Implement the testPointInside() methods in the collision shapes

src/body/CollisionBody.h

@@ -178,7 +178,7 @@ class CollisionBody : public Body {
         friend class DynamicsWorld;
         friend class CollisionDetection;
         friend class BroadPhaseAlgorithm;
-        friend class ProxyConvexMeshShape;
+        friend class ConvexMeshShape;
 };
 
 // Return the type of the body

src/collision/CollisionDetection.h

@@ -148,7 +148,7 @@ class CollisionDetection {
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
-        friend class ProxyConvexMeshShape;
+        friend class ConvexMeshShape;
 };
 
 // Select the narrow-phase collision algorithm to use given two collision shapes

src/collision/ProxyShape.cpp

@@ -25,6 +25,6 @@ ProxyShape::~ProxyShape() {
 bool ProxyShape::testPointInside(const Vector3& worldPoint) {
     const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform;
     const Vector3 localPoint = localToWorld.getInverse() * worldPoint;
-    return mCollisionShape->testPointInside(localPoint);
+    return mCollisionShape->testPointInside(localPoint, this);
 }
 

src/collision/ProxyShape.h

@@ -113,6 +113,7 @@ class ProxyShape {
         friend class CollisionDetection;
         friend class EPAAlgorithm;
         friend class GJKAlgorithm;
+        friend class ConvexMeshShape;
 };
 
 /// Return the collision shape

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -332,28 +332,20 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisi
 }
 
 // Use the GJK Algorithm to find if a point is inside a convex collision shape
-bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, ProxyShape* collisionShape) {
+bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* collisionShape) {
 
     Vector3 suppA;             // Support point of object A
     Vector3 w;                 // Support point of Minkowski difference A-B
-    //Vector3 pA;                // Closest point of object A
-    //Vector3 pB;                // Closest point of object B
     decimal vDotw;
     decimal prevDistSquare;
 
-    // Get the local-space to world-space transforms
-    const Transform localToWorldTransform = collisionShape->getBody()->getTransform() *
-                                 collisionShape->getLocalToBodyTransform();
-    const Transform worldToLocalTransform = localToWorldTransform.getInverse();
-
     // Support point of object B (object B is a single point)
-    const Vector3 suppB = worldToLocalTransform * worldPoint;
+    const Vector3 suppB(localPoint);
 
     // Create a simplex set
     Simplex simplex;
 
-    // Get the previous point V (last cached separating axis)
+    // Initial supporting direction
-    // TODO : Cache separating axis
     Vector3 v(1, 1, 1);
 
     // Initialize the upper bound for the square distance
@@ -363,57 +355,12 @@ bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, ProxyShape* collis
 
         // Compute the support points for original objects (without margins) A and B
         suppA = collisionShape->getLocalSupportPointWithoutMargin(-v);
-        //suppB = body2Tobody1 *
-        //             collisionShape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v);
 
         // Compute the support point for the Minkowski difference A-B
         w = suppA - suppB;
 
         vDotw = v.dot(w);
 
-        /*
-        // If the enlarge objects (with margins) do not intersect
-        if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
-
-            // Cache the current separating axis for frame coherence
-            mCurrentOverlappingPair->setCachedSeparatingAxis(v);
-
-            // No intersection, we return false
-            return false;
-        }
-        */
-
-        /*
-        // If the objects intersect only in the margins
-        if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
-
-            // Compute the closet points of both objects (without the margins)
-            simplex.computeClosestPointsOfAandB(pA, pB);
-
-            // Project those two points on the margins to have the closest points of both
-            // object with the margins
-            decimal dist = sqrt(distSquare);
-            assert(dist > 0.0);
-            pA = (pA - (collisionShape1->getMargin() / dist) * v);
-            pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
-
-            // Compute the contact info
-            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
-            decimal penetrationDepth = margin - dist;
-
-            // Reject the contact if the penetration depth is negative (due too numerical errors)
-            if (penetrationDepth <= 0.0) return false;
-
-            // Create the contact info object
-            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
-                                 ContactPointInfo(collisionShape1, collisionShape2, normal,
-                                                  penetrationDepth, pA, pB);
-
-            // There is an intersection, therefore we return true
-            return true;
-        }
-        */
-
         // Add the new support point to the simplex
         simplex.addPoint(w, suppA, suppB);
 
@@ -421,33 +368,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, ProxyShape* collis
         if (simplex.isAffinelyDependent()) {
 
             return false;
-
-            /*
-            // Compute the closet points of both objects (without the margins)
-            simplex.computeClosestPointsOfAandB(pA, pB);
-
-            // Project those two points on the margins to have the closest points of both
-            // object with the margins
-            decimal dist = sqrt(distSquare);
-            assert(dist > 0.0);
-            pA = (pA - (collisionShape1->getMargin() / dist) * v);
-            pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
-
-            // Compute the contact info
-            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
-            decimal penetrationDepth = margin - dist;
-
-            // Reject the contact if the penetration depth is negative (due too numerical errors)
-            if (penetrationDepth <= 0.0) return false;
-
-            // Create the contact info object
-            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
-                                   ContactPointInfo(collisionShape1, collisionShape2, normal,
-                                                    penetrationDepth, pA, pB);
-
-            // There is an intersection, therefore we return true
-            return true;
-            */
         }
 
         // Compute the point of the simplex closest to the origin
@@ -455,33 +375,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, ProxyShape* collis
         if (!simplex.computeClosestPoint(v)) {
 
             return false;
-
-            /*
-            // Compute the closet points of both objects (without the margins)
-            simplex.computeClosestPointsOfAandB(pA, pB);
-
-            // Project those two points on the margins to have the closest points of both
-            // object with the margins
-            decimal dist = sqrt(distSquare);
-            assert(dist > 0.0);
-            pA = (pA - (collisionShape1->getMargin() / dist) * v);
-            pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
-
-            // Compute the contact info
-            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
-            decimal penetrationDepth = margin - dist;
-
-            // Reject the contact if the penetration depth is negative (due too numerical errors)
-            if (penetrationDepth <= 0.0) return false;
-
-            // Create the contact info object
-            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
-                                 ContactPointInfo(collisionShape1, collisionShape2, normal,
-                                                  penetrationDepth, pA, pB);
-
-            // There is an intersection, therefore we return true
-            return true;
-            */
         }
 
         // Store and update the squared distance of the closest point
@@ -492,49 +385,10 @@ bool GJKAlgorithm::testPointInside(const Vector3& worldPoint, ProxyShape* collis
         if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
 
             return false;
-
-            /*
-            simplex.backupClosestPointInSimplex(v);
-
-            // Get the new squared distance
-            distSquare = v.lengthSquare();
-
-            // Compute the closet points of both objects (without the margins)
-            simplex.computeClosestPointsOfAandB(pA, pB);
-
-            // Project those two points on the margins to have the closest points of both
-            // object with the margins
-            decimal dist = sqrt(distSquare);
-            assert(dist > 0.0);
-            pA = (pA - (collisionShape1->getMargin() / dist) * v);
-            pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
-
-            // Compute the contact info
-            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
-            decimal penetrationDepth = margin - dist;
-
-            // Reject the contact if the penetration depth is negative (due too numerical errors)
-            if (penetrationDepth <= 0.0) return false;
-
-            // Create the contact info object
-            contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
-                                   ContactPointInfo(collisionShape1, collisionShape2, normal,
-                                                    penetrationDepth, pA, pB);
-
-            // There is an intersection, therefore we return true
-            return true;
-            */
         }
     } while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
                                  simplex.getMaxLengthSquareOfAPoint());
 
     // The point is inside the collision shape
     return true;
