Working on triangles mesh raycasting

src/collision/CollisionDetection.cpp

@@ -252,7 +252,6 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
     // For each possible collision pair of bodies
     map<overlappingpairid, OverlappingPair*>::iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
-        ContactPointInfo* contactInfo = NULL;
 
         OverlappingPair* pair = it->second;
 

src/collision/ProxyShape.h

@@ -174,6 +174,7 @@ class ProxyShape {
         friend class EPAAlgorithm;
         friend class GJKAlgorithm;
         friend class ConvexMeshShape;
+
 };
 
 // Return the pointer to the cached collision data

src/collision/RaycastInfo.h

@@ -68,6 +68,12 @@ struct RaycastInfo {
         /// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)
         decimal hitFraction;
 
+        /// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
+        int meshSubpart;
+
+        /// Hit triangle index (only used for triangles mesh and -1 otherwise)
+        int triangleIndex;
+
         /// Pointer to the hit collision body
         CollisionBody* body;
 
@@ -77,7 +83,7 @@ struct RaycastInfo {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        RaycastInfo() : body(NULL), proxyShape(NULL) {
+        RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(NULL), proxyShape(NULL) {
 
         }
 
@@ -108,7 +114,7 @@ class RaycastCallback {
         /// This method will be called for each ProxyShape that is hit by the
         /// ray. You cannot make any assumptions about the order of the
         /// calls. You should use the return value to control the continuation
-        /// of the ray. The return value is the next maxFraction value to use.
+        /// of the ray. The returned value is the next maxFraction value to use.
         /// If you return a fraction of 0.0, it means that the raycast should
         /// terminate. If you return a fraction of 1.0, it indicates that the
         /// ray is not clipped and the ray cast should continue as if no hit

src/collision/broadphase/BroadPhaseAlgorithm.cpp

@@ -264,31 +264,29 @@ void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
     mNbPotentialPairs++;
 }
 
-// Called for a broad-phase shape that has to be tested for raycast
-decimal BroadPhaseAlgorithm::raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest,
-                                                    const Ray& ray,
-                                                    unsigned short raycastWithCategoryMaskBits) const {
-
-     decimal hitFraction = decimal(-1.0);
-
-     // Get the proxy shape from the node
-     ProxyShape* proxyShape = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(nodeId));
-
-     // Check if the raycast filtering mask allows raycast against this shape
-     if ((raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {
-
-         // Ask the collision detection to perform a ray cast test against
-         // the proxy shape of this node because the ray is overlapping
-         // with the shape in the broad-phase
-         hitFraction = raycastTest.raycastAgainstShape(proxyShape, ray);
-     }
-
-     return hitFraction;
-}
-
 // Called when a overlapping node has been found during the call to
 // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
 void AABBOverlapCallback::notifyOverlappingNode(int nodeId) {
 
     mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId);
 }
+
+// Called for a broad-phase shape that has to be tested for raycast
+decimal BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
+
+    decimal hitFraction = decimal(-1.0);
+
+    // Get the proxy shape from the node
+    ProxyShape* proxyShape = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(nodeId));
+
+    // Check if the raycast filtering mask allows raycast against this shape
+    if ((mRaycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {
+
+        // Ask the collision detection to perform a ray cast test against
+        // the proxy shape of this node because the ray is overlapping
+        // with the shape in the broad-phase
+        hitFraction = mRaycastTest.raycastAgainstShape(proxyShape, ray);
+    }
+
+    return hitFraction;
+}

src/collision/broadphase/BroadPhaseAlgorithm.h

@@ -83,18 +83,33 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
 
 };
 
-// Class BroadPhaseRaycastTestCallback
+// Class BroadPhaseRaycastCallback
 /**
- * Callback method called to raycast against a given broad-phase shape
+ * Callback called when the AABB of a leaf node is hit by a ray the
+ * broad-phase Dynamic AABB Tree.
  */
-class BroadPhaseRaycastTestCallback {
+class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback {
+
+    private :
+
+        const DynamicAABBTree& mDynamicAABBTree;
+
+        unsigned short mRaycastWithCategoryMaskBits;
+
+        RaycastTest& mRaycastTest;
 
     public:
 
+        // Constructor
+        BroadPhaseRaycastCallback(const DynamicAABBTree& dynamicAABBTree, unsigned short raycastWithCategoryMaskBits,
+                                  RaycastTest& raycastTest)
+            : mDynamicAABBTree(dynamicAABBTree), mRaycastWithCategoryMaskBits(raycastWithCategoryMaskBits),
+              mRaycastTest(raycastTest) {
+
+        }
+
         // Called for a broad-phase shape that has to be tested for raycast
-        virtual decimal raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest,
-                                               const Ray& ray,
-                                               unsigned short raycastWithCategoryMaskBits) const=0;
+        virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray);
 
 };
 
