OgreNewt_CollisionPrimitives.cpp

Go to the documentation of this file.

#include "External/ogrenewt/OgreNewt_Stdafx.h"
#include "External/ogrenewt/OgreNewt_CollisionPrimitives.h"
#include "External/ogrenewt/OgreNewt_Tools.h"
#include "External/ogrenewt/OgreNewt_RayCast.h"
#include "External/ogrenewt/OgreNewt_World.h"
#include "External/ogrenewt/OgreNewt_CollisionSerializer.h"
#include <OgreEntity.h>
#include <OgreSubMesh.h>
#include <OgreSceneNode.h>
 
namespace OgreNewt
{
 
  namespace CollisionPrimitives
  {
 
    // OgreNewt::CollisionPrimitives::Null
    Null::Null(const OgreNewt::World *world) : Collision(world)
    {
      m_col = NewtonCreateNull(m_world->getNewtonWorld());
    }
 
 
    // OgreNewt::CollisionPrimitives::Box
    Box::Box(const World* world) : ConvexCollision(world)
    {}
 
    Box::Box(const World* world, const Ogre::Vector3& size, int id, const Ogre::Quaternion& orient, const Ogre::Vector3& pos) : ConvexCollision(world)
    {
      dFloat matrix[16];
      OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
 
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      m_col = NewtonCreateBox(m_world->getNewtonWorld(), dFloat(size.x), dFloat(size.y), dFloat(size.z), id, &matrix[0]);
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
    }
 
 
 
    // OgreNewt::CollisionPrimitives::Ellipsoid
    Ellipsoid::Ellipsoid(const World* world) : ConvexCollision(world)
    {
    }
 
    Ellipsoid::Ellipsoid(const World* world, const Ogre::Vector3& size, int id, const Ogre::Quaternion& orient, const Ogre::Vector3& pos) : ConvexCollision(world)
    {
      dFloat matrix[16];
      OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
 
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      NewtonCollision* sphere_col = 0;
      if ((size.x == size.y) && (size.y == size.z))
        m_col = NewtonCreateSphere(m_world->getNewtonWorld(), (float)size.x, id, &matrix[0]);
      else
      {
        float radius = std::min(std::min(size.x, size.y), size.z);
        Ogre::Vector3 scale = Ogre::Vector3(size.x / radius, size.y / radius, size.z / radius);
        m_col = NewtonCreateSphere(m_world->getNewtonWorld(), dFloat(radius), id, &matrix[0]);
        NewtonCollisionSetScale(m_col, dFloat(scale.x), dFloat(scale.y), dFloat(scale.z));
      }
 
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
    }
 
    // OgreNewt::CollisionPrimitives::Cylinder
    Cylinder::Cylinder(const World* world) : ConvexCollision(world)
    {}
 
    Cylinder::Cylinder(const World* world, Ogre::Real radius, Ogre::Real height, int id,
      const Ogre::Quaternion& orient, const Ogre::Vector3& pos) : ConvexCollision(world)
    {
      dFloat matrix[16];
      OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
 
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      m_col = NewtonCreateCylinder(m_world->getNewtonWorld(), dFloat(radius), dFloat(height), id, &matrix[0]);
 
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
    }
 
 
    // OgreNewt::CollisionPrimitives::Capsule
    Capsule::Capsule(const World* world) : ConvexCollision(world)
    {}
 
    Capsule::Capsule(const World* world, Ogre::Real radius, Ogre::Real height, int id,
      const Ogre::Quaternion& orient, const Ogre::Vector3& pos) : ConvexCollision(world)
    {
      dFloat matrix[16];
      OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
 
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      m_col = NewtonCreateCapsule(m_world->getNewtonWorld(), dFloat(radius), dFloat((height - (radius * 2.0))), id, &matrix[0]);
 
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
    }
 
 
    // OgreNewt::CollisionPrimitives::Cone
    Cone::Cone(const World* world) : ConvexCollision(world)
    {}
 
    Cone::Cone(const World* world, Ogre::Real radius, Ogre::Real height, int id,
      const Ogre::Quaternion& orient, const Ogre::Vector3& pos) : ConvexCollision(world)
    {
      dFloat matrix[16];
      OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
 
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      m_col = NewtonCreateCone(m_world->getNewtonWorld(), dFloat(radius), dFloat(height), id, &matrix[0]);
 
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
    }
 
    // OgreNewt::CollisionPrimitives::ChamferCylinder
    ChamferCylinder::ChamferCylinder(const World* world) : ConvexCollision(world)
    {}
 
    ChamferCylinder::ChamferCylinder(const World* world, Ogre::Real radius, Ogre::Real height, int id,
      const Ogre::Quaternion& orient, const Ogre::Vector3& pos) : ConvexCollision(world)
    {
      dFloat matrix[16];
      OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
 
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      m_col = NewtonCreateChamferCylinder(m_world->getNewtonWorld(), dFloat(radius), dFloat(height), id, &matrix[0]);
 
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
    }
 
 
 
    // OgreNewt::CollisionPrimitives::ConvexHull
    ConvexHull::ConvexHull(const World* world) : ConvexCollision(world)
    {}
 