-
-    // The objects (without margins) intersect. Therefore, we run the GJK algorithm
-    // again but on the enlarged objects to compute a simplex polytope that contains
-    // the origin. Then, we give that simplex polytope to the EPA algorithm to compute
-    // the correct penetration depth and contact points between the enlarged objects.
-    //return computePenetrationDepthForEnlargedObjects(collisionShape1, transform1, collisionShape2,
-    //                                                 transform2, contactInfo, v);
 }

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -95,7 +95,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
                                    ContactPointInfo*& contactInfo);
 
         /// Use the GJK Algorithm to find if a point is inside a convex collision shape
-        bool testPointInside(const Vector3& worldPoint, ProxyShape *collisionShape);
+        bool testPointInside(const Vector3& localPoint, ProxyShape *collisionShape);
 };
 
 }

src/collision/shapes/BoxShape.cpp

@@ -72,9 +72,3 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distanc
     // TODO : Implement this method
     return false;
 }
-
-// Return true if a point is inside the collision shape
-bool BoxShape::testPointInside(const Vector3& localPoint) const {
-    // TODO : Implement this method
-    return false;
-}

src/collision/shapes/BoxShape.h

@@ -76,7 +76,7 @@ class BoxShape : public CollisionShape {
                                                           void** cachedCollisionData) const;
 
         /// Return true if a point is inside the collision shape
-        virtual bool testPointInside(const Vector3& localPoint) const;
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
 
     public :
 
@@ -166,6 +166,13 @@ inline bool BoxShape::isEqualTo(const CollisionShape& otherCollisionShape) const
     return (mExtent == otherShape.mExtent);
 }
 
+// Return true if a point is inside the collision shape
+inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
+    return (localPoint.x < mExtent[0] && localPoint.x > -mExtent[0] &&
+            localPoint.y < mExtent[1] && localPoint.y > -mExtent[1] &&
+            localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]);
+}
+
 }
 
 #endif

src/collision/shapes/CapsuleShape.cpp

@@ -139,7 +139,17 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal dis
 }
 
 // Return true if a point is inside the collision shape