@@ -106,7 +121,7 @@ class BroadPhaseRaycastTestCallback {
  * later for collision during the narrow-phase collision detection. A dynamic AABB
  * tree data structure is used for fast broad-phase collision detection.
  */
-class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback {
+class BroadPhaseAlgorithm {
 
     protected :
 
@@ -183,11 +198,6 @@ class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback {
         /// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
         void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2);
 
-        /// Called for a broad-phase shape that has to be tested for raycast
-        virtual decimal raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest,
-                                               const Ray& ray,
-                                               unsigned short raycastWithCategoryMaskBits) const;
-
         /// Compute all the overlapping pairs of collision shapes
         void computeOverlappingPairs();
 
@@ -223,7 +233,10 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
 // Ray casting method
 inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
                                          unsigned short raycastWithCategoryMaskBits) const {
-    mDynamicAABBTree.raycast(ray, raycastTest, raycastWithCategoryMaskBits, *this);
+
+    BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
+
+    mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
 }
 
 }

src/collision/broadphase/DynamicAABBTree.cpp

@@ -631,9 +631,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
 }
 
 // Ray casting method
-void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest,
-                              unsigned short raycastWithCategoryMaskBits,
-                              const BroadPhaseRaycastTestCallback& callback) const {
+void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const {
 
     decimal maxFraction = ray.maxFraction;
 
@@ -668,8 +666,7 @@ void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest,
             Ray rayTemp(ray.point1, ray.point2, maxFraction);
 
             // Call the callback that will raycast again the broad-phase shape
-            decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, raycastTest, rayTemp,
-                                                                  raycastWithCategoryMaskBits);
+            decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp);
 
             // If the user returned a hitFraction of zero, it means that
             // the raycasting should stop here

src/collision/broadphase/DynamicAABBTree.h

@@ -103,6 +103,20 @@ class DynamicAABBTreeOverlapCallback {
         virtual void notifyOverlappingNode(int nodeId)=0;
 };
 
