SO3Engine/SO3PhysicGraph/CollisionTools.cpp

Go to the documentation of this file.

/******************************************************************************************
MOC - Minimal Ogre Collision v 1.0 beta
The MIT License

Copyright (c) 2008 MouseVolcano (Thomas Gradl, Esa Kylli, Erik Biermann, Karolina Sefyrin)

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
******************************************************************************************/
#include "CollisionTools.h"
#include "../SO3SceneGraph/SO3Entity.h"
#include "../SO3SceneGraph/SO3Scene.h"

MFACEDATA::MFACEDATA()
{
  point     = Vector3::ZERO;
  indexFace = 0;
  v1        = Vector3::ZERO;
  v2        = Vector3::ZERO;
  v3        = Vector3::ZERO;
  pReal1    = Vector2::ZERO;
  pReal2    = Vector2::ZERO;
  pReal3    = Vector2::ZERO;
  uvResult  = Vector2::ZERO;
}

MFACEDATA::MFACEDATA(const MFACEDATA& faceData)
{
  point     = faceData.point;
  indexFace = faceData.indexFace;
  v1        = faceData.v1;
  v2        = faceData.v2;
  v3        = faceData.v3;
  pReal1    = faceData.pReal1;
  pReal2    = faceData.pReal2;
  pReal3    = faceData.pReal3;
  uvResult  = faceData.uvResult;
}

