From 1c735e483cf6d151ad050810a05319fa39b822a2 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 26 Oct 2015 18:15:25 +0100 Subject: [PATCH] Working on triangular mesh collision detection --- src/collision/TriangleVertexArray.h | 2 +- .../narrowphase/ConcaveVsConvexAlgorithm.cpp | 10 ++- .../narrowphase/ConcaveVsConvexAlgorithm.h | 2 +- .../narrowphase/DefaultCollisionDispatch.cpp | 1 + src/collision/shapes/ConcaveMeshShape.cpp | 77 ++++++++++++++++++- src/collision/shapes/ConcaveMeshShape.h | 38 ++++----- src/collision/shapes/ConcaveShape.h | 2 +- src/collision/shapes/ConvexMeshShape.cpp | 3 + src/reactphysics3d.h | 3 + 9 files changed, 112 insertions(+), 26 deletions(-) diff --git a/src/collision/TriangleVertexArray.h b/src/collision/TriangleVertexArray.h index 6cf70950..77d03dc3 100644 --- a/src/collision/TriangleVertexArray.h +++ b/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. diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index 91fc431c..f2877a54 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/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()); diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h index fc3a7287..fbbd41c0 100644 --- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h +++ b/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); }; diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 8aac650e..4724ed25 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/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 diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index cc99d244..ed7ae968 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/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; igetNbSubparts(); 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; jgetNbTriangles(); 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; + } + } + } + } +} diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 603da354..385e69b8 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/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 - tensor.setToZero(); -} - -// Update the AABB of a body using its collision shape -/** - * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape - * 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 + // Default inertia tensor + // 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. + tensor.setAllValues(mass, 0, 0, + 0, mass, 0, + 0, 0, mass); } // Return true if a point is inside the collision shape diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h index 569390ce..2ff0f86b 100644 --- a/src/collision/shapes/ConcaveShape.h +++ b/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; }; diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp index 799f67fd..0789e3c5 100644 --- a/src/collision/shapes/ConvexMeshShape.cpp +++ b/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(); diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index 1ef76d5e..66873111 100644 --- a/src/reactphysics3d.h +++ b/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"