From 8b9fdc15a71501ef1990d614ec7a847d809cf98b Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 27 Jun 2020 23:48:19 +0200
Subject: [PATCH 1/7] =?UTF-8?q?Fix=20issue=20in=20SAT=20with=20edge-edge?=
=?UTF-8?q?=20contact=20(wrong=20contact=20normal)=20and=20favor=20face=20?=
=?UTF-8?q?contacts=20over=20edge-edge=20contacts=20in=20polyhedron=20vs?=
=?UTF-8?q?=20polyhedron=20collision=20in=20SAT=C2=A0algorithm=20for=20bet?=
=?UTF-8?q?ter=20stability?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../narrowphase/SAT/SATAlgorithm.cpp | 35 ++++++-------------
1 file changed, 11 insertions(+), 24 deletions(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 54f8e90a..4271b900 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -657,18 +657,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal
- // We use the direction from the centroid to the edge of the shape that is not a triangle
- // to avoid possible degeneracies when axis direction is orthogonal to triangle normal
- Vector3 normal;
- if (isShape1Triangle) {
- normal = polyhedron2->getCentroid() - closestPointPolyhedron2Edge;
- }
- else {
- normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid());
- }
-
- //Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
- Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normal.getUnit();
+ Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * separatingAxisPolyhedron2Space;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
@@ -794,7 +783,15 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
break;
}
- if (penetrationDepth < minPenetrationDepth) {
+ // If the current minimum penetration depth is along a face normal axis (isMinPenetrationFaceNormal=true) and we have found a new
+ // smaller pentration depth along an edge-edge cross-product axis we want to favor the face normal axis because contact manifolds between
+ // faces have more contact points and therefore more stable than the single contact point of an edge-edge collision. It means that if the new minimum
+ // penetration depth from the edge-edge contact is only a little bit smaller than the current minPenetrationDepth (from a face contact), we favor
+ // the face contact and do not generate an edge-edge contact. However, if the new penetration depth from the edge-edge contact is really smaller than
+ // the current one, we generate an edge-edge contact.
+ // To do this, we use a relative and absolute bias to increase a little bit the new penetration depth from the edge-edge contact during the comparison test
+ if ((isMinPenetrationFaceNormal && penetrationDepth1 * SEPARATING_AXIS_RELATIVE_TOLERANCE + SEPARATING_AXIS_ABSOLUTE_TOLERANCE < minPenetrationDepth) ||
+ (!isMinPenetrationFaceNormal && penetrationDepth < minPenetrationDepth)) {
minPenetrationDepth = penetrationDepth;
isMinPenetrationFaceNormalPolyhedron1 = false;
@@ -862,17 +859,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal
- // We use the direction from the centroid to the edge of the shape that is not a triangle
- // to avoid possible degeneracies when axis direction is orthogonal to triangle normal
- Vector3 normal;
- if (isShape1Triangle) {
- normal = polyhedron2->getCentroid() - closestPointPolyhedron2Edge;
- }
- else {
- normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid());
- }
- //Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
- Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normal.getUnit();
+ Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex],
From fa05e1561eea366a47e601663ea0af07f031aee1 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 7 Jul 2020 18:27:01 +0200
Subject: [PATCH 2/7] Use const parameter in PhysicsWorld::setGravity() method
---
include/reactphysics3d/engine/PhysicsWorld.h | 2 +-
src/engine/PhysicsWorld.cpp | 2 +-
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/include/reactphysics3d/engine/PhysicsWorld.h b/include/reactphysics3d/engine/PhysicsWorld.h
index 9aa3da2c..8ebf4ab1 100644
--- a/include/reactphysics3d/engine/PhysicsWorld.h
+++ b/include/reactphysics3d/engine/PhysicsWorld.h
@@ -399,7 +399,7 @@ class PhysicsWorld {
Vector3 getGravity() const;
/// Set the gravity vector of the world
- void setGravity(Vector3& gravity);
+ void setGravity(const Vector3& gravity);
/// Return if the gravity is on
bool isGravityEnabled() const;
diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp
index b188f635..19e55eab 100644
--- a/src/engine/PhysicsWorld.cpp
+++ b/src/engine/PhysicsWorld.cpp
@@ -975,7 +975,7 @@ void PhysicsWorld::setNbIterationsPositionSolver(uint nbIterations) {
/**
* @param gravity The gravity vector (in meter per seconds squared)
*/
-void PhysicsWorld::setGravity(Vector3& gravity) {
+void PhysicsWorld::setGravity(const Vector3& gravity) {
mConfig.gravity = gravity;
From 68212e26d875c1c758fc525159eee738ac94a936 Mon Sep 17 00:00:00 2001
From: manuel5975p <manuel.winkler@gmx.ch>
Date: Sun, 12 Jul 2020 01:01:40 +0200
Subject: [PATCH 3/7] Fix typos in documentation
Add variable type in declaration
Fix PolygonaVertexArray -> PolygonVertexArray
---
documentation/UserManual/ReactPhysics3D-UserManual.tex | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/documentation/UserManual/ReactPhysics3D-UserManual.tex b/documentation/UserManual/ReactPhysics3D-UserManual.tex
index a7a33052..8aaf65bf 100644
--- a/documentation/UserManual/ReactPhysics3D-UserManual.tex
+++ b/documentation/UserManual/ReactPhysics3D-UserManual.tex
@@ -1201,7 +1201,7 @@ indices[16]=2; indices[17]=3; indices[18]=7; indices[19]=6;
indices[20]=0; indices[21]=4; indices[22]=7; indices[23]=3;
// Description of the six faces of the convex mesh
-polygonFaces = new PolygonVertexArray::PolygonFace[6];
+PolygonVertexArray::PolygonFace* polygonFaces = new PolygonVertexArray::PolygonFace[6];
PolygonVertexArray::PolygonFace* face = polygonFaces;
for (int f = 0; f < 6; f++) {
@@ -1215,7 +1215,7 @@ for (int f = 0; f < 6; f++) {
}
// Create the polygon vertex array
-PolygonaVertexArray* polygonVertexArray = new PolygonVertexArray(8, vertices, 3 x sizeof(float),
+PolygonVertexArray* polygonVertexArray = new PolygonVertexArray(8, vertices, 3 x sizeof(float),
indices, sizeof(int), 6, polygonFaces,
PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
From 3b2f973ffdffbcbfaa998bc59bb478a245d631a7 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 19 Jul 2020 01:02:02 +0200
Subject: [PATCH 4/7] Fix issue in Quaternion::Quaternion(Matrix3x3& matrix)
constructor and add corresponding unit test
---
src/mathematics/Quaternion.cpp | 2 +-
test/tests/mathematics/TestQuaternion.h | 15 +++++++++++++++
2 files changed, 16 insertions(+), 1 deletion(-)
diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp
index d1451a3d..a8918f93 100644
--- a/src/mathematics/Quaternion.cpp
+++ b/src/mathematics/Quaternion.cpp
@@ -106,7 +106,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
// Compute the quaternion
x = decimal(0.5) * r;
y = (matrix[0][1] + matrix[1][0]) * s;
- z = (matrix[2][0] - matrix[0][2]) * s;
+ z = (matrix[2][0] + matrix[0][2]) * s;
w = (matrix[2][1] - matrix[1][2]) * s;
}
}
diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h
index 3a25cba9..042aa577 100644
--- a/test/tests/mathematics/TestQuaternion.h
+++ b/test/tests/mathematics/TestQuaternion.h
@@ -90,6 +90,21 @@ class TestQuaternion : public Test {
rp3d_test(approxEqual(quaternion4.z, mQuaternion1.z));
rp3d_test(approxEqual(quaternion4.w, mQuaternion1.w));
+ Matrix3x3 original(0.001743,-0.968608,0.248589,-0.614229,-0.197205,-0.764090,0.789126,-0.151359,-0.595290);
+ Matrix3x3 converted = Quaternion(original).getMatrix();
+ rp3d_test(approxEqual(original[0][0], converted[0][0], 0.0001));
+ rp3d_test(approxEqual(original[0][1], converted[0][1], 0.0001));
+ rp3d_test(approxEqual(original[0][2], converted[0][2], 0.0001));
+ rp3d_test(approxEqual(original[1][0], converted[1][0], 0.0001));
+ rp3d_test(approxEqual(original[1][1], converted[1][1], 0.0001));
+ rp3d_test(approxEqual(original[1][2], converted[1][2], 0.0001));
+ rp3d_test(approxEqual(original[2][0], converted[2][0], 0.0001));
+ rp3d_test(approxEqual(original[2][1], converted[2][1], 0.0001));
+ rp3d_test(approxEqual(original[2][2], converted[2][2], 0.0001));
+
+ std::cout << original.to_string() << std::endl;
+ std::cout << converted.to_string() << std::endl;
+
// Test conversion from Euler angles to quaternion
const decimal PI_OVER_2 = PI * decimal(0.5);
From e7d9e106e92584013b76c4f08269615557687496 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 24 Jul 2020 07:37:09 +0200
Subject: [PATCH 5/7] Remove unused friend class declaration
---
include/reactphysics3d/collision/shapes/CollisionShape.h | 1 -
1 file changed, 1 deletion(-)
diff --git a/include/reactphysics3d/collision/shapes/CollisionShape.h b/include/reactphysics3d/collision/shapes/CollisionShape.h
index 77b56de6..ad7bb3d2 100644
--- a/include/reactphysics3d/collision/shapes/CollisionShape.h
+++ b/include/reactphysics3d/collision/shapes/CollisionShape.h
@@ -164,7 +164,6 @@ class CollisionShape {
friend class Collider;
friend class CollisionBody;
friend class RigidBody;
- friend class PhyscisWorld;
friend class BroadPhaseSystem;
};
From b5a7454f4cfc1a955191e7a8207ebfe1970a2c84 Mon Sep 17 00:00:00 2001
From: SlavicPotato <sp@eth.si>
Date: Tue, 11 Aug 2020 09:14:18 +0200
Subject: [PATCH 6/7] Fix DebugRenderer contact point sphere radius
---
src/utils/DebugRenderer.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/utils/DebugRenderer.cpp b/src/utils/DebugRenderer.cpp
index e403253b..4e1533f7 100644
--- a/src/utils/DebugRenderer.cpp
+++ b/src/utils/DebugRenderer.cpp
@@ -463,7 +463,7 @@ void DebugRenderer::onContact(const CollisionCallback::CallbackData& callbackDat
if (getIsDebugItemDisplayed(DebugItem::CONTACT_POINT)) {
// Contact point
- drawSphere(point, DEFAULT_CONTACT_POINT_SPHERE_RADIUS, mMapDebugItemWithColor[DebugItem::CONTACT_POINT]);
+ drawSphere(point, mContactPointSphereRadius, mMapDebugItemWithColor[DebugItem::CONTACT_POINT]);
}
if (getIsDebugItemDisplayed(DebugItem::CONTACT_NORMAL)) {
From 98ba2f10e6f3c21b684d94e2970c89c8fd6a9499 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 2 Sep 2020 21:59:19 +0200
Subject: [PATCH 7/7] Fix issue with contact manifolds order in islands
creation process
---
include/reactphysics3d/engine/PhysicsWorld.h | 5 +++
.../systems/CollisionDetectionSystem.h | 6 ++--
src/engine/PhysicsWorld.cpp | 20 ++++++++++-
src/systems/CollisionDetectionSystem.cpp | 33 ++++++++++++++-----
4 files changed, 52 insertions(+), 12 deletions(-)
diff --git a/include/reactphysics3d/engine/PhysicsWorld.h b/include/reactphysics3d/engine/PhysicsWorld.h
index 8ebf4ab1..907bf7b5 100644
--- a/include/reactphysics3d/engine/PhysicsWorld.h
+++ b/include/reactphysics3d/engine/PhysicsWorld.h
@@ -241,6 +241,11 @@ class PhysicsWorld {
/// All the islands of bodies of the current frame
Islands mIslands;
+ /// Order in which to process the ContactPairs for contact creation such that
+ /// all the contact manifolds and contact points of a given island are packed together
+ /// This array contains the indices of the ContactPairs.
+ List<uint32> mProcessContactPairsOrderIslands;
+
/// Contact solver system
ContactSolverSystem mContactSolverSystem;
diff --git a/include/reactphysics3d/systems/CollisionDetectionSystem.h b/include/reactphysics3d/systems/CollisionDetectionSystem.h
index b2d24f39..4909aca5 100644
--- a/include/reactphysics3d/systems/CollisionDetectionSystem.h
+++ b/include/reactphysics3d/systems/CollisionDetectionSystem.h
@@ -237,9 +237,6 @@ class CollisionDetectionSystem {
void reducePotentialContactManifolds(List<ContactPair>* contactPairs, List<ContactManifoldInfo>& potentialContactManifolds,
const List<ContactPointInfo>& potentialContactPoints) const;
- /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair
- void createContacts();
-
/// Compute the lost contact pairs (contact pairs in contact in the previous frame but not in the current one)
void computeLostContactPairs();
@@ -279,6 +276,9 @@ class CollisionDetectionSystem {
/// Filter the overlapping pairs to keep only the pairs where two given bodies are involved
void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, List<uint64>& convexPairs, List<uint64>& concavePairs) const;
+ /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair
+ void createContacts();
+
public :
// -------------------- Methods -------------------- //
diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp
index 19e55eab..fc59195d 100644
--- a/src/engine/PhysicsWorld.cpp
+++ b/src/engine/PhysicsWorld.cpp
@@ -59,7 +59,7 @@ PhysicsWorld::PhysicsWorld(MemoryManager& memoryManager, const WorldSettings& wo
mSliderJointsComponents(mMemoryManager.getHeapAllocator()), mCollisionDetection(this, mCollidersComponents, mTransformComponents, mCollisionBodyComponents, mRigidBodyComponents,
mMemoryManager),
mCollisionBodies(mMemoryManager.getHeapAllocator()), mEventListener(nullptr),
- mName(worldSettings.worldName), mIslands(mMemoryManager.getSingleFrameAllocator()),
+ mName(worldSettings.worldName), mIslands(mMemoryManager.getSingleFrameAllocator()), mProcessContactPairsOrderIslands(mMemoryManager.getSingleFrameAllocator()),
mContactSolverSystem(mMemoryManager, *this, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
mCollidersComponents, mConfig.restitutionVelocityThreshold),
mConstraintSolverSystem(*this, mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents,
@@ -342,6 +342,9 @@ void PhysicsWorld::update(decimal timeStep) {
// Create the islands
createIslands();
+ // Create the actual narrow-phase contacts
+ mCollisionDetection.createContacts();
+
// Report the contacts to the user
mCollisionDetection.reportContactsAndTriggers();
@@ -374,6 +377,8 @@ void PhysicsWorld::update(decimal timeStep) {
// Reset the islands
mIslands.clear();
+ mProcessContactPairsOrderIslands.clear(true);
+
// Generate debug rendering primitives (if enabled)
if (mIsDebugRenderingEnabled) {
mDebugRenderer.computeDebugRenderingPrimitives(*this);
@@ -747,6 +752,8 @@ void PhysicsWorld::createIslands() {
RP3D_PROFILE("PhysicsWorld::createIslands()", mProfiler);
+ assert(mProcessContactPairsOrderIslands.size() == 0);
+
// Reset all the isAlreadyInIsland variables of bodies and joints
for (uint b=0; b < mRigidBodyComponents.getNbComponents(); b++) {
@@ -814,6 +821,8 @@ void PhysicsWorld::createIslands() {
if (mRigidBodyComponents.hasComponent(pair.body1Entity) && mRigidBodyComponents.hasComponent(pair.body2Entity)
&& !mCollidersComponents.getIsTrigger(pair.collider1Entity) && !mCollidersComponents.getIsTrigger(pair.collider2Entity)) {
+ mProcessContactPairsOrderIslands.add(contactPairs[p]);
+
assert(pair.potentialContactManifoldsIndices.size() > 0);
nbTotalManifolds += pair.potentialContactManifoldsIndices.size();
@@ -871,6 +880,15 @@ void PhysicsWorld::createIslands() {
}
}
+ for (uint32 i=0; i < (*mCollisionDetection.mCurrentContactPairs).size(); i++) {
+
+ ContactPair& contactPair = (*mCollisionDetection.mCurrentContactPairs)[i];
+
+ if (mRigidBodyComponents.hasComponent(contactPair.body1Entity) && mRigidBodyComponents.hasComponent(contactPair.body2Entity)) {
+
+ }
+ }
+
mCollisionDetection.mMapBodyToContactPairs.clear(true);
}
diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp
index 24f9660c..b794b32d 100644
--- a/src/systems/CollisionDetectionSystem.cpp
+++ b/src/systems/CollisionDetectionSystem.cpp
@@ -532,11 +532,6 @@ void CollisionDetectionSystem::computeNarrowPhase() {
assert(mCurrentContactManifolds->size() == 0);
assert(mCurrentContactPoints->size() == 0);
-
- // Create the actual narrow-phase contacts
- createContacts();
-
- mNarrowPhaseInput.clear();
}
// Compute the narrow-phase collision detection for the testOverlap() methods.
@@ -716,10 +711,30 @@ void CollisionDetectionSystem::createContacts() {
mCurrentContactManifolds->reserve(mCurrentContactPairs->size());
mCurrentContactPoints->reserve(mCurrentContactManifolds->size());
- // For each contact pair
- for (uint p=0; p < (*mCurrentContactPairs).size(); p++) {
+ // We go through all the contact pairs and add the pairs with a least a CollisionBody at the end of the
+ // mProcessContactPairsOrderIslands array because those pairs have not been added during the islands
+ // creation (only the pairs with two RigidBodies are added during island creation)
+ Set<uint32> processedContactPairsIndices(mMemoryManager.getSingleFrameAllocator(), mCurrentContactPairs->size());
+ for (uint32 i=0; i < mWorld->mProcessContactPairsOrderIslands.size(); i++) {
+ processedContactPairsIndices.add(mWorld->mProcessContactPairsOrderIslands[i]);
+ }
+ for (uint32 i=0; i < mCurrentContactPairs->size(); i++) {
+ if (!processedContactPairsIndices.contains(i)) {
+ mWorld->mProcessContactPairsOrderIslands.add(i);
+ }
+ }
- ContactPair& contactPair = (*mCurrentContactPairs)[p];
+ assert(mWorld->mProcessContactPairsOrderIslands.size() == (*mCurrentContactPairs).size());
+
+ // Process the contact pairs in the order defined by the islands such that the contact manifolds and
+ // contact points of a given island are packed together in the array of manifolds and contact points
+ for (uint p=0; p < mWorld->mProcessContactPairsOrderIslands.size(); p++) {
+
+ uint32 contactPairIndex = mWorld->mProcessContactPairsOrderIslands[p];
+
+ processedContactPairsIndices.add(contactPairIndex);
+
+ ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex];
contactPair.contactManifoldsIndex = mCurrentContactManifolds->size();
contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
@@ -772,6 +787,8 @@ void CollisionDetectionSystem::createContacts() {
// Reset the potential contacts
mPotentialContactPoints.clear(true);
mPotentialContactManifolds.clear(true);
+
+ mNarrowPhaseInput.clear();
}
// Compute the lost contact pairs (contact pairs in contact in the previous frame but not in the current one)