    ConvexHull::ConvexHull(const World* world, Ogre::Entity* obj, int id, const Ogre::Quaternion& orient, const Ogre::Vector3& pos, Ogre::Real tolerance, const Ogre::Vector3& scale) : ConvexCollision(world)
    {
      //get the mesh!
      //Ogre::Entity* obj = (Ogre::Entity*)node->getAttachedObject(0);
      Ogre::MeshPtr mesh = obj->getMesh();
 
      //find number of submeshes
      unsigned short sub = mesh->getNumSubMeshes();
 
      size_t total_verts = 0;
 
      Ogre::VertexData* v_data;
      bool addedShared = false;
 
      for (unsigned short i = 0; i < sub; i++)
      {
        Ogre::SubMesh* sub_mesh = mesh->getSubMesh(i);
        if (sub_mesh->useSharedVertices)
        {
          if (!addedShared)
          {
            v_data = mesh->sharedVertexData;
            total_verts += v_data->vertexCount;
 
            addedShared = true;
          }
        }
        else
        {
          v_data = sub_mesh->vertexData;
          total_verts += v_data->vertexCount;
        }
      }
 
      addedShared = false;
 
      //make array to hold vertex positions!
      Ogre::Vector3* vertices = new Ogre::Vector3[total_verts];
      unsigned int offset = 0;
 
      //loop back through, adding vertices as we go!
      for (unsigned short i = 0; i < sub; i++)
      {
        Ogre::SubMesh* sub_mesh = mesh->getSubMesh(i);
        Ogre::VertexDeclaration* v_decl;
        const Ogre::VertexElement* p_elem;
        float* v_Posptr;
        size_t v_count;
 
        v_data = NULL;
 
        if (sub_mesh->useSharedVertices)
        {
          if (!addedShared)
          {
            v_data = mesh->sharedVertexData;
            v_count = v_data->vertexCount;
            v_decl = v_data->vertexDeclaration;
            p_elem = v_decl->findElementBySemantic(Ogre::VES_POSITION);
            addedShared = true;
          }
        }
        else
        {
          v_data = sub_mesh->vertexData;
          v_count = v_data->vertexCount;
          v_decl = v_data->vertexDeclaration;
          p_elem = v_decl->findElementBySemantic(Ogre::VES_POSITION);
        }
 
        if (v_data)
        {
          size_t start = v_data->vertexStart;
          //pointer
          Ogre::HardwareVertexBufferSharedPtr v_sptr = v_data->vertexBufferBinding->getBuffer(p_elem->getSource());
          unsigned char* v_ptr = static_cast<unsigned char*>(v_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
          unsigned char* v_offset;
 
          //loop through vertex data...
          for (size_t j = start; j < (start + v_count); j++)
          {
            //get offset to Position data!
            v_offset = v_ptr + (j * v_sptr->getVertexSize());
            p_elem->baseVertexPointerToElement(v_offset, &v_Posptr);
 
            //now get vertex positions...
            vertices[offset].x = *v_Posptr; v_Posptr++;
            vertices[offset].y = *v_Posptr; v_Posptr++;
            vertices[offset].z = *v_Posptr; v_Posptr++;
 
            vertices[offset] *= scale;
            offset++;
          }
 
          //unlock buffer
          v_sptr->unlock();
        }
      }
 
      dFloat matrix[16];
 
      OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
 
      //okay, let's try making the ConvexHull!
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      m_col = NewtonCreateConvexHull(m_world->getNewtonWorld(), (int)total_verts, (float*)&vertices[0].x, sizeof(Ogre::Vector3), tolerance, id, &matrix[0]);
 
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
 
      delete[]vertices;
 
    }
 
 
    // OgreNewt::CollisionPrimitives::ConvexHull
    ConvexHull::ConvexHull(const World* world, const Ogre::Vector3* verts, int vertcount, int id, const Ogre::Quaternion& orient, const Ogre::Vector3& pos, Ogre::Real tolerance) : ConvexCollision(world)
    {
      dFloat matrix[16];
      OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
 
      //make the collision primitive.
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      m_col = NewtonCreateConvexHull(m_world->getNewtonWorld(), vertcount, (float*)&verts[0].x, sizeof(Ogre::Vector3), tolerance, id, &matrix[0]);
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
    }
 
 
    // OgreNewt::CollisionPrimitives::ConcaveHull
    ConcaveHull::ConcaveHull(const World* world) : ConvexCollision(world)
    {}
 
    ConcaveHull::ConcaveHull(const World* world, Ogre::Entity* obj, int id, Ogre::Real tolerance, const Ogre::Vector3& scale) : ConvexCollision(world)
    {
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      //first create a mesh
      NewtonMesh* const nmesh = NewtonMeshCreate(m_world->getNewtonWorld());
      NewtonMeshBeginFace(nmesh);
 
      Ogre::MeshPtr mesh = obj->getMesh();
      //find number of sub-meshes
      unsigned short sub = mesh->getNumSubMeshes();
 
      for (unsigned short cs = 0; cs < sub; cs++)
      {
        Ogre::SubMesh* sub_mesh = mesh->getSubMesh(cs);
 
        //vertex data!
        Ogre::VertexData* v_data;
 
        if (sub_mesh->useSharedVertices)
        {
          v_data = mesh->sharedVertexData;
        }
        else
        {
          v_data = sub_mesh->vertexData;
        }
 
        //let's find more information about the Vertices...
        Ogre::VertexDeclaration* v_decl = v_data->vertexDeclaration;
        const Ogre::VertexElement* p_elem = v_decl->findElementBySemantic(Ogre::VES_POSITION);
 
        // get pointer!
        Ogre::HardwareVertexBufferSharedPtr v_sptr = v_data->vertexBufferBinding->getBuffer(p_elem->getSource());
        unsigned char* v_ptr = static_cast<unsigned char*>(v_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
 
        //now find more about the index!!
        Ogre::IndexData* i_data = sub_mesh->indexData;
        size_t index_count = i_data->indexCount;
        size_t poly_count = index_count / 3;
 
        // get pointer!
        Ogre::HardwareIndexBufferSharedPtr i_sptr = i_data->indexBuffer;
 
        // 16 or 32 bit indices?
        bool uses32bit = (i_sptr->getType() == Ogre::HardwareIndexBuffer::IT_32BIT);
        Ogre::uint32* i_Longptr;
        Ogre::uint16* i_Shortptr;
 
        if (uses32bit)
        {
          i_Longptr = static_cast<Ogre::uint32*>(i_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
 
        }
        else
        {
          i_Shortptr = static_cast<Ogre::uint16*>(i_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
        }
 
 
        //now loop through the indices, getting polygon info!
        int i_offset = 0;
 
        for (size_t i = 0; i < poly_count; i++)
        {
          Ogre::Vector3 poly_verts[3];
          unsigned char* v_offset;
          float* v_Posptr;
          int idx;
 
          if (uses32bit)
          {
            for (int j = 0; j < 3; j++)
            {
              idx = i_Longptr[i_offset + j];        // index to first vertex!
              v_offset = v_ptr + (idx * v_sptr->getVertexSize());
              p_elem->baseVertexPointerToElement(v_offset, &v_Posptr);
              //now get vertex position from v_Posptr!
              poly_verts[j].x = *v_Posptr; v_Posptr++;
              poly_verts[j].y = *v_Posptr; v_Posptr++;
              poly_verts[j].z = *v_Posptr; v_Posptr++;
 
              poly_verts[j] *= scale;
            }
          }
          else
          {
            for (int j = 0; j < 3; j++)
            {
              idx = i_Shortptr[i_offset + j];       // index to first vertex!
              v_offset = v_ptr + (idx * v_sptr->getVertexSize());
              p_elem->baseVertexPointerToElement(v_offset, &v_Posptr);
              //now get vertex position from v_Posptr!
 
              // switch poly winding.
              poly_verts[j].x = *v_Posptr; v_Posptr++;
              poly_verts[j].y = *v_Posptr; v_Posptr++;
              poly_verts[j].z = *v_Posptr; v_Posptr++;
 
              poly_verts[j] *= scale;
            }
          }
          NewtonMeshAddFace(nmesh, 3, (float*)&poly_verts[0].x, sizeof(Ogre::Vector3), cs);
 
          i_offset += 3;
        }
 
        //unlock the buffers!
        v_sptr->unlock();
        i_sptr->unlock();
 
      }
      //done!
      NewtonMeshEndFace(nmesh);
      NewtonMeshFixTJoints(nmesh);
      NewtonMesh* const omesh = NewtonMeshApproximateConvexDecomposition(nmesh, 0.5f, 0.1f, 10, 1000, NULL, NULL);
 
      if (nmesh)
        NewtonMeshDestroy(nmesh);
 
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      m_col = NewtonCreateCompoundCollisionFromMesh(m_world->getNewtonWorld(), omesh, tolerance, id, id);
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
 
      if (omesh)
        NewtonMeshDestroy(omesh);
 
      //CollisionSerializer serializer = CollisionSerializer();
      //serializer.exportCollision(OgreNewt::CollisionPtr(this), std::string("d:/") + (obj->getName()));
    }
 
    // OgreNewt::CollisionPrimitives::ConcaveHull
    ConcaveHull::ConcaveHull(const World* world, const Ogre::Vector3* verts, int vertcount, int id, Ogre::Real tolerance) : ConvexCollision(world)
    {
    }
 
 
    TreeCollision::TreeCollision(const World* world) : Collision(world)
    {
    }
 
 
    TreeCollision::TreeCollision(const World* world, Ogre::Entity* obj, bool optimize, int id, FaceWinding fw, const Ogre::Vector3& scale) : Collision(world)
    {
      start(id);
 
      Ogre::MeshPtr mesh = obj->getMesh();
 
      //find number of sub-meshes
      unsigned short sub = mesh->getNumSubMeshes();
      int countConvexFaces = 0;
 
      for (unsigned short cs = 0; cs < sub; cs++)
      {
        Ogre::SubMesh* sub_mesh = mesh->getSubMesh(cs);
 
        //vertex data!
        Ogre::VertexData* v_data;
 
        if (sub_mesh->useSharedVertices)
        {
          v_data = mesh->sharedVertexData;
        }
        else
        {
          v_data = sub_mesh->vertexData;
        }
 
        //let's find more information about the Vertices...
        Ogre::VertexDeclaration* v_decl = v_data->vertexDeclaration;
        const Ogre::VertexElement* p_elem = v_decl->findElementBySemantic(Ogre::VES_POSITION);
 
        // get pointer!
        Ogre::HardwareVertexBufferSharedPtr v_sptr = v_data->vertexBufferBinding->getBuffer(p_elem->getSource());
        unsigned char* v_ptr = static_cast<unsigned char*>(v_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
 
        //now find more about the index!!
        Ogre::IndexData* i_data = sub_mesh->indexData;
        size_t index_count = i_data->indexCount;
        size_t poly_count = index_count / 3;
 
        // get pointer!
        Ogre::HardwareIndexBufferSharedPtr i_sptr = i_data->indexBuffer;
 
        // 16 or 32 bit indices?
        bool uses32bit = (i_sptr->getType() == Ogre::HardwareIndexBuffer::IT_32BIT);
        Ogre::uint32* i_Longptr;
        Ogre::uint16* i_Shortptr;
 
        if (uses32bit)
        {
          i_Longptr = static_cast<Ogre::uint32*>(i_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
 
        }
        else
        {
          i_Shortptr = static_cast<Ogre::uint16*>(i_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
        }
        
        //now loop through the indices, getting polygon info!
        int i_offset = 0;
 
        for (size_t i = 0; i < poly_count; i++)
        {
          Ogre::Vector3 poly_verts[3];
          unsigned char* v_offset;
          float* v_Posptr;
          int idx;
 
          if (uses32bit)
          {
            for (int j = 0; j < 3; j++)
            {
              idx = i_Longptr[i_offset + j];        // index to first vertex!
              v_offset = v_ptr + (idx * v_sptr->getVertexSize());
              p_elem->baseVertexPointerToElement(v_offset, &v_Posptr);
              //now get vertex position from v_Posptr!
              poly_verts[j].x = *v_Posptr; v_Posptr++;
              poly_verts[j].y = *v_Posptr; v_Posptr++;
              poly_verts[j].z = *v_Posptr; v_Posptr++;
 
              poly_verts[j] *= scale;
            }
          }
          else
          {
            for (int j = 0; j < 3; j++)
            {
              idx = i_Shortptr[i_offset + j];       // index to first vertex!
              v_offset = v_ptr + (idx * v_sptr->getVertexSize());
              p_elem->baseVertexPointerToElement(v_offset, &v_Posptr);
              //now get vertex position from v_Posptr!
 
              // switch poly winding.
              poly_verts[j].x = *v_Posptr; v_Posptr++;
              poly_verts[j].y = *v_Posptr; v_Posptr++;
              poly_verts[j].z = *v_Posptr; v_Posptr++;
 
              poly_verts[j] *= scale;
            }
          }
 
          if (fw == FW_DEFAULT)
          {
            addPoly(poly_verts, cs);
          }
          else
          {
            Ogre::Vector3 rev_poly_verts[3];
            rev_poly_verts[0] = poly_verts[0];
            rev_poly_verts[0] = poly_verts[2];
            rev_poly_verts[0] = poly_verts[1];
 
            addPoly(rev_poly_verts, cs);
          }
 
          //check concativity the same way newton do to add tree collision faces
          bool convexface = true;
          Ogre::Vector3 p0(poly_verts[2]);
          for (int k = 0; k < 3; k++)
          {
            Ogre::Vector3 p1(poly_verts[k]);
            Ogre::Vector3 edge(p1 - p0);
            float mag2 = edge.dotProduct(edge);
            if (mag2 < 1.0e-6f)
              convexface = false;
            p0 = p1;
          }
 
          if (convexface)
          {
            Ogre::Vector3 edge0(poly_verts[2] - poly_verts[0]);
            Ogre::Vector3 edge1(poly_verts[1] - poly_verts[0]);
            Ogre::Vector3 normal(edge0 * edge1);
            float mag2 = normal.dotProduct(normal);
            if ((mag2 != 0.0) && (mag2 < 1.0e-7f))
              convexface = false;
          }
 
          if (convexface)
            countConvexFaces++;
 
          i_offset += 3;
        }
 
        //unlock the buffers!
        v_sptr->unlock();
        i_sptr->unlock();
 
      }
 
      //done!
      if (countConvexFaces)
        finish(optimize);
      else
      {
        //failed!
        if (m_col)
        {
          NewtonDestroyCollision(m_col);
          m_col = 0;
        }
        
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
      }
 
      //CollisionSerializer serializer = CollisionSerializer();
      //serializer.exportCollision(OgreNewt::CollisionPtr(this), std::string("d:/") + (obj->getName()));
    }
 
    TreeCollision::TreeCollision(const World* world, const Ogre::SceneNode* node, bool optimize, int id, FaceWinding fw, const Ogre::Vector3& scale) : Collision(world)
    {
      start(id);
      AddEntities(node, node, scale, fw);
      finish(optimize);
    }
 
    TreeCollision::TreeCollision(const OgreNewt::World *world, int numVertices, int numIndices, const float *vertices, const int *indices, bool optimize, int id, FaceWinding fw) : OgreNewt::Collision(world)
    {
      start(id);
 
      int numPolys = numIndices / 3;
 
      Ogre::Vector3 *vecVertices = new Ogre::Vector3[numVertices];
 
      for (int curVertex = 0; curVertex < numVertices; curVertex++)
      {
        vecVertices[curVertex].x = vertices[0 + curVertex * 3];
        vecVertices[curVertex].y = vertices[1 + curVertex * 3];
        vecVertices[curVertex].z = vertices[2 + curVertex * 3];
      }
 
      for (int poly = 0; poly < numPolys; poly++)
      {
        Ogre::Vector3 poly_verts[3];
 
        if (fw == FW_DEFAULT)
        {
          poly_verts[0] = vecVertices[indices[0 + poly * 3]];
          poly_verts[1] = vecVertices[indices[1 + poly * 3]];
          poly_verts[2] = vecVertices[indices[2 + poly * 3]];
        }
        else
        {
          poly_verts[0] = vecVertices[indices[0 + poly * 3]];
          poly_verts[2] = vecVertices[indices[1 + poly * 3]];
          poly_verts[1] = vecVertices[indices[2 + poly * 3]];
        }
 
        addPoly(poly_verts, 0);
      }
 
      delete[] vecVertices;
 
      finish(optimize);
    }
 
 
    TreeCollision::TreeCollision(const World* world, int numVertices, Ogre::Vector3* vertices, Ogre::IndexData* indexData, bool optimize, int id, FaceWinding fw) : Collision(world)
    {
      start(id);
 
      unsigned int numPolys = indexData->indexCount / 3;
      Ogre::HardwareIndexBufferSharedPtr hwIndexBuffer = indexData->indexBuffer;
      size_t indexSize = hwIndexBuffer->getIndexSize();
      void* indices = hwIndexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY);
 
      assert((indexSize == 2) || (indexSize == 4));
 
      if (indexSize == 2)
      {
        unsigned short* curIndex = (unsigned short*)indices;
        for (unsigned int poly = 0; poly < numPolys; poly++)
        {
          Ogre::Vector3 poly_verts[3];
 
          //invert vertex winding (otherwise, raycasting won't work???)
          if (fw == FW_DEFAULT)
          {
            poly_verts[0] = vertices[*curIndex]; curIndex++;
            poly_verts[1] = vertices[*curIndex]; curIndex++;
            poly_verts[2] = vertices[*curIndex]; curIndex++;
          }
          else
          {
            poly_verts[0] = vertices[*curIndex]; curIndex++;
            poly_verts[2] = vertices[*curIndex]; curIndex++;
            poly_verts[1] = vertices[*curIndex]; curIndex++;
          }
 
          addPoly(poly_verts, 0);
        }
      }
      else
      {
        unsigned int* curIndex = (unsigned int*)indices;
        for (unsigned int poly = 0; poly < numPolys; poly++)
        {
          Ogre::Vector3 poly_verts[3];
 
          if (fw == FW_DEFAULT)
          {
            poly_verts[0] = vertices[*curIndex]; curIndex++;
            poly_verts[1] = vertices[*curIndex]; curIndex++;
            poly_verts[2] = vertices[*curIndex]; curIndex++;
          }
          else
          {
            poly_verts[0] = vertices[*curIndex]; curIndex++;
            poly_verts[2] = vertices[*curIndex]; curIndex++;
            poly_verts[1] = vertices[*curIndex]; curIndex++;
          }
 
          addPoly(poly_verts, 0);
        }
      }
 
      hwIndexBuffer->unlock();
      finish(optimize);
    }
 
    void TreeCollision::AddEntities(const Ogre::SceneNode* node, const Ogre::SceneNode* mainNode, const Ogre::Vector3 scale, FaceWinding fw)
    {
      Ogre::SceneNode::ObjectMap objmap = node->getAttachedObjects();
      for (unsigned int i = 0; i < objmap.size(); i++)
      {
        Ogre::MovableObject* mobj = objmap[i];
        if (mobj->getMovableType() == "Entity")
        {
          Ogre::Entity* obj = static_cast<Ogre::Entity*>(mobj);
          Ogre::Vector3 npos = node->_getDerivedPosition();
          Ogre::Vector3 nscale = node->_getDerivedScale();
          Ogre::Quaternion nquat = node->_getDerivedOrientation();
          Ogre::MeshPtr mesh = obj->getMesh();
 
          //find number of sub-meshes
          unsigned short sub = mesh->getNumSubMeshes();
 
          for (unsigned short cs = 0; cs < sub; cs++)
          {
            Ogre::SubMesh* sub_mesh = mesh->getSubMesh(cs);
 
            //vertex data!
            Ogre::VertexData* v_data;
 
            if (sub_mesh->useSharedVertices)
            {
              v_data = mesh->sharedVertexData;
            }
            else
            {
              v_data = sub_mesh->vertexData;
            }
 
            //let's find more information about the Vertices...
            Ogre::VertexDeclaration* v_decl = v_data->vertexDeclaration;
            const Ogre::VertexElement* p_elem = v_decl->findElementBySemantic(Ogre::VES_POSITION);
 
            // get pointer!
            Ogre::HardwareVertexBufferSharedPtr v_sptr = v_data->vertexBufferBinding->getBuffer(p_elem->getSource());
            unsigned char* v_ptr = static_cast<unsigned char*>(v_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
 
            //now find more about the index!!
            Ogre::IndexData* i_data = sub_mesh->indexData;
            size_t index_count = i_data->indexCount;
            size_t poly_count = index_count / 3;
 
            // get pointer!
            Ogre::HardwareIndexBufferSharedPtr i_sptr = i_data->indexBuffer;
 
            // 16 or 32 bit indices?
            bool uses32bit = (i_sptr->getType() == Ogre::HardwareIndexBuffer::IT_32BIT);
            Ogre::uint32* i_Longptr;
            Ogre::uint16* i_Shortptr;
 
            if (uses32bit)
            {
              i_Longptr = static_cast<Ogre::uint32*>(i_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
 
            }
            else
            {
              i_Shortptr = static_cast<unsigned short*>(i_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
            }
 
            //now loop through the indices, getting polygon info!
            int i_offset = 0;
 
            for (size_t i = 0; i < poly_count; i++)
            {
              Ogre::Vector3 poly_verts[3];
              unsigned char* v_offset;
              float* v_Posptr;
              int idx;
 
              if (uses32bit)
              {
                for (int j = 0; j < 3; j++)
                {
                  idx = i_Longptr[i_offset + j];        // index to first vertex!
                  v_offset = v_ptr + (idx * v_sptr->getVertexSize());
                  p_elem->baseVertexPointerToElement(v_offset, &v_Posptr);
                  //now get vertex position from v_Posptr!
                  poly_verts[j].x = *v_Posptr; v_Posptr++;
                  poly_verts[j].y = *v_Posptr; v_Posptr++;
                  poly_verts[j].z = *v_Posptr; v_Posptr++;
 
                  poly_verts[j] = mainNode->_getDerivedOrientation().Inverse() * ((nquat * (poly_verts[j] * nscale)) + npos - mainNode->_getDerivedPosition()) / mainNode->_getDerivedScale() * scale;
                }
              }
              else
              {
                for (int j = 0; j < 3; j++)
                {
                  idx = i_Shortptr[i_offset + j];       // index to first vertex!
                  v_offset = v_ptr + (idx * v_sptr->getVertexSize());
                  p_elem->baseVertexPointerToElement(v_offset, &v_Posptr);
                  //now get vertex position from v_Posptr!
 
                  // switch poly winding.
                  poly_verts[j].x = *v_Posptr; v_Posptr++;
                  poly_verts[j].y = *v_Posptr; v_Posptr++;
                  poly_verts[j].z = *v_Posptr; v_Posptr++;
 
                  poly_verts[j] = mainNode->_getDerivedOrientation().Inverse() * ((nquat * (poly_verts[j] * nscale)) + npos - mainNode->_getDerivedPosition()) / mainNode->_getDerivedScale() * scale;
                }
              }
 
              if (fw == FW_DEFAULT)
              {
                addPoly(poly_verts, cs);
              }
              else
              {
                Ogre::Vector3 rev_poly_verts[3];
                rev_poly_verts[0] = poly_verts[0];
                rev_poly_verts[0] = poly_verts[2];
                rev_poly_verts[0] = poly_verts[1];
 
                addPoly(rev_poly_verts, cs);
              }
 
              i_offset += 3;
            }
 
            //unlock the buffers!
            v_sptr->unlock();
            i_sptr->unlock();
          }
        }
      }
 
      // Iterate through all the child-nodes
      Ogre::SceneNode::ConstChildNodeIterator nodei = node->getChildIterator();
 
      while (nodei.hasMoreElements())
      {
        const Ogre::SceneNode* subNode = static_cast<const Ogre::SceneNode*>(nodei.getNext());
        // Add this subnode and its children...
        AddEntities(subNode, mainNode, scale, fw);
      }
    }
 
    void TreeCollision::start(int id)
    {
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      m_col = NewtonCreateTreeCollision(m_world->getNewtonWorld(), id);
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
 
      NewtonTreeCollisionBeginBuild(m_col);
    }
 
    void TreeCollision::addPoly(Ogre::Vector3* polys, unsigned int ID)
    {
      NewtonTreeCollisionAddFace(m_col, 3, (float*)&polys[0].x, sizeof(Ogre::Vector3), ID);
    }
 
    void TreeCollision::finish(bool optimize)
    {
      try
      {
        NewtonTreeCollisionEndBuild(m_col, optimize);
 
        if (!m_col)
          m_col = NewtonCreateNull(m_world->getNewtonWorld());
      }
      catch (std::exception &)
      {
        if (m_col)
        {
          NewtonDestroyCollision(m_col);
          m_col = 0;
        }
 
        if (!m_col)
          m_col = NewtonCreateNull(m_world->getNewtonWorld());
      }
    }
 
    // Newton >= 2.30
    dFloat _CDECL TreeCollision::newtonRayCastCallback(const NewtonBody* const body, const NewtonCollision* const treeCollision, dFloat distance, dFloat *normal, int faceId, void *userData)
    {
      Body* bod = (OgreNewt::Body*)NewtonBodyGetUserData(body);
 
      if (!bod)
        return 0;
 
      ((Raycast*)userData)->userCallback(bod, Ogre::Real(distance), Ogre::Vector3(Ogre::Real(normal[0]), Ogre::Real(normal[1]), Ogre::Real(normal[2])), faceId);
 
      ((Raycast*)userData)->m_treecollisioncallback_bodyalreadyadded = true;
 
      return distance;
    }
 
    /*
        float _CDECL TreeCollision::newtonRayCastCallback(float interception, float *normal, int faceId, void *userData)
        {
            Body* bod = ((Raycast*)userData)->m_treecollisioncallback_lastbody;
 
            if(!bod)
                return 0;
 
            ((Raycast*)userData)->userCallback( bod, interception, Ogre::Vector3(normal[0], normal[1], normal[2]), faceId );
 
            ((Raycast*)userData)->m_treecollisioncallback_bodyalreadyadded = true;
 
            return interception;
        }
*/
    void TreeCollision::setRayCastCallbackactive(bool active, const NewtonCollision *col)
    {
      if (active)
        NewtonTreeCollisionSetUserRayCastCallback(col, newtonRayCastCallback);
      else
        NewtonTreeCollisionSetUserRayCastCallback(col, NULL);
    }
 
    int TreeCollisionSceneParser::count = 0;
 
 
    TreeCollisionSceneParser::TreeCollisionSceneParser(OgreNewt::World* world) : TreeCollision(world)
    {
    }
 
    void TreeCollisionSceneParser::parseScene(Ogre::SceneNode *startNode, int id, bool optimize, FaceWinding fw)
    {
      count = 0;
 
      start(id);
 
      // parse the individual nodes.
      Ogre::Quaternion rootOrient = Ogre::Quaternion::IDENTITY;
      Ogre::Vector3 rootPos = Ogre::Vector3::ZERO;
      Ogre::Vector3 rootScale = startNode->getScale();;
 
      _parseNode(startNode, rootOrient, rootPos, rootScale, fw);
 
      finish(optimize);
    }
 
    void TreeCollisionSceneParser::_parseNode(Ogre::SceneNode *node, const Ogre::Quaternion &curOrient, const Ogre::Vector3 &curPos, const Ogre::Vector3 &curScale, FaceWinding fw)
    {
      // parse this scene node.
      // do children first.
      Ogre::Quaternion thisOrient = curOrient * node->getOrientation();
      Ogre::Vector3 thisPos = curPos + (curOrient * (node->getPosition() * curScale));
      Ogre::Vector3 thisScale = curScale * node->getScale();
 
      Ogre::SceneNode::ChildNodeIterator child_it = node->getChildIterator();
 
      while (child_it.hasMoreElements())
      {
        _parseNode((Ogre::SceneNode*)child_it.getNext(), thisOrient, thisPos, thisScale, fw);
      }
 
 
      // now add the polys from this node.
      //now get the mesh!
      Ogre::SceneNode::ObjectMap objmap = node->getAttachedObjects();
      for (unsigned int co = 0; co < objmap.size(); co++)
      {
        Ogre::MovableObject* obj = objmap[co];
        if (obj->getMovableType() != "Entity")
          continue;
 
        Ogre::Entity* ent = (Ogre::Entity*)obj;
 
        if (!entityFilter(node, ent, fw))
          continue;
 
        Ogre::MeshPtr mesh = ent->getMesh();
 
        //find number of sub-meshes
        unsigned short sub = mesh->getNumSubMeshes();
 
        for (unsigned short cs = 0; cs < sub; cs++)
        {
          Ogre::SubMesh* sub_mesh = mesh->getSubMesh(cs);
 
          //vertex data!
          Ogre::VertexData* v_data;
 
          if (sub_mesh->useSharedVertices)
          {
            v_data = mesh->sharedVertexData;
          }
          else
          {
            v_data = sub_mesh->vertexData;
          }
 
          //let's find more information about the Vertices...
          Ogre::VertexDeclaration* v_decl = v_data->vertexDeclaration;
          const Ogre::VertexElement* p_elem = v_decl->findElementBySemantic(Ogre::VES_POSITION);
 
          // get pointer!
          Ogre::HardwareVertexBufferSharedPtr v_sptr = v_data->vertexBufferBinding->getBuffer(p_elem->getSource());
          unsigned char* v_ptr = static_cast<unsigned char*>(v_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
 
          //now find more about the index!!
          Ogre::IndexData* i_data = sub_mesh->indexData;
          size_t index_count = i_data->indexCount;
          size_t poly_count = index_count / 3;
 
          // get pointer!
          Ogre::HardwareIndexBufferSharedPtr i_sptr = i_data->indexBuffer;
 
          // 16 or 32 bit indices?
          bool uses32bit = (i_sptr->getType() == Ogre::HardwareIndexBuffer::IT_32BIT);
          Ogre::uint32* i_Longptr;
          Ogre::uint16* i_Shortptr;
 
          if (uses32bit)
          {
            i_Longptr = static_cast<Ogre::uint32*>(i_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
          }
          else
          {
            i_Shortptr = static_cast<Ogre::uint16*>(i_sptr->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
          }
 
          //now loop through the indices, getting polygon info!
          int i_offset = 0;
 
          for (size_t i = 0; i < poly_count; i++)
          {
            Ogre::Vector3 poly_verts[3];
            unsigned char* v_offset;
            float* v_Posptr;
            int idx;
 
            if (uses32bit)
            {
              for (int j = 0; j < 3; j++)
              {
                idx = i_Longptr[i_offset + j];        // index to first vertex!
                v_offset = v_ptr + (idx * v_sptr->getVertexSize());
                p_elem->baseVertexPointerToElement(v_offset, &v_Posptr);
                //now get vertex position from v_Posptr!
                poly_verts[j].x = *v_Posptr; v_Posptr++;
                poly_verts[j].y = *v_Posptr; v_Posptr++;
                poly_verts[j].z = *v_Posptr; v_Posptr++;
 
                poly_verts[j] = thisPos + (thisOrient * (poly_verts[j] * curScale));
              }
            }
            else
            {
              for (int j = 0; j < 3; j++)
              {
                idx = i_Shortptr[i_offset + j];       // index to first vertex!
                v_offset = v_ptr + (idx * v_sptr->getVertexSize());
                p_elem->baseVertexPointerToElement(v_offset, &v_Posptr);
                //now get vertex position from v_Posptr!
 
                // switch poly winding.
                poly_verts[j].x = *v_Posptr; v_Posptr++;
                poly_verts[j].y = *v_Posptr; v_Posptr++;
                poly_verts[j].z = *v_Posptr; v_Posptr++;
 
                poly_verts[j] = thisPos + (thisOrient * (poly_verts[j] * curScale));
              }
            }
 
            if (fw == FW_DEFAULT)
            {
              addPoly(poly_verts, cs);
            }
            else
            {
              Ogre::Vector3 rev_poly_verts[3];
              rev_poly_verts[0] = poly_verts[0];
              rev_poly_verts[0] = poly_verts[2];
              rev_poly_verts[0] = poly_verts[1];
 
              addPoly(rev_poly_verts, cs);
            }
 
            i_offset += 3;
          }
 
          //unlock the buffers!
          v_sptr->unlock();
          i_sptr->unlock();
 
        }
      }
 
    }
 
 
 
 
 
    // OgreNewt::CollisionPrimitives::CompoundCollision
    CompoundCollision::CompoundCollision(const World* world) : Collision(world)
    {}
 
    CompoundCollision::CompoundCollision(const World* world, std::vector<OgreNewt::CollisionPtr> col_array, int id) : Collision(world)
    {
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      m_col = NewtonCreateCompoundCollision(world->getNewtonWorld(), id);
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
      else
      {
        //get the number of elements.
        unsigned int num = col_array.size();
        NewtonCompoundCollisionBeginAddRemove(m_col);
        for (unsigned int i = 0; i < num; i++)
        {
          NewtonCompoundCollisionAddSubCollision(m_col, (NewtonCollision*)col_array[i]->getNewtonCollision());
        }
        NewtonCompoundCollisionEndAddRemove(m_col);
      }
    }
 
 
    // OgreNewt::CollisionPrimitives::Pyramid
    Pyramid::Pyramid(const World* world) : ConvexCollision(world)
    {}
 
    Pyramid::Pyramid(const World* world, const Ogre::Vector3& size, int id, const Ogre::Quaternion& orient, const Ogre::Vector3& pos, Ogre::Real tolerance) : ConvexCollision(world)
    {
      dFloat matrix[16];
 
      OgreNewt::Converters::QuatPosToMatrix(orient, pos, &matrix[0]);
 
      // create a simple pyramid collision primitive using the Newton Convex Hull interface.
      // this function places the center of mass 1/3 up y from the base.
 
      dFloat* vertices = new dFloat[15];
      unsigned short idx = 0;
 
      // make the bottom base.
      for (int ix = -1; ix <= 1; ix += 2)
      {
        for (int iz = -1; iz <= 1; iz += 2)
        {
          vertices[idx++] = (size.x / 2.0f) * ix;
          vertices[idx++] = -(size.y / 3.0f);
          vertices[idx++] = (size.z / 2.0f) * iz;
        }
      }
 
      // make the tip.
      vertices[idx++] = 0.0f;
      vertices[idx++] = (size.y*2.0f / 3.0f);
      vertices[idx++] = 0.0f;
 
      //make the collision primitive.
      if (m_col)
      {
        NewtonDestroyCollision(m_col);
        m_col = 0;
      }
 
      m_col = NewtonCreateConvexHull(m_world->getNewtonWorld(), 5, vertices, sizeof(dFloat) * 3, tolerance, id, &matrix[0]);
      if (!m_col)
        m_col = NewtonCreateNull(m_world->getNewtonWorld());
 
      delete[]vertices;
    }
 
    /*
    HeightField::HeightField(const OgreNewt::World *world, int width, int height, int gridsDiagonals, unsigned short *elevationMap, char *attributeMap, Ogre::Real horizontalScale, Ogre::Real verticleScale, int shapeID) : OgreNewt::Collision (world)
    {
      m_col = NewtonCreateHeightFieldCollision(world->getNewtonWorld(),width,height,gridsDiagonals,elevationMap,attributeMap,float(horizontalScale),float(verticleScale),shapeID);
    }
 
    HeightField::HeightField(const OgreNewt::World *world, Ogre::Terrain *terrain, int shapeID) : OgreNewt::Collision (world)
    {
      int width = terrain->getSize();
      int height = width;
      int gridsDiagonals = 0;
 
      char *attributes;
      unsigned short *elevations;
 
      float *hData = terrain->getHeightData();
      float verticleScale = 65535 / (terrain->getMaxHeight() - terrain->getMinHeight());
      Ogre::Real horizontalScale = terrain->getWorldSize() / (terrain->getSize() - 1);
 
      elevations = (unsigned short*) malloc (width * height * sizeof (unsigned short));
      attributes = (char*) malloc (width * height * sizeof (char));
      memset (attributes, 1, width * height * sizeof (char));
 
      //HeightData reversed and mirror on X axis
      int x = 0;
 
      for (int i = width * height - 1; i >= 0; i--)
      {
        elevations[i-width+(x*2)+1] = unsigned short(*(hData)*verticleScale);
        hData++;
        x++;
        if (x == width) x = 0;
      }
 
      createHeightFieldCollision(world, width, height, gridsDiagonals, elevations, attributes, horizontalScale, float(1 / verticleScale), shapeID);
 
      free (elevations);
      free (attributes);
    }
 
    void HeightField::createHeightFieldCollision(const OgreNewt::World *world, int width, int height, int gridsDiagonals, unsigned short *elevationMap, char *attributeMap, Ogre::Real horizontalScale, Ogre::Real verticleScale, int shapeID)
    {
      m_col = NewtonCreateHeightFieldCollision(world->getNewtonWorld(), width, height, gridsDiagonals, elevationMap, attributeMap, float(horizontalScale), float(verticleScale), shapeID);
    }
    */
  }   // end namespace CollisionPrimitives
}   // end namespace OgreNewt