Working on triangular mesh collision detection

src/collision/TriangleVertexArray.h

@@ -36,7 +36,7 @@ namespace reactphysics3d {
  * This class is used to describe the vertices and faces of a triangular mesh.
  * A TriangleVertexArray represents a continuous array of vertices and indexes
  * of a triangular mesh. When you create a TriangleVertexArray, no data is copied
- * into the array. It only stores pointer to the da. The purpose is to allow
+ * into the array. It only stores pointer to the data. The purpose is to allow
  * the user to share vertices data between the physics engine and the rendering
  * part. Therefore, make sure that the data pointed by a TriangleVertexArray
  * remain valid during the TriangleVertexArray life.

src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp

@@ -85,7 +85,7 @@ bool ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
 }
 
 // Test collision between a triangle and the convex mesh shape
-void ConvexVsTriangleCallback::reportTriangle(const Vector3* trianglePoints) {
+void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
 
     // Create a triangle collision shape
     // TODO : Do we need to use a collision margin for a triangle ?
@@ -95,10 +95,16 @@ void ConvexVsTriangleCallback::reportTriangle(const Vector3* trianglePoints) {
     NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(),
                                                                             mConvexShape->getType());
 
+    // If there is no collision algorithm between those two kinds of shapes
+    if (algo == NULL) return;
+
+    // Notify the narrow-phase algorithm about the overlapping pair we are going to test
+    algo->setCurrentOverlappingPair(mOverlappingPair);
+
     // Create the CollisionShapeInfo objects
     CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(),
                                        mOverlappingPair, mConvexProxyShape->getCachedCollisionData());
-    CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, mConcaveProxyShape->getCollisionShape(),
+    CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape,
                                         mConcaveProxyShape->getLocalToWorldTransform(),
                                         mOverlappingPair, mConcaveProxyShape->getCachedCollisionData());
 

src/collision/narrowphase/ConcaveVsConvexAlgorithm.h

@@ -91,7 +91,7 @@ class ConvexVsTriangleCallback : public TriangleCallback {
         }
 
         /// Test collision between a triangle and the convex mesh shape
-        virtual void reportTriangle(const Vector3* trianglePoints);
+        virtual void testTriangle(const Vector3* trianglePoints);
 
 };
 

src/collision/narrowphase/DefaultCollisionDispatch.cpp

@@ -46,6 +46,7 @@ void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection,
     // Initialize the collision algorithms
     mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator);
     mGJKAlgorithm.init(collisionDetection, memoryAllocator);
+    mConcaveVsConvexAlgorithm.init(collisionDetection, memoryAllocator);
 }
 
 // Select and return the narrow-phase collision detection algorithm to

src/collision/shapes/ConcaveMeshShape.cpp

@@ -31,6 +31,8 @@ using namespace reactphysics3d;
 // Constructor
 ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh) : ConcaveShape(CONCAVE_MESH) {
     mTriangleMesh = triangleMesh;
+
+    recalculateBounds();
 }
 
 // Destructor
@@ -65,10 +67,10 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
                 // Get the index of the current vertex in the triangle
                 int vertexIndex;
                 if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
-                    vertexIndex = ((unsigned int*)(indicesStart + j * indexStride))[k];
+                    vertexIndex = ((unsigned int*)(indicesStart + j * 3 * indexStride))[k];
                 }
                 else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
-                    vertexIndex = ((unsigned short*)(indicesStart + j * indexStride))[k];
+                    vertexIndex = ((unsigned short*)(indicesStart + j * 3 * indexStride))[k];
                 }
 
                 // Get the vertices components of the triangle
@@ -90,7 +92,7 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
             if (localAABB.testCollisionTriangleAABB(trianglePoints)) {
 
                 // Call the callback to report this triangle
-                callback.reportTriangle(trianglePoints);
+                callback.testTriangle(trianglePoints);
             }
         }
     }
@@ -104,3 +106,72 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
 
     return false;
 }