-bool CapsuleShape::testPointInside(const Vector3& localPoint) const {
+bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
-    // TODO : Implement this method
+
-    return false;
+    const decimal diffYCenterSphere1 = localPoint.y - mHalfHeight;
+    const decimal diffYCenterSphere2 = localPoint.y + mHalfHeight;
+    const decimal xSquare = localPoint.x * localPoint.x;
+    const decimal zSquare = localPoint.z * localPoint.z;
+    const decimal squareRadius = mRadius * mRadius;
+
+    // Return true if the point is inside the cylinder or one of the two spheres of the capsule
+    return ((xSquare + zSquare) < squareRadius &&
+            localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) ||
+            (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
+            (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
 }

src/collision/shapes/CapsuleShape.h

@@ -73,7 +73,7 @@ class CapsuleShape : public CollisionShape {
                                                           void** cachedCollisionData) const;
 
         /// Return true if a point is inside the collision shape
-        virtual bool testPointInside(const Vector3& localPoint) const;
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
 
     public :
 

src/collision/shapes/CollisionShape.h

@@ -83,7 +83,7 @@ class CollisionShape {
                                                           void** cachedCollisionData) const=0;
 
         /// Return true if a point is inside the collision shape
-        virtual bool testPointInside(const Vector3& worldPoint) const=0;
+        virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
 
         /// Raycast method
         virtual bool raycast(const Ray& ray, decimal distance = RAYCAST_INFINITY_DISTANCE) const=0;

src/collision/shapes/ConeShape.cpp

@@ -105,9 +105,3 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distan
     // TODO : Implement this method
     return false;
 }
-
-// Return true if a point is inside the collision shape
-bool ConeShape::testPointInside(const Vector3& localPoint) const {
-    // TODO : Implement this method
-    return false;
-}

src/collision/shapes/ConeShape.h

@@ -81,7 +81,7 @@ class ConeShape : public CollisionShape {
                                                           void** cachedCollisionData) const;
 
         /// Return true if a point is inside the collision shape
-        virtual bool testPointInside(const Vector3& localPoint) const;
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
         
     public :
 
@@ -171,6 +171,14 @@ inline bool ConeShape::isEqualTo(const CollisionShape& otherCollisionShape) cons
     return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
 }
 
+// Return true if a point is inside the collision shape
+inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
+    const decimal radiusHeight = mRadius * (-localPoint.y + mHalfHeight) /
+                                          (mHalfHeight * decimal(2.0));
+    return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) &&
+           (localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight);
+}
+
 }
 
 #endif

src/collision/shapes/ConvexMeshShape.h

@@ -102,7 +102,7 @@ class ConvexMeshShape : public CollisionShape {
                                                           void** cachedCollisionData) const;
 
         /// Return true if a point is inside the collision shape
-        virtual bool testPointInside(const Vector3& localPoint) const;
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
 
     public :
 
@@ -237,9 +237,12 @@ inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
 }
 
 // Return true if a point is inside the collision shape
-inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint) const {
+inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
-    // TODO : Implement this
+                                             ProxyShape* proxyShape) const {
-    return false;
+
+    // Use the GJK algorithm to test if the point is inside the convex mesh
+    return proxyShape->mBody->mWorld.mCollisionDetection.
+           mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
 }
 
 }

src/collision/shapes/CylinderShape.cpp

@@ -98,9 +98,3 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal di
     // TODO : Implement this method
     return false;
 }
-
-// Return true if a point is inside the collision shape
-bool CylinderShape::testPointInside(const Vector3& localPoint) const {
-    // TODO : Implement this method
-    return false;
-}

src/collision/shapes/CylinderShape.h

@@ -78,7 +78,7 @@ class CylinderShape : public CollisionShape {
                                                           void** cachedCollisionData) const;
 
         /// Return true if a point is inside the collision shape
-        virtual bool testPointInside(const Vector3& localPoint) const;
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
 
     public :
 
@@ -168,6 +168,11 @@ inline bool CylinderShape::isEqualTo(const CollisionShape& otherCollisionShape)
     return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
 }
 
+// Return true if a point is inside the collision shape
+inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{
+    return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius &&
+            localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight);
+}
 
 }
 

src/collision/shapes/SphereShape.cpp

@@ -57,9 +57,3 @@ bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal dist
     // TODO : Implement this method
     return false;
 }
-
-// Return true if a point is inside the collision shape
-bool SphereShape::testPointInside(const Vector3& localPoint) const {
-    // TODO : Implement this method
-    return false;
-}

src/collision/shapes/SphereShape.h

@@ -68,7 +68,7 @@ class SphereShape : public CollisionShape {
                                                           void** cachedCollisionData) const;
 
         /// Return true if a point is inside the collision shape
-        virtual bool testPointInside(const Vector3& localPoint) const;
+        virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
 
     public :
 
@@ -188,6 +188,11 @@ inline bool SphereShape::isEqualTo(const CollisionShape& otherCollisionShape) co
     return (mRadius == otherShape.mRadius);
 }
 
+// Return true if a point is inside the collision shape
+inline bool SphereShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
+    return (localPoint.lengthSquare() < mRadius * mRadius);
+}
+
 }
 
 #endif

src/engine/CollisionWorld.h

@@ -132,7 +132,7 @@ class CollisionWorld {
         friend class CollisionDetection;
         friend class CollisionBody;
         friend class RigidBody;
-        friend class ProxyConvexMeshShape;
+        friend class ConvexMeshShape;
 };
 
 // Return an iterator to the beginning of the bodies of the physics world