From 6e9a84823a77c5f1aef7e94e44f654ffb3b17021 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 17 Jul 2017 08:05:40 +0200 Subject: [PATCH] Fix issues in collision detection --- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 31 +++++++++++++------ .../narrowphase/SAT/SATAlgorithm.cpp | 4 +-- .../SphereVsConvexPolyhedronAlgorithm.cpp | 5 +++ src/collision/shapes/BoxShape.cpp | 12 ++++--- src/mathematics/mathematics_functions.cpp | 5 +++ src/mathematics/mathematics_functions.h | 3 ++ .../CollisionDetectionScene.cpp | 8 ++--- 7 files changed, 48 insertions(+), 20 deletions(-) mode change 100644 => 100755 src/mathematics/mathematics_functions.cpp mode change 100644 => 100755 src/mathematics/mathematics_functions.h diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 142a7fc9..b538dfa7 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -29,6 +29,7 @@ #include "GJK/GJKAlgorithm.h" #include "collision/shapes/CapsuleShape.h" #include "collision/shapes/ConvexPolyhedronShape.h" +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -47,40 +48,52 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { - // GJK has found a shallow contact. If the contact normal is parallel to a face of the - // polyhedron mesh, we would like to create two contact points instead of a single one - // (as in the deep contact case with SAT algorithm) + // GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the + // capsule inner segment and parallel to the contact point normal, we would like to create + // two contact points instead of a single one (as in the deep contact case with SAT algorithm) // Get the contact point created by GJK ContactPointInfo* contactPoint = contactManifoldInfo.getFirstContactPointInfo(); + assert(contactPoint != nullptr); bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; - assert(isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); - assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); // Get the collision shapes const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); // For each face of the polyhedron - // For each face of the convex mesh for (uint f = 0; f < polyhedron->getNbFaces(); f++) { // Get the face HalfEdgeStructure::Face face = polyhedron->getFace(f); const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; // Get the face normal const Vector3 faceNormal = polyhedron->getFaceNormal(f); const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal; - // If the polyhedron face normal is parallel to the computed GJK contact normal - if (areParallelVectors(faceNormalWorld, contactPoint->normal)) { + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleInnerSegmentWorld = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA); + + bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0); + bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal); + + // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal + // is in direction of the contact normal (from the polyhedron point of view). + if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentWorld) + && areParallelVectors(faceNormalWorld, contactPoint->normal)) { // Remove the previous contact point computed by GJK contactManifoldInfo.reset(); diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 3bd3e796..d204a88e 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -296,7 +296,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* } } - // We the shapes are still overlapping in the same axis as in + // If the shapes are still overlapping in the same axis as in the previous frame // the previous frame, we skip the whole SAT algorithm if (!isTemporalCoherenceValid) { @@ -470,7 +470,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outFaceNormalCapsuleSpace, nullptr); const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]); const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint; - const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal); + const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(outFaceNormalCapsuleSpace); return penetrationDepth; } diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index bb43bd33..068a79ff 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -41,6 +41,11 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar GJKAlgorithm gjkAlgorithm; GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE || + narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index e1c97844..af6a30b8 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -57,15 +57,15 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin) std::vector face0; face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3); std::vector face1; - face1.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2); + face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2); std::vector face2; - face2.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5); + face2.push_back(4); face2.push_back(7); face2.push_back(6); face2.push_back(5); std::vector face3; - face3.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7); + face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7); std::vector face4; - face4.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0); + face4.push_back(4); face4.push_back(5); face4.push_back(1); face4.push_back(0); std::vector face5; - face5.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3); + face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3); mHalfEdgeStructure.addFace(face0); mHalfEdgeStructure.addFace(face1); @@ -73,6 +73,8 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin) mHalfEdgeStructure.addFace(face3); mHalfEdgeStructure.addFace(face4); mHalfEdgeStructure.addFace(face5); + + mHalfEdgeStructure.init(); } // Return the local inertia tensor of the collision shape diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp old mode 100644 new mode 100755 index e01c749a..24329732 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -65,6 +65,11 @@ bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& v return vector1.cross(vector2).lengthSquare() < decimal(0.00001); } +/// Return true if two vectors are orthogonal +bool reactphysics3d::areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) { + return std::abs(vector1.dot(vector2)) < decimal(0.00001); +} + /// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h old mode 100644 new mode 100755 index c695f342..5ee03a19 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -79,6 +79,9 @@ inline bool sameSign(decimal a, decimal b) { /// Return true if two vectors are parallel bool areParallelVectors(const Vector3& vector1, const Vector3& vector2); +/// Return true if two vectors are orthogonal +bool areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2); + /// Clamp a vector such that it is no longer than a given maximum length Vector3 clamp(const Vector3& vector, decimal maxLength); diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 6351028a..e8f09670 100755 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -61,7 +61,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mSphere1->setSleepingColor(mRedColorDemo); // ---------- Sphere 2 ---------- // - openglframework::Vector3 position2(0, 0, 0); + openglframework::Vector3 position2(12, 8, 0); // Create a sphere and a corresponding collision body in the dynamics world mSphere2 = new Sphere(2, position2, mCollisionWorld, mMeshFolderPath); @@ -72,7 +72,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mSphere2->setSleepingColor(mRedColorDemo); // ---------- Capsule 1 ---------- // - openglframework::Vector3 position3(8, 0, 0); + openglframework::Vector3 position3(0, -12, 0); // Create a cylinder and a corresponding collision body in the dynamics world mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath); @@ -94,7 +94,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mCapsule2->setSleepingColor(mRedColorDemo); // ---------- Box 1 ---------- // - openglframework::Vector3 position5(0, -12, 0); + openglframework::Vector3 position5(0, -0, 0); // Create a cylinder and a corresponding collision body in the dynamics world mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath); @@ -105,7 +105,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mBox1->setSleepingColor(mRedColorDemo); // ---------- Box 2 ---------- // - openglframework::Vector3 position6(0, 12, 0); + openglframework::Vector3 position6(0, 8, 0); // Create a cylinder and a corresponding collision body in the dynamics world mBox2 = new Box(openglframework::Vector3(3, 2, 5), position6, mCollisionWorld, mMeshFolderPath);