+// Class DynamicAABBTreeRaycastCallback
+/**
+ * Raycast callback in the Dynamic AABB Tree called when the AABB of a leaf
+ * node is hit by the ray.
+ */
+class DynamicAABBTreeRaycastCallback {
+
+    public:
+
+        // Called when the AABB of a leaf node is hit by a ray
+        virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray)=0;
+
+};
+
 // Class DynamicAABBTree
 /**
  * This class implements a dynamic AABB tree that is used for broad-phase
@@ -208,9 +222,7 @@ class DynamicAABBTree {
                                                 DynamicAABBTreeOverlapCallback& callback) const;
 
         /// Ray casting method
-        void raycast(const Ray& ray, RaycastTest& raycastTest,
-                     unsigned short raycastWithCategoryMaskBits,
-                     const BroadPhaseRaycastTestCallback& callback) const;
+        void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const;
 
         /// Compute the height of the tree
         int computeHeight();

src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp

@@ -85,7 +85,6 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
 void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
 
     // Create a triangle collision shape
-    // TODO : Do we need to use a collision margin for a triangle ?
     TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2]);
 
     // Select the collision algorithm to use between the triangle and the convex shape

src/collision/shapes/ConcaveMeshShape.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "ConcaveMeshShape.h"
+#include "collision/shapes/TriangleShape.h"
 
 using namespace reactphysics3d;
 
@@ -157,10 +158,67 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
 }
 
 // Raycast method with feedback information
+/// Note that only the first triangle hit by the ray in the mesh will be returned, even if
+/// the ray hits many triangles.
 bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
 
+    // Create the callback object that will compute ray casting against triangles
+    ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray);
 
-    // TODO : Implement this
+    // Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray.
+    // The raycastCallback object will then compute ray casting against the triangles
+    // in the hit AABBs.
+    mDynamicAABBTree.raycast(ray, raycastCallback);
 
-    return false;
+    return raycastCallback.getIsHit();
+}
+
+// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
+decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
+
+    // Add the id of the hit AABB node into
+    mHitAABBNodes.push_back(nodeId);
+
+    return ray.maxFraction;
+}
+
+// Raycast all collision shapes that have been collected
+void ConcaveMeshRaycastCallback::raycastTriangles() {
+
+    std::vector<int>::const_iterator it;
+    decimal smallestHitFraction = mRay.maxFraction;
+
+    for (it = mHitAABBNodes.begin(); it != mHitAABBNodes.end(); ++it) {
+
+        // Get the node data (triangle index and mesh subpart index)
+        int32* data = mDynamicAABBTree.getNodeDataInt(*it);
+
+        // Get the triangle vertices for this node from the concave mesh shape
+        Vector3 trianglePoints[3];
+        mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
+
+        // Create a triangle collision shape
+        TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2]);
+
+        // Ray casting test against the collision shape
+        RaycastInfo raycastInfo;
+        bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
+
+        // If the ray hit the collision shape
+        if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
+
+            assert(raycastInfo.hitFraction >= decimal(0.0));
+
+            mRaycastInfo.body = raycastInfo.body;
+            mRaycastInfo.proxyShape = raycastInfo.proxyShape;
+            mRaycastInfo.hitFraction = raycastInfo.hitFraction;
+            mRaycastInfo.worldPoint = raycastInfo.worldPoint;
+            mRaycastInfo.worldNormal = raycastInfo.worldNormal;
+            mRaycastInfo.meshSubpart = data[0];
+            mRaycastInfo.triangleIndex = data[1];
+
+            smallestHitFraction = raycastInfo.hitFraction;
+            mIsHit = true;
+        }
+    }
 }

src/collision/shapes/ConcaveMeshShape.h

@@ -63,6 +63,41 @@ class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback
 
 };
 
+/// Class ConcaveMeshRaycastCallback
+class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
+
+    private :
+
+        std::vector<int32> mHitAABBNodes;
+        const DynamicAABBTree& mDynamicAABBTree;
+        const ConcaveMeshShape& mConcaveMeshShape;
+        ProxyShape* mProxyShape;
+        RaycastInfo& mRaycastInfo;
+        const Ray& mRay;
+        bool mIsHit;
+
+    public:
+
+        // Constructor
+        ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape,
+                                   ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray)
+            : mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape),
+              mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false) {
+
+        }
+
+        /// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
+        virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray);
+
+        /// Raycast all collision shapes that have been collected
+        void raycastTriangles();
+
+        /// Return true if a raycast hit has been found
+        bool getIsHit() const {
+            return mIsHit;
+        }
+};
+
 // TODO : Implement raycasting with this collision shape
 
 // TODO : Make possible for the user to have a scaling factor on the mesh
@@ -101,9 +136,6 @@ class ConcaveMeshShape : public ConcaveShape {
         virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
                                                           void** cachedCollisionData) const;
 
-        /// Return true if a point is inside the collision shape
-        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
-
         /// Raycast method with feedback information
         virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
 
@@ -141,6 +173,7 @@ class ConcaveMeshShape : public ConcaveShape {
         // ---------- Friendship ----------- //
 
         friend class ConvexTriangleAABBOverlapCallback;
+        friend class ConcaveMeshRaycastCallback;
 };
 
 // Return the number of bytes used by the collision shape
@@ -209,13 +242,6 @@ inline void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decim
                         0, 0, mass);
 }
 
-// Return true if a point is inside the collision shape
-inline bool ConcaveMeshShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
-
-    // TODO : Implement this
-    return false;
-}
-
 // Called when a overlapping node has been found during the call to
 // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
 inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) {

src/collision/shapes/ConcaveShape.h

@@ -68,6 +68,9 @@ class ConcaveShape : public CollisionShape {
         /// Private assignment operator
         ConcaveShape& operator=(const ConcaveShape& shape);
 
+        /// Return true if a point is inside the collision shape
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -90,6 +93,11 @@ inline bool ConcaveShape::isConvex() const {
     return false;
 }
 
+// Return true if a point is inside the collision shape
+inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
+    return false;
+}
+
 }
 
 #endif

src/collision/shapes/SphereShape.h

@@ -97,7 +97,7 @@ class SphereShape : public ConvexShape {
         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);
+        virtual void computeAABB(AABB& aabb, const Transform& transform) const;
 };
 
 // Get the radius of the sphere
@@ -183,7 +183,7 @@ inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal ma
  *                  computed in world-space coordinates
  * @param transform Transform used to compute the AABB of the collision shape
  */
-inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) {
+inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) const {
 
     // Get the local extents in x,y and z direction
     Vector3 extents(mMargin, mMargin, mMargin);

src/collision/shapes/TriangleShape.cpp

@@ -51,10 +51,57 @@ TriangleShape::~TriangleShape() {
 }
 
 // Raycast method with feedback information
+/// This method use the line vs triangle raycasting technique described in
+/// Real-time Collision Detection by Christer Ericson.
 bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
 
-    // TODO : Implement this
+    // TODO : For all collision shape, try to perform raycasting in local-space and
+    //        compute world-space hit point, normal in upper classes when local-space
+    //        hit points are returned
+    const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
+    const Transform worldToLocalTransform = localToWorldTransform.getInverse();
+    const Vector3 point1 = worldToLocalTransform * ray.point1;
+    const Vector3 point2 = worldToLocalTransform * ray.point2;
 
-    return false;
+    const Vector3 pq = point2 - point1;
+    const Vector3 pa = mPoints[0] - point1;
+    const Vector3 pb = mPoints[1] - point1;
+    const Vector3 pc = mPoints[2] - point1;
+
+    // Test if the line PQ is inside the eges BC, CA and AB. We use the triple
+    // product for this test.
+    const Vector3 m = pq.cross(pc);
+    decimal u = pb.dot(m);
+    if (u < decimal(0.0)) return false;
+    
+    decimal v = -pa.dot(m);
+    if (v < decimal(0.0)) return false;
+
+    decimal w = pa.dot(pq.cross(pb));
+    if (w < decimal(0.0)) return false;
+
+    // If the line PQ is in the triangle plane (case where u=v=w=0)
+    if (u < MACHINE_EPSILON && u < MACHINE_EPSILON && v < MACHINE_EPSILON) return false;
+
+    // Compute the barycentric coordinates (u, v, w) to determine the
+    // intersection point R, R = u * a + v * b + w * c
+    decimal denom = decimal(1.0) / (u + v + w);
+    u *= denom;
+    v *= denom;
+    w *= denom;
+
+    // Compute the local hit point using the barycentric coordinates
+    const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2];
+
+    const Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]);
+
+    raycastInfo.body = proxyShape->getBody();
+    raycastInfo.proxyShape = proxyShape;
+    raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
+    raycastInfo.hitFraction = (localHitPoint - point1).length() / pq.length();
+    raycastInfo.worldNormal = localToWorldTransform.getOrientation() * localHitNormal;
+    raycastInfo.worldNormal.normalize();
+
+    return true;
 }
 

src/collision/shapes/TriangleShape.h