+
+// Recompute the bounds of the mesh
+void ConcaveMeshShape::recalculateBounds() {
+
+    bool isFirstVertex = true;
+
+    // For each sub-part of the mesh
+    for (int i=0; i<mTriangleMesh->getNbSubparts(); i++) {
+
+        // Get the triangle vertex array of the current sub-part
+        TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(i);
+
+        TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
+        TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
+        unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
+        unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
+        int vertexStride = triangleVertexArray->getVerticesStride();
+        int indexStride = triangleVertexArray->getIndicesStride();
+
+        // For each triangle of the concave mesh
+        for (int j=0; j<triangleVertexArray->getNbTriangles(); j++) {
+
+            // For each vertex of the triangle
+            for (int k=0; k < 3; k++) {
+
+                // Get the index of the current vertex in the triangle
+                int vertexIndex;
+                if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
+                    vertexIndex = ((unsigned int*)(indicesStart + j * indexStride))[k];
+                }
+                else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
+                    vertexIndex = ((unsigned short*)(indicesStart + j * indexStride))[k];
+                }
+
+                Vector3 vertex;
+
+                // Get the vertices components of the triangle
+                if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
+                    const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
+                    vertex[0] = decimal(vertices[0]);
+                    vertex[1] = decimal(vertices[1]);
+                    vertex[2] = decimal(vertices[2]);
+                }
+                else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
+                    const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
+                    vertex[0] = decimal(vertices[0]);
+                    vertex[1] = decimal(vertices[1]);
+                    vertex[2] = decimal(vertices[2]);
+                }
+
+                if (isFirstVertex) {
+                    mMinBounds.setAllValues(vertex.x, vertex.y, vertex.z);
+                    mMaxBounds.setAllValues(vertex.x, vertex.y, vertex.z);
+                    isFirstVertex = false;
+                }
+                else {
+                    if (vertex.x > mMaxBounds.x) mMaxBounds.x = vertex.x;
+                    if (vertex.x < mMinBounds.x) mMinBounds.x = vertex.x;
+
+                    if (vertex.y > mMaxBounds.y) mMaxBounds.y = vertex.y;
+                    if (vertex.y < mMinBounds.y) mMinBounds.y = vertex.y;
+
+                    if (vertex.z > mMaxBounds.z) mMaxBounds.z = vertex.z;
+                    if (vertex.z < mMinBounds.z) mMinBounds.z = vertex.z;
+                }
+            }
+        }
+    }
+}

src/collision/shapes/ConcaveMeshShape.h

@@ -51,6 +51,12 @@ class ConcaveMeshShape : public ConcaveShape {
         /// Triangle mesh
         TriangleMesh* mTriangleMesh;
 
+        /// Mesh minimum bounds in the three local x, y and z directions
+        Vector3 mMinBounds;
+
+        /// Mesh maximum bounds in the three local x, y and z directions
+        Vector3 mMaxBounds;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -76,6 +82,10 @@ class ConcaveMeshShape : public ConcaveShape {
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const;
 
+        /// Recompute the bounds of the mesh
+        // TODO : Check if we need this when AABB tree is used
+        void recalculateBounds();
+
     public:
 
         /// Constructor
@@ -90,11 +100,8 @@ class ConcaveMeshShape : public ConcaveShape {
         /// Return the local inertia tensor of the collision shape
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
 
-        /// Update the AABB of a body using its collision shape
-        virtual void computeAABB(AABB& aabb, const Transform& transform);
-
         /// Use a callback method on all triangles of the concave shape inside a given AABB
-        virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0;
+        virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
 };
 
 // Return the number of bytes used by the collision shape
@@ -125,7 +132,8 @@ inline Vector3 ConcaveMeshShape::getLocalSupportPointWithoutMargin(const Vector3
  */
 inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
 
-    // TODO : Implement this
+    min = mMinBounds;
+    max = mMaxBounds;
 }
 
 // Return the local inertia tensor of the sphere
@@ -136,19 +144,13 @@ inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
  */
 inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
 
-    // TODO : Implement this
+    // Default inertia tensor
-    tensor.setToZero();
+    // Note that this is not very realistic for a concave triangle mesh.
-}
+    // However, in most cases, it will only be used static bodies and therefore,
-
+    // the inertia tensor is not used.
-// Update the AABB of a body using its collision shape
+    tensor.setAllValues(mass, 0, 0,
-/**
+                        0, mass, 0,
- * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
+                        0, 0, mass);
- *                  computed in world-space coordinates
- * @param transform Transform used to compute the AABB of the collision shape
- */
-inline void ConcaveMeshShape::computeAABB(AABB& aabb, const Transform& transform) {
-
-    // TODO : Implement this
 }
 
 // Return true if a point is inside the collision shape

src/collision/shapes/ConcaveShape.h

@@ -42,7 +42,7 @@ class TriangleCallback {
     public:
 
         /// Report a triangle
-        virtual void reportTriangle(const Vector3* trianglePoints)=0;
+        virtual void testTriangle(const Vector3* trianglePoints)=0;
 
 };
 

src/collision/shapes/ConvexMeshShape.cpp

@@ -178,6 +178,9 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
 // Recompute the bounds of the mesh
 void ConvexMeshShape::recalculateBounds() {
 
+    // TODO : Only works if the local origin is inside the mesh
+    //        => Make it more robust (init with first vertex of mesh instead)
+
     mMinBounds.setToZero();
     mMaxBounds.setToZero();
 

src/reactphysics3d.h

@@ -50,9 +50,12 @@
 #include "collision/shapes/CylinderShape.h"
 #include "collision/shapes/CapsuleShape.h"
 #include "collision/shapes/ConvexMeshShape.h"
+#include "collision/shapes/ConcaveMeshShape.h"
 #include "collision/shapes/AABB.h"
 #include "collision/ProxyShape.h"
 #include "collision/RaycastInfo.h"
+#include "collision/TriangleMesh.h"
+#include "collision/TriangleVertexArray.h"
 #include "constraint/BallAndSocketJoint.h"
 #include "constraint/SliderJoint.h"
 #include "constraint/HingeJoint.h"