From 3c1b819fdab60131aa5c7e3aed07cd0b7569bd32 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 9 Aug 2014 10:28:37 +0200 Subject: [PATCH] Implement the testPointInside() methods in the collision shapes --- src/body/CollisionBody.h | 2 +- src/collision/CollisionDetection.h | 2 +- src/collision/ProxyShape.cpp | 2 +- src/collision/ProxyShape.h | 1 + .../narrowphase/GJK/GJKAlgorithm.cpp | 152 +----------------- src/collision/narrowphase/GJK/GJKAlgorithm.h | 2 +- src/collision/shapes/BoxShape.cpp | 6 - src/collision/shapes/BoxShape.h | 9 +- src/collision/shapes/CapsuleShape.cpp | 16 +- src/collision/shapes/CapsuleShape.h | 2 +- src/collision/shapes/CollisionShape.h | 2 +- src/collision/shapes/ConeShape.cpp | 6 - src/collision/shapes/ConeShape.h | 10 +- src/collision/shapes/ConvexMeshShape.h | 11 +- src/collision/shapes/CylinderShape.cpp | 6 - src/collision/shapes/CylinderShape.h | 7 +- src/collision/shapes/SphereShape.cpp | 6 - src/collision/shapes/SphereShape.h | 7 +- src/engine/CollisionWorld.h | 2 +- 19 files changed, 60 insertions(+), 191 deletions(-) diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index e2f542b9..7e12f783 100644 --- a/src/body/CollisionBody.h +++ b/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 diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 00ec5b5a..cedf69fd 100644 --- a/src/collision/CollisionDetection.h +++ b/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 diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp index 396bef0d..81e0d322 100644 --- a/src/collision/ProxyShape.cpp +++ b/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); } diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index e4646692..8abf88e1 100644 --- a/src/collision/ProxyShape.h +++ b/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 diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index bd3f5757..88992d94 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/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) - // TODO : Cache separating axis + // Initial supporting direction 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); } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index 6f1e7211..b3040be5 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/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); }; } diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 1e680650..314251e8 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/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; -} diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index bd6b2d65..76b6a4d1 100644 --- a/src/collision/shapes/BoxShape.h +++ b/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 diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp index be12a362..74f19f1d 100644 --- a/src/collision/shapes/CapsuleShape.cpp +++ b/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 { - // TODO : Implement this method - return false; +bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { + + 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; } diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index ea83f889..3759db0c 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/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 : diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index e931d94b..e7581a77 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/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; diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp index 82f7a119..ad39ed41 100644 --- a/src/collision/shapes/ConeShape.cpp +++ b/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; -} diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h index 2cfdddd7..ef56bc5b 100644 --- a/src/collision/shapes/ConeShape.h +++ b/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 diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 43e4df94..e57d1243 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/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 { - // TODO : Implement this - return false; +inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint, + ProxyShape* proxyShape) const { + + // Use the GJK algorithm to test if the point is inside the convex mesh + return proxyShape->mBody->mWorld.mCollisionDetection. + mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); } } diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp index c451dde3..9a809e7d 100644 --- a/src/collision/shapes/CylinderShape.cpp +++ b/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; -} diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h index b0037f65..46106f5d 100644 --- a/src/collision/shapes/CylinderShape.h +++ b/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); +} } diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index fe4fae4f..ec252200 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/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; -} diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index 4271474b..45a42439 100644 --- a/src/collision/shapes/SphereShape.h +++ b/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 diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index fb3209e9..52ae8b01 100644 --- a/src/engine/CollisionWorld.h +++ b/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