@@ -92,7 +92,11 @@ class TriangleShape : public ConvexShape {
         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);
+        virtual void computeAABB(AABB& aabb, const Transform& transform) const;
+
+        // ---------- Friendship ---------- //
+
+        friend class ConcaveMeshRaycastCallback;
 };
 
 // Return the number of bytes used by the collision shape
@@ -155,7 +159,7 @@ inline void TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal
  *                  computed in world-space coordinates
  * @param transform Transform used to compute the AABB of the collision shape
  */
-inline void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) {
+inline void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) const {
 
     const Vector3 worldPoint1 = transform * mPoints[0];
     const Vector3 worldPoint2 = transform * mPoints[1];

test/tests/collision/TestRaycast.h

@@ -36,6 +36,7 @@
 #include "collision/shapes/ConeShape.h"
 #include "collision/shapes/ConvexMeshShape.h"
 #include "collision/shapes/CylinderShape.h"
+#include "collision/shapes/TriangleShape.h"
 
 // TODO : Add test for concave shape here
 
@@ -115,6 +116,7 @@ class TestRaycast : public Test {
         CollisionBody* mConvexMeshBodyEdgesInfo;
         CollisionBody* mCylinderBody;
         CollisionBody* mCompoundBody;
+        CollisionBody* mTriangleBody;
 
         // Transform
         Transform mBodyTransform;
@@ -130,7 +132,7 @@ class TestRaycast : public Test {
         ConvexMeshShape* mConvexMeshShape;
         ConvexMeshShape* mConvexMeshShapeEdgesInfo;
         CylinderShape* mCylinderShape;
-
+        TriangleShape* mTriangleShape;
 
         // Proxy Shapes
         ProxyShape* mBoxProxyShape;
@@ -142,6 +144,7 @@ class TestRaycast : public Test {
         ProxyShape* mCylinderProxyShape;
         ProxyShape* mCompoundSphereProxyShape;
         ProxyShape* mCompoundCylinderProxyShape;
+        ProxyShape* mTriangleProxyShape;
 
     public :
 
@@ -156,8 +159,11 @@ class TestRaycast : public Test {
             mWorld = new CollisionWorld();
 
             // Body transform
+            // TODO : Uncomment this
             Vector3 position(-3, 2, 7);
             Quaternion orientation(PI / 5, PI / 6, PI / 7);
+//            Vector3 position(0, 0, 0);
+//            Quaternion orientation = Quaternion::identity();
             mBodyTransform = Transform(position, orientation);
 
             // Create the bodies
@@ -169,10 +175,14 @@ class TestRaycast : public Test {
             mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform);
             mCylinderBody = mWorld->createCollisionBody(mBodyTransform);
             mCompoundBody = mWorld->createCollisionBody(mBodyTransform);
+            mTriangleBody = mWorld->createCollisionBody(mBodyTransform);
 
             // Collision shape transform
+            // TODO : Uncomment this
             Vector3 shapePosition(1, -4, -3);
             Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3);
+//            Vector3 shapePosition(0, 0, 0);
+//            Quaternion shapeOrientation = Quaternion::identity();
             mShapeTransform = Transform(shapePosition, shapeOrientation);
 
             // Compute the the transform from a local shape point to world-space
@@ -185,6 +195,12 @@ class TestRaycast : public Test {
             mSphereShape = new SphereShape(3);
             mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform);
 
+            const Vector3 triangleVertex1(100, 100, 0);
+            const Vector3 triangleVertex2(105, 100, 0);
+            const Vector3 triangleVertex3(100, 103, 0);
+            mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3);
+            mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform);
+
             mCapsuleShape = new CapsuleShape(2, 5);
             mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform);
 
@@ -249,6 +265,7 @@ class TestRaycast : public Test {
             mCylinderProxyShape->setCollisionCategoryBits(CATEGORY2);
             mCompoundSphereProxyShape->setCollisionCategoryBits(CATEGORY2);
             mCompoundCylinderProxyShape->setCollisionCategoryBits(CATEGORY2);
+            mTriangleProxyShape->setCollisionCategoryBits(CATEGORY1);
         }
 
         /// Destructor
@@ -260,6 +277,7 @@ class TestRaycast : public Test {
             delete mConvexMeshShape;
             delete mConvexMeshShapeEdgesInfo;
             delete mCylinderShape;
+            delete mTriangleShape;
         }
 
         /// Run the tests
@@ -271,6 +289,7 @@ class TestRaycast : public Test {
             testConvexMesh();
             testCylinder();
             testCompound();
+            testTriangle();
         }
 
         /// Test the ProxyBoxShape::raycast(), CollisionBody::raycast() and
@@ -935,6 +954,119 @@ class TestRaycast : public Test {
             test(mCallback.isHit);
         }
 
+        /// Test the ProxySphereShape::raycast(), CollisionBody::raycast() and
+        /// CollisionWorld::raycast() methods.
+        void testTriangle() {
+
+            // ----- Test feedback data ----- //
+            Vector3 point1 = mLocalShapeToWorld * Vector3(101, 101, 400);
+            Vector3 point2 = mLocalShapeToWorld * Vector3(101, 101, -200);
+            Ray ray(point1, point2);
+            Vector3 hitPoint = mLocalShapeToWorld * Vector3(101, 101, 0);
+
+            mCallback.shapeToTest = mTriangleProxyShape;
+
+            // CollisionWorld::raycast()
+            mCallback.reset();
+            mWorld->raycast(ray, &mCallback);
+            test(mCallback.isHit);
+            test(mCallback.raycastInfo.body == mTriangleBody);
+            test(mCallback.raycastInfo.proxyShape == mTriangleProxyShape);
+            test(approxEqual(mCallback.raycastInfo.hitFraction, 0.2, epsilon));
+            test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint.x, epsilon));
+            test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint.y, epsilon));
+            test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint.z, epsilon));
+
+            // Correct category filter mask
+            mCallback.reset();
+            mWorld->raycast(ray, &mCallback, CATEGORY1);
+            test(mCallback.isHit);
+
+            // Wrong category filter mask
+            mCallback.reset();
+            mWorld->raycast(ray, &mCallback, CATEGORY2);
+            test(!mCallback.isHit);
+
+            // CollisionBody::raycast()
+            RaycastInfo raycastInfo2;
+            test(mTriangleBody->raycast(ray, raycastInfo2));
+            test(raycastInfo2.body == mTriangleBody);
+            test(raycastInfo2.proxyShape == mTriangleProxyShape);
+            test(approxEqual(raycastInfo2.hitFraction, 0.2, epsilon));
+            test(approxEqual(raycastInfo2.worldPoint.x, hitPoint.x, epsilon));
+            test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon));
+            test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon));
+
+            // ProxyCollisionShape::raycast()
+            RaycastInfo raycastInfo3;
+            test(mTriangleProxyShape->raycast(ray, raycastInfo3));
+            test(raycastInfo3.body == mTriangleBody);
+            test(raycastInfo3.proxyShape == mTriangleProxyShape);
+            test(approxEqual(raycastInfo3.hitFraction, 0.2, epsilon));
+            test(approxEqual(raycastInfo3.worldPoint.x, hitPoint.x, epsilon));
+            test(approxEqual(raycastInfo3.worldPoint.y, hitPoint.y, epsilon));
+            test(approxEqual(raycastInfo3.worldPoint.z, hitPoint.z, epsilon));
+
+            Ray ray1(mLocalShapeToWorld * Vector3(-10, 10, 4), mLocalShapeToWorld * Vector3(15, 6, -4));
+            Ray ray2(mLocalShapeToWorld * Vector3(102, 107, 5), mLocalShapeToWorld * Vector3(102, 107, -5));
+            Ray ray3(mLocalShapeToWorld * Vector3(106, 102, 6), mLocalShapeToWorld * Vector3(106, 102, -8));
+
+            Ray ray4(mLocalShapeToWorld * Vector3(100.2, 101, 5), mLocalShapeToWorld * Vector3(100.2, 101, -5));
+            Ray ray5(mLocalShapeToWorld * Vector3(100.5, 101.5, 4), mLocalShapeToWorld * Vector3(100.5, 101.5, -54));
+            Ray ray6(mLocalShapeToWorld * Vector3(102, 101, 1), mLocalShapeToWorld * Vector3(102, 102, -1));
+
+            // ----- Test raycast miss ----- //
+            test(!mTriangleBody->raycast(ray1, raycastInfo3));
+            test(!mTriangleProxyShape->raycast(ray1, raycastInfo3));
+            mCallback.reset();
+            mWorld->raycast(ray1, &mCallback);
+            test(!mCallback.isHit);
+            mCallback.reset();
+            mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(0.01)), &mCallback);
+            test(!mCallback.isHit);
+            mCallback.reset();
+            mWorld->raycast(Ray(ray1.point1, ray1.point2, decimal(100.0)), &mCallback);
+            test(!mCallback.isHit);
+
+            test(!mTriangleBody->raycast(ray2, raycastInfo3));
+            test(!mTriangleProxyShape->raycast(ray2, raycastInfo3));
+            mCallback.reset();
+            mWorld->raycast(ray2, &mCallback);
+            test(!mCallback.isHit);
+
+            test(!mTriangleBody->raycast(ray3, raycastInfo3));
+            test(!mTriangleProxyShape->raycast(ray3, raycastInfo3));
+            mCallback.reset();
+            mWorld->raycast(ray3, &mCallback);
+            test(!mCallback.isHit);
+
+            // ----- Test raycast hits ----- //
+            test(mTriangleBody->raycast(ray4, raycastInfo3));
+            test(mTriangleProxyShape->raycast(ray4, raycastInfo3));
+            mCallback.reset();
+            mWorld->raycast(ray4, &mCallback);
+            test(mCallback.isHit);
+            mCallback.reset();
+            mWorld->raycast(Ray(ray4.point1, ray4.point2, decimal(0.8)), &mCallback);
+            test(mCallback.isHit);
+
+            test(mTriangleBody->raycast(ray5, raycastInfo3));
+            test(mTriangleProxyShape->raycast(ray5, raycastInfo3));
+            mCallback.reset();
+            mWorld->raycast(ray5, &mCallback);
+            test(mCallback.isHit);
+            mCallback.reset();
+            mWorld->raycast(Ray(ray5.point1, ray5.point2, decimal(0.8)), &mCallback);
+            test(mCallback.isHit);
+
+            test(mTriangleBody->raycast(ray6, raycastInfo3));
+            test(mTriangleProxyShape->raycast(ray6, raycastInfo3));
+            mCallback.reset();
+            mWorld->raycast(ray6, &mCallback);
+            mCallback.reset();
+            mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0.8)), &mCallback);
+        }
+
         /// Test the ProxyConeShape::raycast(), CollisionBody::raycast() and
         /// CollisionWorld::raycast() methods.
         void testCone() {

testbed/scenes/collisionshapes/CollisionShapesScene.cpp

@@ -45,9 +45,6 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
     // Gravity vector in the dynamics world
     rp3d::Vector3 gravity(0, -9.81, 0);
 
-    // Time step for the physics simulation
-    rp3d::decimal timeStep = 1.0f / 60.0f;
-
     // Create the dynamics world for the physics simulation
     mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
 

testbed/scenes/concavemesh/ConcaveMeshScene.cpp

@@ -165,7 +165,7 @@ void ConcaveMeshScene::reset() {
     // Reset the transform
     mConcaveMesh->resetTransform(rp3d::Transform::identity());
 
-    rp3d::Vector3 spherePos(10.1, 10, 0);
+    rp3d::Vector3 spherePos(0, 15, 0);
     rp3d::Transform sphereTransform(spherePos, rp3d::Quaternion::identity());
     mSphere->resetTransform(sphereTransform);
 }

testbed/scenes/raycast/RaycastScene.cpp

@@ -32,9 +32,9 @@ using namespace raycastscene;
 
 // Constructor
 RaycastScene::RaycastScene(const std::string& name)
-       : SceneDemo(name, SCENE_RADIUS, false), mCurrentBodyIndex(-1), mAreNormalsDisplayed(false),
-         mMeshFolderPath("meshes/"), mRaycastManager(mPhongShader, mMeshFolderPath),
-         mVBOVertices(GL_ARRAY_BUFFER) {
+       : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
+         mRaycastManager(mPhongShader, mMeshFolderPath), mCurrentBodyIndex(-1),
+         mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) {
 
     mIsContactPointsDisplayed = true;
 
@@ -122,6 +122,16 @@ RaycastScene::RaycastScene(const std::string& name)
     mConvexMesh->setColor(mGreyColorDemo);
     mConvexMesh->setSleepingColor(mRedColorDemo);
 
+    // ---------- Concave Mesh ---------- //
+    openglframework::Vector3 position8(0, 0, 0);
+
+    // Create a convex mesh and a corresponding collision body in the dynamics world
+    mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath);
+
+    // Set the color
+    mConcaveMesh->setColor(mGreyColorDemo);
+    mConcaveMesh->setSleepingColor(mRedColorDemo);
+
     // Create the lines that will be used for raycasting
     createLines();
 
@@ -173,6 +183,7 @@ void RaycastScene::changeBody() {
     mCapsule->getCollisionBody()->setIsActive(false);
     mConvexMesh->getCollisionBody()->setIsActive(false);
     mDumbbell->getCollisionBody()->setIsActive(false);
+    mConcaveMesh->getCollisionBody()->setIsActive(false);
 
     switch(mCurrentBodyIndex) {
         case 0: mSphere->getCollisionBody()->setIsActive(true);
@@ -189,6 +200,8 @@ void RaycastScene::changeBody() {
                 break;
         case 6: mDumbbell->getCollisionBody()->setIsActive(true);
                 break;
+        case 7: mConcaveMesh->getCollisionBody()->setIsActive(true);
+                break;
 
     }
 }
@@ -237,9 +250,15 @@ RaycastScene::~RaycastScene() {
     // Destroy the corresponding rigid body from the dynamics world
     mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody());
 
-    // Destroy the convex mesh
+    // Destroy the dumbbell
     delete mDumbbell;
 
+    // Destroy the corresponding rigid body from the dynamics world
+    mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody());
+
+    // Destroy the convex mesh
+    delete mConcaveMesh;
+
     mRaycastManager.resetPoints();
 
     // Destroy the static data for the visual contact points
@@ -345,8 +364,8 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader,
     if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix);
     if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix);
     if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix);
+    if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix);
 
-    //mPhongShader.unbind();
     shader.unbind();
 }
 

testbed/scenes/raycast/RaycastScene.h

@@ -39,6 +39,7 @@
 #include "Capsule.h"
 #include "Line.h"
 #include "ConvexMesh.h"
+#include "ConcaveMesh.h"
 #include "Dumbbell.h"
 #include "VisualContactPoint.h"
 #include "../common/Viewer.h"
@@ -58,7 +59,7 @@ const float CAPSULE_HEIGHT = 5.0f;
 const float DUMBBELL_HEIGHT = 5.0f;
 const int NB_RAYS = 100;
 const float RAY_LENGTH = 30.0f;
-const int NB_BODIES = 7;
+const int NB_BODIES = 8;
 
 // Raycast manager
 class RaycastManager : public rp3d::RaycastCallback {
@@ -145,6 +146,7 @@ class RaycastScene : public SceneDemo {
         Capsule* mCapsule;
         ConvexMesh* mConvexMesh;
         Dumbbell* mDumbbell;
+        ConcaveMesh* mConcaveMesh;
 
         /// Collision world used for the physics simulation
         rp3d::CollisionWorld* mCollisionWorld;