namespace MOC {

#ifdef ETM_TERRAIN
CollisionTools::CollisionTools(Ogre::SceneManager* sceneMgr, const ET::TerrainInfo* terrainInfo)
{
        mRaySceneQuery = sceneMgr->createRayQuery(Ogre::Ray());
    if(NULL == mRaySceneQuery)
    {
      // LOG_ERROR << "Failed to create Ogre::RaySceneQuery instance" << ENDLOG;
      return;
    }
    mRaySceneQuery->setSortByDistance(true);
    
    mTSMRaySceneQuery = NULL;
    
        mTerrainInfo = terrainInfo;
        
        _heightAdjust = 0.0f;
}
#endif

CollisionTools::CollisionTools(Ogre::SceneManager* sceneMgr)
{
        mSceneMgr = sceneMgr;
        
        mRaySceneQuery = mSceneMgr->createRayQuery(Ogre::Ray());
    if(NULL == mRaySceneQuery)
    {
      // LOG_ERROR << "Failed to create Ogre::RaySceneQuery instance" << ENDLOG;
      return;
    }
    mRaySceneQuery->setSortByDistance(true);
    
    mTSMRaySceneQuery =  mSceneMgr->createRayQuery(Ogre::Ray());
    
    _heightAdjust = 0.0f;       
}

CollisionTools::~CollisionTools()
{
        if(mRaySceneQuery != NULL)
                delete mRaySceneQuery;
                
        if(mTSMRaySceneQuery != NULL)
                delete mTSMRaySceneQuery;
}

bool CollisionTools::collidesWithEntity(const Vector3& fromPoint, const Vector3& toPoint, const float collisionRadius, const float rayHeightLevel, const uint32 queryMask)
{
        Vector3 fromPointAdj(fromPoint.x, fromPoint.y + rayHeightLevel, fromPoint.z);
        Vector3 toPointAdj(toPoint.x, toPoint.y + rayHeightLevel, toPoint.z);   
        Vector3 normal = toPointAdj - fromPointAdj;
        float distToDest = normal.normalise();

        Vector3 myResult(0, 0, 0);
        Ogre::Entity* myObject = NULL;
        float distToColl = 0.0f;

        if(raycastFromPoint(fromPointAdj, normal, myResult, (ulong&)myObject, distToColl, queryMask))
        {
                distToColl -= collisionRadius; 
                return (distToColl <= distToDest);
        }
        else
        {
                return false;
        }
}

float CollisionTools::getTSMHeightAt(const float x, const float z) {
        float y=0.0f;

    static Ray updateRay;
    
    updateRay.setOrigin(Vector3(x,9999,z));
    updateRay.setDirection(Vector3::NEGATIVE_UNIT_Y);
    
    mTSMRaySceneQuery->setRay(updateRay);
    RaySceneQueryResult& qryResult = mTSMRaySceneQuery->execute();
    
    RaySceneQueryResult::iterator i = qryResult.begin();
    if(i != qryResult.end() && i->worldFragment)
    {
        y=i->worldFragment->singleIntersection.y;           
    }           
        return y;
}

void CollisionTools::calculateY(SceneNode*n, const bool doTerrainCheck, const bool doGridCheck, const float gridWidth, const uint32 queryMask)
{
        Vector3 pos = n->getPosition();
        
        float x = pos.x;
        float z = pos.z;
        float y = pos.y;
        
        Vector3 myResult(0,0,0);
        Ogre::Entity* myObject=NULL;
        float distToColl = 0.0f;

        float terrY = 0, colY = 0, colY2 = 0;

        if( raycastFromPoint(Vector3(x,y,z),Vector3::NEGATIVE_UNIT_Y,myResult,(ulong&)myObject, distToColl, queryMask)){
                if(myObject != NULL) {
                        colY = myResult.y;
                } else {
                        colY = -99999;
                }
        }
        
        //if doGridCheck is on, repeat not to fall through small holes for example when crossing a hangbridge
        if(doGridCheck) {               
                if( raycastFromPoint(Vector3(x,y,z)+(n->getOrientation()*Vector3(0,0,gridWidth)),Vector3::NEGATIVE_UNIT_Y,myResult,(ulong&)myObject, distToColl, queryMask)){
                        if(myObject != NULL) {
                                colY = myResult.y;
                        } else {
                                colY = -99999;
                        }       
                }
                if(colY<colY2) 
      colY = colY2;
        }
        
        // set the parameter to false if you are not using ETM or TSM
        if(doTerrainCheck) {
        
#ifdef ETM_TERRAIN
                // ETM height value
                terrY = mTerrainInfo->getHeightAt(x,z);
#else   
                // TSM height value
                terrY = getTSMHeightAt(x,z);
#endif

                if(terrY < colY ) {
                        n->setPosition(x,colY+_heightAdjust,z);
                } else {
                        n->setPosition(x,terrY+_heightAdjust,z);
                }
        } else {
                if(!doTerrainCheck && colY == -99999) colY = y;
                n->setPosition(x,colY+_heightAdjust,z);
        }
}

// raycast from a point in to the scene.
// returns success or failure.
// on success the point is returned in the result.
bool CollisionTools::raycastFromPoint(const Vector3 &point,
                                        const Vector3 &normal,
                                                                                Vector3 &result,ulong &target,float &closest_distance,
                                                                                const uint32 queryMask)
{
    // create the ray to test
    static Ogre::Ray ray;
        ray.setOrigin(point);
        ray.setDirection(normal);

        int index = 0;

        return raycast(ray, result, target, closest_distance,index,NULL, queryMask,false);
}

bool CollisionTools::raycast(const Ray &ray, Vector3 &result,ulong &target,float &closest_distance,int &indexSubEntity,MFACEDATA* faceData, const uint32 queryMask,bool extendedInfo)
{
        target = NULL;
        bool priority = false;
  // check we are initialised
  if(mRaySceneQuery != NULL)
  {
    // create a query object
    mRaySceneQuery->setRay(ray);
    mRaySceneQuery->setSortByDistance(true);
    mRaySceneQuery->setQueryMask(queryMask);
    // execute the query, returns a vector of hits
    if(mRaySceneQuery->execute().size() <= 0)
    {
      // raycast did not hit an objects bounding box
      return (false);
    }
  }
  else
  {
    return (false);
  }

        closest_distance = -1.0f;
        float* pReal1;
        float* pReal2;
        float* pReal3;
  Ogre::Vector3 closest_result;
  Ogre::RaySceneQueryResult &query_result = mRaySceneQuery->getLastResults();
  for (size_t qr_idx = 0; qr_idx < query_result.size()&&!priority; qr_idx++)
  {
    if((closest_distance >= 0.0f) &&
       (closest_distance < query_result[qr_idx].distance))
    {
      //break;
    }
    // only check this result if its a hit against an entity
    if((query_result[qr_idx].movable != NULL)  &&
       (query_result[qr_idx].movable->getMovableType().compare("Entity") == 0)) 
    {
      // get the entity to check
      Ogre::Entity* pentity = static_cast<Ogre::Entity*>(query_result[qr_idx].movable);                                         
      bool new_closest_found = false;
      String name = pentity->getName();
                        
      SScene* mScene  = getSceneByName(mSceneMgr->getName());
      if(mScene==NULL)
        return false;

      SEntity* curEntity = static_cast <SEntity*> (mScene->GetNode(name));
      if(curEntity==NULL)
        return false;

      if(curEntity->getSceneNodeIsMouseClick()&&pentity->isVisible())
      {
        int numSubMeshes = pentity->getMesh()->getNumSubMeshes();
        size_t vertex_count;
        size_t index_count;
        Ogre::Vector3* vertices;
        unsigned long* indices;
              for(int i = 0; i<numSubMeshes; i++)
              {
          Ogre::Node* pentityParentNode = pentity->getParentNode();
          Ogre::SubMesh* submesh = pentity->getMesh()->getSubMesh(i);
                      GetSubMeshInformation(curEntity, submesh, i, vertex_count, vertices, index_count, indices,             
                                                  pentityParentNode->_getDerivedPosition(),
                                                  pentityParentNode->_getDerivedOrientation(),
                                                  pentityParentNode->_getDerivedScale());

                      Ogre::RenderOperation tmpRenderOperation;
                      submesh->_getRenderOperation(tmpRenderOperation);

                      if(tmpRenderOperation.operationType == Ogre::RenderOperation::OT_TRIANGLE_LIST) 
                      {
                              for (int j = 0; j < static_cast<int>(index_count); j += 3) 
                              { 
              std::pair<bool, Ogre::Real> hit = Ogre::Math::intersects(ray, vertices[indices[j]], vertices[indices[j+1]], vertices[indices[j+2]], true, false); 
                                      if(hit.first)
                                      {
                                              if(curEntity->getSceneNodeIsMouseForeground()) 
                  closest_distance = -1.0f;
                                        
                if((closest_distance < 0.0f) || (hit.second < closest_distance))
                                              {
                                                      // this is the closest so far, save it off
                                                      closest_distance = hit.second;
                  new_closest_found = true;

                                                      faceData->indexFace = j/3;
                                                      if(faceData!=NULL && extendedInfo)
                                                      {
                                                              faceData->v1 = vertices[indices[j]];
                                                              faceData->v2 = vertices[indices[j+1]];
                                                              faceData->v3 = vertices[indices[j+2]];
                                                              faceData->point = ray.getPoint(hit.second);
                                                              Ogre::VertexData* vertex_data = submesh->useSharedVertices ? submesh->parent->sharedVertexData : submesh->vertexData;
                    const Ogre::VertexElement* vertex_element1 = vertex_data->vertexDeclaration->findElementBySemantic(Ogre::VES_TEXTURE_COORDINATES);
                                                        
                                                              if(vertex_data->vertexDeclaration->getElementCount()>2)
                                                              {
                                                                      Ogre::HardwareVertexBufferSharedPtr vbuf1 = vertex_data->vertexBufferBinding->getBuffer(vertex_element1->getSource());
                                                                      unsigned char* vertex1 = static_cast<unsigned char*>(vbuf1->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
                                                                      vertex1 += indices[j]*vbuf1->getVertexSize();
                                                                      vertex_element1->baseVertexPointerToElement(vertex1, &pReal1);
                                                                      vbuf1->unlock();

                                                                      const Ogre::VertexElement* vertex_element2 = vertex_data->vertexDeclaration->findElementBySemantic(Ogre::VES_TEXTURE_COORDINATES);
                                                                      Ogre::HardwareVertexBufferSharedPtr vbuf2 = vertex_data->vertexBufferBinding->getBuffer(vertex_element2->getSource());
                                                                      unsigned char* vertex2 = static_cast<unsigned char*>(vbuf2->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
                                                                      vertex2 += indices[j+1]*vbuf2->getVertexSize();
                                                                      vertex_element2->baseVertexPointerToElement(vertex2, &pReal2);
                                                                      vbuf2->unlock();

                                                                      const Ogre::VertexElement* vertex_element3 = vertex_data->vertexDeclaration->findElementBySemantic(Ogre::VES_TEXTURE_COORDINATES);
                                                                      Ogre::HardwareVertexBufferSharedPtr vbuf3 = vertex_data->vertexBufferBinding->getBuffer(vertex_element3->getSource());
                                                                      unsigned char* vertex3 = static_cast<unsigned char*>(vbuf3->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
                                                                      vertex3 += indices[j+2]*vbuf3->getVertexSize();
                                                                      vertex_element3->baseVertexPointerToElement(vertex3, &pReal3);
                                                                      vbuf3->unlock();

                                                                      faceData->pReal1.x = pReal1[0];
                                                                      faceData->pReal1.y = pReal1[1];
                                                                      faceData->pReal2.x = pReal2[0];
                                                                      faceData->pReal2.y = pReal2[1];
                                                                      faceData->pReal3.x = pReal3[0];
                                                                      faceData->pReal3.y = pReal3[1];
                                                        
                                                                      Ogre::Vector3 d1 = faceData->point - faceData->v1; // Distance au vertex1
                                                                      Ogre::Vector3 d2 = faceData->point - faceData->v2; // Distance au vertex2
                                                                      Ogre::Vector3 d3 = faceData->point - faceData->v3; // Distance au vertex3

                                                                      Ogre::Vector3 d12 =  faceData->v1 - faceData->v2;
                                                                      Ogre::Vector3 d13 =  faceData->v1 - faceData->v3;
                                                                      Ogre::Vector3 d23 =  faceData->v2 - faceData->v3;

                                                                      Vector3 B = GetBarycentricCoordinates(faceData->v1, faceData->v2, faceData->v3, faceData->point);
                                                                      Vector2 T = faceData->pReal1*B.x + faceData->pReal2*B.y + faceData->pReal3*B.z;

                                                                      faceData->uvResult.x = abs(T.x) - abs((int)T.x);
                                                                      faceData->uvResult.y = abs(T.y) - abs((int)T.y);

                                                                      if(T.x<0)
                        faceData->uvResult.x = 1 - faceData->uvResult.x;
                                                                      if(T.y<0) 
                        faceData->uvResult.y = 1 - faceData->uvResult.y;

                                                                      if(curEntity->getSceneNodeIsMouseForeground())
                        priority = true;
                                                              }
                                                      }
                                              }
                                      }
                              }
                      }
                      delete[] vertices;
                      delete[] indices;
              }
      }

      if(new_closest_found)
      {
              target = (ulong)pentity;
              closest_result = ray.getPoint(closest_distance);               
      }
    }
  }

  // return the result
  if(closest_distance >= 0.0f)
  {
      // raycast success
        result = closest_result;
      return (true);
  }
  else
  {
      // raycast failed
      return (false);
  } 
}
Vector3 CollisionTools::GetBarycentricCoordinates(const Vector3 &P1, const Vector3 &P2, const Vector3 &P3, const Vector3 &P)
{
        Vector3 Coordinates(0.0);
        Real denom = (-P1.x * P3.y - P2.x * P1.y + P2.x * P3.y + P1.y * P3.x + P2.y * P1.x - P2.y * P3.x);
        if(fabs(denom) >= 1e-6) 
        {
                Coordinates.x = (P2.x * P3.y - P2.y * P3.x - P.x * P3.y + P3.x * P.y - P2.x * P.y + P2.y * P.x) / denom;
                Coordinates.y = -(-P1.x * P.y + P1.x * P3.y + P1.y * P.x - P.x * P3.y + P3.x * P.y - P1.y * P3.x) / denom;
        }
        else
        {
                denom = (-P1.x * P3.z - P2.x * P1.z + P2.x * P3.z + P1.z * P3.x + P2.z * P1.x - P2.z * P3.x);
                if(fabs(denom) >= 1e-6) 
                {
                        Coordinates.x = (P2.x * P3.z - P2.z * P3.x - P.x * P3.z + P3.x * P.z - P2.x * P.z + P2.z * P.x) / denom;
                        Coordinates.y = -(-P1.x * P.z + P1.x * P3.z + P1.z * P.x - P.x * P3.z + P3.x * P.z - P1.z * P3.x) / denom;
                }
                else
                {
                        denom = (-P1.y * P3.z - P2.y * P1.z + P2.y * P3.z + P1.z * P3.y + P2.z * P1.y - P2.z * P3.y);
                        if(fabs(denom) >= 1e-6) 
                        {
                                Coordinates.x = (P2.y * P3.z - P2.z * P3.y - P.y * P3.z + P3.y * P.z - P2.y * P.z + P2.z * P.y) / denom;
                                Coordinates.y = -(-P1.y * P.z + P1.y * P3.z + P1.z * P.y - P.y * P3.z + P3.y * P.z - P1.z * P3.y) / denom;
                        }
                }
        }
        Coordinates.z = 1 - Coordinates.x - Coordinates.y;
        return Coordinates;
}
// Get the mesh information for the given mesh.
// Code found on this forum link: http://www.ogre3d.org/wiki/index.php/RetrieveVertexData

void CollisionTools::GetMeshInformation(const Entity* mEntity,
                                size_t &vertex_count,
                                Ogre::Vector4* &vertices,
                                size_t &index_count,
                                unsigned long* &indices,
                                const Ogre::Vector3 &position,
                                const Ogre::Quaternion &orient,
                                const Ogre::Vector3 &scale)
{
    bool added_shared = false;
    size_t current_offset = 0;
    size_t shared_offset = 0;
    size_t next_offset = 0;
    size_t index_offset = 0;

    vertex_count = index_count = 0;

        Ogre::MeshPtr mesh = mEntity->getMesh();

        bool useSoftwareBlendingVertices = mEntity->hasSkeleton();
        bool useSoftwareMorphingVertices = mEntity->hasVertexAnimation();

    // Calculate how many vertices and indices we're going to need
    for (unsigned short i = 0; i < mesh->getNumSubMeshes(); ++i)
    {
        Ogre::SubMesh* submesh = mesh->getSubMesh( i );
        // We only need to add the shared vertices once
        if(submesh->useSharedVertices)
        {
            if( !added_shared )
            {
                vertex_count += mesh->sharedVertexData->vertexCount;
                added_shared = true;
            }
        }
        else
        {
            vertex_count += submesh->vertexData->vertexCount;
        }
        // Add the indices
        index_count += submesh->indexData->indexCount;
    }
    // Allocate space for the vertices and indices
    vertices = new Ogre::Vector4[vertex_count];
    indices = new unsigned long[index_count];

    added_shared = false;

    // Run through the sub meshes again, adding the data into the arrays
    for ( unsigned short i = 0; i < mesh->getNumSubMeshes(); ++i)
    {
        Ogre::SubMesh* submesh = mesh->getSubMesh(i);

        //Ogre::VertexData* vertex_data = submesh->useSharedVertices ? mesh->sharedVertexData : submesh->vertexData;
                Ogre::VertexData* vertex_data;

                //When there is animation:
                if(useSoftwareBlendingVertices&&useSoftwareMorphingVertices)
                        vertex_data = submesh->useSharedVertices ? mEntity->_getSkelAnimVertexData() : mEntity->getSubEntity(i)->_getSkelAnimVertexData();
                else if(!useSoftwareBlendingVertices&&useSoftwareMorphingVertices)
                        vertex_data = submesh->useSharedVertices ? mEntity->_getSoftwareVertexAnimVertexData() : mEntity->getSubEntity(i)->_getSoftwareVertexAnimVertexData();
                else
                        vertex_data = submesh->useSharedVertices ? mesh->sharedVertexData : submesh->vertexData;

        if((!submesh->useSharedVertices)||(submesh->useSharedVertices && !added_shared))
        {
            if(submesh->useSharedVertices)
            {
                added_shared = true;
                shared_offset = current_offset;
            }

            const Ogre::VertexElement* posElem =
                vertex_data->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION);

            Ogre::HardwareVertexBufferSharedPtr vbuf =
                vertex_data->vertexBufferBinding->getBuffer(posElem->getSource());

            unsigned char* vertex =
                static_cast<unsigned char*>(vbuf->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));

            float* pReal;

            for( size_t j = 0; j < vertex_data->vertexCount; ++j, vertex += vbuf->getVertexSize())
            {
                posElem->baseVertexPointerToElement(vertex, &pReal);

                Ogre::Vector3 pt(pReal[0], pReal[1], pReal[2]);

                vertices[current_offset + j] = (orient * (pt * scale)) + position;
                                vertices[current_offset + j].w = (float)(i);
            }

            vbuf->unlock();
            next_offset += vertex_data->vertexCount;
        }


        Ogre::IndexData* index_data = submesh->indexData;
        size_t numTris = index_data->indexCount / 3;
        Ogre::HardwareIndexBufferSharedPtr ibuf = index_data->indexBuffer;

        bool use32bitindexes = (ibuf->getType() == Ogre::HardwareIndexBuffer::IT_32BIT);

        unsigned long*  pLong = static_cast<unsigned long*>(ibuf->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
        unsigned short* pShort = reinterpret_cast<unsigned short*>(pLong);


        size_t offset = (submesh->useSharedVertices)? shared_offset : current_offset;

        if( use32bitindexes )
        {
            for ( size_t k = 0; k < numTris*3; ++k)
            {
                indices[index_offset++] = pLong[k] + static_cast<unsigned long>(offset);
            }
        }
        else
        {
            for ( size_t k = 0; k < numTris*3; ++k)
            {
                indices[index_offset++] = static_cast<unsigned long>(pShort[k]) +
                    static_cast<unsigned long>(offset);
            }
        }

        ibuf->unlock();
        current_offset = next_offset;
    }
}


void CollisionTools::GetSubMeshInformation(SEntity* mEntity,
                                                                        Ogre::SubMesh* submesh,
                                                                        int i,
                                                                        size_t &vertex_count,
                                                                        Ogre::Vector3* &vertices,
                                                                        size_t &index_count,
                                                                        unsigned long* &indices,
                                                                        const Ogre::Vector3 &position,
                                                                        const Ogre::Quaternion &orient,
                                                                        const Ogre::Vector3 &scale)
{
        bool added_shared = false;
        size_t current_offset = 0;
        size_t shared_offset = 0;
        size_t next_offset = 0;
        size_t index_offset = 0;
        vertex_count = index_count = 0;

        Ogre::MeshPtr mesh = mEntity->getOgreEntityPointer()->getMesh();

        bool useSoftwareBlendingVertices = mEntity->getOgreEntityPointer()->hasSkeleton();
        bool useSoftwareMorphingVertices = mEntity->getOgreEntityPointer()->hasVertexAnimation();


        // We only need to add the shared vertices once
        if(submesh->useSharedVertices)
        {
                if(!added_shared)
                {
                        vertex_count += mesh->sharedVertexData->vertexCount;
                        added_shared = true;
                }
        }
        else
        {
                vertex_count += submesh->vertexData->vertexCount;
        }

        // Add the indices
        index_count += submesh->indexData->indexCount;

        vertices = new Ogre::Vector3[vertex_count];
        indices = new unsigned long[index_count];

        added_shared = false;

        Ogre::VertexData* vertex_data;
        if(useSoftwareBlendingVertices&&mEntity->getOgreEntityPointer()->_isAnimated())
  {
                vertex_data = submesh->useSharedVertices ? mEntity->getOgreEntityPointer()->_getSkelAnimVertexData() : mEntity->getOgreEntityPointer()->getSubEntity(i)->_getSkelAnimVertexData();
  }
        else if(!useSoftwareBlendingVertices&&useSoftwareMorphingVertices&&mEntity->getOgreEntityPointer()->_isAnimated()&&mEntity->getOgreEntityPointer()->getMesh()->getPoseCount()==0)
                vertex_data = submesh->useSharedVertices ? mEntity->getOgreEntityPointer()->_getSoftwareVertexAnimVertexData() : mEntity->getOgreEntityPointer()->getSubEntity(i)->_getSoftwareVertexAnimVertexData();
        else
                vertex_data = submesh->useSharedVertices ? mesh->sharedVertexData : submesh->vertexData;

        if((!submesh->useSharedVertices)||(submesh->useSharedVertices && !added_shared))
        {
                if(submesh->useSharedVertices)
                {
                        added_shared = true;
                        shared_offset = current_offset;
                }
                const Ogre::VertexElement * posElem =
                        vertex_data->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION);

                Ogre::HardwareVertexBufferSharedPtr vbuf =
                        vertex_data->vertexBufferBinding->getBuffer(posElem->getSource());

                unsigned char* vertex =
                        static_cast<unsigned char*>(vbuf->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));

                float* pReal;
                for( size_t j = 0; j < vertex_data->vertexCount; ++j, vertex += vbuf->getVertexSize())
                {
                        posElem->baseVertexPointerToElement(vertex, &pReal);

                        Ogre::Vector3 pt(pReal[0], pReal[1], pReal[2]);

                        vertices[current_offset + j] = (orient * (pt * scale)) + position;
                }

                vbuf->unlock();
                next_offset += vertex_data->vertexCount;
        }
        Ogre::IndexData* index_data = submesh->indexData;
        size_t numTris = index_data->indexCount / 3;
        Ogre::HardwareIndexBufferSharedPtr ibuf = index_data->indexBuffer;

        bool use32bitindexes = (ibuf->getType() == Ogre::HardwareIndexBuffer::IT_32BIT);

        unsigned long*  pLong = static_cast<unsigned long*>(ibuf->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
        unsigned short* pShort = reinterpret_cast<unsigned short*>(pLong);


        size_t offset = (submesh->useSharedVertices)? shared_offset : current_offset;

        if( use32bitindexes )
        {
                for ( size_t k = 0; k < numTris*3; ++k)
                {
                        indices[index_offset++] = pLong[k] + static_cast<unsigned long>(offset);
                }
        }
        else
        {
                for ( size_t k = 0; k < numTris*3; ++k)
                {
                        indices[index_offset++] = static_cast<unsigned long>(pShort[k]) +
                                static_cast<unsigned long>(offset);
                }
        }

        ibuf->unlock();
        current_offset = next_offset;
        // }
} 
void CollisionTools::setHeightAdjust(const float heightadjust) {
        _heightAdjust = heightadjust;
}

float CollisionTools::getHeightAdjust(void) {
        return _heightAdjust;
}

};