From 8a69dc89fa9a830e72a56c8ff836af49f7e23ef6 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 21 Aug 2016 12:34:27 +0200
Subject: [PATCH 001/133] Add missing override keyword
---
src/collision/shapes/BoxShape.h | 2 +-
src/collision/shapes/CapsuleShape.h | 2 +-
src/collision/shapes/ConcaveShape.h | 2 +-
src/collision/shapes/ConeShape.h | 2 +-
src/collision/shapes/ConvexMeshShape.h | 2 +-
src/collision/shapes/CylinderShape.h | 2 +-
src/collision/shapes/SphereShape.h | 2 +-
src/collision/shapes/TriangleShape.h | 2 +-
8 files changed, 8 insertions(+), 8 deletions(-)
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index 2d0fef75..776356ba 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -100,7 +100,7 @@ class BoxShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const;
+ virtual bool isPolyhedron() const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h
index 542fb12f..9c0a4545 100644
--- a/src/collision/shapes/CapsuleShape.h
+++ b/src/collision/shapes/CapsuleShape.h
@@ -102,7 +102,7 @@ class CapsuleShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const;
+ virtual bool isPolyhedron() const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h
index 4af49338..c5657b2b 100644
--- a/src/collision/shapes/ConcaveShape.h
+++ b/src/collision/shapes/ConcaveShape.h
@@ -105,7 +105,7 @@ class ConcaveShape : public CollisionShape {
virtual bool isConvex() const override;
/// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const;
+ virtual bool isPolyhedron() const override;
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0;
diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h
index 65ddb35e..16102163 100644
--- a/src/collision/shapes/ConeShape.h
+++ b/src/collision/shapes/ConeShape.h
@@ -108,7 +108,7 @@ class ConeShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const;
+ virtual bool isPolyhedron() const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index aeb1edbe..66b624aa 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -141,7 +141,7 @@ class ConvexMeshShape : public ConvexShape {
void addEdge(uint v1, uint v2);
/// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const;
+ virtual bool isPolyhedron() const override;
/// Return true if the edges information is used to speed up the collision detection
bool isEdgesInformationUsed() const;
diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h
index b58c454d..e0744a80 100644
--- a/src/collision/shapes/CylinderShape.h
+++ b/src/collision/shapes/CylinderShape.h
@@ -99,7 +99,7 @@ class CylinderShape : public ConvexShape {
decimal getHeight() const;
/// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const;
+ virtual bool isPolyhedron() const override;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling) override;
diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h
index 58f20e6f..fe55a7a8 100644
--- a/src/collision/shapes/SphereShape.h
+++ b/src/collision/shapes/SphereShape.h
@@ -81,7 +81,7 @@ class SphereShape : public ConvexShape {
decimal getRadius() const;
/// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const;
+ virtual bool isPolyhedron() const override;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling) override;
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index d1c82f70..29732f8f 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -117,7 +117,7 @@ class TriangleShape : public ConvexShape {
Vector3 getVertex(int index) const;
/// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const;
+ virtual bool isPolyhedron() const override;
// ---------- Friendship ---------- //
From e069a25f08bfee44b84ed88ab0b7b65dd7e0b4b7 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 10 Sep 2016 11:18:52 +0200
Subject: [PATCH 002/133] Start refactoring the contact solver
---
src/engine/ContactSolver.cpp | 1308 ++++++++++++++++++++++------------
src/engine/ContactSolver.h | 317 ++++++--
src/engine/DynamicsWorld.cpp | 10 +-
src/mathematics/Vector3.cpp | 8 +-
4 files changed, 1114 insertions(+), 529 deletions(-)
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 331e8045..22359b10 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -38,12 +38,15 @@ const decimal ContactSolver::BETA = decimal(0.2);
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP= decimal(0.01);
+// TODO : Enable warmstarting again
+
// Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
:mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr),
- mContactConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
+ mContactConstraints(nullptr), mPenetrationConstraints(nullptr),
+ mFrictionConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
- mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
+ mIsWarmStartingActive(false), mIsSplitImpulseActive(true),
mIsSolveFrictionAtContactManifoldCenterActive(true) {
}
@@ -64,9 +67,21 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
mNbContactManifolds = island->getNbContactManifolds();
+ mNbFrictionConstraints = 0;
+ mNbPenetrationConstraints = 0;
+
+ // TODO : Try to do faster allocation here
mContactConstraints = new ContactManifoldSolver[mNbContactManifolds];
assert(mContactConstraints != nullptr);
+ // TODO : Count exactly the number of constraints to allocate here (do not reallocate each frame)
+ mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4];
+ assert(mPenetrationConstraints != nullptr);
+
+ // TODO : Do not reallocate each frame)
+ mFrictionConstraints = new FrictionConstraint[mNbContactManifolds];
+ assert(mFrictionConstraints != nullptr);
+
// For each contact manifold of the island
ContactManifold** contactManifolds = island->getContactManifold();
for (uint i=0; i<mNbContactManifolds; i++) {
@@ -83,248 +98,367 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
assert(body1 != nullptr);
assert(body2 != nullptr);
+ // TODO : Check if we have a better way to find the body index
+ uint indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
+ uint indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
+
+ mFrictionConstraints[mNbFrictionConstraints].indexBody1 = indexBody1;
+ mFrictionConstraints[mNbFrictionConstraints].indexBody2 = indexBody2;
+
// Get the position of the two bodies
const Vector3& x1 = body1->mCenterOfMassWorld;
const Vector3& x2 = body2->mCenterOfMassWorld;
+ // Get the velocities of the bodies
+ const Vector3& v1 = mLinearVelocities[indexBody1];
+ const Vector3& w1 = mAngularVelocities[indexBody1];
+ const Vector3& v2 = mLinearVelocities[indexBody2];
+ const Vector3& w2 = mAngularVelocities[indexBody2];
+
+ // Get the inertia tensors of both bodies
+ Matrix3x3 I1 = body1->getInertiaTensorInverseWorld();
+ Matrix3x3 I2 = body2->getInertiaTensorInverseWorld();
+
+ mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 = I1;
+ mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 = I2;
+
// Initialize the internal contact manifold structure using the external
// contact manifold
- internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
- internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
- internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
- internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
- internalManifold.massInverseBody1 = body1->mMassInverse;
- internalManifold.massInverseBody2 = body2->mMassInverse;
- internalManifold.nbContacts = externalManifold->getNbContactPoints();
- internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
- internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
- internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
+ mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 = body1->mMassInverse;
+ mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 = body2->mMassInverse;
+ //internalManifold.nbContacts = externalManifold->getNbContactPoints();
+ decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2);
+ mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
+ mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
internalManifold.externalContactManifold = externalManifold;
- internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
- internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
+ //internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
+ //internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
+
+ bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
+ bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
// If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
- internalManifold.frictionPointBody1 = Vector3::zero();
- internalManifold.frictionPointBody2 = Vector3::zero();
+ //if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 = Vector3::zero();
+ mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 = Vector3::zero();
+ mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero();
+ //}
+
+ // Compute the inverse K matrix for the rolling resistance constraint
+ mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.setToZero();
+ if (mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) {
+ mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance = I1 + I2;
+ mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance = mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.getInverse();
}
+ int nbContacts = 0;
+
// For each contact point of the contact manifold
for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
- ContactPointSolver& contactPoint = internalManifold.contacts[c];
-
// Get a contact point
ContactPoint* externalContact = externalManifold->getContactPoint(c);
+ mPenetrationConstraints[mNbPenetrationConstraints].indexBody1 = indexBody1;
+ mPenetrationConstraints[mNbPenetrationConstraints].indexBody2 = indexBody2;
+ mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody1 = I1;
+ mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 = I2;
+ mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 = body1->mMassInverse;
+ mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse;
+ mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor = restitutionFactor;
+ mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints;
+
// Get the contact point on the two bodies
Vector3 p1 = externalContact->getWorldPointOnBody1();
Vector3 p2 = externalContact->getWorldPointOnBody2();
- contactPoint.externalContact = externalContact;
- contactPoint.normal = externalContact->getNormal();
- contactPoint.r1 = p1 - x1;
- contactPoint.r2 = p2 - x2;
- contactPoint.penetrationDepth = externalContact->getPenetrationDepth();
- contactPoint.isRestingContact = externalContact->getIsRestingContact();
+ mPenetrationConstraints[mNbPenetrationConstraints].r1 = p1 - x1;
+ mPenetrationConstraints[mNbPenetrationConstraints].r2 = p2 - x2;
+
+ //mPenetrationConstraints[penConstIndex].externalContact = externalContact;
+ mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal();
+ mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth();
+ //mPenetrationConstraints[penConstIndex].isRestingContact = externalContact->getIsRestingContact();
externalContact->setIsRestingContact(true);
- contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1();
- contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2();
- contactPoint.penetrationImpulse = 0.0;
- contactPoint.friction1Impulse = 0.0;
- contactPoint.friction2Impulse = 0.0;
- contactPoint.rollingResistanceImpulse = Vector3::zero();
+ //mPenetrationConstraints[penConstIndex].oldFrictionVector1 = externalContact->getFrictionVector1();
+ //mPenetrationConstraints[penConstIndex].oldFrictionVector2 = externalContact->getFrictionVector2();
+ mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = 0.0;
+ //mPenetrationConstraints[penConstIndex].friction1Impulse = 0.0;
+ //mPenetrationConstraints[penConstIndex].friction2Impulse = 0.0;
+ //mPenetrationConstraints[penConstIndex].rollingResistanceImpulse = Vector3::zero();
// If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
- internalManifold.frictionPointBody1 += p1;
- internalManifold.frictionPointBody2 += p2;
- }
- }
-
- // If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
-
- internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts);
- internalManifold.frictionPointBody2 /=static_cast<decimal>(internalManifold.nbContacts);
- internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1;
- internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2;
- internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
- internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
-
- // If warm starting is active
- if (mIsWarmStartingActive) {
-
- // Initialize the accumulated impulses with the previous step accumulated impulses
- internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
- internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
- internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
- }
- else {
-
- // Initialize the accumulated impulses to zero
- internalManifold.friction1Impulse = 0.0;
- internalManifold.friction2Impulse = 0.0;
- internalManifold.frictionTwistImpulse = 0.0;
- internalManifold.rollingResistanceImpulse = Vector3(0, 0, 0);
- }
- }
- }
-
- // Fill-in all the matrices needed to solve the LCP problem
- initializeContactConstraints();
-}
-
-// Initialize the contact constraints before solving the system
-void ContactSolver::initializeContactConstraints() {
-
- // For each contact constraint
- for (uint c=0; c<mNbContactManifolds; c++) {
-
- ContactManifoldSolver& manifold = mContactConstraints[c];
-
- // Get the inertia tensors of both bodies
- Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
- Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
-
- // If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
- manifold.normal = Vector3(0.0, 0.0, 0.0);
- }
-
- // Get the velocities of the bodies
- const Vector3& v1 = mLinearVelocities[manifold.indexBody1];
- const Vector3& w1 = mAngularVelocities[manifold.indexBody1];
- const Vector3& v2 = mLinearVelocities[manifold.indexBody2];
- const Vector3& w2 = mAngularVelocities[manifold.indexBody2];
-
- // For each contact point constraint
- for (uint i=0; i<manifold.nbContacts; i++) {
-
- ContactPointSolver& contactPoint = manifold.contacts[i];
- ContactPoint* externalContact = contactPoint.externalContact;
+ //if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 += p1;
+ mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 += p2;
+ //}
// Compute the velocity difference
- Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
-
- contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
- contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
+ Vector3 deltaV = v2 + w2.cross(mPenetrationConstraints[mNbPenetrationConstraints].r2) - v1 - w1.cross(mPenetrationConstraints[mNbPenetrationConstraints].r1);
+ mPenetrationConstraints[mNbPenetrationConstraints].r1CrossN = mPenetrationConstraints[mNbPenetrationConstraints].r1.cross(mPenetrationConstraints[mNbPenetrationConstraints].normal);
+ mPenetrationConstraints[mNbPenetrationConstraints].r2CrossN = mPenetrationConstraints[mNbPenetrationConstraints].r2.cross(mPenetrationConstraints[mNbPenetrationConstraints].normal);
// Compute the inverse mass matrix K for the penetration constraint
- decimal massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
- ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
- massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) /
+ decimal massPenetration = mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 + mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 +
+ ((mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody1 * mPenetrationConstraints[mNbPenetrationConstraints].r1CrossN ).cross(mPenetrationConstraints[mNbPenetrationConstraints].r1)).dot(mPenetrationConstraints[mNbPenetrationConstraints].normal) +
+ ((mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 * mPenetrationConstraints[mNbPenetrationConstraints].r2CrossN ).cross(mPenetrationConstraints[mNbPenetrationConstraints].r2)).dot(mPenetrationConstraints[mNbPenetrationConstraints].normal);
+ massPenetration > decimal(0.0) ? mPenetrationConstraints[mNbPenetrationConstraints].inversePenetrationMass = decimal(1.0) /
massPenetration :
decimal(0.0);
- // If we do not solve the friction constraints at the center of the contact manifold
- if (!mIsSolveFrictionAtContactManifoldCenterActive) {
-
- // Compute the friction vectors
- computeFrictionVectors(deltaV, contactPoint);
-
- contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
- contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2);
- contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1);
- contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2);
-
- // Compute the inverse mass matrix K for the friction
- // constraints at each contact point
- decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(
- contactPoint.frictionVector1) +
- ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(
- contactPoint.frictionVector1);
- decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(
- contactPoint.frictionVector2) +
- ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(
- contactPoint.frictionVector2);
- friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) /
- friction1Mass :
- decimal(0.0);
- friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) /
- friction2Mass :
- decimal(0.0);
- }
-
// Compute the restitution velocity bias "b". We compute this here instead
// of inside the solve() method because we need to use the velocity difference
// at the beginning of the contact. Note that if it is a resting contact (normal
// velocity bellow a given threshold), we do not add a restitution velocity bias
- contactPoint.restitutionBias = 0.0;
- decimal deltaVDotN = deltaV.dot(contactPoint.normal);
+ mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = 0.0;
+ decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[mNbPenetrationConstraints].normal);
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
- contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
+ mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor * deltaVDotN;
}
// If the warm starting of the contact solver is active
if (mIsWarmStartingActive) {
// Get the cached accumulated impulses from the previous step
- contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
- contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
- contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
- contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
+ mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = externalContact->getPenetrationImpulse();
}
// Initialize the split impulses to zero
- contactPoint.penetrationSplitImpulse = 0.0;
+ mPenetrationConstraints[mNbPenetrationConstraints].penetrationSplitImpulse = 0.0;
// If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
- manifold.normal += contactPoint.normal;
- }
- }
+ //if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal;
+ //}
- // Compute the inverse K matrix for the rolling resistance constraint
- manifold.inverseRollingResistance.setToZero();
- if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) {
- manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2;
- manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse();
+ mNbPenetrationConstraints++;
+ nbContacts++;
}
// If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ //if (mIsSolveFrictionAtContactManifoldCenterActive) {
- manifold.normal.normalize();
+ //mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero();
+ mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 /= nbContacts;
+ mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 /= nbContacts;
+ mFrictionConstraints[mNbFrictionConstraints].r1Friction = mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 - x1;
+ mFrictionConstraints[mNbFrictionConstraints].r2Friction = mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 - x2;
+ mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector1 = externalManifold->getFrictionVector1();
+ mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector2 = externalManifold->getFrictionVector2();
- Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
- v1 - w1.cross(manifold.r1Friction);
+ // If warm starting is active
+ if (mIsWarmStartingActive) {
+
+ // Initialize the accumulated impulses with the previous step accumulated impulses
+ mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = externalManifold->getFrictionImpulse1();
+ mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = externalManifold->getFrictionImpulse2();
+ mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
+ }
+ else {
+
+ // Initialize the accumulated impulses to zero
+ mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = 0.0;
+ mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = 0.0;
+ mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = 0.0;
+ mFrictionConstraints[mNbFrictionConstraints].rollingResistanceImpulse = Vector3(0, 0, 0);
+ }
+
+ mFrictionConstraints[mNbFrictionConstraints].normal.normalize();
+
+ Vector3 deltaVFrictionPoint = v2 + w2.cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction) -
+ v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction);
// Compute the friction vectors
- computeFrictionVectors(deltaVFrictionPoint, manifold);
+ computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]);
- // Compute the inverse mass matrix K for the friction constraints at the center of
- // the contact manifold
- manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
- manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
- manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
- manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2);
- decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
- manifold.frictionVector1) +
- ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
- manifold.frictionVector1);
- decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
- manifold.frictionVector2) +
- ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
- manifold.frictionVector2);
- decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
- manifold.normal) +
- manifold.normal.dot(manifold.inverseInertiaTensorBody2 *
- manifold.normal);
- friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass
+ // Compute the inverse mass matrix K for the friction constraints at the center of the contact manifold
+ mFrictionConstraints[mNbFrictionConstraints].r1CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
+ mFrictionConstraints[mNbFrictionConstraints].r1CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
+ mFrictionConstraints[mNbFrictionConstraints].r2CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
+ mFrictionConstraints[mNbFrictionConstraints].r2CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
+ decimal friction1Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
+ ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
+ mFrictionConstraints[mNbFrictionConstraints].frictionVector1) +
+ ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
+ mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
+ decimal friction2Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
+ ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
+ mFrictionConstraints[mNbFrictionConstraints].frictionVector2) +
+ ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
+ mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
+ decimal frictionTwistMass = mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 *
+ mFrictionConstraints[mNbFrictionConstraints].normal) +
+ mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 *
+ mFrictionConstraints[mNbFrictionConstraints].normal);
+ friction1Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction1Mass = decimal(1.0)/friction1Mass
: decimal(0.0);
- friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass
+ friction2Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction2Mass = decimal(1.0)/friction2Mass
: decimal(0.0);
- frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) /
+ frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) /
frictionTwistMass :
decimal(0.0);
- }
+ //}
+
+ mNbFrictionConstraints++;
}
+
+ // Fill-in all the matrices needed to solve the LCP problem
+ //initializeContactConstraints();
+}
+
+// TODO : Delete this method
+// Initialize the contact constraints before solving the system
+void ContactSolver::initializeContactConstraints() {
+
+ PROFILE("ContactSolver::initializeContactConstraints()");
+
+ // For each contact constraint
+ //for (uint c=0; c<mNbContactManifolds; c++) {
+
+ // ContactManifoldSolver& manifold = mContactConstraints[c];
+
+// // Get the inertia tensors of both bodies
+// Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
+// Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
+
+ // If we solve the friction constraints at the center of the contact manifold
+// if (mIsSolveFrictionAtContactManifoldCenterActive) {
+// manifold.normal = Vector3(0.0, 0.0, 0.0);
+// }
+
+ // Get the velocities of the bodies
+// const Vector3& v1 = mLinearVelocities[manifold.indexBody1];
+// const Vector3& w1 = mAngularVelocities[manifold.indexBody1];
+// const Vector3& v2 = mLinearVelocities[manifold.indexBody2];
+// const Vector3& w2 = mAngularVelocities[manifold.indexBody2];
+
+ // For each contact point constraint
+ //for (uint i=0; i<manifold.nbContacts; i++) {
+
+ //ContactPointSolver& contactPoint = manifold.contacts[i];
+ //ContactPoint* externalContact = contactPoint.externalContact;
+
+// // Compute the velocity difference
+// Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+
+// contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
+// contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
+
+// // Compute the inverse mass matrix K for the penetration constraint
+// decimal massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 +
+// ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
+// ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
+// massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) /
+// massPenetration :
+// decimal(0.0);
+
+ // If we do not solve the friction constraints at the center of the contact manifold
+// if (!mIsSolveFrictionAtContactManifoldCenterActive) {
+
+// // Compute the friction vectors
+// computeFrictionVectors(deltaV, contactPoint);
+
+// contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
+// contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2);
+// contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1);
+// contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2);
+
+// // Compute the inverse mass matrix K for the friction
+// // constraints at each contact point
+// decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
+// ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(
+// contactPoint.frictionVector1) +
+// ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(
+// contactPoint.frictionVector1);
+// decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
+// ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(
+// contactPoint.frictionVector2) +
+// ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(
+// contactPoint.frictionVector2);
+// friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) /
+// friction1Mass :
+// decimal(0.0);
+// friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) /
+// friction2Mass :
+// decimal(0.0);
+// }
+
+ // Compute the restitution velocity bias "b". We compute this here instead
+ // of inside the solve() method because we need to use the velocity difference
+ // at the beginning of the contact. Note that if it is a resting contact (normal
+ // velocity bellow a given threshold), we do not add a restitution velocity bias
+// contactPoint.restitutionBias = 0.0;
+// decimal deltaVDotN = deltaV.dot(contactPoint.normal);
+// if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
+// contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
+// }
+
+// // If the warm starting of the contact solver is active
+// if (mIsWarmStartingActive) {
+
+// // Get the cached accumulated impulses from the previous step
+// contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
+// contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
+// contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
+// contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
+// }
+
+// // Initialize the split impulses to zero
+// contactPoint.penetrationSplitImpulse = 0.0;
+
+// // If we solve the friction constraints at the center of the contact manifold
+// if (mIsSolveFrictionAtContactManifoldCenterActive) {
+// manifold.normal += contactPoint.normal;
+// }
+ //}
+
+// // Compute the inverse K matrix for the rolling resistance constraint
+// manifold.inverseRollingResistance.setToZero();
+// if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) {
+// manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2;
+// manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse();
+// }
+
+ // If we solve the friction constraints at the center of the contact manifold
+ //if (mIsSolveFrictionAtContactManifoldCenterActive) {
+
+// manifold.normal.normalize();
+
+// Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
+// v1 - w1.cross(manifold.r1Friction);
+
+// // Compute the friction vectors
+// computeFrictionVectors(deltaVFrictionPoint, manifold);
+
+// // Compute the inverse mass matrix K for the friction constraints at the center of
+// // the contact manifold
+// manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
+// manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
+// manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
+// manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2);
+// decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
+// ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
+// manifold.frictionVector1) +
+// ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
+// manifold.frictionVector1);
+// decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
+// ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
+// manifold.frictionVector2) +
+// ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
+// manifold.frictionVector2);
+// decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
+// manifold.normal) +
+// manifold.normal.dot(manifold.inverseInertiaTensorBody2 *
+// manifold.normal);
+// friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass
+// : decimal(0.0);
+// friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass
+// : decimal(0.0);
+// frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) /
+// frictionTwistMass :
+// decimal(0.0);
+ //}
+ //}
}
// Warm start the solver.
@@ -333,6 +467,9 @@ void ContactSolver::initializeContactConstraints() {
/// the solution of the linear system
void ContactSolver::warmStart() {
+ /*
+ PROFILE("ContactSolver::warmStart()");
+
// Check that warm starting is active
if (!mIsWarmStartingActive) return;
@@ -498,285 +635,516 @@ void ContactSolver::warmStart() {
contactManifold.rollingResistanceImpulse = Vector3::zero();
}
}
+ */
}
-// Solve the contacts
-void ContactSolver::solve() {
+// Reset the total penetration impulse of friction constraints
+void ContactSolver::resetTotalPenetrationImpulse() {
- PROFILE("ContactSolver::solve()");
+ for (uint i=0; i<mNbFrictionConstraints; i++) {
+ mFrictionConstraints[i].totalPenetrationImpulse = decimal(0.0);
+ }
+}
+
+// Solve the penetration constraints
+void ContactSolver::solvePenetrationConstraints() {
+
+ PROFILE("ContactSolver::solvePenetrationConstraints()");
+
+ // TODO : Check that the PenetrationConstraint struct only contains variables that are
+ // used in this method, nothing more
+
+ // TODO : Maybe solve split impulses and normal impulses separately
decimal deltaLambda;
decimal lambdaTemp;
- // For each contact manifold
- for (uint c=0; c<mNbContactManifolds; c++) {
-
- ContactManifoldSolver& contactManifold = mContactConstraints[c];
-
- decimal sumPenetrationImpulse = 0.0;
+ for (uint i=0; i<mNbPenetrationConstraints; i++) {
// Get the constrained velocities
- const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
- const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
- const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
- const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
+ Vector3& v1 = mLinearVelocities[mPenetrationConstraints[i].indexBody1];
+ Vector3& w1 = mAngularVelocities[mPenetrationConstraints[i].indexBody1];
+ Vector3& v2 = mLinearVelocities[mPenetrationConstraints[i].indexBody2];
+ Vector3& w2 = mAngularVelocities[mPenetrationConstraints[i].indexBody2];
- for (uint i=0; i<contactManifold.nbContacts; i++) {
+ // Compute J*v
+ Vector3 deltaV = v2 + w2.cross(mPenetrationConstraints[i].r2) - v1 - w1.cross(mPenetrationConstraints[i].r1);
+ decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[i].normal);
+ decimal Jv = deltaVDotN;
- ContactPointSolver& contactPoint = contactManifold.contacts[i];
+ // Compute the bias "b" of the constraint
+ decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
+ decimal biasPenetrationDepth = 0.0;
+ if (mPenetrationConstraints[i].penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
+ max(0.0f, float(mPenetrationConstraints[i].penetrationDepth - SLOP));
+ decimal b = biasPenetrationDepth + mPenetrationConstraints[i].restitutionBias;
- // --------- Penetration --------- //
-
- // Compute J*v
- Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
- decimal deltaVDotN = deltaV.dot(contactPoint.normal);
- decimal Jv = deltaVDotN;
-
- // Compute the bias "b" of the constraint
- decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
- decimal biasPenetrationDepth = 0.0;
- if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
- max(0.0f, float(contactPoint.penetrationDepth - SLOP));
- decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
-
- // Compute the Lagrange multiplier lambda
- if (mIsSplitImpulseActive) {
- deltaLambda = - (Jv + contactPoint.restitutionBias) *
- contactPoint.inversePenetrationMass;
- }
- else {
- deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
- }
- lambdaTemp = contactPoint.penetrationImpulse;
- contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
- deltaLambda, decimal(0.0));
- deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
-
- // Compute the impulse P=J^T * lambda
- const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
- contactPoint);
-
- // Apply the impulse to the bodies of the constraint
- applyImpulse(impulsePenetration, contactManifold);
-
- sumPenetrationImpulse += contactPoint.penetrationImpulse;
-
- // If the split impulse position correction is active
- if (mIsSplitImpulseActive) {
-
- // Split impulse (position correction)
- const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
- const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
- const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
- const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
- Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
- v1Split - w1Split.cross(contactPoint.r1);
- decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
- decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
- contactPoint.inversePenetrationMass;
- decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
- contactPoint.penetrationSplitImpulse = std::max(
- contactPoint.penetrationSplitImpulse +
- deltaLambdaSplit, decimal(0.0));
- deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
-
- // Compute the impulse P=J^T * lambda
- const Impulse splitImpulsePenetration = computePenetrationImpulse(
- deltaLambdaSplit, contactPoint);
-
- applySplitImpulse(splitImpulsePenetration, contactManifold);
- }
-
- // If we do not solve the friction constraints at the center of the contact manifold
- if (!mIsSolveFrictionAtContactManifoldCenterActive) {
-
- // --------- Friction 1 --------- //
-
- // Compute J*v
- deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
- Jv = deltaV.dot(contactPoint.frictionVector1);
-
- // Compute the Lagrange multiplier lambda
- deltaLambda = -Jv;
- deltaLambda *= contactPoint.inverseFriction1Mass;
- decimal frictionLimit = contactManifold.frictionCoefficient *
- contactPoint.penetrationImpulse;
- lambdaTemp = contactPoint.friction1Impulse;
- contactPoint.friction1Impulse = std::max(-frictionLimit,
- std::min(contactPoint.friction1Impulse
- + deltaLambda, frictionLimit));
- deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
-
- // Compute the impulse P=J^T * lambda
- const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
- contactPoint);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction1, contactManifold);
-
- // --------- Friction 2 --------- //
-
- // Compute J*v
- deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
- Jv = deltaV.dot(contactPoint.frictionVector2);
-
- // Compute the Lagrange multiplier lambda
- deltaLambda = -Jv;
- deltaLambda *= contactPoint.inverseFriction2Mass;
- frictionLimit = contactManifold.frictionCoefficient *
- contactPoint.penetrationImpulse;
- lambdaTemp = contactPoint.friction2Impulse;
- contactPoint.friction2Impulse = std::max(-frictionLimit,
- std::min(contactPoint.friction2Impulse
- + deltaLambda, frictionLimit));
- deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
-
- // Compute the impulse P=J^T * lambda
- const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
- contactPoint);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction2, contactManifold);
-
- // --------- Rolling resistance constraint --------- //
-
- if (contactManifold.rollingResistanceFactor > 0) {
-
- // Compute J*v
- const Vector3 JvRolling = w2 - w1;
-
- // Compute the Lagrange multiplier lambda
- Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
- decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
- Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse;
- contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse +
- deltaLambdaRolling, rollingLimit);
- deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling;
-
- // Compute the impulse P=J^T * lambda
- const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling,
- Vector3::zero(), deltaLambdaRolling);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseRolling, contactManifold);
- }
- }
+ // Compute the Lagrange multiplier lambda
+ if (mIsSplitImpulseActive) {
+ deltaLambda = - (Jv + mPenetrationConstraints[i].restitutionBias) *
+ mPenetrationConstraints[i].inversePenetrationMass;
}
+ else {
+ deltaLambda = - (Jv + b) * mPenetrationConstraints[i].inversePenetrationMass;
+ }
+ lambdaTemp = mPenetrationConstraints[i].penetrationImpulse;
+ mPenetrationConstraints[i].penetrationImpulse = std::max(mPenetrationConstraints[i].penetrationImpulse +
+ deltaLambda, decimal(0.0));
+ deltaLambda = mPenetrationConstraints[i].penetrationImpulse - lambdaTemp;
- // If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ // Add the penetration impulse to the total impulse of the corresponding friction constraint
+ mFrictionConstraints[mPenetrationConstraints[i].indexFrictionConstraint].totalPenetrationImpulse += mPenetrationConstraints[i].penetrationImpulse;
- // ------ First friction constraint at the center of the contact manifol ------ //
+ // Update the velocities of the body 1 by applying the impulse P=J^T * lambda
+ Vector3 linearImpulse = mPenetrationConstraints[i].normal * deltaLambda;
+ v1 += mPenetrationConstraints[i].massInverseBody1 * (-linearImpulse);
+ w1 += mPenetrationConstraints[i].inverseInertiaTensorBody1 * (-mPenetrationConstraints[i].r1CrossN * deltaLambda);
- // Compute J*v
- Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
- - v1 - w1.cross(contactManifold.r1Friction);
- decimal Jv = deltaV.dot(contactManifold.frictionVector1);
+ // Update the velocities of the body 1 by applying the impulse P=J^T * lambda
+ v2 += mPenetrationConstraints[i].massInverseBody2 * linearImpulse;
+ w2 += mPenetrationConstraints[i].inverseInertiaTensorBody2 * (mPenetrationConstraints[i].r2CrossN * deltaLambda);
- // Compute the Lagrange multiplier lambda
- decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
- decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
- lambdaTemp = contactManifold.friction1Impulse;
- contactManifold.friction1Impulse = std::max(-frictionLimit,
- std::min(contactManifold.friction1Impulse +
- deltaLambda, frictionLimit));
- deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
+ // If the split impulse position correction is active
+ if (mIsSplitImpulseActive) {
- // Compute the impulse P=J^T * lambda
- Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
- Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
- Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
- Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
- const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
+ // Split impulse (position correction)
+ const Vector3& v1Split = mSplitLinearVelocities[mPenetrationConstraints[i].indexBody1];
+ const Vector3& w1Split = mSplitAngularVelocities[mPenetrationConstraints[i].indexBody1];
+ const Vector3& v2Split = mSplitLinearVelocities[mPenetrationConstraints[i].indexBody2];
+ const Vector3& w2Split = mSplitAngularVelocities[mPenetrationConstraints[i].indexBody2];
+ Vector3 deltaVSplit = v2Split + w2Split.cross(mPenetrationConstraints[i].r2) -
+ v1Split - w1Split.cross(mPenetrationConstraints[i].r1);
+ decimal JvSplit = deltaVSplit.dot(mPenetrationConstraints[i].normal);
+ decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
+ mPenetrationConstraints[i].inversePenetrationMass;
+ decimal lambdaTempSplit = mPenetrationConstraints[i].penetrationSplitImpulse;
+ mPenetrationConstraints[i].penetrationSplitImpulse = std::max(
+ mPenetrationConstraints[i].penetrationSplitImpulse +
+ deltaLambdaSplit, decimal(0.0));
+ deltaLambda = mPenetrationConstraints[i].penetrationSplitImpulse - lambdaTempSplit;
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction1, contactManifold);
+ // Update the velocities of the body 1 by applying the impulse P=J^T * lambda
+ Vector3 linearImpulse = mPenetrationConstraints[i].normal * deltaLambdaSplit;
+ mSplitLinearVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].massInverseBody1 * (-linearImpulse);
+ mSplitAngularVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].inverseInertiaTensorBody1 * (-mPenetrationConstraints[i].r1CrossN * deltaLambdaSplit);
- // ------ Second friction constraint at the center of the contact manifol ----- //
-
- // Compute J*v
- deltaV = v2 + w2.cross(contactManifold.r2Friction)
- - v1 - w1.cross(contactManifold.r1Friction);
- Jv = deltaV.dot(contactManifold.frictionVector2);
-
- // Compute the Lagrange multiplier lambda
- deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
- frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
- lambdaTemp = contactManifold.friction2Impulse;
- contactManifold.friction2Impulse = std::max(-frictionLimit,
- std::min(contactManifold.friction2Impulse +
- deltaLambda, frictionLimit));
- deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
-
- // Compute the impulse P=J^T * lambda
- linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
- angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
- linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
- angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
- const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction2, contactManifold);
-
- // ------ Twist friction constraint at the center of the contact manifol ------ //
-
- // Compute J*v
- deltaV = w2 - w1;
- Jv = deltaV.dot(contactManifold.normal);
-
- deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
- frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
- lambdaTemp = contactManifold.frictionTwistImpulse;
- contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
- std::min(contactManifold.frictionTwistImpulse
- + deltaLambda, frictionLimit));
- deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
-
- // Compute the impulse P=J^T * lambda
- linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
- angularImpulseBody1 = -contactManifold.normal * deltaLambda;
- linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
- angularImpulseBody2 = contactManifold.normal * deltaLambda;
- const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseTwistFriction, contactManifold);
-
- // --------- Rolling resistance constraint at the center of the contact manifold --------- //
-
- if (contactManifold.rollingResistanceFactor > 0) {
-
- // Compute J*v
- const Vector3 JvRolling = w2 - w1;
-
- // Compute the Lagrange multiplier lambda
- Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
- decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
- Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
- contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse +
- deltaLambdaRolling, rollingLimit);
- deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
-
- // Compute the impulse P=J^T * lambda
- angularImpulseBody1 = -deltaLambdaRolling;
- angularImpulseBody2 = deltaLambdaRolling;
- const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1,
- Vector3::zero(), angularImpulseBody2);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseRolling, contactManifold);
- }
+ // Update the velocities of the body 1 by applying the impulse P=J^T * lambda
+ mSplitLinearVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].massInverseBody2 * linearImpulse;
+ mSplitAngularVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].inverseInertiaTensorBody2 * (mPenetrationConstraints[i].r2CrossN * deltaLambdaSplit);
}
}
}
+// Solve the friction constraints
+void ContactSolver::solveFrictionConstraints() {
+
+ // TODO : Check that the FrictionConstraint struct only contains variables that are
+ // used in this method, nothing more
+
+ PROFILE("ContactSolver::solveFrictionConstraints()");
+
+ for (uint i=0; i<mNbFrictionConstraints; i++) {
+
+ // Get the constrained velocities
+ Vector3& v1 = mLinearVelocities[mFrictionConstraints[i].indexBody1];
+ Vector3& w1 = mAngularVelocities[mFrictionConstraints[i].indexBody1];
+ Vector3& v2 = mLinearVelocities[mFrictionConstraints[i].indexBody2];
+ Vector3& w2 = mAngularVelocities[mFrictionConstraints[i].indexBody2];
+
+ // ------ First friction constraint at the center of the contact manifol ------ //
+
+ // Compute J*v
+ Vector3 deltaV = v2 + w2.cross(mFrictionConstraints[i].r2Friction)
+ - v1 - w1.cross(mFrictionConstraints[i].r1Friction);
+ decimal Jv = deltaV.dot(mFrictionConstraints[i].frictionVector1);
+
+ // Compute the Lagrange multiplier lambda
+ decimal deltaLambda = -Jv * mFrictionConstraints[i].inverseFriction1Mass;
+ decimal frictionLimit = mFrictionConstraints[i].frictionCoefficient * mFrictionConstraints[i].totalPenetrationImpulse;
+ decimal lambdaTemp = mFrictionConstraints[i].friction1Impulse;
+ mFrictionConstraints[i].friction1Impulse = std::max(-frictionLimit,
+ std::min(mFrictionConstraints[i].friction1Impulse +
+ deltaLambda, frictionLimit));
+ deltaLambda = mFrictionConstraints[i].friction1Impulse - lambdaTemp;
+
+ // Compute the impulse P=J^T * lambda
+ Vector3 linearImpulseBody2 = mFrictionConstraints[i].frictionVector1 * deltaLambda;
+ Vector3 linearImpulseBody1 = -linearImpulseBody2;
+ Vector3 angularImpulseBody1 = -mFrictionConstraints[i].r1CrossT1 * deltaLambda;
+ Vector3 angularImpulseBody2 = mFrictionConstraints[i].r2CrossT1 * deltaLambda;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ v1 += mFrictionConstraints[i].massInverseBody1 * linearImpulseBody1;
+ w1 += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ v2 += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
+ w2 += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+
+ // ------ Second friction constraint at the center of the contact manifol ----- //
+
+ // Compute J*v
+ deltaV = v2 + w2.cross(mFrictionConstraints[i].r2Friction)
+ - v1 - w1.cross(mFrictionConstraints[i].r1Friction);
+ Jv = deltaV.dot(mFrictionConstraints[i].frictionVector2);
+
+ // Compute the Lagrange multiplier lambda
+ deltaLambda = -Jv * mFrictionConstraints[i].inverseFriction2Mass;
+ frictionLimit = mFrictionConstraints[i].frictionCoefficient * mFrictionConstraints[i].totalPenetrationImpulse;
+ lambdaTemp = mFrictionConstraints[i].friction2Impulse;
+ mFrictionConstraints[i].friction2Impulse = std::max(-frictionLimit,
+ std::min(mFrictionConstraints[i].friction2Impulse +
+ deltaLambda, frictionLimit));
+ deltaLambda = mFrictionConstraints[i].friction2Impulse - lambdaTemp;
+
+ // Compute the impulse P=J^T * lambda
+ linearImpulseBody2 = mFrictionConstraints[i].frictionVector2 * deltaLambda;
+ linearImpulseBody1 = -linearImpulseBody2;
+ angularImpulseBody1 = -mFrictionConstraints[i].r1CrossT2 * deltaLambda;
+ angularImpulseBody2 = mFrictionConstraints[i].r2CrossT2 * deltaLambda;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ v1 += mFrictionConstraints[i].massInverseBody1 * linearImpulseBody1;
+ w1 += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ v2 += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
+ w2 += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+
+ // ------ Twist friction constraint at the center of the contact manifol ------ //
+
+ // Compute J*v
+ deltaV = w2 - w1;
+ Jv = deltaV.dot(mFrictionConstraints[i].normal);
+
+ deltaLambda = -Jv * (mFrictionConstraints[i].inverseTwistFrictionMass);
+ frictionLimit = mFrictionConstraints[i].frictionCoefficient * mFrictionConstraints[i].totalPenetrationImpulse;
+ lambdaTemp = mFrictionConstraints[i].frictionTwistImpulse;
+ mFrictionConstraints[i].frictionTwistImpulse = std::max(-frictionLimit,
+ std::min(mFrictionConstraints[i].frictionTwistImpulse
+ + deltaLambda, frictionLimit));
+ deltaLambda = mFrictionConstraints[i].frictionTwistImpulse - lambdaTemp;
+
+ // Compute the impulse P=J^T * lambda
+ linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
+ linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);
+ angularImpulseBody2 = mFrictionConstraints[i].normal * deltaLambda;
+ angularImpulseBody1 = -angularImpulseBody2;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ v1 += mFrictionConstraints[i].massInverseBody1 * linearImpulseBody1;
+ w1 += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ v2 += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
+ w2 += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+
+ // --------- Rolling resistance constraint at the center of the contact manifold --------- //
+
+ if (mFrictionConstraints[i].rollingResistanceFactor > 0) {
+
+ // Compute J*v
+ const Vector3 JvRolling = w2 - w1;
+
+ // Compute the Lagrange multiplier lambda
+ Vector3 deltaLambdaRolling = mFrictionConstraints[i].inverseRollingResistance * (-JvRolling);
+ decimal rollingLimit = mFrictionConstraints[i].rollingResistanceFactor * mFrictionConstraints[i].totalPenetrationImpulse;
+ Vector3 lambdaTempRolling = mFrictionConstraints[i].rollingResistanceImpulse;
+ mFrictionConstraints[i].rollingResistanceImpulse = clamp(mFrictionConstraints[i].rollingResistanceImpulse +
+ deltaLambdaRolling, rollingLimit);
+ deltaLambdaRolling = mFrictionConstraints[i].rollingResistanceImpulse - lambdaTempRolling;
+
+ // Compute the impulse P=J^T * lambda
+ angularImpulseBody1 = -deltaLambdaRolling;
+ angularImpulseBody2 = deltaLambdaRolling;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ w1 += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ w2 += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+ }
+ }
+}
+
+// Solve the contacts
+//void ContactSolver::solve() {
+
+// PROFILE("ContactSolver::solve()");
+
+// decimal deltaLambda;
+// decimal lambdaTemp;
+
+// // For each contact manifold
+// for (uint c=0; c<mNbContactManifolds; c++) {
+
+// ContactManifoldSolver& contactManifold = mContactConstraints[c];
+
+// decimal sumPenetrationImpulse = 0.0;
+
+// // Get the constrained velocities
+// const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
+// const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
+// const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
+// const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
+
+// for (uint i=0; i<contactManifold.nbContacts; i++) {
+
+// ContactPointSolver& contactPoint = contactManifold.contacts[i];
+
+// // --------- Penetration --------- //
+
+// // Compute J*v
+// Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+// decimal deltaVDotN = deltaV.dot(contactPoint.normal);
+// decimal Jv = deltaVDotN;
+
+// // Compute the bias "b" of the constraint
+// decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
+// decimal biasPenetrationDepth = 0.0;
+// if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
+// max(0.0f, float(contactPoint.penetrationDepth - SLOP));
+// decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
+
+// // Compute the Lagrange multiplier lambda
+// if (mIsSplitImpulseActive) {
+// deltaLambda = - (Jv + contactPoint.restitutionBias) *
+// contactPoint.inversePenetrationMass;
+// }
+// else {
+// deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
+// }
+// lambdaTemp = contactPoint.penetrationImpulse;
+// contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
+// deltaLambda, decimal(0.0));
+// deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
+
+// // Compute the impulse P=J^T * lambda
+// const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
+// contactPoint);
+
+// // Apply the impulse to the bodies of the constraint
+// applyImpulse(impulsePenetration, contactManifold);
+
+// sumPenetrationImpulse += contactPoint.penetrationImpulse;
+
+// // If the split impulse position correction is active
+// if (mIsSplitImpulseActive) {
+
+// // Split impulse (position correction)
+// const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
+// const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
+// const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
+// const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
+// Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
+// v1Split - w1Split.cross(contactPoint.r1);
+// decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
+// decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
+// contactPoint.inversePenetrationMass;
+// decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
+// contactPoint.penetrationSplitImpulse = std::max(
+// contactPoint.penetrationSplitImpulse +
+// deltaLambdaSplit, decimal(0.0));
+// deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
+
+// // Compute the impulse P=J^T * lambda
+// const Impulse splitImpulsePenetration = computePenetrationImpulse(
+// deltaLambdaSplit, contactPoint);
+
+// applySplitImpulse(splitImpulsePenetration, contactManifold);
+// }
+
+// // If we do not solve the friction constraints at the center of the contact manifold
+// if (!mIsSolveFrictionAtContactManifoldCenterActive) {
+
+// // --------- Friction 1 --------- //
+
+// // Compute J*v
+// deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+// Jv = deltaV.dot(contactPoint.frictionVector1);
+
+// // Compute the Lagrange multiplier lambda
+// deltaLambda = -Jv;
+// deltaLambda *= contactPoint.inverseFriction1Mass;
+// decimal frictionLimit = contactManifold.frictionCoefficient *
+// contactPoint.penetrationImpulse;
+// lambdaTemp = contactPoint.friction1Impulse;
+// contactPoint.friction1Impulse = std::max(-frictionLimit,
+// std::min(contactPoint.friction1Impulse
+// + deltaLambda, frictionLimit));
+// deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
+
+// // Compute the impulse P=J^T * lambda
+// const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
+// contactPoint);
+
+// // Apply the impulses to the bodies of the constraint
+// applyImpulse(impulseFriction1, contactManifold);
+
+// // --------- Friction 2 --------- //
+
+// // Compute J*v
+// deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+// Jv = deltaV.dot(contactPoint.frictionVector2);
+
+// // Compute the Lagrange multiplier lambda
+// deltaLambda = -Jv;
+// deltaLambda *= contactPoint.inverseFriction2Mass;
+// frictionLimit = contactManifold.frictionCoefficient *
+// contactPoint.penetrationImpulse;
+// lambdaTemp = contactPoint.friction2Impulse;
+// contactPoint.friction2Impulse = std::max(-frictionLimit,
+// std::min(contactPoint.friction2Impulse
+// + deltaLambda, frictionLimit));
+// deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
+
+// // Compute the impulse P=J^T * lambda
+// const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
+// contactPoint);
+
+// // Apply the impulses to the bodies of the constraint
+// applyImpulse(impulseFriction2, contactManifold);
+
+// // --------- Rolling resistance constraint --------- //
+
+// if (contactManifold.rollingResistanceFactor > 0) {
+
+// // Compute J*v
+// const Vector3 JvRolling = w2 - w1;
+
+// // Compute the Lagrange multiplier lambda
+// Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
+// decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
+// Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse;
+// contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse +
+// deltaLambdaRolling, rollingLimit);
+// deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling;
+
+// // Compute the impulse P=J^T * lambda
+// const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling,
+// Vector3::zero(), deltaLambdaRolling);
+
+// // Apply the impulses to the bodies of the constraint
+// applyImpulse(impulseRolling, contactManifold);
+// }
+// }
+ //}
+
+ // If we solve the friction constraints at the center of the contact manifold
+// if (mIsSolveFrictionAtContactManifoldCenterActive) {
+
+// // ------ First friction constraint at the center of the contact manifol ------ //
+
+// // Compute J*v
+// Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
+// - v1 - w1.cross(contactManifold.r1Friction);
+// decimal Jv = deltaV.dot(contactManifold.frictionVector1);
+
+// // Compute the Lagrange multiplier lambda
+// decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
+// decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+// lambdaTemp = contactManifold.friction1Impulse;
+// contactManifold.friction1Impulse = std::max(-frictionLimit,
+// std::min(contactManifold.friction1Impulse +
+// deltaLambda, frictionLimit));
+// deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
+
+// // Compute the impulse P=J^T * lambda
+// Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
+// Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
+// Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
+// Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
+// const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
+// linearImpulseBody2, angularImpulseBody2);
+
+// // Apply the impulses to the bodies of the constraint
+// applyImpulse(impulseFriction1, contactManifold);
+
+// // ------ Second friction constraint at the center of the contact manifol ----- //
+
+// // Compute J*v
+// deltaV = v2 + w2.cross(contactManifold.r2Friction)
+// - v1 - w1.cross(contactManifold.r1Friction);
+// Jv = deltaV.dot(contactManifold.frictionVector2);
+
+// // Compute the Lagrange multiplier lambda
+// deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
+// frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+// lambdaTemp = contactManifold.friction2Impulse;
+// contactManifold.friction2Impulse = std::max(-frictionLimit,
+// std::min(contactManifold.friction2Impulse +
+// deltaLambda, frictionLimit));
+// deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
+
+// // Compute the impulse P=J^T * lambda
+// linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
+// angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
+// linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
+// angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
+// const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
+// linearImpulseBody2, angularImpulseBody2);
+
+// // Apply the impulses to the bodies of the constraint
+// applyImpulse(impulseFriction2, contactManifold);
+
+// // ------ Twist friction constraint at the center of the contact manifol ------ //
+
+// // Compute J*v
+// deltaV = w2 - w1;
+// Jv = deltaV.dot(contactManifold.normal);
+
+// deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
+// frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+// lambdaTemp = contactManifold.frictionTwistImpulse;
+// contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
+// std::min(contactManifold.frictionTwistImpulse
+// + deltaLambda, frictionLimit));
+// deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
+
+// // Compute the impulse P=J^T * lambda
+// linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
+// angularImpulseBody1 = -contactManifold.normal * deltaLambda;
+// linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
+// angularImpulseBody2 = contactManifold.normal * deltaLambda;
+// const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
+// linearImpulseBody2, angularImpulseBody2);
+
+// // Apply the impulses to the bodies of the constraint
+// applyImpulse(impulseTwistFriction, contactManifold);
+
+// // --------- Rolling resistance constraint at the center of the contact manifold --------- //
+
+// if (contactManifold.rollingResistanceFactor > 0) {
+
+// // Compute J*v
+// const Vector3 JvRolling = w2 - w1;
+
+// // Compute the Lagrange multiplier lambda
+// Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
+// decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
+// Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
+// contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse +
+// deltaLambdaRolling, rollingLimit);
+// deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
+
+// // Compute the impulse P=J^T * lambda
+// angularImpulseBody1 = -deltaLambdaRolling;
+// angularImpulseBody2 = deltaLambdaRolling;
+// const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1,
+// Vector3::zero(), angularImpulseBody2);
+
+// // Apply the impulses to the bodies of the constraint
+// applyImpulse(impulseRolling, contactManifold);
+// }
+// }
+// }
+//}
+
// Store the computed impulses to use them to
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
+ /*
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
@@ -802,12 +1170,16 @@ void ContactSolver::storeImpulses() {
manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1);
manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2);
}
+ */
}
+/*
// Apply an impulse to the two bodies of a constraint
void ContactSolver::applyImpulse(const Impulse& impulse,
const ContactManifoldSolver& manifold) {
+ PROFILE("ContactSolver::applyImpulse()");
+
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
impulse.linearImpulseBody1;
@@ -820,7 +1192,9 @@ void ContactSolver::applyImpulse(const Impulse& impulse,
mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
+*/
+/*
// Apply an impulse to the two bodies of a constraint
void ContactSolver::applySplitImpulse(const Impulse& impulse,
const ContactManifoldSolver& manifold) {
@@ -837,46 +1211,48 @@ void ContactSolver::applySplitImpulse(const Impulse& impulse,
mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
+*/
+// TODO : Delete this
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
// for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal.
-void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
- ContactPointSolver& contactPoint) const {
+//void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
+// ContactPointSolver& contactPoint) const {
- assert(contactPoint.normal.length() > 0.0);
+// assert(contactPoint.normal.length() > 0.0);
- // Compute the velocity difference vector in the tangential plane
- Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
- Vector3 tangentVelocity = deltaVelocity - normalVelocity;
+// // Compute the velocity difference vector in the tangential plane
+// Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
+// Vector3 tangentVelocity = deltaVelocity - normalVelocity;
- // If the velocty difference in the tangential plane is not zero
- decimal lengthTangenVelocity = tangentVelocity.length();
- if (lengthTangenVelocity > MACHINE_EPSILON) {
+// // If the velocty difference in the tangential plane is not zero
+// decimal lengthTangenVelocity = tangentVelocity.length();
+// if (lengthTangenVelocity > MACHINE_EPSILON) {
- // Compute the first friction vector in the direction of the tangent
- // velocity difference
- contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
- }
- else {
+// // Compute the first friction vector in the direction of the tangent
+// // velocity difference
+// contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
+// }
+// else {
- // Get any orthogonal vector to the normal as the first friction vector
- contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector();
- }
+// // Get any orthogonal vector to the normal as the first friction vector
+// contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector();
+// }
- // The second friction vector is computed by the cross product of the firs
- // friction vector and the contact normal
- contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit();
-}
+// // The second friction vector is computed by the cross product of the firs
+// // friction vector and the contact normal
+// contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit();
+//}
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
// for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
- ContactManifoldSolver& contact) const {
+ FrictionConstraint& frictionConstraint) const {
- assert(contact.normal.length() > 0.0);
+ assert(frictionConstraint.normal.length() > MACHINE_EPSILON);
// Compute the velocity difference vector in the tangential plane
- Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal;
+ Vector3 normalVelocity = deltaVelocity.dot(frictionConstraint.normal) * frictionConstraint.normal;
Vector3 tangentVelocity = deltaVelocity - normalVelocity;
// If the velocty difference in the tangential plane is not zero
@@ -885,17 +1261,17 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
// Compute the first friction vector in the direction of the tangent
// velocity difference
- contact.frictionVector1 = tangentVelocity / lengthTangenVelocity;
+ frictionConstraint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
}
else {
// Get any orthogonal vector to the normal as the first friction vector
- contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector();
+ frictionConstraint.frictionVector1 = frictionConstraint.normal.getOneUnitOrthogonalVector();
}
// The second friction vector is computed by the cross product of the firs
// friction vector and the contact normal
- contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit();
+ frictionConstraint.frictionVector2 = frictionConstraint.normal.cross(frictionConstraint.frictionVector1).getUnit();
}
// Clean up the constraint solver
@@ -905,4 +1281,14 @@ void ContactSolver::cleanup() {
delete[] mContactConstraints;
mContactConstraints = nullptr;
}
+
+ if (mPenetrationConstraints != nullptr) {
+ delete[] mPenetrationConstraints;
+ mPenetrationConstraints = nullptr;
+ }
+
+ if (mFrictionConstraints != nullptr) {
+ delete[] mFrictionConstraints;
+ mFrictionConstraints = nullptr;
+ }
}
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index ae2005f4..ef0087a0 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -39,7 +39,6 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
-
// Class Contact Solver
/**
* This class represents the contact solver that is used to solve rigid bodies contacts.
@@ -113,6 +112,156 @@ class ContactSolver {
private:
+ struct PenetrationConstraint {
+
+ /// Index of body 1 in the constraint solver
+ uint indexBody1;
+
+ /// Index of body 2 in the constraint solver
+ uint indexBody2;
+
+ /// Normal vector of the contact
+ Vector3 normal;
+
+ /// Vector from the body 1 center to the contact point
+ Vector3 r1;
+
+ /// Vector from the body 2 center to the contact point
+ Vector3 r2;
+
+ /// Cross product of r1 with the contact normal
+ Vector3 r1CrossN;
+
+ /// Cross product of r2 with the contact normal
+ Vector3 r2CrossN;
+
+ /// Penetration depth
+ decimal penetrationDepth;
+
+ /// Velocity restitution bias
+ decimal restitutionBias;
+
+ /// Mix of the restitution factor for two bodies
+ decimal restitutionFactor;
+
+ /// Accumulated normal impulse
+ decimal penetrationImpulse;
+
+ /// Accumulated split impulse for penetration correction
+ decimal penetrationSplitImpulse;
+
+ /// Inverse of the mass of body 1
+ decimal massInverseBody1;
+
+ /// Inverse of the mass of body 2
+ decimal massInverseBody2;
+
+ /// Inverse of the matrix K for the penenetration
+ decimal inversePenetrationMass;
+
+ /// Inverse inertia tensor of body 1
+ Matrix3x3 inverseInertiaTensorBody1;
+
+ /// Inverse inertia tensor of body 2
+ Matrix3x3 inverseInertiaTensorBody2;
+
+ /// Index of the corresponding friction constraint
+ uint indexFrictionConstraint;
+ };
+
+ struct FrictionConstraint {
+
+ /// Index of body 1 in the constraint solver
+ uint indexBody1;
+
+ /// Index of body 2 in the constraint solver
+ uint indexBody2;
+
+ /// R1 vector for the friction constraints
+ Vector3 r1Friction;
+
+ /// R2 vector for the friction constraints
+ Vector3 r2Friction;
+
+ /// Point on body 1 where to apply the friction constraints
+ Vector3 frictionPointBody1;
+
+ /// Point on body 2 where to apply the friction constraints
+ Vector3 frictionPointBody2;
+
+ /// Average normal vector of the contact manifold
+ Vector3 normal;
+
+ /// Accumulated impulse in the 1st friction direction
+ decimal friction1Impulse;
+
+ /// Accumulated impulse in the 2nd friction direction
+ decimal friction2Impulse;
+
+ /// Twist friction impulse at contact manifold center
+ decimal frictionTwistImpulse;
+
+ /// Accumulated rolling resistance impulse
+ Vector3 rollingResistanceImpulse;
+
+ /// Rolling resistance factor between the two bodies
+ decimal rollingResistanceFactor;
+
+ /// Mix friction coefficient for the two bodies
+ decimal frictionCoefficient;
+
+ /// First friction vector in the tangent plane
+ Vector3 frictionVector1;
+
+ /// Second friction vector in the tangent plane
+ Vector3 frictionVector2;
+
+ /// Old 1st friction direction at contact manifold center
+ Vector3 oldFrictionVector1;
+
+ /// Old 2nd friction direction at contact manifold center
+ Vector3 oldFrictionVector2;
+
+ /// Cross product of r1 with 1st friction vector
+ Vector3 r1CrossT1;
+
+ /// Cross product of r1 with 2nd friction vector
+ Vector3 r1CrossT2;
+
+ /// Cross product of r2 with 1st friction vector
+ Vector3 r2CrossT1;
+
+ /// Cross product of r2 with 2nd friction vector
+ Vector3 r2CrossT2;
+
+ /// Total of the all the corresponding penetration impulses
+ decimal totalPenetrationImpulse;
+
+ /// Inverse of the matrix K for the 1st friction
+ decimal inverseFriction1Mass;
+
+ /// Inverse of the matrix K for the 2nd friction
+ decimal inverseFriction2Mass;
+
+ /// Matrix K for the twist friction constraint
+ decimal inverseTwistFrictionMass;
+
+ /// Matrix K for the rolling resistance constraint
+ Matrix3x3 inverseRollingResistance;
+
+ /// Inverse of the mass of body 1
+ decimal massInverseBody1;
+
+ /// Inverse of the mass of body 2
+ decimal massInverseBody2;
+
+ /// Inverse inertia tensor of body 1
+ Matrix3x3 inverseInertiaTensorBody1;
+
+ /// Inverse inertia tensor of body 2
+ Matrix3x3 inverseInertiaTensorBody2;
+ };
+
// Structure ContactPointSolver
/**
* Contact solver internal data structure that to store all the
@@ -120,6 +269,30 @@ class ContactSolver {
*/
struct ContactPointSolver {
+ /// Index of body 1 in the constraint solver
+ uint indexBody1;
+
+ /// Index of body 2 in the constraint solver
+ uint indexBody2;
+
+ /// Inverse of the mass of body 1
+ decimal massInverseBody1;
+
+ /// Inverse of the mass of body 2
+ decimal massInverseBody2;
+
+ /// Inverse inertia tensor of body 1
+ Matrix3x3 inverseInertiaTensorBody1;
+
+ /// Inverse inertia tensor of body 2
+ Matrix3x3 inverseInertiaTensorBody2;
+
+ /// Point on body 1 where to apply the friction constraints
+ Vector3 frictionPointBody1;
+
+ /// Point on body 2 where to apply the friction constraints
+ Vector3 frictionPointBody2;
+
/// Accumulated normal impulse
decimal penetrationImpulse;
@@ -139,10 +312,10 @@ class ContactSolver {
Vector3 normal;
/// First friction vector in the tangent plane
- Vector3 frictionVector1;
+ //Vector3 frictionVector1;
/// Second friction vector in the tangent plane
- Vector3 frictionVector2;
+ //Vector3 frictionVector2;
/// Old first friction vector in the tangent plane
Vector3 oldFrictionVector1;
@@ -157,22 +330,22 @@ class ContactSolver {
Vector3 r2;
/// Cross product of r1 with 1st friction vector
- Vector3 r1CrossT1;
+ //Vector3 r1CrossT1;
/// Cross product of r1 with 2nd friction vector
- Vector3 r1CrossT2;
+ //Vector3 r1CrossT2;
/// Cross product of r2 with 1st friction vector
- Vector3 r2CrossT1;
+ //Vector3 r2CrossT1;
/// Cross product of r2 with 2nd friction vector
- Vector3 r2CrossT2;
+ //Vector3 r2CrossT2;
/// Cross product of r1 with the contact normal
- Vector3 r1CrossN;
+ //Vector3 r1CrossN;
/// Cross product of r2 with the contact normal
- Vector3 r2CrossN;
+ //Vector3 r2CrossN;
/// Penetration depth
decimal penetrationDepth;
@@ -181,7 +354,7 @@ class ContactSolver {
decimal restitutionBias;
/// Inverse of the matrix K for the penenetration
- decimal inversePenetrationMass;
+ //decimal inversePenetrationMass;
/// Inverse of the matrix K for the 1st friction
decimal inverseFriction1Mass;
@@ -204,40 +377,40 @@ class ContactSolver {
struct ContactManifoldSolver {
/// Index of body 1 in the constraint solver
- uint indexBody1;
+ //uint indexBody1;
/// Index of body 2 in the constraint solver
- uint indexBody2;
+ //uint indexBody2;
/// Inverse of the mass of body 1
- decimal massInverseBody1;
+ //decimal massInverseBody1;
// Inverse of the mass of body 2
- decimal massInverseBody2;
+ //decimal massInverseBody2;
/// Inverse inertia tensor of body 1
- Matrix3x3 inverseInertiaTensorBody1;
+ //Matrix3x3 inverseInertiaTensorBody1;
/// Inverse inertia tensor of body 2
- Matrix3x3 inverseInertiaTensorBody2;
+ //Matrix3x3 inverseInertiaTensorBody2;
/// Contact point constraints
- ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
+ //ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Number of contact points
- uint nbContacts;
+ //uint nbContacts;
/// True if the body 1 is of type dynamic
- bool isBody1DynamicType;
+ //bool isBody1DynamicType;
/// True if the body 2 is of type dynamic
- bool isBody2DynamicType;
+ //bool isBody2DynamicType;
/// Mix of the restitution factor for two bodies
- decimal restitutionFactor;
+ //decimal restitutionFactor;
/// Mix friction coefficient for the two bodies
- decimal frictionCoefficient;
+ //decimal frictionCoefficient;
/// Rolling resistance factor between the two bodies
decimal rollingResistanceFactor;
@@ -248,67 +421,67 @@ class ContactSolver {
// - Variables used when friction constraints are apply at the center of the manifold-//
/// Average normal vector of the contact manifold
- Vector3 normal;
+ //Vector3 normal;
/// Point on body 1 where to apply the friction constraints
- Vector3 frictionPointBody1;
+ //Vector3 frictionPointBody1;
/// Point on body 2 where to apply the friction constraints
- Vector3 frictionPointBody2;
+ //Vector3 frictionPointBody2;
/// R1 vector for the friction constraints
- Vector3 r1Friction;
+ //Vector3 r1Friction;
/// R2 vector for the friction constraints
- Vector3 r2Friction;
+ //Vector3 r2Friction;
/// Cross product of r1 with 1st friction vector
- Vector3 r1CrossT1;
+ //Vector3 r1CrossT1;
/// Cross product of r1 with 2nd friction vector
- Vector3 r1CrossT2;
+ //Vector3 r1CrossT2;
/// Cross product of r2 with 1st friction vector
- Vector3 r2CrossT1;
+ //Vector3 r2CrossT1;
/// Cross product of r2 with 2nd friction vector
- Vector3 r2CrossT2;
+ //Vector3 r2CrossT2;
/// Matrix K for the first friction constraint
- decimal inverseFriction1Mass;
+ //decimal inverseFriction1Mass;
/// Matrix K for the second friction constraint
- decimal inverseFriction2Mass;
+ //decimal inverseFriction2Mass;
/// Matrix K for the twist friction constraint
- decimal inverseTwistFrictionMass;
+ //decimal inverseTwistFrictionMass;
/// Matrix K for the rolling resistance constraint
- Matrix3x3 inverseRollingResistance;
+ //Matrix3x3 inverseRollingResistance;
/// First friction direction at contact manifold center
- Vector3 frictionVector1;
+ //Vector3 frictionVector1;
/// Second friction direction at contact manifold center
- Vector3 frictionVector2;
+ //Vector3 frictionVector2;
/// Old 1st friction direction at contact manifold center
- Vector3 oldFrictionVector1;
+ //Vector3 oldFrictionVector1;
/// Old 2nd friction direction at contact manifold center
- Vector3 oldFrictionVector2;
+ //Vector3 oldFrictionVector2;
/// First friction direction impulse at manifold center
- decimal friction1Impulse;
+ //decimal friction1Impulse;
/// Second friction direction impulse at manifold center
- decimal friction2Impulse;
+ //decimal friction2Impulse;
/// Twist friction impulse at contact manifold center
- decimal frictionTwistImpulse;
+ //decimal frictionTwistImpulse;
/// Rolling resistance impulse
- Vector3 rollingResistanceImpulse;
+ //Vector3 rollingResistanceImpulse;
};
// -------------------- Constants --------------------- //
@@ -336,6 +509,14 @@ class ContactSolver {
/// Contact constraints
ContactManifoldSolver* mContactConstraints;
+ PenetrationConstraint* mPenetrationConstraints;
+
+ FrictionConstraint* mFrictionConstraints;
+
+ uint mNbPenetrationConstraints;
+
+ uint mNbFrictionConstraints;
+
/// Number of contact constraints
uint mNbContactManifolds;
@@ -364,11 +545,11 @@ class ContactSolver {
void initializeContactConstraints();
/// Apply an impulse to the two bodies of a constraint
- void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
+ //void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
/// Apply an impulse to the two bodies of a constraint
- void applySplitImpulse(const Impulse& impulse,
- const ContactManifoldSolver& manifold);
+ //void applySplitImpulse(const Impulse& impulse,
+ // const ContactManifoldSolver& manifold);
/// Compute the collision restitution factor from the restitution factor of each body
decimal computeMixedRestitutionFactor(RigidBody *body1,
@@ -381,29 +562,30 @@ class ContactSolver {
/// Compute th mixed rolling resistance factor between two bodies
decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const;
+ // TODO : Delete this
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
/// plane for a contact point. The two vectors have to be
/// such that : t1 x t2 = contactNormal.
- void computeFrictionVectors(const Vector3& deltaVelocity,
- ContactPointSolver &contactPoint) const;
+// void computeFrictionVectors(const Vector3& deltaVelocity,
+// ContactPointSolver &contactPoint) const;
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
/// plane for a contact manifold. The two vectors have to be
/// such that : t1 x t2 = contactNormal.
void computeFrictionVectors(const Vector3& deltaVelocity,
- ContactManifoldSolver& contactPoint) const;
+ FrictionConstraint& frictionConstraint) const;
/// Compute a penetration constraint impulse
- const Impulse computePenetrationImpulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint) const;
+// const Impulse computePenetrationImpulse(decimal deltaLambda,
+// const PenetrationConstraint& constraint) const;
/// Compute the first friction constraint impulse
const Impulse computeFriction1Impulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint) const;
+ const FrictionConstraint& contactPoint) const;
/// Compute the second friction constraint impulse
const Impulse computeFriction2Impulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint) const;
+ const FrictionConstraint& contactPoint) const;
public:
@@ -434,7 +616,16 @@ class ContactSolver {
void storeImpulses();
/// Solve the contacts
- void solve();
+ //void solve();
+
+ /// Reset the total penetration impulse of friction constraints
+ void resetTotalPenetrationImpulse();
+
+ /// Solve the penetration constraints
+ void solvePenetrationConstraints();
+
+ /// Solve the friction constraints
+ void solveFrictionConstraints();
/// Return true if the split impulses position correction technique is used for contacts
bool isSplitImpulseActive() const;
@@ -502,7 +693,7 @@ inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
RigidBody *body2) const {
// Use the geometric mean to compute the mixed friction coefficient
- return sqrt(body1->getMaterial().getFrictionCoefficient() *
+ return std::sqrt(body1->getMaterial().getFrictionCoefficient() *
body2->getMaterial().getFrictionCoefficient());
}
@@ -513,16 +704,16 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1,
}
// Compute a penetration constraint impulse
-inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint)
- const {
- return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda,
- contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda);
-}
+//inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
+// const PenetrationConstraint& constraint)
+// const {
+// return Impulse(-constraint.normal * deltaLambda, -constraint.r1CrossN * deltaLambda,
+// constraint.normal * deltaLambda, constraint.r2CrossN * deltaLambda);
+//}
// Compute the first friction constraint impulse
inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint)
+ const FrictionConstraint& contactPoint)
const {
return Impulse(-contactPoint.frictionVector1 * deltaLambda,
-contactPoint.r1CrossT1 * deltaLambda,
@@ -532,7 +723,7 @@ inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
// Compute the second friction constraint impulse
inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint)
+ const FrictionConstraint& contactPoint)
const {
return Impulse(-contactPoint.frictionVector2 * deltaLambda,
-contactPoint.r1CrossT2 * deltaLambda,
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 2f601edd..4bd7aa19 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -353,6 +353,8 @@ void DynamicsWorld::solveContactsAndConstraints() {
PROFILE("DynamicsWorld::solveContactsAndConstraints()");
+ // TODO : Do not solve per island but solve every constraints at once
+
// Set the velocities arrays
mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
@@ -398,7 +400,13 @@ void DynamicsWorld::solveContactsAndConstraints() {
}
// Solve the contacts
- if (isContactsToSolve) mContactSolver.solve();
+ if (isContactsToSolve) {
+
+ mContactSolver.resetTotalPenetrationImpulse();
+
+ mContactSolver.solvePenetrationConstraints();
+ mContactSolver.solveFrictionConstraints();
+ }
}
// Cache the lambda values in order to use them in the next
diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp
index ed29a37f..ab2d126d 100644
--- a/src/mathematics/Vector3.cpp
+++ b/src/mathematics/Vector3.cpp
@@ -65,17 +65,17 @@ Vector3 Vector3::getOneUnitOrthogonalVector() const {
assert(length() > MACHINE_EPSILON);
// Get the minimum element of the vector
- Vector3 vectorAbs(fabs(x), fabs(y), fabs(z));
+ Vector3 vectorAbs(std::fabs(x), std::fabs(y), std::fabs(z));
int minElement = vectorAbs.getMinAxis();
if (minElement == 0) {
- return Vector3(0.0, -z, y) / sqrt(y*y + z*z);
+ return Vector3(0.0, -z, y) / std::sqrt(y*y + z*z);
}
else if (minElement == 1) {
- return Vector3(-z, 0.0, x) / sqrt(x*x + z*z);
+ return Vector3(-z, 0.0, x) / std::sqrt(x*x + z*z);
}
else {
- return Vector3(-y, x, 0.0) / sqrt(x*x + y*y);
+ return Vector3(-y, x, 0.0) / std::sqrt(x*x + y*y);
}
}
From b4f13308de720524e91f07a03c82744d59b6eba5 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 13 Sep 2016 22:58:17 +0200
Subject: [PATCH 003/133] Optimize warmstarting in contact solver
---
src/engine/ContactSolver.cpp | 141 +++++++++++++++++++++++++++++++++--
src/engine/ContactSolver.h | 24 ++++++
src/engine/DynamicsWorld.cpp | 4 +-
src/engine/Island.h | 4 +-
4 files changed, 164 insertions(+), 9 deletions(-)
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 22359b10..4e2e0eec 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -38,15 +38,13 @@ const decimal ContactSolver::BETA = decimal(0.2);
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP= decimal(0.01);
-// TODO : Enable warmstarting again
-
// Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
:mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr),
mContactConstraints(nullptr), mPenetrationConstraints(nullptr),
mFrictionConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
- mIsWarmStartingActive(false), mIsSplitImpulseActive(true),
+ mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
mIsSolveFrictionAtContactManifoldCenterActive(true) {
}
@@ -83,7 +81,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
assert(mFrictionConstraints != nullptr);
// For each contact manifold of the island
- ContactManifold** contactManifolds = island->getContactManifold();
+ ContactManifold** contactManifolds = island->getContactManifolds();
for (uint i=0; i<mNbContactManifolds; i++) {
ContactManifold* externalManifold = contactManifolds[i];
@@ -104,6 +102,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
mFrictionConstraints[mNbFrictionConstraints].indexBody1 = indexBody1;
mFrictionConstraints[mNbFrictionConstraints].indexBody2 = indexBody2;
+ mFrictionConstraints[mNbFrictionConstraints].contactManifold = externalManifold;
// Get the position of the two bodies
const Vector3& x1 = body1->mCenterOfMassWorld;
@@ -131,6 +130,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
internalManifold.externalContactManifold = externalManifold;
+ mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false;
//internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
//internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
@@ -167,6 +167,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse;
mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor = restitutionFactor;
mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints;
+ mPenetrationConstraints[mNbPenetrationConstraints].contactPoint = externalContact;
// Get the contact point on the two bodies
Vector3 p1 = externalContact->getWorldPointOnBody1();
@@ -178,7 +179,10 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
//mPenetrationConstraints[penConstIndex].externalContact = externalContact;
mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal();
mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth();
- //mPenetrationConstraints[penConstIndex].isRestingContact = externalContact->getIsRestingContact();
+ mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact = externalContact->getIsRestingContact();
+
+ mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint |= mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact;
+
externalContact->setIsRestingContact(true);
//mPenetrationConstraints[penConstIndex].oldFrictionVector1 = externalContact->getFrictionVector1();
//mPenetrationConstraints[penConstIndex].oldFrictionVector2 = externalContact->getFrictionVector2();
@@ -467,9 +471,116 @@ void ContactSolver::initializeContactConstraints() {
/// the solution of the linear system
void ContactSolver::warmStart() {
- /*
PROFILE("ContactSolver::warmStart()");
+ // Penetration constraints
+ for (uint i=0; i<mNbPenetrationConstraints; i++) {
+
+ // If it is not a new contact (this contact was already existing at last time step)
+ if (mPenetrationConstraints[i].isRestingContact) {
+
+ Vector3 linearImpulse = mPenetrationConstraints[i].normal * mPenetrationConstraints[i].penetrationImpulse;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].massInverseBody1 *
+ (-linearImpulse);
+ mAngularVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].inverseInertiaTensorBody1 *
+ (-mPenetrationConstraints[i].r1CrossN * mPenetrationConstraints[i].penetrationImpulse);
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].massInverseBody2 *
+ linearImpulse;
+ mAngularVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].inverseInertiaTensorBody2 *
+ (-mPenetrationConstraints[i].r1CrossN * mPenetrationConstraints[i].penetrationImpulse);
+
+ }
+ else { // If it is a new contact point
+
+ // Initialize the accumulated impulses to zero
+ mPenetrationConstraints[i].penetrationImpulse = 0.0;
+ }
+ }
+
+ // Friction constraints
+ for (uint i=0; i<mNbFrictionConstraints; i++) {
+
+ // If we solve the friction constraints at the center of the contact manifold and there is
+ // at least one resting contact point in the contact manifold
+ if (mFrictionConstraints[i].hasAtLeastOneRestingContactPoint) {
+
+ // Project the old friction impulses (with old friction vectors) into the new friction
+ // vectors to get the new friction impulses
+ Vector3 oldFrictionImpulse = mFrictionConstraints[i].friction1Impulse * mFrictionConstraints[i].oldFrictionVector1 +
+ mFrictionConstraints[i].friction2Impulse * mFrictionConstraints[i].oldFrictionVector2;
+ mFrictionConstraints[i].friction1Impulse = oldFrictionImpulse.dot(mFrictionConstraints[i].frictionVector1);
+ mFrictionConstraints[i].friction2Impulse = oldFrictionImpulse.dot(mFrictionConstraints[i].frictionVector2);
+
+ // ------ First friction constraint at the center of the contact manifold ------ //
+
+ // Compute the impulse P = J^T * lambda
+ Vector3 linearImpulseBody2 = mFrictionConstraints[i].frictionVector1 * mFrictionConstraints[i].friction1Impulse;
+ Vector3 angularImpulseBody1 = -mFrictionConstraints[i].r1CrossT1 * mFrictionConstraints[i].friction1Impulse;
+ Vector3 angularImpulseBody2 = mFrictionConstraints[i].r2CrossT1 * mFrictionConstraints[i].friction1Impulse;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].massInverseBody1 * (-linearImpulseBody2);
+ mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
+ mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+
+ // ------ Second friction constraint at the center of the contact manifold ----- //
+
+ // Compute the impulse P = J^T * lambda
+ angularImpulseBody1 = -mFrictionConstraints[i].r1CrossT2 * mFrictionConstraints[i].friction2Impulse;
+ linearImpulseBody2 = mFrictionConstraints[i].frictionVector2 * mFrictionConstraints[i].friction2Impulse;
+ angularImpulseBody2 = mFrictionConstraints[i].r2CrossT2 * mFrictionConstraints[i].friction2Impulse;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].massInverseBody1 * (-linearImpulseBody2);
+ mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
+ mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+
+ // ------ Twist friction constraint at the center of the contact manifold ------ //
+
+ // Compute the impulse P = J^T * lambda
+ angularImpulseBody2 = mFrictionConstraints[i].normal * mFrictionConstraints[i].frictionTwistImpulse;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * (-angularImpulseBody2);
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+
+ // ------ Rolling resistance at the center of the contact manifold ------ //
+
+ // Compute the impulse P = J^T * lambda
+ angularImpulseBody2 = mFrictionConstraints[i].rollingResistanceImpulse;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * (-angularImpulseBody2);
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+ }
+ else { // If it is a new contact manifold
+
+ // Initialize the accumulated impulses to zero
+ mFrictionConstraints[i].friction1Impulse = 0.0;
+ mFrictionConstraints[i].friction2Impulse = 0.0;
+ mFrictionConstraints[i].frictionTwistImpulse = 0.0;
+ mFrictionConstraints[i].rollingResistanceImpulse.setToZero();
+ }
+ }
+
+
+ /*
+
+
// Check that warm starting is active
if (!mIsWarmStartingActive) return;
@@ -1144,6 +1255,24 @@ void ContactSolver::solveFrictionConstraints() {
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
+ // Penetration constraints
+ for (uint i=0; i<mNbPenetrationConstraints; i++) {
+
+ mPenetrationConstraints[i].contactPoint->setPenetrationImpulse(mPenetrationConstraints[i].penetrationImpulse);
+
+ }
+
+ // Friction constraints
+ for (uint i=0; i<mNbFrictionConstraints; i++) {
+
+ mFrictionConstraints[i].contactManifold->setFrictionImpulse1(mFrictionConstraints[i].friction1Impulse);
+ mFrictionConstraints[i].contactManifold->setFrictionImpulse2(mFrictionConstraints[i].friction2Impulse);
+ mFrictionConstraints[i].contactManifold->setFrictionTwistImpulse(mFrictionConstraints[i].frictionTwistImpulse);
+ mFrictionConstraints[i].contactManifold->setRollingResistanceImpulse(mFrictionConstraints[i].rollingResistanceImpulse);
+ mFrictionConstraints[i].contactManifold->setFrictionVector1(mFrictionConstraints[i].frictionVector1);
+ mFrictionConstraints[i].contactManifold->setFrictionVector2(mFrictionConstraints[i].frictionVector2);
+ }
+
/*
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index ef0087a0..f3f5dfa7 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -114,6 +114,8 @@ class ContactSolver {
struct PenetrationConstraint {
+ // TODO : Pack bools into a single value
+
/// Index of body 1 in the constraint solver
uint indexBody1;
@@ -167,10 +169,18 @@ class ContactSolver {
/// Index of the corresponding friction constraint
uint indexFrictionConstraint;
+
+ /// Pointer to the corresponding contact point
+ ContactPoint* contactPoint;
+
+ /// True if this constraint is for a resting contact
+ bool isRestingContact;
};
struct FrictionConstraint {
+ // TODO : Pack bools into a single value
+
/// Index of body 1 in the constraint solver
uint indexBody1;
@@ -260,6 +270,12 @@ class ContactSolver {
/// Inverse inertia tensor of body 2
Matrix3x3 inverseInertiaTensorBody2;
+
+ /// Pointer to the corresponding contact manifold
+ ContactManifold* contactManifold;
+
+ /// True if the original contact manifold has at least one resting contact
+ bool hasAtLeastOneRestingContactPoint;
};
// Structure ContactPointSolver
@@ -639,6 +655,9 @@ class ContactSolver {
/// Clean up the constraint solver
void cleanup();
+
+ /// Return true if warmstarting is active
+ bool IsWarmStartingActive() const;
};
// Set the split velocities arrays
@@ -731,6 +750,11 @@ inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
contactPoint.r2CrossT2 * deltaLambda);
}
+// Return true if warmstarting is active
+inline bool ContactSolver::IsWarmStartingActive() const {
+ return mIsWarmStartingActive;
+}
+
}
#endif
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 4bd7aa19..189ef0e0 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -381,7 +381,9 @@ void DynamicsWorld::solveContactsAndConstraints() {
mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
// Warm start the contact solver
- mContactSolver.warmStart();
+ if (mContactSolver.IsWarmStartingActive()) {
+ mContactSolver.warmStart();
+ }
}
// If there are constraints
diff --git a/src/engine/Island.h b/src/engine/Island.h
index 1b086939..512a0d1b 100644
--- a/src/engine/Island.h
+++ b/src/engine/Island.h
@@ -114,7 +114,7 @@ class Island {
RigidBody** getBodies();
/// Return a pointer to the array of contact manifolds
- ContactManifold** getContactManifold();
+ ContactManifold** getContactManifolds();
/// Return a pointer to the array of joints
Joint** getJoints();
@@ -164,7 +164,7 @@ inline RigidBody** Island::getBodies() {
}
// Return a pointer to the array of contact manifolds
-inline ContactManifold** Island::getContactManifold() {
+inline ContactManifold** Island::getContactManifolds() {
return mContactManifolds;
}
From 1a26241fa85a5e373df004bc6ceb3a9fccd4804f Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 16 Sep 2016 20:02:38 +0200
Subject: [PATCH 004/133] Fix issue in contact solver
---
src/engine/ContactSolver.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 4e2e0eec..a0db1119 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -832,7 +832,7 @@ void ContactSolver::solvePenetrationConstraints() {
mPenetrationConstraints[i].penetrationSplitImpulse = std::max(
mPenetrationConstraints[i].penetrationSplitImpulse +
deltaLambdaSplit, decimal(0.0));
- deltaLambda = mPenetrationConstraints[i].penetrationSplitImpulse - lambdaTempSplit;
+ deltaLambdaSplit = mPenetrationConstraints[i].penetrationSplitImpulse - lambdaTempSplit;
// Update the velocities of the body 1 by applying the impulse P=J^T * lambda
Vector3 linearImpulse = mPenetrationConstraints[i].normal * deltaLambdaSplit;
From 92460791e651bf62216a14992c4bfb7da5926952 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 21 Sep 2016 22:01:14 +0200
Subject: [PATCH 005/133] Refactor PoolAllocator and add SingleFrameAllocator
---
...{MemoryAllocator.cpp => PoolAllocator.cpp} | 16 ++--
.../{MemoryAllocator.h => PoolAllocator.h} | 12 +--
src/memory/SingleFrameAllocator.cpp | 80 +++++++++++++++++++
src/memory/SingleFrameAllocator.h | 76 ++++++++++++++++++
4 files changed, 170 insertions(+), 14 deletions(-)
rename src/memory/{MemoryAllocator.cpp => PoolAllocator.cpp} (95%)
rename src/memory/{MemoryAllocator.h => PoolAllocator.h} (96%)
create mode 100644 src/memory/SingleFrameAllocator.cpp
create mode 100644 src/memory/SingleFrameAllocator.h
diff --git a/src/memory/MemoryAllocator.cpp b/src/memory/PoolAllocator.cpp
similarity index 95%
rename from src/memory/MemoryAllocator.cpp
rename to src/memory/PoolAllocator.cpp
index 46c47e14..dc1306bc 100644
--- a/src/memory/MemoryAllocator.cpp
+++ b/src/memory/PoolAllocator.cpp
@@ -24,19 +24,19 @@
********************************************************************************/
// Libraries
-#include "MemoryAllocator.h"
+#include "PoolAllocator.h"
#include <cstdlib>
#include <cassert>
using namespace reactphysics3d;
// Initialization of static variables
-bool MemoryAllocator::isMapSizeToHeadIndexInitialized = false;
-size_t MemoryAllocator::mUnitSizes[NB_HEAPS];
-int MemoryAllocator::mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1];
+bool PoolAllocator::isMapSizeToHeadIndexInitialized = false;
+size_t PoolAllocator::mUnitSizes[NB_HEAPS];
+int PoolAllocator::mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1];
// Constructor
-MemoryAllocator::MemoryAllocator() {
+PoolAllocator::PoolAllocator() {
// Allocate some memory to manage the blocks
mNbAllocatedMemoryBlocks = 64;
@@ -78,7 +78,7 @@ MemoryAllocator::MemoryAllocator() {
}
// Destructor
-MemoryAllocator::~MemoryAllocator() {
+PoolAllocator::~PoolAllocator() {
// Release the memory allocated for each block
for (uint i=0; i<mNbCurrentMemoryBlocks; i++) {
@@ -96,7 +96,7 @@ MemoryAllocator::~MemoryAllocator() {
// Allocate memory of a given size (in bytes) and return a pointer to the
// allocated memory.
-void* MemoryAllocator::allocate(size_t size) {
+void* PoolAllocator::allocate(size_t size) {
// We cannot allocate zero bytes
if (size == 0) return nullptr;
@@ -164,7 +164,7 @@ void* MemoryAllocator::allocate(size_t size) {
}
// Release previously allocated memory.
-void MemoryAllocator::release(void* pointer, size_t size) {
+void PoolAllocator::release(void* pointer, size_t size) {
// Cannot release a 0-byte allocated memory
if (size == 0) return;
diff --git a/src/memory/MemoryAllocator.h b/src/memory/PoolAllocator.h
similarity index 96%
rename from src/memory/MemoryAllocator.h
rename to src/memory/PoolAllocator.h
index 77006961..2af762ca 100644
--- a/src/memory/MemoryAllocator.h
+++ b/src/memory/PoolAllocator.h
@@ -23,8 +23,8 @@
* *
********************************************************************************/
-#ifndef REACTPHYSICS3D_MEMORY_ALLOCATOR_H
-#define REACTPHYSICS3D_MEMORY_ALLOCATOR_H
+#ifndef REACTPHYSICS3D_POOL_ALLOCATOR_H
+#define REACTPHYSICS3D_POOL_ALLOCATOR_H
// Libraries
#include <cstring>
@@ -33,14 +33,14 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
-// Class MemoryAllocator
+// Class PoolAllocator
/**
* This class is used to efficiently allocate memory on the heap.
* It allows us to allocate small blocks of memory (smaller or equal to 1024 bytes)
* efficiently. This implementation is inspired by the small block allocator
* described here : http://www.codeproject.com/useritems/Small_Block_Allocator.asp
*/
-class MemoryAllocator {
+class PoolAllocator {
private :
@@ -126,10 +126,10 @@ class MemoryAllocator {
// -------------------- Methods -------------------- //
/// Constructor
- MemoryAllocator();
+ PoolAllocator();
/// Destructor
- ~MemoryAllocator();
+ ~PoolAllocator();
/// Allocate memory of a given size (in bytes) and return a pointer to the
/// allocated memory.
diff --git a/src/memory/SingleFrameAllocator.cpp b/src/memory/SingleFrameAllocator.cpp
new file mode 100644
index 00000000..a2e436cd
--- /dev/null
+++ b/src/memory/SingleFrameAllocator.cpp
@@ -0,0 +1,80 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "SingleFrameAllocator.h"
+#include <cstdlib>
+#include <cassert>
+
+using namespace reactphysics3d;
+
+// Constructor
+SingleFrameAllocator::SingleFrameAllocator(size_t totalSizeBytes)
+ : mTotalSizeBytes(totalSizeBytes), mCurrentOffset(0) {
+
+ // Allocate a whole block of memory at the beginning
+ mMemoryBufferStart = static_cast<char*>(malloc(mTotalSizeBytes));
+ assert(mMemoryBufferStart != nullptr);
+}
+
+// Destructor
+SingleFrameAllocator::~SingleFrameAllocator() {
+
+ // Release the memory allocated at the beginning
+ free(mMemoryBufferStart);
+}
+
+
+// Allocate memory of a given size (in bytes) and return a pointer to the
+// allocated memory.
+void* SingleFrameAllocator::allocate(size_t size) {
+
+ // Check that there is enough remaining memory in the buffer
+ if (static_cast<size_t>(mCurrentOffset) + size > mTotalSizeBytes) {
+
+ // This should never occur. If it does, you must increase the initial
+ // size of memory of this allocator
+ assert(false);
+
+ // Return null
+ return nullptr;
+ }
+
+ // Next available memory location
+ void* nextAvailableMemory = mMemoryBufferStart + mCurrentOffset;
+
+ // Increment the offset
+ mCurrentOffset += size;
+
+ // Return the next available memory location
+ return nextAvailableMemory;
+}
+
+// Reset the marker of the current allocated memory
+void SingleFrameAllocator::reset() {
+
+ // Reset the current offset at the beginning of the block
+ mCurrentOffset = 0;
+}
diff --git a/src/memory/SingleFrameAllocator.h b/src/memory/SingleFrameAllocator.h
new file mode 100644
index 00000000..e3151f0a
--- /dev/null
+++ b/src/memory/SingleFrameAllocator.h
@@ -0,0 +1,76 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H
+#define REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H
+
+// Libraries
+#include <cstring>
+#include "configuration.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class SingleFrameAllocator
+/**
+ * This class represent a memory allocator used to efficiently allocate
+ * memory on the heap that is used during a single frame.
+ */
+class SingleFrameAllocator {
+
+ private :
+
+ // -------------------- Attributes -------------------- //
+
+ /// Total size (in bytes) of memory of the allocator
+ size_t mTotalSizeBytes;
+
+ /// Pointer to the beginning of the allocated memory block
+ char* mMemoryBufferStart;
+
+ /// Pointer to the next available memory location in the buffer
+ int mCurrentOffset;
+
+ public :
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ SingleFrameAllocator(size_t totalSizeBytes);
+
+ /// Destructor
+ ~SingleFrameAllocator();
+
+ /// Allocate memory of a given size (in bytes)
+ void* allocate(size_t size);
+
+ /// Reset the marker of the current allocated memory
+ void reset();
+
+};
+
+}
+
+#endif
From e014f00afc71bf62e78d02fba1e2fea3e957fbb3 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 21 Sep 2016 22:03:45 +0200
Subject: [PATCH 006/133] Refactor memory allocator and refactor contact solver
---
CMakeLists.txt | 6 +-
src/body/CollisionBody.cpp | 10 +-
src/body/CollisionBody.h | 2 +-
src/body/RigidBody.cpp | 4 +-
src/body/RigidBody.h | 4 +-
src/collision/CollisionDetection.cpp | 22 +-
src/collision/CollisionDetection.h | 8 +-
src/collision/ContactManifold.cpp | 2 +-
src/collision/ContactManifold.h | 6 +-
src/collision/ContactManifoldSet.cpp | 2 +-
src/collision/ContactManifoldSet.h | 4 +-
src/collision/narrowphase/CollisionDispatch.h | 2 +-
.../narrowphase/DefaultCollisionDispatch.cpp | 2 +-
.../narrowphase/DefaultCollisionDispatch.h | 2 +-
src/collision/narrowphase/EPA/EPAAlgorithm.h | 8 +-
src/collision/narrowphase/GJK/GJKAlgorithm.h | 4 +-
.../narrowphase/GJK/VoronoiSimplex.cpp | 1 -
.../narrowphase/NarrowPhaseAlgorithm.cpp | 2 +-
.../narrowphase/NarrowPhaseAlgorithm.h | 6 +-
src/collision/shapes/CollisionShape.h | 2 +-
src/configuration.h | 3 +
src/engine/CollisionWorld.cpp | 6 +-
src/engine/CollisionWorld.h | 6 +-
src/engine/ContactSolver.cpp | 805 ++----------------
src/engine/ContactSolver.h | 249 +-----
src/engine/DynamicsWorld.cpp | 103 ++-
src/engine/DynamicsWorld.h | 3 +
src/engine/Island.cpp | 2 +-
src/engine/Island.h | 6 +-
src/engine/OverlappingPair.cpp | 2 +-
src/engine/OverlappingPair.h | 2 +-
31 files changed, 201 insertions(+), 1085 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e85693d7..69f20522 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -174,8 +174,10 @@ SET (REACTPHYSICS3D_SOURCES
"src/mathematics/Vector3.h"
"src/mathematics/Ray.h"
"src/mathematics/Vector3.cpp"
- "src/memory/MemoryAllocator.h"
- "src/memory/MemoryAllocator.cpp"
+ "src/memory/PoolAllocator.h"
+ "src/memory/PoolAllocator.cpp"
+ "src/memory/SingleFrameAllocator.h"
+ "src/memory/SingleFrameAllocator.cpp"
"src/memory/Stack.h"
)
diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index daa4a71e..db22f606 100644
--- a/src/body/CollisionBody.cpp
+++ b/src/body/CollisionBody.cpp
@@ -70,7 +70,7 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform) {
// Create a new proxy collision shape to attach the collision shape to the body
- ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
+ ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, decimal(1));
@@ -116,7 +116,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
}
current->~ProxyShape();
- mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
+ mWorld.mPoolAllocator.release(current, sizeof(ProxyShape));
mNbCollisionShapes--;
return;
}
@@ -136,7 +136,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
}
elementToRemove->~ProxyShape();
- mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape));
+ mWorld.mPoolAllocator.release(elementToRemove, sizeof(ProxyShape));
mNbCollisionShapes--;
return;
}
@@ -162,7 +162,7 @@ void CollisionBody::removeAllCollisionShapes() {
}
current->~ProxyShape();
- mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
+ mWorld.mPoolAllocator.release(current, sizeof(ProxyShape));
// Get the next element in the list
current = nextElement;
@@ -181,7 +181,7 @@ void CollisionBody::resetContactManifoldsList() {
// Delete the current element
currentElement->~ContactManifoldListElement();
- mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement));
+ mWorld.mPoolAllocator.release(currentElement, sizeof(ContactManifoldListElement));
currentElement = nextElement;
}
diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h
index 5fda6958..bd5c7818 100644
--- a/src/body/CollisionBody.h
+++ b/src/body/CollisionBody.h
@@ -34,7 +34,7 @@
#include "collision/shapes/AABB.h"
#include "collision/shapes/CollisionShape.h"
#include "collision/RaycastInfo.h"
-#include "memory/MemoryAllocator.h"
+#include "memory/PoolAllocator.h"
#include "configuration.h"
/// Namespace reactphysics3d
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index ff77b228..74ee8a05 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -166,7 +166,7 @@ void RigidBody::setMass(decimal mass) {
}
// Remove a joint from the joints list
-void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint) {
+void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint) {
assert(joint != nullptr);
assert(mJointsList != nullptr);
@@ -214,7 +214,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
decimal mass) {
// Create a new proxy collision shape to attach the collision shape to the body
- ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
+ ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, mass);
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index 8fb36d0e..0ffc6257 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -31,7 +31,7 @@
#include "CollisionBody.h"
#include "engine/Material.h"
#include "mathematics/mathematics.h"
-#include "memory/MemoryAllocator.h"
+#include "memory/PoolAllocator.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
@@ -104,7 +104,7 @@ class RigidBody : public CollisionBody {
// -------------------- Methods -------------------- //
/// Remove a joint from the joints list
- void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint);
+ void removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint);
/// Update the transform of the body after a change of the center of mass
void updateTransformWithCenterOfMass();
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 7d9f6fb2..c3caa97a 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -41,7 +41,7 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
-CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator)
+CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator)
: mMemoryAllocator(memoryAllocator),
mWorld(world), mBroadPhaseAlgorithm(*this),
mIsCollisionShapesAdded(false) {
@@ -189,7 +189,7 @@ void CollisionDetection::computeNarrowPhase() {
// Destroy the overlapping pair
itToRemove->second->~OverlappingPair();
- mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
+ mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove);
continue;
}
@@ -294,7 +294,7 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
// Destroy the overlapping pair
itToRemove->second->~OverlappingPair();
- mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
+ mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove);
continue;
}
@@ -370,8 +370,8 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
shape2->getCollisionShape()->getType());
// Create the overlapping pair and add it into the set of overlapping pairs
- OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair)))
- OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator);
+ OverlappingPair* newPair = new (mWorld->mPoolAllocator.allocate(sizeof(OverlappingPair)))
+ OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mPoolAllocator);
assert(newPair != nullptr);
#ifndef NDEBUG
@@ -400,7 +400,7 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
// Destroy the overlapping pair
itToRemove->second->~OverlappingPair();
- mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
+ mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove);
}
else {
@@ -434,7 +434,7 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo) {
// Create a new contact
- ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint)))
+ ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(contactInfo);
// Add the contact to the contact manifold set of the corresponding overlapping pair
@@ -477,7 +477,7 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
- void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
+ void* allocatedMemory1 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement1 = new (allocatedMemory1)
ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList);
@@ -485,7 +485,7 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
// Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body
- void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
+ void* allocatedMemory2 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
@@ -520,8 +520,8 @@ EventListener* CollisionDetection::getWorldEventListener() {
}
/// Return a reference to the world memory allocator
-MemoryAllocator& CollisionDetection::getWorldMemoryAllocator() {
- return mWorld->mMemoryAllocator;
+PoolAllocator& CollisionDetection::getWorldMemoryAllocator() {
+ return mWorld->mPoolAllocator;
}
// Called by a narrow-phase collision algorithm when a new contact has been found
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 75f278ba..0fe486c4 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -32,7 +32,7 @@
#include "engine/OverlappingPair.h"
#include "engine/EventListener.h"
#include "narrowphase/DefaultCollisionDispatch.h"
-#include "memory/MemoryAllocator.h"
+#include "memory/PoolAllocator.h"
#include "constraint/ContactPoint.h"
#include <vector>
#include <set>
@@ -93,7 +93,7 @@ class CollisionDetection : public NarrowPhaseCallback {
NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES];
/// Reference to the memory allocator
- MemoryAllocator& mMemoryAllocator;
+ PoolAllocator& mMemoryAllocator;
/// Pointer to the physics world
CollisionWorld* mWorld;
@@ -143,7 +143,7 @@ class CollisionDetection : public NarrowPhaseCallback {
// -------------------- Methods -------------------- //
/// Constructor
- CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator);
+ CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator);
/// Destructor
~CollisionDetection() = default;
@@ -220,7 +220,7 @@ class CollisionDetection : public NarrowPhaseCallback {
EventListener* getWorldEventListener();
/// Return a reference to the world memory allocator
- MemoryAllocator& getWorldMemoryAllocator();
+ PoolAllocator& getWorldMemoryAllocator();
/// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) override;
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index cfa905ff..ff15bce6 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -31,7 +31,7 @@ using namespace reactphysics3d;
// Constructor
ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
- MemoryAllocator& memoryAllocator, short normalDirectionId)
+ PoolAllocator& memoryAllocator, short normalDirectionId)
: mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index eb7716df..2f77e9b2 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -31,7 +31,7 @@
#include "body/CollisionBody.h"
#include "collision/ProxyShape.h"
#include "constraint/ContactPoint.h"
-#include "memory/MemoryAllocator.h"
+#include "memory/PoolAllocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -126,7 +126,7 @@ class ContactManifold {
bool mIsAlreadyInIsland;
/// Reference to the memory allocator
- MemoryAllocator& mMemoryAllocator;
+ PoolAllocator& mMemoryAllocator;
// -------------------- Methods -------------------- //
@@ -151,7 +151,7 @@ class ContactManifold {
/// Constructor
ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
- MemoryAllocator& memoryAllocator, short int normalDirectionId);
+ PoolAllocator& memoryAllocator, short int normalDirectionId);
/// Destructor
~ContactManifold();
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index 54e3614e..26f3a66f 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -30,7 +30,7 @@ using namespace reactphysics3d;
// Constructor
ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
- MemoryAllocator& memoryAllocator, int nbMaxManifolds)
+ PoolAllocator& memoryAllocator, int nbMaxManifolds)
: mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator) {
assert(nbMaxManifolds >= 1);
diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h
index b167f506..21944e36 100644
--- a/src/collision/ContactManifoldSet.h
+++ b/src/collision/ContactManifoldSet.h
@@ -61,7 +61,7 @@ class ContactManifoldSet {
ProxyShape* mShape2;
/// Reference to the memory allocator
- MemoryAllocator& mMemoryAllocator;
+ PoolAllocator& mMemoryAllocator;
/// Contact manifolds of the set
ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET];
@@ -88,7 +88,7 @@ class ContactManifoldSet {
/// Constructor
ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
- MemoryAllocator& memoryAllocator, int nbMaxManifolds);
+ PoolAllocator& memoryAllocator, int nbMaxManifolds);
/// Destructor
~ContactManifoldSet();
diff --git a/src/collision/narrowphase/CollisionDispatch.h b/src/collision/narrowphase/CollisionDispatch.h
index 8fa24f87..6e027721 100644
--- a/src/collision/narrowphase/CollisionDispatch.h
+++ b/src/collision/narrowphase/CollisionDispatch.h
@@ -51,7 +51,7 @@ class CollisionDispatch {
/// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection,
- MemoryAllocator* memoryAllocator) {
+ PoolAllocator* memoryAllocator) {
}
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
index 109b5de3..f6812d74 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
@@ -31,7 +31,7 @@ using namespace reactphysics3d;
/// Initialize the collision dispatch configuration
void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection,
- MemoryAllocator* memoryAllocator) {
+ PoolAllocator* memoryAllocator) {
// Initialize the collision algorithms
mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator);
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h
index 9bf15b11..5dd07bf2 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.h
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.h
@@ -63,7 +63,7 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection,
- MemoryAllocator* memoryAllocator) override;
+ PoolAllocator* memoryAllocator) override;
/// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes.
diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h
index f585ac15..05a78eea 100644
--- a/src/collision/narrowphase/EPA/EPAAlgorithm.h
+++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h
@@ -34,7 +34,7 @@
#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
#include "mathematics/mathematics.h"
#include "TriangleEPA.h"
-#include "memory/MemoryAllocator.h"
+#include "memory/PoolAllocator.h"
#include <algorithm>
/// ReactPhysics3D namespace
@@ -88,7 +88,7 @@ class EPAAlgorithm {
// -------------------- Attributes -------------------- //
/// Reference to the memory allocator
- MemoryAllocator* mMemoryAllocator;
+ PoolAllocator* mMemoryAllocator;
/// Triangle comparison operator
TriangleComparison mTriangleComparison;
@@ -120,7 +120,7 @@ class EPAAlgorithm {
EPAAlgorithm& operator=(const EPAAlgorithm& algorithm) = delete;
/// Initalize the algorithm
- void init(MemoryAllocator* memoryAllocator);
+ void init(PoolAllocator* memoryAllocator);
/// Compute the penetration depth with EPA algorithm.
bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
@@ -151,7 +151,7 @@ inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA**
}
// Initalize the algorithm
-inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) {
+inline void EPAAlgorithm::init(PoolAllocator* memoryAllocator) {
mMemoryAllocator = memoryAllocator;
}
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h
index e81ee9b3..b6cce5c2 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.h
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h
@@ -94,7 +94,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
/// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection,
- MemoryAllocator* memoryAllocator) override;
+ PoolAllocator* memoryAllocator) override;
/// Compute a contact info if the two bounding volumes collide.
virtual void testCollision(const CollisionShapeInfo& shape1Info,
@@ -110,7 +110,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
// Initalize the algorithm
inline void GJKAlgorithm::init(CollisionDetection* collisionDetection,
- MemoryAllocator* memoryAllocator) {
+ PoolAllocator* memoryAllocator) {
NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator);
mAlgoEPA.init(memoryAllocator);
}
diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp
index 382ccc9b..590182ac 100644
--- a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp
+++ b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp
@@ -563,7 +563,6 @@ bool VoronoiSimplex::computeClosestPointOnTetrahedron(const Vector3& a, const Ve
if (squareDist < closestSquareDistance) {
// Use it as the current closest point
- closestSquareDistance = squareDist;
baryCoordsAB.setAllValues(0.0, triangleBaryCoords[0]);
baryCoordsCD.setAllValues(triangleBaryCoords[2], triangleBaryCoords[1]);
bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 1, 3, 2);
diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp b/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp
index 0fbbe5fe..782f8d6f 100644
--- a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp
+++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp
@@ -36,7 +36,7 @@ NarrowPhaseAlgorithm::NarrowPhaseAlgorithm()
}
// Initalize the algorithm
-void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) {
+void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator) {
mCollisionDetection = collisionDetection;
mMemoryAllocator = memoryAllocator;
}
diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
index 30b216f0..0759d138 100644
--- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h
+++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
@@ -29,7 +29,7 @@
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
-#include "memory/MemoryAllocator.h"
+#include "memory/PoolAllocator.h"
#include "engine/OverlappingPair.h"
#include "collision/CollisionShapeInfo.h"
@@ -71,7 +71,7 @@ class NarrowPhaseAlgorithm {
CollisionDetection* mCollisionDetection;
/// Pointer to the memory allocator
- MemoryAllocator* mMemoryAllocator;
+ PoolAllocator* mMemoryAllocator;
/// Overlapping pair of the bodies currently tested for collision
OverlappingPair* mCurrentOverlappingPair;
@@ -93,7 +93,7 @@ class NarrowPhaseAlgorithm {
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
/// Initalize the algorithm
- virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator);
+ virtual void init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator);
/// Set the current overlapping pair of bodies
void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index 1e2e3f14..b33ed4f1 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -34,7 +34,7 @@
#include "mathematics/Ray.h"
#include "AABB.h"
#include "collision/RaycastInfo.h"
-#include "memory/MemoryAllocator.h"
+#include "memory/PoolAllocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
diff --git a/src/configuration.h b/src/configuration.h
index 4364c548..87747756 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -144,6 +144,9 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
/// least one concave collision shape.
constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
+/// Size (in bytes) of the single frame allocator
+constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 1048576;
+
}
#endif
diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp
index 68e37625..b1b004ec 100644
--- a/src/engine/CollisionWorld.cpp
+++ b/src/engine/CollisionWorld.cpp
@@ -33,7 +33,7 @@ using namespace std;
// Constructor
CollisionWorld::CollisionWorld()
- : mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0),
+ : mCollisionDetection(this, mPoolAllocator), mCurrentBodyID(0),
mEventListener(nullptr) {
}
@@ -66,7 +66,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
// Create the collision body
- CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody)))
+ CollisionBody* collisionBody = new (mPoolAllocator.allocate(sizeof(CollisionBody)))
CollisionBody(transform, *this, bodyID);
assert(collisionBody != nullptr);
@@ -97,7 +97,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
mBodies.erase(collisionBody);
// Free the object from the memory allocator
- mMemoryAllocator.release(collisionBody, sizeof(CollisionBody));
+ mPoolAllocator.release(collisionBody, sizeof(CollisionBody));
}
// Return the next available body ID
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index 77189e89..d04acb4e 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -39,7 +39,7 @@
#include "collision/CollisionDetection.h"
#include "constraint/Joint.h"
#include "constraint/ContactPoint.h"
-#include "memory/MemoryAllocator.h"
+#include "memory/PoolAllocator.h"
#include "EventListener.h"
/// Namespace reactphysics3d
@@ -72,8 +72,8 @@ class CollisionWorld {
/// List of free ID for rigid bodies
std::vector<luint> mFreeBodiesIDs;
- /// Memory allocator
- MemoryAllocator mMemoryAllocator;
+ /// Pool Memory allocator
+ PoolAllocator mPoolAllocator;
/// Pointer to an event listener object
EventListener* mEventListener;
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index a0db1119..b271aa6e 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -39,55 +39,75 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP= decimal(0.01);
// Constructor
-ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
+ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
+ SingleFrameAllocator& singleFrameAllocator)
:mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr),
- mContactConstraints(nullptr), mPenetrationConstraints(nullptr),
- mFrictionConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
+ mSingleFrameAllocator(singleFrameAllocator),
+ mPenetrationConstraints(nullptr), mFrictionConstraints(nullptr),
+ mLinearVelocities(nullptr), mAngularVelocities(nullptr),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
mIsSolveFrictionAtContactManifoldCenterActive(true) {
}
+// Initialize the contact constraints
+void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
+
+ mTimeStep = timeStep;
+
+ // TODO : Try not to count manifolds here
+ // Count the contact manifolds
+ uint nbContactManifolds = 0;
+ for (uint i = 0; i < nbIslands; i++) {
+ nbContactManifolds += islands[i]->getNbContactManifolds();
+ }
+
+ mNbFrictionConstraints = 0;
+ mNbPenetrationConstraints = 0;
+
+ mPenetrationConstraints = nullptr;
+ mFrictionConstraints = nullptr;
+
+ if (nbContactManifolds == 0) return;
+
+ // TODO : Count exactly the number of constraints to allocate here
+ uint nbPenetrationConstraints = nbContactManifolds * MAX_CONTACT_POINTS_IN_MANIFOLD;
+ mPenetrationConstraints = static_cast<PenetrationConstraint*>(mSingleFrameAllocator.allocate(sizeof(PenetrationConstraint) * nbPenetrationConstraints));
+ //mPenetrationConstraints = new PenetrationConstraint[nbPenetrationConstraints];
+ assert(mPenetrationConstraints != nullptr);
+ //mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4];
+
+ mFrictionConstraints = static_cast<FrictionConstraint*>(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds));
+ //mFrictionConstraints = new FrictionConstraint[nbContactManifolds];
+ assert(mFrictionConstraints != nullptr);
+ //mFrictionConstraints = new FrictionConstraint[mNbContactManifolds];
+
+ // For each island of the world
+ for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) {
+ initializeForIsland(islands[islandIndex]);
+ }
+
+ // Warmstarting
+ warmStart();
+}
+
// Initialize the constraint solver for a given island
-void ContactSolver::initializeForIsland(decimal dt, Island* island) {
+void ContactSolver::initializeForIsland(Island* island) {
PROFILE("ContactSolver::initializeForIsland()");
assert(island != nullptr);
assert(island->getNbBodies() > 0);
- assert(island->getNbContactManifolds() > 0);
assert(mSplitLinearVelocities != nullptr);
assert(mSplitAngularVelocities != nullptr);
- // Set the current time step
- mTimeStep = dt;
-
- mNbContactManifolds = island->getNbContactManifolds();
-
- mNbFrictionConstraints = 0;
- mNbPenetrationConstraints = 0;
-
- // TODO : Try to do faster allocation here
- mContactConstraints = new ContactManifoldSolver[mNbContactManifolds];
- assert(mContactConstraints != nullptr);
-
- // TODO : Count exactly the number of constraints to allocate here (do not reallocate each frame)
- mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4];
- assert(mPenetrationConstraints != nullptr);
-
- // TODO : Do not reallocate each frame)
- mFrictionConstraints = new FrictionConstraint[mNbContactManifolds];
- assert(mFrictionConstraints != nullptr);
-
// For each contact manifold of the island
ContactManifold** contactManifolds = island->getContactManifolds();
- for (uint i=0; i<mNbContactManifolds; i++) {
+ for (uint i=0; i<island->getNbContactManifolds(); i++) {
ContactManifold* externalManifold = contactManifolds[i];
- ContactManifoldSolver& internalManifold = mContactConstraints[i];
-
assert(externalManifold->getNbContactPoints() > 0);
// Get the two bodies of the contact
@@ -100,6 +120,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
uint indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
uint indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
+ new (mFrictionConstraints + mNbFrictionConstraints) FrictionConstraint();
mFrictionConstraints[mNbFrictionConstraints].indexBody1 = indexBody1;
mFrictionConstraints[mNbFrictionConstraints].indexBody2 = indexBody2;
mFrictionConstraints[mNbFrictionConstraints].contactManifold = externalManifold;
@@ -129,7 +150,6 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2);
mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
- internalManifold.externalContactManifold = externalManifold;
mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false;
//internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
//internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
@@ -159,6 +179,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
// Get a contact point
ContactPoint* externalContact = externalManifold->getContactPoint(c);
+ new (mPenetrationConstraints + mNbPenetrationConstraints) PenetrationConstraint();
mPenetrationConstraints[mNbPenetrationConstraints].indexBody1 = indexBody1;
mPenetrationConstraints[mNbPenetrationConstraints].indexBody2 = indexBody2;
mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody1 = I1;
@@ -301,168 +322,19 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) /
frictionTwistMass :
decimal(0.0);
- //}
mNbFrictionConstraints++;
}
-
- // Fill-in all the matrices needed to solve the LCP problem
- //initializeContactConstraints();
}
-// TODO : Delete this method
-// Initialize the contact constraints before solving the system
-void ContactSolver::initializeContactConstraints() {
+// Solve the contact constraints of one iteration of the solve
+void ContactSolver::solve() {
- PROFILE("ContactSolver::initializeContactConstraints()");
-
- // For each contact constraint
- //for (uint c=0; c<mNbContactManifolds; c++) {
+ assert(mTimeStep > decimal(0.0));
- // ContactManifoldSolver& manifold = mContactConstraints[c];
-
-// // Get the inertia tensors of both bodies
-// Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
-// Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
-
- // If we solve the friction constraints at the center of the contact manifold
-// if (mIsSolveFrictionAtContactManifoldCenterActive) {
-// manifold.normal = Vector3(0.0, 0.0, 0.0);
-// }
-
- // Get the velocities of the bodies
-// const Vector3& v1 = mLinearVelocities[manifold.indexBody1];
-// const Vector3& w1 = mAngularVelocities[manifold.indexBody1];
-// const Vector3& v2 = mLinearVelocities[manifold.indexBody2];
-// const Vector3& w2 = mAngularVelocities[manifold.indexBody2];
-
- // For each contact point constraint
- //for (uint i=0; i<manifold.nbContacts; i++) {
-
- //ContactPointSolver& contactPoint = manifold.contacts[i];
- //ContactPoint* externalContact = contactPoint.externalContact;
-
-// // Compute the velocity difference
-// Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
-
-// contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
-// contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
-
-// // Compute the inverse mass matrix K for the penetration constraint
-// decimal massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 +
-// ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
-// ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
-// massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) /
-// massPenetration :
-// decimal(0.0);
-
- // If we do not solve the friction constraints at the center of the contact manifold
-// if (!mIsSolveFrictionAtContactManifoldCenterActive) {
-
-// // Compute the friction vectors
-// computeFrictionVectors(deltaV, contactPoint);
-
-// contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
-// contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2);
-// contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1);
-// contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2);
-
-// // Compute the inverse mass matrix K for the friction
-// // constraints at each contact point
-// decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
-// ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(
-// contactPoint.frictionVector1) +
-// ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(
-// contactPoint.frictionVector1);
-// decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
-// ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(
-// contactPoint.frictionVector2) +
-// ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(
-// contactPoint.frictionVector2);
-// friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) /
-// friction1Mass :
-// decimal(0.0);
-// friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) /
-// friction2Mass :
-// decimal(0.0);
-// }
-
- // Compute the restitution velocity bias "b". We compute this here instead
- // of inside the solve() method because we need to use the velocity difference
- // at the beginning of the contact. Note that if it is a resting contact (normal
- // velocity bellow a given threshold), we do not add a restitution velocity bias
-// contactPoint.restitutionBias = 0.0;
-// decimal deltaVDotN = deltaV.dot(contactPoint.normal);
-// if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
-// contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
-// }
-
-// // If the warm starting of the contact solver is active
-// if (mIsWarmStartingActive) {
-
-// // Get the cached accumulated impulses from the previous step
-// contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
-// contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
-// contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
-// contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
-// }
-
-// // Initialize the split impulses to zero
-// contactPoint.penetrationSplitImpulse = 0.0;
-
-// // If we solve the friction constraints at the center of the contact manifold
-// if (mIsSolveFrictionAtContactManifoldCenterActive) {
-// manifold.normal += contactPoint.normal;
-// }
- //}
-
-// // Compute the inverse K matrix for the rolling resistance constraint
-// manifold.inverseRollingResistance.setToZero();
-// if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) {
-// manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2;
-// manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse();
-// }
-
- // If we solve the friction constraints at the center of the contact manifold
- //if (mIsSolveFrictionAtContactManifoldCenterActive) {
-
-// manifold.normal.normalize();
-
-// Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
-// v1 - w1.cross(manifold.r1Friction);
-
-// // Compute the friction vectors
-// computeFrictionVectors(deltaVFrictionPoint, manifold);
-
-// // Compute the inverse mass matrix K for the friction constraints at the center of
-// // the contact manifold
-// manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
-// manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
-// manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
-// manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2);
-// decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
-// ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
-// manifold.frictionVector1) +
-// ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
-// manifold.frictionVector1);
-// decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
-// ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
-// manifold.frictionVector2) +
-// ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
-// manifold.frictionVector2);
-// decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
-// manifold.normal) +
-// manifold.normal.dot(manifold.inverseInertiaTensorBody2 *
-// manifold.normal);
-// friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass
-// : decimal(0.0);
-// friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass
-// : decimal(0.0);
-// frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) /
-// frictionTwistMass :
-// decimal(0.0);
- //}
- //}
+ resetTotalPenetrationImpulse();
+ solvePenetrationConstraints();
+ solveFrictionConstraints();
}
// Warm start the solver.
@@ -576,177 +448,6 @@ void ContactSolver::warmStart() {
mFrictionConstraints[i].rollingResistanceImpulse.setToZero();
}
}
-
-
- /*
-
-
- // Check that warm starting is active
- if (!mIsWarmStartingActive) return;
-
- // For each constraint
- for (uint c=0; c<mNbContactManifolds; c++) {
-
- ContactManifoldSolver& contactManifold = mContactConstraints[c];
-
- bool atLeastOneRestingContactPoint = false;
-
- for (uint i=0; i<contactManifold.nbContacts; i++) {
-
- ContactPointSolver& contactPoint = contactManifold.contacts[i];
-
- // If it is not a new contact (this contact was already existing at last time step)
- if (contactPoint.isRestingContact) {
-
- atLeastOneRestingContactPoint = true;
-
- // --------- Penetration --------- //
-
- // Compute the impulse P = J^T * lambda
- const Impulse impulsePenetration = computePenetrationImpulse(
- contactPoint.penetrationImpulse, contactPoint);
-
- // Apply the impulse to the bodies of the constraint
- applyImpulse(impulsePenetration, contactManifold);
-
- // If we do not solve the friction constraints at the center of the contact manifold
- if (!mIsSolveFrictionAtContactManifoldCenterActive) {
-
- // Project the old friction impulses (with old friction vectors) into
- // the new friction vectors to get the new friction impulses
- Vector3 oldFrictionImpulse = contactPoint.friction1Impulse *
- contactPoint.oldFrictionVector1 +
- contactPoint.friction2Impulse *
- contactPoint.oldFrictionVector2;
- contactPoint.friction1Impulse = oldFrictionImpulse.dot(
- contactPoint.frictionVector1);
- contactPoint.friction2Impulse = oldFrictionImpulse.dot(
- contactPoint.frictionVector2);
-
- // --------- Friction 1 --------- //
-
- // Compute the impulse P = J^T * lambda
- const Impulse impulseFriction1 = computeFriction1Impulse(
- contactPoint.friction1Impulse, contactPoint);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction1, contactManifold);
-
- // --------- Friction 2 --------- //
-
- // Compute the impulse P=J^T * lambda
- const Impulse impulseFriction2 = computeFriction2Impulse(
- contactPoint.friction2Impulse, contactPoint);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction2, contactManifold);
-
- // ------ Rolling resistance------ //
-
- if (contactManifold.rollingResistanceFactor > 0) {
-
- // Compute the impulse P = J^T * lambda
- const Impulse impulseRollingResistance(Vector3::zero(), -contactPoint.rollingResistanceImpulse,
- Vector3::zero(), contactPoint.rollingResistanceImpulse);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseRollingResistance, contactManifold);
- }
- }
- }
- else { // If it is a new contact point
-
- // Initialize the accumulated impulses to zero
- contactPoint.penetrationImpulse = 0.0;
- contactPoint.friction1Impulse = 0.0;
- contactPoint.friction2Impulse = 0.0;
- contactPoint.rollingResistanceImpulse = Vector3::zero();
- }
- }
-
- // If we solve the friction constraints at the center of the contact manifold and there is
- // at least one resting contact point in the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) {
-
- // Project the old friction impulses (with old friction vectors) into the new friction
- // vectors to get the new friction impulses
- Vector3 oldFrictionImpulse = contactManifold.friction1Impulse *
- contactManifold.oldFrictionVector1 +
- contactManifold.friction2Impulse *
- contactManifold.oldFrictionVector2;
- contactManifold.friction1Impulse = oldFrictionImpulse.dot(
- contactManifold.frictionVector1);
- contactManifold.friction2Impulse = oldFrictionImpulse.dot(
- contactManifold.frictionVector2);
-
- // ------ First friction constraint at the center of the contact manifold ------ //
-
- // Compute the impulse P = J^T * lambda
- Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 *
- contactManifold.friction1Impulse;
- Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 *
- contactManifold.friction1Impulse;
- Vector3 linearImpulseBody2 = contactManifold.frictionVector1 *
- contactManifold.friction1Impulse;
- Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 *
- contactManifold.friction1Impulse;
- const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction1, contactManifold);
-
- // ------ Second friction constraint at the center of the contact manifold ----- //
-
- // Compute the impulse P = J^T * lambda
- linearImpulseBody1 = -contactManifold.frictionVector2 *
- contactManifold.friction2Impulse;
- angularImpulseBody1 = -contactManifold.r1CrossT2 *
- contactManifold.friction2Impulse;
- linearImpulseBody2 = contactManifold.frictionVector2 *
- contactManifold.friction2Impulse;
- angularImpulseBody2 = contactManifold.r2CrossT2 *
- contactManifold.friction2Impulse;
- const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction2, contactManifold);
-
- // ------ Twist friction constraint at the center of the contact manifold ------ //
-
- // Compute the impulse P = J^T * lambda
- linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
- angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse;
- linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);
- angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse;
- const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseTwistFriction, contactManifold);
-
- // ------ Rolling resistance at the center of the contact manifold ------ //
-
- // Compute the impulse P = J^T * lambda
- angularImpulseBody1 = -contactManifold.rollingResistanceImpulse;
- angularImpulseBody2 = contactManifold.rollingResistanceImpulse;
- const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1,
- Vector3::zero(), angularImpulseBody2);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseRollingResistance, contactManifold);
- }
- else { // If it is a new contact manifold
-
- // Initialize the accumulated impulses to zero
- contactManifold.friction1Impulse = 0.0;
- contactManifold.friction2Impulse = 0.0;
- contactManifold.frictionTwistImpulse = 0.0;
- contactManifold.rollingResistanceImpulse = Vector3::zero();
- }
- }
- */
}
// Reset the total penetration impulse of friction constraints
@@ -978,288 +679,13 @@ void ContactSolver::solveFrictionConstraints() {
}
}
-// Solve the contacts
-//void ContactSolver::solve() {
-
-// PROFILE("ContactSolver::solve()");
-
-// decimal deltaLambda;
-// decimal lambdaTemp;
-
-// // For each contact manifold
-// for (uint c=0; c<mNbContactManifolds; c++) {
-
-// ContactManifoldSolver& contactManifold = mContactConstraints[c];
-
-// decimal sumPenetrationImpulse = 0.0;
-
-// // Get the constrained velocities
-// const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
-// const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
-// const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
-// const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
-
-// for (uint i=0; i<contactManifold.nbContacts; i++) {
-
-// ContactPointSolver& contactPoint = contactManifold.contacts[i];
-
-// // --------- Penetration --------- //
-
-// // Compute J*v
-// Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
-// decimal deltaVDotN = deltaV.dot(contactPoint.normal);
-// decimal Jv = deltaVDotN;
-
-// // Compute the bias "b" of the constraint
-// decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
-// decimal biasPenetrationDepth = 0.0;
-// if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
-// max(0.0f, float(contactPoint.penetrationDepth - SLOP));
-// decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
-
-// // Compute the Lagrange multiplier lambda
-// if (mIsSplitImpulseActive) {
-// deltaLambda = - (Jv + contactPoint.restitutionBias) *
-// contactPoint.inversePenetrationMass;
-// }
-// else {
-// deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
-// }
-// lambdaTemp = contactPoint.penetrationImpulse;
-// contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
-// deltaLambda, decimal(0.0));
-// deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
-
-// // Compute the impulse P=J^T * lambda
-// const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
-// contactPoint);
-
-// // Apply the impulse to the bodies of the constraint
-// applyImpulse(impulsePenetration, contactManifold);
-
-// sumPenetrationImpulse += contactPoint.penetrationImpulse;
-
-// // If the split impulse position correction is active
-// if (mIsSplitImpulseActive) {
-
-// // Split impulse (position correction)
-// const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
-// const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
-// const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
-// const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
-// Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
-// v1Split - w1Split.cross(contactPoint.r1);
-// decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
-// decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
-// contactPoint.inversePenetrationMass;
-// decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
-// contactPoint.penetrationSplitImpulse = std::max(
-// contactPoint.penetrationSplitImpulse +
-// deltaLambdaSplit, decimal(0.0));
-// deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
-
-// // Compute the impulse P=J^T * lambda
-// const Impulse splitImpulsePenetration = computePenetrationImpulse(
-// deltaLambdaSplit, contactPoint);
-
-// applySplitImpulse(splitImpulsePenetration, contactManifold);
-// }
-
-// // If we do not solve the friction constraints at the center of the contact manifold
-// if (!mIsSolveFrictionAtContactManifoldCenterActive) {
-
-// // --------- Friction 1 --------- //
-
-// // Compute J*v
-// deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
-// Jv = deltaV.dot(contactPoint.frictionVector1);
-
-// // Compute the Lagrange multiplier lambda
-// deltaLambda = -Jv;
-// deltaLambda *= contactPoint.inverseFriction1Mass;
-// decimal frictionLimit = contactManifold.frictionCoefficient *
-// contactPoint.penetrationImpulse;
-// lambdaTemp = contactPoint.friction1Impulse;
-// contactPoint.friction1Impulse = std::max(-frictionLimit,
-// std::min(contactPoint.friction1Impulse
-// + deltaLambda, frictionLimit));
-// deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
-
-// // Compute the impulse P=J^T * lambda
-// const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
-// contactPoint);
-
-// // Apply the impulses to the bodies of the constraint
-// applyImpulse(impulseFriction1, contactManifold);
-
-// // --------- Friction 2 --------- //
-
-// // Compute J*v
-// deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
-// Jv = deltaV.dot(contactPoint.frictionVector2);
-
-// // Compute the Lagrange multiplier lambda
-// deltaLambda = -Jv;
-// deltaLambda *= contactPoint.inverseFriction2Mass;
-// frictionLimit = contactManifold.frictionCoefficient *
-// contactPoint.penetrationImpulse;
-// lambdaTemp = contactPoint.friction2Impulse;
-// contactPoint.friction2Impulse = std::max(-frictionLimit,
-// std::min(contactPoint.friction2Impulse
-// + deltaLambda, frictionLimit));
-// deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
-
-// // Compute the impulse P=J^T * lambda
-// const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
-// contactPoint);
-
-// // Apply the impulses to the bodies of the constraint
-// applyImpulse(impulseFriction2, contactManifold);
-
-// // --------- Rolling resistance constraint --------- //
-
-// if (contactManifold.rollingResistanceFactor > 0) {
-
-// // Compute J*v
-// const Vector3 JvRolling = w2 - w1;
-
-// // Compute the Lagrange multiplier lambda
-// Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
-// decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
-// Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse;
-// contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse +
-// deltaLambdaRolling, rollingLimit);
-// deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling;
-
-// // Compute the impulse P=J^T * lambda
-// const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling,
-// Vector3::zero(), deltaLambdaRolling);
-
-// // Apply the impulses to the bodies of the constraint
-// applyImpulse(impulseRolling, contactManifold);
-// }
-// }
- //}
-
- // If we solve the friction constraints at the center of the contact manifold
-// if (mIsSolveFrictionAtContactManifoldCenterActive) {
-
-// // ------ First friction constraint at the center of the contact manifol ------ //
-
-// // Compute J*v
-// Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
-// - v1 - w1.cross(contactManifold.r1Friction);
-// decimal Jv = deltaV.dot(contactManifold.frictionVector1);
-
-// // Compute the Lagrange multiplier lambda
-// decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
-// decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
-// lambdaTemp = contactManifold.friction1Impulse;
-// contactManifold.friction1Impulse = std::max(-frictionLimit,
-// std::min(contactManifold.friction1Impulse +
-// deltaLambda, frictionLimit));
-// deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
-
-// // Compute the impulse P=J^T * lambda
-// Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
-// Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
-// Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
-// Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
-// const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
-// linearImpulseBody2, angularImpulseBody2);
-
-// // Apply the impulses to the bodies of the constraint
-// applyImpulse(impulseFriction1, contactManifold);
-
-// // ------ Second friction constraint at the center of the contact manifol ----- //
-
-// // Compute J*v
-// deltaV = v2 + w2.cross(contactManifold.r2Friction)
-// - v1 - w1.cross(contactManifold.r1Friction);
-// Jv = deltaV.dot(contactManifold.frictionVector2);
-
-// // Compute the Lagrange multiplier lambda
-// deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
-// frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
-// lambdaTemp = contactManifold.friction2Impulse;
-// contactManifold.friction2Impulse = std::max(-frictionLimit,
-// std::min(contactManifold.friction2Impulse +
-// deltaLambda, frictionLimit));
-// deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
-
-// // Compute the impulse P=J^T * lambda
-// linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
-// angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
-// linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
-// angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
-// const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
-// linearImpulseBody2, angularImpulseBody2);
-
-// // Apply the impulses to the bodies of the constraint
-// applyImpulse(impulseFriction2, contactManifold);
-
-// // ------ Twist friction constraint at the center of the contact manifol ------ //
-
-// // Compute J*v
-// deltaV = w2 - w1;
-// Jv = deltaV.dot(contactManifold.normal);
-
-// deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
-// frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
-// lambdaTemp = contactManifold.frictionTwistImpulse;
-// contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
-// std::min(contactManifold.frictionTwistImpulse
-// + deltaLambda, frictionLimit));
-// deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
-
-// // Compute the impulse P=J^T * lambda
-// linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
-// angularImpulseBody1 = -contactManifold.normal * deltaLambda;
-// linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
-// angularImpulseBody2 = contactManifold.normal * deltaLambda;
-// const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
-// linearImpulseBody2, angularImpulseBody2);
-
-// // Apply the impulses to the bodies of the constraint
-// applyImpulse(impulseTwistFriction, contactManifold);
-
-// // --------- Rolling resistance constraint at the center of the contact manifold --------- //
-
-// if (contactManifold.rollingResistanceFactor > 0) {
-
-// // Compute J*v
-// const Vector3 JvRolling = w2 - w1;
-
-// // Compute the Lagrange multiplier lambda
-// Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
-// decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
-// Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
-// contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse +
-// deltaLambdaRolling, rollingLimit);
-// deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
-
-// // Compute the impulse P=J^T * lambda
-// angularImpulseBody1 = -deltaLambdaRolling;
-// angularImpulseBody2 = deltaLambdaRolling;
-// const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1,
-// Vector3::zero(), angularImpulseBody2);
-
-// // Apply the impulses to the bodies of the constraint
-// applyImpulse(impulseRolling, contactManifold);
-// }
-// }
-// }
-//}
-
// Store the computed impulses to use them to
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
// Penetration constraints
for (uint i=0; i<mNbPenetrationConstraints; i++) {
-
mPenetrationConstraints[i].contactPoint->setPenetrationImpulse(mPenetrationConstraints[i].penetrationImpulse);
-
}
// Friction constraints
@@ -1274,105 +700,17 @@ void ContactSolver::storeImpulses() {
}
/*
- // For each contact manifold
- for (uint c=0; c<mNbContactManifolds; c++) {
+ if (mPenetrationConstraints != nullptr) {
+ // TODO : Delete this
+ delete[] mPenetrationConstraints;
+ }
- ContactManifoldSolver& manifold = mContactConstraints[c];
-
- for (uint i=0; i<manifold.nbContacts; i++) {
-
- ContactPointSolver& contactPoint = manifold.contacts[i];
-
- contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse);
- contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse);
- contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse);
- contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse);
-
- contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1);
- contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2);
- }
-
- manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse);
- manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse);
- manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse);
- manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse);
- manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1);
- manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2);
+ if (mFrictionConstraints != nullptr) {
+ delete[] mFrictionConstraints;
}
*/
}
-/*
-// Apply an impulse to the two bodies of a constraint
-void ContactSolver::applyImpulse(const Impulse& impulse,
- const ContactManifoldSolver& manifold) {
-
- PROFILE("ContactSolver::applyImpulse()");
-
- // Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
- impulse.linearImpulseBody1;
- mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
- impulse.angularImpulseBody1;
-
- // Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
- impulse.linearImpulseBody2;
- mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
- impulse.angularImpulseBody2;
-}
-*/
-
-/*
-// Apply an impulse to the two bodies of a constraint
-void ContactSolver::applySplitImpulse(const Impulse& impulse,
- const ContactManifoldSolver& manifold) {
-
- // Update the velocities of the body 1 by applying the impulse P
- mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
- impulse.linearImpulseBody1;
- mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
- impulse.angularImpulseBody1;
-
- // Update the velocities of the body 1 by applying the impulse P
- mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
- impulse.linearImpulseBody2;
- mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
- impulse.angularImpulseBody2;
-}
-*/
-
-// TODO : Delete this
-// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
-// for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal.
-//void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
-// ContactPointSolver& contactPoint) const {
-
-// assert(contactPoint.normal.length() > 0.0);
-
-// // Compute the velocity difference vector in the tangential plane
-// Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
-// Vector3 tangentVelocity = deltaVelocity - normalVelocity;
-
-// // If the velocty difference in the tangential plane is not zero
-// decimal lengthTangenVelocity = tangentVelocity.length();
-// if (lengthTangenVelocity > MACHINE_EPSILON) {
-
-// // Compute the first friction vector in the direction of the tangent
-// // velocity difference
-// contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
-// }
-// else {
-
-// // Get any orthogonal vector to the normal as the first friction vector
-// contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector();
-// }
-
-// // The second friction vector is computed by the cross product of the firs
-// // friction vector and the contact normal
-// contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit();
-//}
-
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
// for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
@@ -1402,22 +740,3 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
// friction vector and the contact normal
frictionConstraint.frictionVector2 = frictionConstraint.normal.cross(frictionConstraint.frictionVector1).getUnit();
}
-
-// Clean up the constraint solver
-void ContactSolver::cleanup() {
-
- if (mContactConstraints != nullptr) {
- delete[] mContactConstraints;
- mContactConstraints = nullptr;
- }
-
- if (mPenetrationConstraints != nullptr) {
- delete[] mPenetrationConstraints;
- mPenetrationConstraints = nullptr;
- }
-
- if (mFrictionConstraints != nullptr) {
- delete[] mFrictionConstraints;
- mFrictionConstraints = nullptr;
- }
-}
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index f3f5dfa7..e5d5840d 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -31,6 +31,7 @@
#include "configuration.h"
#include "constraint/Joint.h"
#include "collision/ContactManifold.h"
+#include "memory/SingleFrameAllocator.h"
#include "Island.h"
#include "Impulse.h"
#include <map>
@@ -278,228 +279,6 @@ class ContactSolver {
bool hasAtLeastOneRestingContactPoint;
};
- // Structure ContactPointSolver
- /**
- * Contact solver internal data structure that to store all the
- * information relative to a contact point
- */
- struct ContactPointSolver {
-
- /// Index of body 1 in the constraint solver
- uint indexBody1;
-
- /// Index of body 2 in the constraint solver
- uint indexBody2;
-
- /// Inverse of the mass of body 1
- decimal massInverseBody1;
-
- /// Inverse of the mass of body 2
- decimal massInverseBody2;
-
- /// Inverse inertia tensor of body 1
- Matrix3x3 inverseInertiaTensorBody1;
-
- /// Inverse inertia tensor of body 2
- Matrix3x3 inverseInertiaTensorBody2;
-
- /// Point on body 1 where to apply the friction constraints
- Vector3 frictionPointBody1;
-
- /// Point on body 2 where to apply the friction constraints
- Vector3 frictionPointBody2;
-
- /// Accumulated normal impulse
- decimal penetrationImpulse;
-
- /// Accumulated impulse in the 1st friction direction
- decimal friction1Impulse;
-
- /// Accumulated impulse in the 2nd friction direction
- decimal friction2Impulse;
-
- /// Accumulated split impulse for penetration correction
- decimal penetrationSplitImpulse;
-
- /// Accumulated rolling resistance impulse
- Vector3 rollingResistanceImpulse;
-
- /// Normal vector of the contact
- Vector3 normal;
-
- /// First friction vector in the tangent plane
- //Vector3 frictionVector1;
-
- /// Second friction vector in the tangent plane
- //Vector3 frictionVector2;
-
- /// Old first friction vector in the tangent plane
- Vector3 oldFrictionVector1;
-
- /// Old second friction vector in the tangent plane
- Vector3 oldFrictionVector2;
-
- /// Vector from the body 1 center to the contact point
- Vector3 r1;
-
- /// Vector from the body 2 center to the contact point
- Vector3 r2;
-
- /// Cross product of r1 with 1st friction vector
- //Vector3 r1CrossT1;
-
- /// Cross product of r1 with 2nd friction vector
- //Vector3 r1CrossT2;
-
- /// Cross product of r2 with 1st friction vector
- //Vector3 r2CrossT1;
-
- /// Cross product of r2 with 2nd friction vector
- //Vector3 r2CrossT2;
-
- /// Cross product of r1 with the contact normal
- //Vector3 r1CrossN;
-
- /// Cross product of r2 with the contact normal
- //Vector3 r2CrossN;
-
- /// Penetration depth
- decimal penetrationDepth;
-
- /// Velocity restitution bias
- decimal restitutionBias;
-
- /// Inverse of the matrix K for the penenetration
- //decimal inversePenetrationMass;
-
- /// Inverse of the matrix K for the 1st friction
- decimal inverseFriction1Mass;
-
- /// Inverse of the matrix K for the 2nd friction
- decimal inverseFriction2Mass;
-
- /// True if the contact was existing last time step
- bool isRestingContact;
-
- /// Pointer to the external contact
- ContactPoint* externalContact;
- };
-
- // Structure ContactManifoldSolver
- /**
- * Contact solver internal data structure to store all the
- * information relative to a contact manifold.
- */
- struct ContactManifoldSolver {
-
- /// Index of body 1 in the constraint solver
- //uint indexBody1;
-
- /// Index of body 2 in the constraint solver
- //uint indexBody2;
-
- /// Inverse of the mass of body 1
- //decimal massInverseBody1;
-
- // Inverse of the mass of body 2
- //decimal massInverseBody2;
-
- /// Inverse inertia tensor of body 1
- //Matrix3x3 inverseInertiaTensorBody1;
-
- /// Inverse inertia tensor of body 2
- //Matrix3x3 inverseInertiaTensorBody2;
-
- /// Contact point constraints
- //ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
-
- /// Number of contact points
- //uint nbContacts;
-
- /// True if the body 1 is of type dynamic
- //bool isBody1DynamicType;
-
- /// True if the body 2 is of type dynamic
- //bool isBody2DynamicType;
-
- /// Mix of the restitution factor for two bodies
- //decimal restitutionFactor;
-
- /// Mix friction coefficient for the two bodies
- //decimal frictionCoefficient;
-
- /// Rolling resistance factor between the two bodies
- decimal rollingResistanceFactor;
-
- /// Pointer to the external contact manifold
- ContactManifold* externalContactManifold;
-
- // - Variables used when friction constraints are apply at the center of the manifold-//
-
- /// Average normal vector of the contact manifold
- //Vector3 normal;
-
- /// Point on body 1 where to apply the friction constraints
- //Vector3 frictionPointBody1;
-
- /// Point on body 2 where to apply the friction constraints
- //Vector3 frictionPointBody2;
-
- /// R1 vector for the friction constraints
- //Vector3 r1Friction;
-
- /// R2 vector for the friction constraints
- //Vector3 r2Friction;
-
- /// Cross product of r1 with 1st friction vector
- //Vector3 r1CrossT1;
-
- /// Cross product of r1 with 2nd friction vector
- //Vector3 r1CrossT2;
-
- /// Cross product of r2 with 1st friction vector
- //Vector3 r2CrossT1;
-
- /// Cross product of r2 with 2nd friction vector
- //Vector3 r2CrossT2;
-
- /// Matrix K for the first friction constraint
- //decimal inverseFriction1Mass;
-
- /// Matrix K for the second friction constraint
- //decimal inverseFriction2Mass;
-
- /// Matrix K for the twist friction constraint
- //decimal inverseTwistFrictionMass;
-
- /// Matrix K for the rolling resistance constraint
- //Matrix3x3 inverseRollingResistance;
-
- /// First friction direction at contact manifold center
- //Vector3 frictionVector1;
-
- /// Second friction direction at contact manifold center
- //Vector3 frictionVector2;
-
- /// Old 1st friction direction at contact manifold center
- //Vector3 oldFrictionVector1;
-
- /// Old 2nd friction direction at contact manifold center
- //Vector3 oldFrictionVector2;
-
- /// First friction direction impulse at manifold center
- //decimal friction1Impulse;
-
- /// Second friction direction impulse at manifold center
- //decimal friction2Impulse;
-
- /// Twist friction impulse at contact manifold center
- //decimal frictionTwistImpulse;
-
- /// Rolling resistance impulse
- //Vector3 rollingResistanceImpulse;
- };
-
// -------------------- Constants --------------------- //
/// Beta value for the penetration depth position correction without split impulses
@@ -519,12 +298,12 @@ class ContactSolver {
/// Split angular velocities for the position contact solver (split impulse)
Vector3* mSplitAngularVelocities;
+ /// Reference to the single frame memory allocator
+ SingleFrameAllocator& mSingleFrameAllocator;
+
/// Current time step
decimal mTimeStep;
- /// Contact constraints
- ContactManifoldSolver* mContactConstraints;
-
PenetrationConstraint* mPenetrationConstraints;
FrictionConstraint* mFrictionConstraints;
@@ -533,9 +312,6 @@ class ContactSolver {
uint mNbFrictionConstraints;
- /// Number of contact constraints
- uint mNbContactManifolds;
-
/// Array of linear velocities
Vector3* mLinearVelocities;
@@ -557,8 +333,8 @@ class ContactSolver {
// -------------------- Methods -------------------- //
- /// Initialize the contact constraints before solving the system
- void initializeContactConstraints();
+ /// Initialize the constraint solver for a given island
+ void initializeForIsland(Island* island);
/// Apply an impulse to the two bodies of a constraint
//void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
@@ -608,13 +384,17 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Constructor
- ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
+ ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
+ SingleFrameAllocator& singleFrameAllocator);
/// Destructor
~ContactSolver() = default;
- /// Initialize the constraint solver for a given island
- void initializeForIsland(decimal dt, Island* island);
+ /// Initialize the contact constraints
+ void init(Island** islands, uint nbIslands, decimal timeStep);
+
+ /// Solve the contact constraints of one iteration of the solve
+ void solve();
/// Set the split velocities arrays
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
@@ -653,9 +433,6 @@ class ContactSolver {
/// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
- /// Clean up the constraint solver
- void cleanup();
-
/// Return true if warmstarting is active
bool IsWarmStartingActive() const;
};
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 189ef0e0..0b1110af 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -40,7 +40,8 @@ using namespace std;
*/
DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
: CollisionWorld(),
- mContactSolver(mMapBodyToConstrainedVelocityIndex),
+ mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES),
+ mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator),
mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
@@ -82,10 +83,10 @@ DynamicsWorld::~DynamicsWorld() {
mIslands[i]->~Island();
// Release the allocated memory for the island
- mMemoryAllocator.release(mIslands[i], sizeof(Island));
+ mPoolAllocator.release(mIslands[i], sizeof(Island));
}
if (mNbIslandsCapacity > 0) {
- mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
+ mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
}
// Release the memory allocated for the bodies velocity arrays
@@ -161,6 +162,9 @@ void DynamicsWorld::update(decimal timeStep) {
// Reset the external force and torque applied to the bodies
resetBodiesForceAndTorque();
+
+ // Reset the single frame memory allocator
+ mSingleFrameAllocator.reset();
}
// Integrate position and orientation of the rigid bodies.
@@ -364,27 +368,28 @@ void DynamicsWorld::solveContactsAndConstraints() {
mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
mConstrainedOrientations);
- // ---------- Solve velocity constraints for joints and contacts ---------- //
+ // Initialize the contact solver
+ mContactSolver.init(mIslands, mNbIslands, mTimeStep);
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Check if there are contacts and constraints to solve
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
- bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
- if (!isConstraintsToSolve && !isContactsToSolve) continue;
+ //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
+ //if (!isConstraintsToSolve && !isContactsToSolve) continue;
// If there are contacts in the current island
- if (isContactsToSolve) {
+// if (isContactsToSolve) {
- // Initialize the solver
- mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
+// // Initialize the solver
+// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
- // Warm start the contact solver
- if (mContactSolver.IsWarmStartingActive()) {
- mContactSolver.warmStart();
- }
- }
+// // Warm start the contact solver
+// if (mContactSolver.IsWarmStartingActive()) {
+// mContactSolver.warmStart();
+// }
+// }
// If there are constraints
if (isConstraintsToSolve) {
@@ -392,32 +397,40 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Initialize the constraint solver
mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
}
+ }
// For each iteration of the velocity solver
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
- // Solve the constraints
- if (isConstraintsToSolve) {
- mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
+ for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+ // Solve the constraints
+ bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
+ if (isConstraintsToSolve) {
+ mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
+ }
}
+ mContactSolver.solve();
+
// Solve the contacts
- if (isContactsToSolve) {
+// if (isContactsToSolve) {
- mContactSolver.resetTotalPenetrationImpulse();
+// mContactSolver.resetTotalPenetrationImpulse();
- mContactSolver.solvePenetrationConstraints();
- mContactSolver.solveFrictionConstraints();
- }
+// mContactSolver.solvePenetrationConstraints();
+// mContactSolver.solveFrictionConstraints();
+// }
}
// Cache the lambda values in order to use them in the next
// step and cleanup the contact solver
- if (isContactsToSolve) {
- mContactSolver.storeImpulses();
- mContactSolver.cleanup();
- }
- }
+// if (isContactsToSolve) {
+// mContactSolver.storeImpulses();
+// mContactSolver.cleanup();
+// }
+ //}
+
+ mContactSolver.storeImpulses();
}
// Solve the position error correction of the constraints
@@ -456,7 +469,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
// Create the rigid body
- RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
+ RigidBody* rigidBody = new (mPoolAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
*this, bodyID);
assert(rigidBody != nullptr);
@@ -497,7 +510,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
mRigidBodies.erase(rigidBody);
// Free the object from the memory allocator
- mMemoryAllocator.release(rigidBody, sizeof(RigidBody));
+ mPoolAllocator.release(rigidBody, sizeof(RigidBody));
}
// Create a joint between two bodies in the world and return a pointer to the new joint
@@ -515,7 +528,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
// Ball-and-Socket joint
case JointType::BALLSOCKETJOINT:
{
- void* allocatedMemory = mMemoryAllocator.allocate(sizeof(BallAndSocketJoint));
+ void* allocatedMemory = mPoolAllocator.allocate(sizeof(BallAndSocketJoint));
const BallAndSocketJointInfo& info = static_cast<const BallAndSocketJointInfo&>(
jointInfo);
newJoint = new (allocatedMemory) BallAndSocketJoint(info);
@@ -525,7 +538,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
// Slider joint
case JointType::SLIDERJOINT:
{
- void* allocatedMemory = mMemoryAllocator.allocate(sizeof(SliderJoint));
+ void* allocatedMemory = mPoolAllocator.allocate(sizeof(SliderJoint));
const SliderJointInfo& info = static_cast<const SliderJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) SliderJoint(info);
break;
@@ -534,7 +547,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
// Hinge joint
case JointType::HINGEJOINT:
{
- void* allocatedMemory = mMemoryAllocator.allocate(sizeof(HingeJoint));
+ void* allocatedMemory = mPoolAllocator.allocate(sizeof(HingeJoint));
const HingeJointInfo& info = static_cast<const HingeJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) HingeJoint(info);
break;
@@ -543,7 +556,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
// Fixed joint
case JointType::FIXEDJOINT:
{
- void* allocatedMemory = mMemoryAllocator.allocate(sizeof(FixedJoint));
+ void* allocatedMemory = mPoolAllocator.allocate(sizeof(FixedJoint));
const FixedJointInfo& info = static_cast<const FixedJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) FixedJoint(info);
break;
@@ -596,8 +609,8 @@ void DynamicsWorld::destroyJoint(Joint* joint) {
mJoints.erase(joint);
// Remove the joint from the joint list of the bodies involved in the joint
- joint->mBody1->removeJointFromJointsList(mMemoryAllocator, joint);
- joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint);
+ joint->mBody1->removeJointFromJointsList(mPoolAllocator, joint);
+ joint->mBody2->removeJointFromJointsList(mPoolAllocator, joint);
size_t nbBytes = joint->getSizeInBytes();
@@ -605,7 +618,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) {
joint->~Joint();
// Release the allocated memory
- mMemoryAllocator.release(joint, nbBytes);
+ mPoolAllocator.release(joint, nbBytes);
}
// Add the joint to the list of joints of the two bodies involved in the joint
@@ -614,13 +627,13 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
assert(joint != nullptr);
// Add the joint at the beginning of the linked list of joints of the first body
- void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(JointListElement));
+ void* allocatedMemory1 = mPoolAllocator.allocate(sizeof(JointListElement));
JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint,
joint->mBody1->mJointsList);
joint->mBody1->mJointsList = jointListElement1;
// Add the joint at the beginning of the linked list of joints of the second body
- void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(JointListElement));
+ void* allocatedMemory2 = mPoolAllocator.allocate(sizeof(JointListElement));
JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint,
joint->mBody2->mJointsList);
joint->mBody2->mJointsList = jointListElement2;
@@ -646,16 +659,16 @@ void DynamicsWorld::computeIslands() {
mIslands[i]->~Island();
// Release the allocated memory for the island
- mMemoryAllocator.release(mIslands[i], sizeof(Island));
+ mPoolAllocator.release(mIslands[i], sizeof(Island));
}
// Allocate and create the array of islands
if (mNbIslandsCapacity != nbBodies && nbBodies > 0) {
if (mNbIslandsCapacity > 0) {
- mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
+ mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
}
mNbIslandsCapacity = nbBodies;
- mIslands = (Island**)mMemoryAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity);
+ mIslands = (Island**)mPoolAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity);
}
mNbIslands = 0;
@@ -672,7 +685,7 @@ void DynamicsWorld::computeIslands() {
// Create a stack (using an array) for the rigid bodies to visit during the Depth First Search
size_t nbBytesStack = sizeof(RigidBody*) * nbBodies;
- RigidBody** stackBodiesToVisit = (RigidBody**)mMemoryAllocator.allocate(nbBytesStack);
+ RigidBody** stackBodiesToVisit = (RigidBody**)mPoolAllocator.allocate(nbBytesStack);
// For each rigid body of the world
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
@@ -695,10 +708,10 @@ void DynamicsWorld::computeIslands() {
body->mIsAlreadyInIsland = true;
// Create the new island
- void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island));
+ void* allocatedMemoryIsland = mPoolAllocator.allocate(sizeof(Island));
mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies,
nbContactManifolds,
- mJoints.size(), mMemoryAllocator);
+ mJoints.size(), mPoolAllocator);
// While there are still some bodies to visit in the stack
while (stackIndex > 0) {
@@ -790,7 +803,7 @@ void DynamicsWorld::computeIslands() {
}
// Release the allocated memory for the stack of bodies to visit
- mMemoryAllocator.release(stackBodiesToVisit, nbBytesStack);
+ mPoolAllocator.release(stackBodiesToVisit, nbBytesStack);
}
// Put bodies to sleep if needed.
diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h
index 7dc715c9..542e0f4b 100644
--- a/src/engine/DynamicsWorld.h
+++ b/src/engine/DynamicsWorld.h
@@ -50,6 +50,9 @@ class DynamicsWorld : public CollisionWorld {
// -------------------- Attributes -------------------- //
+ /// Single frame Memory allocator
+ SingleFrameAllocator mSingleFrameAllocator;
+
/// Contact solver
ContactSolver mContactSolver;
diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp
index 41560bde..8cf743a3 100644
--- a/src/engine/Island.cpp
+++ b/src/engine/Island.cpp
@@ -30,7 +30,7 @@ using namespace reactphysics3d;
// Constructor
Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
- MemoryAllocator& memoryAllocator)
+ PoolAllocator& memoryAllocator)
: mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0),
mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) {
diff --git a/src/engine/Island.h b/src/engine/Island.h
index 512a0d1b..227a78e5 100644
--- a/src/engine/Island.h
+++ b/src/engine/Island.h
@@ -27,7 +27,7 @@
#define REACTPHYSICS3D_ISLAND_H
// Libraries
-#include "memory/MemoryAllocator.h"
+#include "memory/PoolAllocator.h"
#include "body/RigidBody.h"
#include "constraint/Joint.h"
#include "collision/ContactManifold.h"
@@ -64,7 +64,7 @@ class Island {
uint mNbJoints;
/// Reference to the memory allocator
- MemoryAllocator& mMemoryAllocator;
+ PoolAllocator& mMemoryAllocator;
/// Number of bytes allocated for the bodies array
size_t mNbAllocatedBytesBodies;
@@ -81,7 +81,7 @@ class Island {
/// Constructor
Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
- MemoryAllocator& memoryAllocator);
+ PoolAllocator& memoryAllocator);
/// Destructor
~Island();
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index 7efca599..48658f5c 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -31,7 +31,7 @@ using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
- int nbMaxContactManifolds, MemoryAllocator& memoryAllocator)
+ int nbMaxContactManifolds, PoolAllocator& memoryAllocator)
: mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds),
mCachedSeparatingAxis(0.0, 1.0, 0.0) {
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index 7995b0d2..70ee1c3f 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -63,7 +63,7 @@ class OverlappingPair {
/// Constructor
OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
- int nbMaxContactManifolds, MemoryAllocator& memoryAllocator);
+ int nbMaxContactManifolds, PoolAllocator& memoryAllocator);
/// Destructor
~OverlappingPair() = default;
From 8f4979f4a2e50419dc25a5696c83dca45932e047 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 22 Sep 2016 23:24:03 +0200
Subject: [PATCH 007/133] Allocate memory in the SingleFrameAllocator in the
update() method
---
src/engine/DynamicsWorld.cpp | 96 +++++++++---------------------------
src/engine/DynamicsWorld.h | 6 ---
src/engine/Island.cpp | 24 +++------
src/engine/Island.h | 16 +-----
4 files changed, 34 insertions(+), 108 deletions(-)
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 0b1110af..91312546 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -49,8 +49,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr),
mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr),
mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr),
- mConstrainedOrientations(nullptr), mNbIslands(0),
- mNbIslandsCapacity(0), mIslands(nullptr), mNbBodiesCapacity(0),
+ mConstrainedOrientations(nullptr), mNbIslands(0), mIslands(nullptr),
mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY),
mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY),
mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) {
@@ -76,29 +75,6 @@ DynamicsWorld::~DynamicsWorld() {
destroyRigidBody(*itToRemove);
}
- // Release the memory allocated for the islands
- for (uint i=0; i<mNbIslands; i++) {
-
- // Call the island destructor
- mIslands[i]->~Island();
-
- // Release the allocated memory for the island
- mPoolAllocator.release(mIslands[i], sizeof(Island));
- }
- if (mNbIslandsCapacity > 0) {
- mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
- }
-
- // Release the memory allocated for the bodies velocity arrays
- if (mNbBodiesCapacity > 0) {
- delete[] mSplitLinearVelocities;
- delete[] mSplitAngularVelocities;
- delete[] mConstrainedLinearVelocities;
- delete[] mConstrainedAngularVelocities;
- delete[] mConstrainedPositions;
- delete[] mConstrainedOrientations;
- }
-
assert(mJoints.size() == 0);
assert(mRigidBodies.size() == 0);
@@ -245,31 +221,26 @@ void DynamicsWorld::updateBodiesState() {
// Initialize the bodies velocities arrays for the next simulation step.
void DynamicsWorld::initVelocityArrays() {
+ PROFILE("DynamicsWorld::initVelocityArrays()");
+
// Allocate memory for the bodies velocity arrays
uint nbBodies = mRigidBodies.size();
- if (mNbBodiesCapacity != nbBodies && nbBodies > 0) {
- if (mNbBodiesCapacity > 0) {
- delete[] mSplitLinearVelocities;
- delete[] mSplitAngularVelocities;
- }
- mNbBodiesCapacity = nbBodies;
- // TODO : Use better memory allocation here
- mSplitLinearVelocities = new Vector3[mNbBodiesCapacity];
- mSplitAngularVelocities = new Vector3[mNbBodiesCapacity];
- mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity];
- mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity];
- mConstrainedPositions = new Vector3[mNbBodiesCapacity];
- mConstrainedOrientations = new Quaternion[mNbBodiesCapacity];
- assert(mSplitLinearVelocities != nullptr);
- assert(mSplitAngularVelocities != nullptr);
- assert(mConstrainedLinearVelocities != nullptr);
- assert(mConstrainedAngularVelocities != nullptr);
- assert(mConstrainedPositions != nullptr);
- assert(mConstrainedOrientations != nullptr);
- }
+
+ mSplitLinearVelocities = static_cast<Vector3*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3)));
+ mSplitAngularVelocities = static_cast<Vector3*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3)));
+ mConstrainedLinearVelocities = static_cast<Vector3*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3)));
+ mConstrainedAngularVelocities = static_cast<Vector3*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3)));
+ mConstrainedPositions = static_cast<Vector3*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3)));
+ mConstrainedOrientations = static_cast<Quaternion*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Quaternion)));
+ assert(mSplitLinearVelocities != nullptr);
+ assert(mSplitAngularVelocities != nullptr);
+ assert(mConstrainedLinearVelocities != nullptr);
+ assert(mConstrainedAngularVelocities != nullptr);
+ assert(mConstrainedPositions != nullptr);
+ assert(mConstrainedOrientations != nullptr);
// Reset the velocities arrays
- for (uint i=0; i<mNbBodiesCapacity; i++) {
+ for (uint i=0; i<nbBodies; i++) {
mSplitLinearVelocities[i].setToZero();
mSplitAngularVelocities[i].setToZero();
}
@@ -652,24 +623,9 @@ void DynamicsWorld::computeIslands() {
uint nbBodies = mRigidBodies.size();
- // Clear all the islands
- for (uint i=0; i<mNbIslands; i++) {
-
- // Call the island destructor
- mIslands[i]->~Island();
-
- // Release the allocated memory for the island
- mPoolAllocator.release(mIslands[i], sizeof(Island));
- }
-
- // Allocate and create the array of islands
- if (mNbIslandsCapacity != nbBodies && nbBodies > 0) {
- if (mNbIslandsCapacity > 0) {
- mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
- }
- mNbIslandsCapacity = nbBodies;
- mIslands = (Island**)mPoolAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity);
- }
+ // Allocate and create the array of islands pointer. This memory is allocated
+ // in the single frame allocator
+ mIslands = static_cast<Island**>(mSingleFrameAllocator.allocate(sizeof(Island*) * nbBodies));
mNbIslands = 0;
int nbContactManifolds = 0;
@@ -685,7 +641,7 @@ void DynamicsWorld::computeIslands() {
// Create a stack (using an array) for the rigid bodies to visit during the Depth First Search
size_t nbBytesStack = sizeof(RigidBody*) * nbBodies;
- RigidBody** stackBodiesToVisit = (RigidBody**)mPoolAllocator.allocate(nbBytesStack);
+ RigidBody** stackBodiesToVisit = static_cast<RigidBody**>(mSingleFrameAllocator.allocate(nbBytesStack));
// For each rigid body of the world
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
@@ -708,10 +664,9 @@ void DynamicsWorld::computeIslands() {
body->mIsAlreadyInIsland = true;
// Create the new island
- void* allocatedMemoryIsland = mPoolAllocator.allocate(sizeof(Island));
- mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies,
- nbContactManifolds,
- mJoints.size(), mPoolAllocator);
+ void* allocatedMemoryIsland = mSingleFrameAllocator.allocate(sizeof(Island));
+ mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, mJoints.size(),
+ mSingleFrameAllocator);
// While there are still some bodies to visit in the stack
while (stackIndex > 0) {
@@ -801,9 +756,6 @@ void DynamicsWorld::computeIslands() {
mNbIslands++;
}
-
- // Release the allocated memory for the stack of bodies to visit
- mPoolAllocator.release(stackBodiesToVisit, nbBytesStack);
}
// Put bodies to sleep if needed.
diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h
index 542e0f4b..dec383a1 100644
--- a/src/engine/DynamicsWorld.h
+++ b/src/engine/DynamicsWorld.h
@@ -109,15 +109,9 @@ class DynamicsWorld : public CollisionWorld {
/// Number of islands in the world
uint mNbIslands;
- /// Current allocated capacity for the islands
- uint mNbIslandsCapacity;
-
/// Array with all the islands of awaken bodies
Island** mIslands;
- /// Current allocated capacity for the bodies
- uint mNbBodiesCapacity;
-
/// Sleep linear velocity threshold
decimal mSleepLinearVelocity;
diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp
index 8cf743a3..19eef304 100644
--- a/src/engine/Island.cpp
+++ b/src/engine/Island.cpp
@@ -29,26 +29,18 @@
using namespace reactphysics3d;
// Constructor
-Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
- PoolAllocator& memoryAllocator)
+Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, SingleFrameAllocator& allocator)
: mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0),
- mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) {
+ mNbContactManifolds(0), mNbJoints(0) {
- // Allocate memory for the arrays
- mNbAllocatedBytesBodies = sizeof(RigidBody*) * nbMaxBodies;
- mBodies = (RigidBody**) mMemoryAllocator.allocate(mNbAllocatedBytesBodies);
- mNbAllocatedBytesContactManifolds = sizeof(ContactManifold*) * nbMaxContactManifolds;
- mContactManifolds = (ContactManifold**) mMemoryAllocator.allocate(
- mNbAllocatedBytesContactManifolds);
- mNbAllocatedBytesJoints = sizeof(Joint*) * nbMaxJoints;
- mJoints = (Joint**) mMemoryAllocator.allocate(mNbAllocatedBytesJoints);
+ // Allocate memory for the arrays on the single frame allocator
+ mBodies = static_cast<RigidBody**>(allocator.allocate(sizeof(RigidBody*) * nbMaxBodies));
+ mContactManifolds = static_cast<ContactManifold**>(allocator.allocate(sizeof(ContactManifold*) * nbMaxContactManifolds));
+ mJoints = static_cast<Joint**>(allocator.allocate(sizeof(Joint*) * nbMaxJoints));
}
// Destructor
Island::~Island() {
-
- // Release the memory of the arrays
- mMemoryAllocator.release(mBodies, mNbAllocatedBytesBodies);
- mMemoryAllocator.release(mContactManifolds, mNbAllocatedBytesContactManifolds);
- mMemoryAllocator.release(mJoints, mNbAllocatedBytesJoints);
+ // This destructor is never called because memory is allocated on the
+ // single frame allocator
}
diff --git a/src/engine/Island.h b/src/engine/Island.h
index 227a78e5..5f370004 100644
--- a/src/engine/Island.h
+++ b/src/engine/Island.h
@@ -27,7 +27,7 @@
#define REACTPHYSICS3D_ISLAND_H
// Libraries
-#include "memory/PoolAllocator.h"
+#include "memory/SingleFrameAllocator.h"
#include "body/RigidBody.h"
#include "constraint/Joint.h"
#include "collision/ContactManifold.h"
@@ -63,25 +63,13 @@ class Island {
/// Current number of joints in the island
uint mNbJoints;
- /// Reference to the memory allocator
- PoolAllocator& mMemoryAllocator;
-
- /// Number of bytes allocated for the bodies array
- size_t mNbAllocatedBytesBodies;
-
- /// Number of bytes allocated for the contact manifolds array
- size_t mNbAllocatedBytesContactManifolds;
-
- /// Number of bytes allocated for the joints array
- size_t mNbAllocatedBytesJoints;
-
public:
// -------------------- Methods -------------------- //
/// Constructor
Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
- PoolAllocator& memoryAllocator);
+ SingleFrameAllocator& allocator);
/// Destructor
~Island();
From c59781519167421c05525e0b48586b16df64f5e8 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 26 Sep 2016 22:51:30 +0200
Subject: [PATCH 008/133] Remove unecessary variables in constraints and cache
inverse inertia world tensor of bodies
---
src/body/RigidBody.cpp | 17 ++++
src/body/RigidBody.h | 23 +++--
src/engine/ContactSolver.cpp | 163 +++++++++++++----------------------
src/engine/ContactSolver.h | 13 ---
src/engine/DynamicsWorld.cpp | 3 +
5 files changed, 97 insertions(+), 122 deletions(-)
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index 74ee8a05..6e6a55e6 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -46,6 +46,9 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde
// Compute the inverse mass
mMassInverse = decimal(1.0) / mInitMass;
+
+ // Update the world inverse inertia tensor
+ updateInertiaTensorInverseWorld();
}
// Destructor
@@ -90,11 +93,15 @@ void RigidBody::setType(BodyType type) {
mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
+ mInertiaTensorInverseWorld.setToZero();
}
else { // If it is a dynamic body
mMassInverse = decimal(1.0) / mInitMass;
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+
+ // Update the world inverse inertia tensor
+ updateInertiaTensorInverseWorld();
}
// Awake the body
@@ -125,6 +132,9 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+
+ // Update the world inverse inertia tensor
+ updateInertiaTensorInverseWorld();
}
// Set the local center of mass of the body (in local-space coordinates)
@@ -314,6 +324,9 @@ void RigidBody::setTransform(const Transform& transform) {
// Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+ // Update the world inverse inertia tensor
+ updateInertiaTensorInverseWorld();
+
// Update the broad-phase state of the body
updateBroadPhaseState();
}
@@ -326,6 +339,7 @@ void RigidBody::recomputeMassInformation() {
mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
+ mInertiaTensorInverseWorld.setToZero();
mCenterOfMassLocal.setToZero();
// If it is STATIC or KINEMATIC body
@@ -386,6 +400,9 @@ void RigidBody::recomputeMassInformation() {
// Compute the local inverse inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+ // Update the world inverse inertia tensor
+ updateInertiaTensorInverseWorld();
+
// Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
}
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index 0ffc6257..a38b53d5 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -83,6 +83,9 @@ class RigidBody : public CollisionBody {
/// Inverse of the inertia tensor of the body
Matrix3x3 mInertiaTensorLocalInverse;
+ /// Inverse of the world inertia tensor of the body
+ Matrix3x3 mInertiaTensorInverseWorld;
+
/// Inverse of the mass of the body
decimal mMassInverse;
@@ -112,6 +115,9 @@ class RigidBody : public CollisionBody {
/// Update the broad-phase state for this body (because it has moved for instance)
virtual void updateBroadPhaseState() const override;
+ /// Update the world inverse inertia tensor of the body
+ void updateInertiaTensorInverseWorld();
+
public :
// -------------------- Methods -------------------- //
@@ -291,12 +297,19 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
*/
inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
- // TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
- // INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
-
// Compute and return the inertia tensor in world coordinates
- return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse *
- mTransform.getOrientation().getMatrix().getTranspose();
+ return mInertiaTensorInverseWorld;
+}
+
+// Update the world inverse inertia tensor of the body
+/// The inertia tensor I_w in world coordinates is computed with the
+/// local inverse inertia tensor I_b^-1 in body coordinates
+/// by I_w = R * I_b^-1 * R^T
+/// where R is the rotation matrix (and R^T its transpose) of the
+/// current orientation quaternion of the body
+inline void RigidBody::updateInertiaTensorInverseWorld() {
+ Matrix3x3 orientation = mTransform.getOrientation().getMatrix();
+ mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose();
}
// Return true if the gravity needs to be applied to this rigid body
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index b271aa6e..d2218136 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -36,7 +36,7 @@ using namespace std;
// Constants initialization
const decimal ContactSolver::BETA = decimal(0.2);
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
-const decimal ContactSolver::SLOP= decimal(0.01);
+const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
@@ -74,14 +74,10 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
// TODO : Count exactly the number of constraints to allocate here
uint nbPenetrationConstraints = nbContactManifolds * MAX_CONTACT_POINTS_IN_MANIFOLD;
mPenetrationConstraints = static_cast<PenetrationConstraint*>(mSingleFrameAllocator.allocate(sizeof(PenetrationConstraint) * nbPenetrationConstraints));
- //mPenetrationConstraints = new PenetrationConstraint[nbPenetrationConstraints];
assert(mPenetrationConstraints != nullptr);
- //mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4];
mFrictionConstraints = static_cast<FrictionConstraint*>(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds));
- //mFrictionConstraints = new FrictionConstraint[nbContactManifolds];
assert(mFrictionConstraints != nullptr);
- //mFrictionConstraints = new FrictionConstraint[mNbContactManifolds];
// For each island of the world
for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) {
@@ -146,23 +142,17 @@ void ContactSolver::initializeForIsland(Island* island) {
// contact manifold
mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 = body1->mMassInverse;
mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 = body2->mMassInverse;
- //internalManifold.nbContacts = externalManifold->getNbContactPoints();
decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2);
mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false;
- //internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
- //internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
- // If we solve the friction constraints at the center of the contact manifold
- //if (mIsSolveFrictionAtContactManifoldCenterActive) {
- mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 = Vector3::zero();
- mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 = Vector3::zero();
- mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero();
- //}
+ Vector3 frictionPointBody1;
+ Vector3 frictionPointBody2;
+ mFrictionConstraints[mNbFrictionConstraints].normal.setToZero();
// Compute the inverse K matrix for the rolling resistance constraint
mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.setToZero();
@@ -186,7 +176,6 @@ void ContactSolver::initializeForIsland(Island* island) {
mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 = I2;
mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 = body1->mMassInverse;
mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse;
- mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor = restitutionFactor;
mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints;
mPenetrationConstraints[mNbPenetrationConstraints].contactPoint = externalContact;
@@ -196,8 +185,6 @@ void ContactSolver::initializeForIsland(Island* island) {
mPenetrationConstraints[mNbPenetrationConstraints].r1 = p1 - x1;
mPenetrationConstraints[mNbPenetrationConstraints].r2 = p2 - x2;
-
- //mPenetrationConstraints[penConstIndex].externalContact = externalContact;
mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal();
mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth();
mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact = externalContact->getIsRestingContact();
@@ -205,18 +192,10 @@ void ContactSolver::initializeForIsland(Island* island) {
mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint |= mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact;
externalContact->setIsRestingContact(true);
- //mPenetrationConstraints[penConstIndex].oldFrictionVector1 = externalContact->getFrictionVector1();
- //mPenetrationConstraints[penConstIndex].oldFrictionVector2 = externalContact->getFrictionVector2();
mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = 0.0;
- //mPenetrationConstraints[penConstIndex].friction1Impulse = 0.0;
- //mPenetrationConstraints[penConstIndex].friction2Impulse = 0.0;
- //mPenetrationConstraints[penConstIndex].rollingResistanceImpulse = Vector3::zero();
- // If we solve the friction constraints at the center of the contact manifold
- //if (mIsSolveFrictionAtContactManifoldCenterActive) {
- mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 += p1;
- mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 += p2;
- //}
+ frictionPointBody1 += p1;
+ frictionPointBody2 += p2;
// Compute the velocity difference
Vector3 deltaV = v2 + w2.cross(mPenetrationConstraints[mNbPenetrationConstraints].r2) - v1 - w1.cross(mPenetrationConstraints[mNbPenetrationConstraints].r1);
@@ -238,7 +217,7 @@ void ContactSolver::initializeForIsland(Island* island) {
mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = 0.0;
decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[mNbPenetrationConstraints].normal);
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
- mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = mPenetrationConstraints[mNbPenetrationConstraints].restitutionFactor * deltaVDotN;
+ mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = restitutionFactor * deltaVDotN;
}
// If the warm starting of the contact solver is active
@@ -251,77 +230,70 @@ void ContactSolver::initializeForIsland(Island* island) {
// Initialize the split impulses to zero
mPenetrationConstraints[mNbPenetrationConstraints].penetrationSplitImpulse = 0.0;
- // If we solve the friction constraints at the center of the contact manifold
- //if (mIsSolveFrictionAtContactManifoldCenterActive) {
- mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal;
- //}
+ mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal;
mNbPenetrationConstraints++;
nbContacts++;
}
- // If we solve the friction constraints at the center of the contact manifold
- //if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ frictionPointBody1 /= nbContacts;
+ frictionPointBody2 /= nbContacts;
+ mFrictionConstraints[mNbFrictionConstraints].r1Friction = frictionPointBody1 - x1;
+ mFrictionConstraints[mNbFrictionConstraints].r2Friction = frictionPointBody2 - x2;
+ mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector1 = externalManifold->getFrictionVector1();
+ mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector2 = externalManifold->getFrictionVector2();
- //mFrictionConstraints[mNbFrictionConstraints].normal = Vector3::zero();
- mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 /= nbContacts;
- mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 /= nbContacts;
- mFrictionConstraints[mNbFrictionConstraints].r1Friction = mFrictionConstraints[mNbFrictionConstraints].frictionPointBody1 - x1;
- mFrictionConstraints[mNbFrictionConstraints].r2Friction = mFrictionConstraints[mNbFrictionConstraints].frictionPointBody2 - x2;
- mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector1 = externalManifold->getFrictionVector1();
- mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector2 = externalManifold->getFrictionVector2();
+ // If warm starting is active
+ if (mIsWarmStartingActive) {
- // If warm starting is active
- if (mIsWarmStartingActive) {
+ // Initialize the accumulated impulses with the previous step accumulated impulses
+ mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = externalManifold->getFrictionImpulse1();
+ mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = externalManifold->getFrictionImpulse2();
+ mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
+ }
+ else {
- // Initialize the accumulated impulses with the previous step accumulated impulses
- mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = externalManifold->getFrictionImpulse1();
- mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = externalManifold->getFrictionImpulse2();
- mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
- }
- else {
+ // Initialize the accumulated impulses to zero
+ mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = 0.0;
+ mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = 0.0;
+ mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = 0.0;
+ mFrictionConstraints[mNbFrictionConstraints].rollingResistanceImpulse = Vector3(0, 0, 0);
+ }
- // Initialize the accumulated impulses to zero
- mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = 0.0;
- mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = 0.0;
- mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = 0.0;
- mFrictionConstraints[mNbFrictionConstraints].rollingResistanceImpulse = Vector3(0, 0, 0);
- }
+ mFrictionConstraints[mNbFrictionConstraints].normal.normalize();
- mFrictionConstraints[mNbFrictionConstraints].normal.normalize();
+ Vector3 deltaVFrictionPoint = v2 + w2.cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction) -
+ v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction);
- Vector3 deltaVFrictionPoint = v2 + w2.cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction) -
- v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction);
+ // Compute the friction vectors
+ computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]);
- // Compute the friction vectors
- computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]);
-
- // Compute the inverse mass matrix K for the friction constraints at the center of the contact manifold
- mFrictionConstraints[mNbFrictionConstraints].r1CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
- mFrictionConstraints[mNbFrictionConstraints].r1CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
- mFrictionConstraints[mNbFrictionConstraints].r2CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
- mFrictionConstraints[mNbFrictionConstraints].r2CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
- decimal friction1Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
- ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
- mFrictionConstraints[mNbFrictionConstraints].frictionVector1) +
- ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
- mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
- decimal friction2Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
- ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
- mFrictionConstraints[mNbFrictionConstraints].frictionVector2) +
- ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
- mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
- decimal frictionTwistMass = mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 *
- mFrictionConstraints[mNbFrictionConstraints].normal) +
- mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 *
- mFrictionConstraints[mNbFrictionConstraints].normal);
- friction1Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction1Mass = decimal(1.0)/friction1Mass
- : decimal(0.0);
- friction2Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction2Mass = decimal(1.0)/friction2Mass
- : decimal(0.0);
- frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) /
- frictionTwistMass :
- decimal(0.0);
+ // Compute the inverse mass matrix K for the friction constraints at the center of the contact manifold
+ mFrictionConstraints[mNbFrictionConstraints].r1CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
+ mFrictionConstraints[mNbFrictionConstraints].r1CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
+ mFrictionConstraints[mNbFrictionConstraints].r2CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
+ mFrictionConstraints[mNbFrictionConstraints].r2CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
+ decimal friction1Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
+ ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
+ mFrictionConstraints[mNbFrictionConstraints].frictionVector1) +
+ ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
+ mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
+ decimal friction2Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
+ ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
+ mFrictionConstraints[mNbFrictionConstraints].frictionVector2) +
+ ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
+ mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
+ decimal frictionTwistMass = mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 *
+ mFrictionConstraints[mNbFrictionConstraints].normal) +
+ mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 *
+ mFrictionConstraints[mNbFrictionConstraints].normal);
+ friction1Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction1Mass = decimal(1.0)/friction1Mass
+ : decimal(0.0);
+ friction2Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction2Mass = decimal(1.0)/friction2Mass
+ : decimal(0.0);
+ frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) /
+ frictionTwistMass :
+ decimal(0.0);
mNbFrictionConstraints++;
}
@@ -463,9 +435,6 @@ void ContactSolver::solvePenetrationConstraints() {
PROFILE("ContactSolver::solvePenetrationConstraints()");
- // TODO : Check that the PenetrationConstraint struct only contains variables that are
- // used in this method, nothing more
-
// TODO : Maybe solve split impulses and normal impulses separately
decimal deltaLambda;
@@ -550,9 +519,6 @@ void ContactSolver::solvePenetrationConstraints() {
// Solve the friction constraints
void ContactSolver::solveFrictionConstraints() {
- // TODO : Check that the FrictionConstraint struct only contains variables that are
- // used in this method, nothing more
-
PROFILE("ContactSolver::solveFrictionConstraints()");
for (uint i=0; i<mNbFrictionConstraints; i++) {
@@ -698,17 +664,6 @@ void ContactSolver::storeImpulses() {
mFrictionConstraints[i].contactManifold->setFrictionVector1(mFrictionConstraints[i].frictionVector1);
mFrictionConstraints[i].contactManifold->setFrictionVector2(mFrictionConstraints[i].frictionVector2);
}
-
- /*
- if (mPenetrationConstraints != nullptr) {
- // TODO : Delete this
- delete[] mPenetrationConstraints;
- }
-
- if (mFrictionConstraints != nullptr) {
- delete[] mFrictionConstraints;
- }
- */
}
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index e5d5840d..0e710c6f 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -115,8 +115,6 @@ class ContactSolver {
struct PenetrationConstraint {
- // TODO : Pack bools into a single value
-
/// Index of body 1 in the constraint solver
uint indexBody1;
@@ -144,9 +142,6 @@ class ContactSolver {
/// Velocity restitution bias
decimal restitutionBias;
- /// Mix of the restitution factor for two bodies
- decimal restitutionFactor;
-
/// Accumulated normal impulse
decimal penetrationImpulse;
@@ -180,8 +175,6 @@ class ContactSolver {
struct FrictionConstraint {
- // TODO : Pack bools into a single value
-
/// Index of body 1 in the constraint solver
uint indexBody1;
@@ -194,12 +187,6 @@ class ContactSolver {
/// R2 vector for the friction constraints
Vector3 r2Friction;
- /// Point on body 1 where to apply the friction constraints
- Vector3 frictionPointBody1;
-
- /// Point on body 2 where to apply the friction constraints
- Vector3 frictionPointBody2;
-
/// Average normal vector of the contact manifold
Vector3 normal;
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 91312546..b8a48ddc 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -212,6 +212,9 @@ void DynamicsWorld::updateBodiesState() {
// Update the transform of the body (using the new center of mass and new orientation)
bodies[b]->updateTransformWithCenterOfMass();
+ // Update the world inverse inertia tensor of the body
+ bodies[b]->updateInertiaTensorInverseWorld();
+
// Update the broad-phase state of the body
bodies[b]->updateBroadPhaseState();
}
From 54be20c5d3bd9400aa71c1767faff8167fa9a808 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 2 Oct 2016 15:10:19 +0200
Subject: [PATCH 009/133] Increase the default size of the single frame memory
allocator
---
src/configuration.h | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/configuration.h b/src/configuration.h
index 87747756..c14783d8 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -145,7 +145,7 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
/// Size (in bytes) of the single frame allocator
-constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 1048576;
+constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 15728640; // 15 Mb
}
From 25fddd6fb2481f727ce1c1f1d688d8abaa635d16 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 8 Oct 2016 01:18:56 +0200
Subject: [PATCH 010/133] Back to previous contact solver
---
src/engine/ContactSolver.cpp | 1161 ++++++++++++++++++++--------------
src/engine/ContactSolver.h | 319 +++++-----
src/engine/DynamicsWorld.cpp | 57 +-
3 files changed, 878 insertions(+), 659 deletions(-)
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index d2218136..d8c3a644 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -36,14 +36,13 @@ using namespace std;
// Constants initialization
const decimal ContactSolver::BETA = decimal(0.2);
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
-const decimal ContactSolver::SLOP = decimal(0.01);
+const decimal ContactSolver::SLOP= decimal(0.01);
// Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
- SingleFrameAllocator& singleFrameAllocator)
+ SingleFrameAllocator& allocator)
:mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr),
- mSingleFrameAllocator(singleFrameAllocator),
- mPenetrationConstraints(nullptr), mFrictionConstraints(nullptr),
+ mContactConstraints(nullptr), mSingleFrameAllocator(allocator),
mLinearVelocities(nullptr), mAngularVelocities(nullptr),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
@@ -51,59 +50,33 @@ ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocity
}
-// Initialize the contact constraints
-void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
-
- mTimeStep = timeStep;
-
- // TODO : Try not to count manifolds here
- // Count the contact manifolds
- uint nbContactManifolds = 0;
- for (uint i = 0; i < nbIslands; i++) {
- nbContactManifolds += islands[i]->getNbContactManifolds();
- }
-
- mNbFrictionConstraints = 0;
- mNbPenetrationConstraints = 0;
-
- mPenetrationConstraints = nullptr;
- mFrictionConstraints = nullptr;
-
- if (nbContactManifolds == 0) return;
-
- // TODO : Count exactly the number of constraints to allocate here
- uint nbPenetrationConstraints = nbContactManifolds * MAX_CONTACT_POINTS_IN_MANIFOLD;
- mPenetrationConstraints = static_cast<PenetrationConstraint*>(mSingleFrameAllocator.allocate(sizeof(PenetrationConstraint) * nbPenetrationConstraints));
- assert(mPenetrationConstraints != nullptr);
-
- mFrictionConstraints = static_cast<FrictionConstraint*>(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds));
- assert(mFrictionConstraints != nullptr);
-
- // For each island of the world
- for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) {
- initializeForIsland(islands[islandIndex]);
- }
-
- // Warmstarting
- warmStart();
-}
-
// Initialize the constraint solver for a given island
-void ContactSolver::initializeForIsland(Island* island) {
+void ContactSolver::initializeForIsland(decimal dt, Island* island) {
PROFILE("ContactSolver::initializeForIsland()");
assert(island != nullptr);
assert(island->getNbBodies() > 0);
+ assert(island->getNbContactManifolds() > 0);
assert(mSplitLinearVelocities != nullptr);
assert(mSplitAngularVelocities != nullptr);
+ // Set the current time step
+ mTimeStep = dt;
+
+ mNbContactManifolds = island->getNbContactManifolds();
+
+ mContactConstraints = new ContactManifoldSolver[mNbContactManifolds];
+ assert(mContactConstraints != nullptr);
+
// For each contact manifold of the island
ContactManifold** contactManifolds = island->getContactManifolds();
- for (uint i=0; i<island->getNbContactManifolds(); i++) {
+ for (uint i=0; i<mNbContactManifolds; i++) {
ContactManifold* externalManifold = contactManifolds[i];
+ ContactManifoldSolver& internalManifold = mContactConstraints[i];
+
assert(externalManifold->getNbContactPoints() > 0);
// Get the two bodies of the contact
@@ -112,535 +85,692 @@ void ContactSolver::initializeForIsland(Island* island) {
assert(body1 != nullptr);
assert(body2 != nullptr);
- // TODO : Check if we have a better way to find the body index
- uint indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
- uint indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
-
- new (mFrictionConstraints + mNbFrictionConstraints) FrictionConstraint();
- mFrictionConstraints[mNbFrictionConstraints].indexBody1 = indexBody1;
- mFrictionConstraints[mNbFrictionConstraints].indexBody2 = indexBody2;
- mFrictionConstraints[mNbFrictionConstraints].contactManifold = externalManifold;
-
// Get the position of the two bodies
const Vector3& x1 = body1->mCenterOfMassWorld;
const Vector3& x2 = body2->mCenterOfMassWorld;
- // Get the velocities of the bodies
- const Vector3& v1 = mLinearVelocities[indexBody1];
- const Vector3& w1 = mAngularVelocities[indexBody1];
- const Vector3& v2 = mLinearVelocities[indexBody2];
- const Vector3& w2 = mAngularVelocities[indexBody2];
-
- // Get the inertia tensors of both bodies
- Matrix3x3 I1 = body1->getInertiaTensorInverseWorld();
- Matrix3x3 I2 = body2->getInertiaTensorInverseWorld();
-
- mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 = I1;
- mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 = I2;
-
// Initialize the internal contact manifold structure using the external
// contact manifold
- mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 = body1->mMassInverse;
- mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 = body2->mMassInverse;
- decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2);
- mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
- mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
- mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false;
+ internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
+ internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
+ internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
+ internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
+ internalManifold.massInverseBody1 = body1->mMassInverse;
+ internalManifold.massInverseBody2 = body2->mMassInverse;
+ internalManifold.nbContacts = externalManifold->getNbContactPoints();
+ internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
+ internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
+ internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
+ internalManifold.externalContactManifold = externalManifold;
+ internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
+ internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
- bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
- bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
-
- Vector3 frictionPointBody1;
- Vector3 frictionPointBody2;
- mFrictionConstraints[mNbFrictionConstraints].normal.setToZero();
-
- // Compute the inverse K matrix for the rolling resistance constraint
- mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.setToZero();
- if (mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) {
- mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance = I1 + I2;
- mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance = mFrictionConstraints[mNbFrictionConstraints].inverseRollingResistance.getInverse();
+ // If we solve the friction constraints at the center of the contact manifold
+ if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ internalManifold.frictionPointBody1 = Vector3::zero();
+ internalManifold.frictionPointBody2 = Vector3::zero();
}
- int nbContacts = 0;
-
// For each contact point of the contact manifold
for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
+ ContactPointSolver& contactPoint = internalManifold.contacts[c];
+
// Get a contact point
ContactPoint* externalContact = externalManifold->getContactPoint(c);
- new (mPenetrationConstraints + mNbPenetrationConstraints) PenetrationConstraint();
- mPenetrationConstraints[mNbPenetrationConstraints].indexBody1 = indexBody1;
- mPenetrationConstraints[mNbPenetrationConstraints].indexBody2 = indexBody2;
- mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody1 = I1;
- mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 = I2;
- mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 = body1->mMassInverse;
- mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 = body2->mMassInverse;
- mPenetrationConstraints[mNbPenetrationConstraints].indexFrictionConstraint = mNbFrictionConstraints;
- mPenetrationConstraints[mNbPenetrationConstraints].contactPoint = externalContact;
-
// Get the contact point on the two bodies
Vector3 p1 = externalContact->getWorldPointOnBody1();
Vector3 p2 = externalContact->getWorldPointOnBody2();
- mPenetrationConstraints[mNbPenetrationConstraints].r1 = p1 - x1;
- mPenetrationConstraints[mNbPenetrationConstraints].r2 = p2 - x2;
- mPenetrationConstraints[mNbPenetrationConstraints].normal = externalContact->getNormal();
- mPenetrationConstraints[mNbPenetrationConstraints].penetrationDepth = externalContact->getPenetrationDepth();
- mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact = externalContact->getIsRestingContact();
-
- mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint |= mPenetrationConstraints[mNbPenetrationConstraints].isRestingContact;
-
+ contactPoint.externalContact = externalContact;
+ contactPoint.normal = externalContact->getNormal();
+ contactPoint.r1 = p1 - x1;
+ contactPoint.r2 = p2 - x2;
+ contactPoint.penetrationDepth = externalContact->getPenetrationDepth();
+ contactPoint.isRestingContact = externalContact->getIsRestingContact();
externalContact->setIsRestingContact(true);
- mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = 0.0;
+ contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1();
+ contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2();
+ contactPoint.penetrationImpulse = 0.0;
+ contactPoint.friction1Impulse = 0.0;
+ contactPoint.friction2Impulse = 0.0;
+ contactPoint.rollingResistanceImpulse = Vector3::zero();
- frictionPointBody1 += p1;
- frictionPointBody2 += p2;
+ // If we solve the friction constraints at the center of the contact manifold
+ if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ internalManifold.frictionPointBody1 += p1;
+ internalManifold.frictionPointBody2 += p2;
+ }
+ }
+
+ // If we solve the friction constraints at the center of the contact manifold
+ if (mIsSolveFrictionAtContactManifoldCenterActive) {
+
+ internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts);
+ internalManifold.frictionPointBody2 /=static_cast<decimal>(internalManifold.nbContacts);
+ internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1;
+ internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2;
+ internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
+ internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
+
+ // If warm starting is active
+ if (mIsWarmStartingActive) {
+
+ // Initialize the accumulated impulses with the previous step accumulated impulses
+ internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
+ internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
+ internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
+ }
+ else {
+
+ // Initialize the accumulated impulses to zero
+ internalManifold.friction1Impulse = 0.0;
+ internalManifold.friction2Impulse = 0.0;
+ internalManifold.frictionTwistImpulse = 0.0;
+ internalManifold.rollingResistanceImpulse = Vector3(0, 0, 0);
+ }
+ }
+ }
+
+ // Fill-in all the matrices needed to solve the LCP problem
+ initializeContactConstraints();
+}
+
+// Initialize the contact constraints before solving the system
+void ContactSolver::initializeContactConstraints() {
+
+ // For each contact constraint
+ for (uint c=0; c<mNbContactManifolds; c++) {
+
+ ContactManifoldSolver& manifold = mContactConstraints[c];
+
+ // Get the inertia tensors of both bodies
+ Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
+ Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
+
+ // If we solve the friction constraints at the center of the contact manifold
+ if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ manifold.normal = Vector3(0.0, 0.0, 0.0);
+ }
+
+ // Get the velocities of the bodies
+ const Vector3& v1 = mLinearVelocities[manifold.indexBody1];
+ const Vector3& w1 = mAngularVelocities[manifold.indexBody1];
+ const Vector3& v2 = mLinearVelocities[manifold.indexBody2];
+ const Vector3& w2 = mAngularVelocities[manifold.indexBody2];
+
+ // For each contact point constraint
+ for (uint i=0; i<manifold.nbContacts; i++) {
+
+ ContactPointSolver& contactPoint = manifold.contacts[i];
+ ContactPoint* externalContact = contactPoint.externalContact;
// Compute the velocity difference
- Vector3 deltaV = v2 + w2.cross(mPenetrationConstraints[mNbPenetrationConstraints].r2) - v1 - w1.cross(mPenetrationConstraints[mNbPenetrationConstraints].r1);
- mPenetrationConstraints[mNbPenetrationConstraints].r1CrossN = mPenetrationConstraints[mNbPenetrationConstraints].r1.cross(mPenetrationConstraints[mNbPenetrationConstraints].normal);
- mPenetrationConstraints[mNbPenetrationConstraints].r2CrossN = mPenetrationConstraints[mNbPenetrationConstraints].r2.cross(mPenetrationConstraints[mNbPenetrationConstraints].normal);
+ Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+
+ contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
+ contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
// Compute the inverse mass matrix K for the penetration constraint
- decimal massPenetration = mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody1 + mPenetrationConstraints[mNbPenetrationConstraints].massInverseBody2 +
- ((mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody1 * mPenetrationConstraints[mNbPenetrationConstraints].r1CrossN ).cross(mPenetrationConstraints[mNbPenetrationConstraints].r1)).dot(mPenetrationConstraints[mNbPenetrationConstraints].normal) +
- ((mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody2 * mPenetrationConstraints[mNbPenetrationConstraints].r2CrossN ).cross(mPenetrationConstraints[mNbPenetrationConstraints].r2)).dot(mPenetrationConstraints[mNbPenetrationConstraints].normal);
- massPenetration > decimal(0.0) ? mPenetrationConstraints[mNbPenetrationConstraints].inversePenetrationMass = decimal(1.0) /
+ decimal massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 +
+ ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
+ ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
+ massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) /
massPenetration :
decimal(0.0);
+ // If we do not solve the friction constraints at the center of the contact manifold
+ if (!mIsSolveFrictionAtContactManifoldCenterActive) {
+
+ // Compute the friction vectors
+ computeFrictionVectors(deltaV, contactPoint);
+
+ contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
+ contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2);
+ contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1);
+ contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2);
+
+ // Compute the inverse mass matrix K for the friction
+ // constraints at each contact point
+ decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
+ ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(
+ contactPoint.frictionVector1) +
+ ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(
+ contactPoint.frictionVector1);
+ decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
+ ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(
+ contactPoint.frictionVector2) +
+ ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(
+ contactPoint.frictionVector2);
+ friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) /
+ friction1Mass :
+ decimal(0.0);
+ friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) /
+ friction2Mass :
+ decimal(0.0);
+ }
+
// Compute the restitution velocity bias "b". We compute this here instead
// of inside the solve() method because we need to use the velocity difference
// at the beginning of the contact. Note that if it is a resting contact (normal
// velocity bellow a given threshold), we do not add a restitution velocity bias
- mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = 0.0;
- decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[mNbPenetrationConstraints].normal);
+ contactPoint.restitutionBias = 0.0;
+ decimal deltaVDotN = deltaV.dot(contactPoint.normal);
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
- mPenetrationConstraints[mNbPenetrationConstraints].restitutionBias = restitutionFactor * deltaVDotN;
+ contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
}
// If the warm starting of the contact solver is active
if (mIsWarmStartingActive) {
// Get the cached accumulated impulses from the previous step
- mPenetrationConstraints[mNbPenetrationConstraints].penetrationImpulse = externalContact->getPenetrationImpulse();
+ contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
+ contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
+ contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
+ contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
}
// Initialize the split impulses to zero
- mPenetrationConstraints[mNbPenetrationConstraints].penetrationSplitImpulse = 0.0;
+ contactPoint.penetrationSplitImpulse = 0.0;
- mFrictionConstraints[mNbFrictionConstraints].normal += mPenetrationConstraints[mNbPenetrationConstraints].normal;
-
- mNbPenetrationConstraints++;
- nbContacts++;
+ // If we solve the friction constraints at the center of the contact manifold
+ if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ manifold.normal += contactPoint.normal;
+ }
}
- frictionPointBody1 /= nbContacts;
- frictionPointBody2 /= nbContacts;
- mFrictionConstraints[mNbFrictionConstraints].r1Friction = frictionPointBody1 - x1;
- mFrictionConstraints[mNbFrictionConstraints].r2Friction = frictionPointBody2 - x2;
- mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector1 = externalManifold->getFrictionVector1();
- mFrictionConstraints[mNbFrictionConstraints].oldFrictionVector2 = externalManifold->getFrictionVector2();
-
- // If warm starting is active
- if (mIsWarmStartingActive) {
-
- // Initialize the accumulated impulses with the previous step accumulated impulses
- mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = externalManifold->getFrictionImpulse1();
- mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = externalManifold->getFrictionImpulse2();
- mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
- }
- else {
-
- // Initialize the accumulated impulses to zero
- mFrictionConstraints[mNbFrictionConstraints].friction1Impulse = 0.0;
- mFrictionConstraints[mNbFrictionConstraints].friction2Impulse = 0.0;
- mFrictionConstraints[mNbFrictionConstraints].frictionTwistImpulse = 0.0;
- mFrictionConstraints[mNbFrictionConstraints].rollingResistanceImpulse = Vector3(0, 0, 0);
+ // Compute the inverse K matrix for the rolling resistance constraint
+ manifold.inverseRollingResistance.setToZero();
+ if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) {
+ manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2;
+ manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse();
}
- mFrictionConstraints[mNbFrictionConstraints].normal.normalize();
+ // If we solve the friction constraints at the center of the contact manifold
+ if (mIsSolveFrictionAtContactManifoldCenterActive) {
- Vector3 deltaVFrictionPoint = v2 + w2.cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction) -
- v1 - w1.cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction);
+ manifold.normal.normalize();
- // Compute the friction vectors
- computeFrictionVectors(deltaVFrictionPoint, mFrictionConstraints[mNbFrictionConstraints]);
+ Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
+ v1 - w1.cross(manifold.r1Friction);
- // Compute the inverse mass matrix K for the friction constraints at the center of the contact manifold
- mFrictionConstraints[mNbFrictionConstraints].r1CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
- mFrictionConstraints[mNbFrictionConstraints].r1CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r1Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
- mFrictionConstraints[mNbFrictionConstraints].r2CrossT1 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
- mFrictionConstraints[mNbFrictionConstraints].r2CrossT2 = mFrictionConstraints[mNbFrictionConstraints].r2Friction.cross(mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
- decimal friction1Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
- ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
- mFrictionConstraints[mNbFrictionConstraints].frictionVector1) +
- ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT1).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
- mFrictionConstraints[mNbFrictionConstraints].frictionVector1);
- decimal friction2Mass = mFrictionConstraints[mNbFrictionConstraints].massInverseBody1 + mFrictionConstraints[mNbFrictionConstraints].massInverseBody2 +
- ((I1 * mFrictionConstraints[mNbFrictionConstraints].r1CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r1Friction)).dot(
- mFrictionConstraints[mNbFrictionConstraints].frictionVector2) +
- ((I2 * mFrictionConstraints[mNbFrictionConstraints].r2CrossT2).cross(mFrictionConstraints[mNbFrictionConstraints].r2Friction)).dot(
- mFrictionConstraints[mNbFrictionConstraints].frictionVector2);
- decimal frictionTwistMass = mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody1 *
- mFrictionConstraints[mNbFrictionConstraints].normal) +
- mFrictionConstraints[mNbFrictionConstraints].normal.dot(mFrictionConstraints[mNbFrictionConstraints].inverseInertiaTensorBody2 *
- mFrictionConstraints[mNbFrictionConstraints].normal);
- friction1Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction1Mass = decimal(1.0)/friction1Mass
- : decimal(0.0);
- friction2Mass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseFriction2Mass = decimal(1.0)/friction2Mass
- : decimal(0.0);
- frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) /
- frictionTwistMass :
- decimal(0.0);
+ // Compute the friction vectors
+ computeFrictionVectors(deltaVFrictionPoint, manifold);
- mNbFrictionConstraints++;
+ // Compute the inverse mass matrix K for the friction constraints at the center of
+ // the contact manifold
+ manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
+ manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
+ manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
+ manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2);
+ decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
+ ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
+ manifold.frictionVector1) +
+ ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
+ manifold.frictionVector1);
+ decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
+ ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
+ manifold.frictionVector2) +
+ ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
+ manifold.frictionVector2);
+ decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
+ manifold.normal) +
+ manifold.normal.dot(manifold.inverseInertiaTensorBody2 *
+ manifold.normal);
+ friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass
+ : decimal(0.0);
+ friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass
+ : decimal(0.0);
+ frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) /
+ frictionTwistMass :
+ decimal(0.0);
+ }
}
}
-// Solve the contact constraints of one iteration of the solve
-void ContactSolver::solve() {
-
- assert(mTimeStep > decimal(0.0));
-
- resetTotalPenetrationImpulse();
- solvePenetrationConstraints();
- solveFrictionConstraints();
-}
-
// Warm start the solver.
/// For each constraint, we apply the previous impulse (from the previous step)
/// at the beginning. With this technique, we will converge faster towards
/// the solution of the linear system
void ContactSolver::warmStart() {
- PROFILE("ContactSolver::warmStart()");
+ // Check that warm starting is active
+ if (!mIsWarmStartingActive) return;
- // Penetration constraints
- for (uint i=0; i<mNbPenetrationConstraints; i++) {
+ // For each constraint
+ for (uint c=0; c<mNbContactManifolds; c++) {
- // If it is not a new contact (this contact was already existing at last time step)
- if (mPenetrationConstraints[i].isRestingContact) {
+ ContactManifoldSolver& contactManifold = mContactConstraints[c];
- Vector3 linearImpulse = mPenetrationConstraints[i].normal * mPenetrationConstraints[i].penetrationImpulse;
+ bool atLeastOneRestingContactPoint = false;
- // Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].massInverseBody1 *
- (-linearImpulse);
- mAngularVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].inverseInertiaTensorBody1 *
- (-mPenetrationConstraints[i].r1CrossN * mPenetrationConstraints[i].penetrationImpulse);
+ for (uint i=0; i<contactManifold.nbContacts; i++) {
- // Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].massInverseBody2 *
- linearImpulse;
- mAngularVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].inverseInertiaTensorBody2 *
- (-mPenetrationConstraints[i].r1CrossN * mPenetrationConstraints[i].penetrationImpulse);
+ ContactPointSolver& contactPoint = contactManifold.contacts[i];
+ // If it is not a new contact (this contact was already existing at last time step)
+ if (contactPoint.isRestingContact) {
+
+ atLeastOneRestingContactPoint = true;
+
+ // --------- Penetration --------- //
+
+ // Compute the impulse P = J^T * lambda
+ const Impulse impulsePenetration = computePenetrationImpulse(
+ contactPoint.penetrationImpulse, contactPoint);
+
+ // Apply the impulse to the bodies of the constraint
+ applyImpulse(impulsePenetration, contactManifold);
+
+ // If we do not solve the friction constraints at the center of the contact manifold
+ if (!mIsSolveFrictionAtContactManifoldCenterActive) {
+
+ // Project the old friction impulses (with old friction vectors) into
+ // the new friction vectors to get the new friction impulses
+ Vector3 oldFrictionImpulse = contactPoint.friction1Impulse *
+ contactPoint.oldFrictionVector1 +
+ contactPoint.friction2Impulse *
+ contactPoint.oldFrictionVector2;
+ contactPoint.friction1Impulse = oldFrictionImpulse.dot(
+ contactPoint.frictionVector1);
+ contactPoint.friction2Impulse = oldFrictionImpulse.dot(
+ contactPoint.frictionVector2);
+
+ // --------- Friction 1 --------- //
+
+ // Compute the impulse P = J^T * lambda
+ const Impulse impulseFriction1 = computeFriction1Impulse(
+ contactPoint.friction1Impulse, contactPoint);
+
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseFriction1, contactManifold);
+
+ // --------- Friction 2 --------- //
+
+ // Compute the impulse P=J^T * lambda
+ const Impulse impulseFriction2 = computeFriction2Impulse(
+ contactPoint.friction2Impulse, contactPoint);
+
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseFriction2, contactManifold);
+
+ // ------ Rolling resistance------ //
+
+ if (contactManifold.rollingResistanceFactor > 0) {
+
+ // Compute the impulse P = J^T * lambda
+ const Impulse impulseRollingResistance(Vector3::zero(), -contactPoint.rollingResistanceImpulse,
+ Vector3::zero(), contactPoint.rollingResistanceImpulse);
+
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseRollingResistance, contactManifold);
+ }
+ }
+ }
+ else { // If it is a new contact point
+
+ // Initialize the accumulated impulses to zero
+ contactPoint.penetrationImpulse = 0.0;
+ contactPoint.friction1Impulse = 0.0;
+ contactPoint.friction2Impulse = 0.0;
+ contactPoint.rollingResistanceImpulse = Vector3::zero();
+ }
}
- else { // If it is a new contact point
-
- // Initialize the accumulated impulses to zero
- mPenetrationConstraints[i].penetrationImpulse = 0.0;
- }
- }
-
- // Friction constraints
- for (uint i=0; i<mNbFrictionConstraints; i++) {
// If we solve the friction constraints at the center of the contact manifold and there is
// at least one resting contact point in the contact manifold
- if (mFrictionConstraints[i].hasAtLeastOneRestingContactPoint) {
+ if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) {
// Project the old friction impulses (with old friction vectors) into the new friction
// vectors to get the new friction impulses
- Vector3 oldFrictionImpulse = mFrictionConstraints[i].friction1Impulse * mFrictionConstraints[i].oldFrictionVector1 +
- mFrictionConstraints[i].friction2Impulse * mFrictionConstraints[i].oldFrictionVector2;
- mFrictionConstraints[i].friction1Impulse = oldFrictionImpulse.dot(mFrictionConstraints[i].frictionVector1);
- mFrictionConstraints[i].friction2Impulse = oldFrictionImpulse.dot(mFrictionConstraints[i].frictionVector2);
+ Vector3 oldFrictionImpulse = contactManifold.friction1Impulse *
+ contactManifold.oldFrictionVector1 +
+ contactManifold.friction2Impulse *
+ contactManifold.oldFrictionVector2;
+ contactManifold.friction1Impulse = oldFrictionImpulse.dot(
+ contactManifold.frictionVector1);
+ contactManifold.friction2Impulse = oldFrictionImpulse.dot(
+ contactManifold.frictionVector2);
// ------ First friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
- Vector3 linearImpulseBody2 = mFrictionConstraints[i].frictionVector1 * mFrictionConstraints[i].friction1Impulse;
- Vector3 angularImpulseBody1 = -mFrictionConstraints[i].r1CrossT1 * mFrictionConstraints[i].friction1Impulse;
- Vector3 angularImpulseBody2 = mFrictionConstraints[i].r2CrossT1 * mFrictionConstraints[i].friction1Impulse;
+ Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 *
+ contactManifold.friction1Impulse;
+ Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 *
+ contactManifold.friction1Impulse;
+ Vector3 linearImpulseBody2 = contactManifold.frictionVector1 *
+ contactManifold.friction1Impulse;
+ Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 *
+ contactManifold.friction1Impulse;
+ const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
+ linearImpulseBody2, angularImpulseBody2);
- // Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].massInverseBody1 * (-linearImpulseBody2);
- mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
-
- // Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
- mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseFriction1, contactManifold);
// ------ Second friction constraint at the center of the contact manifold ----- //
// Compute the impulse P = J^T * lambda
- angularImpulseBody1 = -mFrictionConstraints[i].r1CrossT2 * mFrictionConstraints[i].friction2Impulse;
- linearImpulseBody2 = mFrictionConstraints[i].frictionVector2 * mFrictionConstraints[i].friction2Impulse;
- angularImpulseBody2 = mFrictionConstraints[i].r2CrossT2 * mFrictionConstraints[i].friction2Impulse;
+ linearImpulseBody1 = -contactManifold.frictionVector2 *
+ contactManifold.friction2Impulse;
+ angularImpulseBody1 = -contactManifold.r1CrossT2 *
+ contactManifold.friction2Impulse;
+ linearImpulseBody2 = contactManifold.frictionVector2 *
+ contactManifold.friction2Impulse;
+ angularImpulseBody2 = contactManifold.r2CrossT2 *
+ contactManifold.friction2Impulse;
+ const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
+ linearImpulseBody2, angularImpulseBody2);
- // Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].massInverseBody1 * (-linearImpulseBody2);
- mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
-
- // Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
- mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseFriction2, contactManifold);
// ------ Twist friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
- angularImpulseBody2 = mFrictionConstraints[i].normal * mFrictionConstraints[i].frictionTwistImpulse;
+ linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
+ angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse;
+ linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);
+ angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse;
+ const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
+ linearImpulseBody2, angularImpulseBody2);
- // Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * (-angularImpulseBody2);
-
- // Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseTwistFriction, contactManifold);
// ------ Rolling resistance at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
- angularImpulseBody2 = mFrictionConstraints[i].rollingResistanceImpulse;
+ angularImpulseBody1 = -contactManifold.rollingResistanceImpulse;
+ angularImpulseBody2 = contactManifold.rollingResistanceImpulse;
+ const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1,
+ Vector3::zero(), angularImpulseBody2);
- // Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[mFrictionConstraints[i].indexBody1] += mFrictionConstraints[i].inverseInertiaTensorBody1 * (-angularImpulseBody2);
-
- // Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[mFrictionConstraints[i].indexBody2] += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseRollingResistance, contactManifold);
}
else { // If it is a new contact manifold
// Initialize the accumulated impulses to zero
- mFrictionConstraints[i].friction1Impulse = 0.0;
- mFrictionConstraints[i].friction2Impulse = 0.0;
- mFrictionConstraints[i].frictionTwistImpulse = 0.0;
- mFrictionConstraints[i].rollingResistanceImpulse.setToZero();
+ contactManifold.friction1Impulse = 0.0;
+ contactManifold.friction2Impulse = 0.0;
+ contactManifold.frictionTwistImpulse = 0.0;
+ contactManifold.rollingResistanceImpulse = Vector3::zero();
}
}
}
-// Reset the total penetration impulse of friction constraints
-void ContactSolver::resetTotalPenetrationImpulse() {
+// Solve the contacts
+void ContactSolver::solve() {
- for (uint i=0; i<mNbFrictionConstraints; i++) {
- mFrictionConstraints[i].totalPenetrationImpulse = decimal(0.0);
- }
-}
-
-// Solve the penetration constraints
-void ContactSolver::solvePenetrationConstraints() {
-
- PROFILE("ContactSolver::solvePenetrationConstraints()");
-
- // TODO : Maybe solve split impulses and normal impulses separately
+ PROFILE("ContactSolver::solve()");
decimal deltaLambda;
decimal lambdaTemp;
- for (uint i=0; i<mNbPenetrationConstraints; i++) {
+ // For each contact manifold
+ for (uint c=0; c<mNbContactManifolds; c++) {
+
+ ContactManifoldSolver& contactManifold = mContactConstraints[c];
+
+ decimal sumPenetrationImpulse = 0.0;
// Get the constrained velocities
- Vector3& v1 = mLinearVelocities[mPenetrationConstraints[i].indexBody1];
- Vector3& w1 = mAngularVelocities[mPenetrationConstraints[i].indexBody1];
- Vector3& v2 = mLinearVelocities[mPenetrationConstraints[i].indexBody2];
- Vector3& w2 = mAngularVelocities[mPenetrationConstraints[i].indexBody2];
+ const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
+ const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
+ const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
+ const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
- // Compute J*v
- Vector3 deltaV = v2 + w2.cross(mPenetrationConstraints[i].r2) - v1 - w1.cross(mPenetrationConstraints[i].r1);
- decimal deltaVDotN = deltaV.dot(mPenetrationConstraints[i].normal);
- decimal Jv = deltaVDotN;
+ for (uint i=0; i<contactManifold.nbContacts; i++) {
- // Compute the bias "b" of the constraint
- decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
- decimal biasPenetrationDepth = 0.0;
- if (mPenetrationConstraints[i].penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
- max(0.0f, float(mPenetrationConstraints[i].penetrationDepth - SLOP));
- decimal b = biasPenetrationDepth + mPenetrationConstraints[i].restitutionBias;
+ ContactPointSolver& contactPoint = contactManifold.contacts[i];
- // Compute the Lagrange multiplier lambda
- if (mIsSplitImpulseActive) {
- deltaLambda = - (Jv + mPenetrationConstraints[i].restitutionBias) *
- mPenetrationConstraints[i].inversePenetrationMass;
- }
- else {
- deltaLambda = - (Jv + b) * mPenetrationConstraints[i].inversePenetrationMass;
- }
- lambdaTemp = mPenetrationConstraints[i].penetrationImpulse;
- mPenetrationConstraints[i].penetrationImpulse = std::max(mPenetrationConstraints[i].penetrationImpulse +
- deltaLambda, decimal(0.0));
- deltaLambda = mPenetrationConstraints[i].penetrationImpulse - lambdaTemp;
-
- // Add the penetration impulse to the total impulse of the corresponding friction constraint
- mFrictionConstraints[mPenetrationConstraints[i].indexFrictionConstraint].totalPenetrationImpulse += mPenetrationConstraints[i].penetrationImpulse;
-
- // Update the velocities of the body 1 by applying the impulse P=J^T * lambda
- Vector3 linearImpulse = mPenetrationConstraints[i].normal * deltaLambda;
- v1 += mPenetrationConstraints[i].massInverseBody1 * (-linearImpulse);
- w1 += mPenetrationConstraints[i].inverseInertiaTensorBody1 * (-mPenetrationConstraints[i].r1CrossN * deltaLambda);
-
- // Update the velocities of the body 1 by applying the impulse P=J^T * lambda
- v2 += mPenetrationConstraints[i].massInverseBody2 * linearImpulse;
- w2 += mPenetrationConstraints[i].inverseInertiaTensorBody2 * (mPenetrationConstraints[i].r2CrossN * deltaLambda);
-
- // If the split impulse position correction is active
- if (mIsSplitImpulseActive) {
-
- // Split impulse (position correction)
- const Vector3& v1Split = mSplitLinearVelocities[mPenetrationConstraints[i].indexBody1];
- const Vector3& w1Split = mSplitAngularVelocities[mPenetrationConstraints[i].indexBody1];
- const Vector3& v2Split = mSplitLinearVelocities[mPenetrationConstraints[i].indexBody2];
- const Vector3& w2Split = mSplitAngularVelocities[mPenetrationConstraints[i].indexBody2];
- Vector3 deltaVSplit = v2Split + w2Split.cross(mPenetrationConstraints[i].r2) -
- v1Split - w1Split.cross(mPenetrationConstraints[i].r1);
- decimal JvSplit = deltaVSplit.dot(mPenetrationConstraints[i].normal);
- decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
- mPenetrationConstraints[i].inversePenetrationMass;
- decimal lambdaTempSplit = mPenetrationConstraints[i].penetrationSplitImpulse;
- mPenetrationConstraints[i].penetrationSplitImpulse = std::max(
- mPenetrationConstraints[i].penetrationSplitImpulse +
- deltaLambdaSplit, decimal(0.0));
- deltaLambdaSplit = mPenetrationConstraints[i].penetrationSplitImpulse - lambdaTempSplit;
-
- // Update the velocities of the body 1 by applying the impulse P=J^T * lambda
- Vector3 linearImpulse = mPenetrationConstraints[i].normal * deltaLambdaSplit;
- mSplitLinearVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].massInverseBody1 * (-linearImpulse);
- mSplitAngularVelocities[mPenetrationConstraints[i].indexBody1] += mPenetrationConstraints[i].inverseInertiaTensorBody1 * (-mPenetrationConstraints[i].r1CrossN * deltaLambdaSplit);
-
- // Update the velocities of the body 1 by applying the impulse P=J^T * lambda
- mSplitLinearVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].massInverseBody2 * linearImpulse;
- mSplitAngularVelocities[mPenetrationConstraints[i].indexBody2] += mPenetrationConstraints[i].inverseInertiaTensorBody2 * (mPenetrationConstraints[i].r2CrossN * deltaLambdaSplit);
- }
- }
-}
-
-// Solve the friction constraints
-void ContactSolver::solveFrictionConstraints() {
-
- PROFILE("ContactSolver::solveFrictionConstraints()");
-
- for (uint i=0; i<mNbFrictionConstraints; i++) {
-
- // Get the constrained velocities
- Vector3& v1 = mLinearVelocities[mFrictionConstraints[i].indexBody1];
- Vector3& w1 = mAngularVelocities[mFrictionConstraints[i].indexBody1];
- Vector3& v2 = mLinearVelocities[mFrictionConstraints[i].indexBody2];
- Vector3& w2 = mAngularVelocities[mFrictionConstraints[i].indexBody2];
-
- // ------ First friction constraint at the center of the contact manifol ------ //
-
- // Compute J*v
- Vector3 deltaV = v2 + w2.cross(mFrictionConstraints[i].r2Friction)
- - v1 - w1.cross(mFrictionConstraints[i].r1Friction);
- decimal Jv = deltaV.dot(mFrictionConstraints[i].frictionVector1);
-
- // Compute the Lagrange multiplier lambda
- decimal deltaLambda = -Jv * mFrictionConstraints[i].inverseFriction1Mass;
- decimal frictionLimit = mFrictionConstraints[i].frictionCoefficient * mFrictionConstraints[i].totalPenetrationImpulse;
- decimal lambdaTemp = mFrictionConstraints[i].friction1Impulse;
- mFrictionConstraints[i].friction1Impulse = std::max(-frictionLimit,
- std::min(mFrictionConstraints[i].friction1Impulse +
- deltaLambda, frictionLimit));
- deltaLambda = mFrictionConstraints[i].friction1Impulse - lambdaTemp;
-
- // Compute the impulse P=J^T * lambda
- Vector3 linearImpulseBody2 = mFrictionConstraints[i].frictionVector1 * deltaLambda;
- Vector3 linearImpulseBody1 = -linearImpulseBody2;
- Vector3 angularImpulseBody1 = -mFrictionConstraints[i].r1CrossT1 * deltaLambda;
- Vector3 angularImpulseBody2 = mFrictionConstraints[i].r2CrossT1 * deltaLambda;
-
- // Update the velocities of the body 1 by applying the impulse P
- v1 += mFrictionConstraints[i].massInverseBody1 * linearImpulseBody1;
- w1 += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
-
- // Update the velocities of the body 1 by applying the impulse P
- v2 += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
- w2 += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
-
- // ------ Second friction constraint at the center of the contact manifol ----- //
-
- // Compute J*v
- deltaV = v2 + w2.cross(mFrictionConstraints[i].r2Friction)
- - v1 - w1.cross(mFrictionConstraints[i].r1Friction);
- Jv = deltaV.dot(mFrictionConstraints[i].frictionVector2);
-
- // Compute the Lagrange multiplier lambda
- deltaLambda = -Jv * mFrictionConstraints[i].inverseFriction2Mass;
- frictionLimit = mFrictionConstraints[i].frictionCoefficient * mFrictionConstraints[i].totalPenetrationImpulse;
- lambdaTemp = mFrictionConstraints[i].friction2Impulse;
- mFrictionConstraints[i].friction2Impulse = std::max(-frictionLimit,
- std::min(mFrictionConstraints[i].friction2Impulse +
- deltaLambda, frictionLimit));
- deltaLambda = mFrictionConstraints[i].friction2Impulse - lambdaTemp;
-
- // Compute the impulse P=J^T * lambda
- linearImpulseBody2 = mFrictionConstraints[i].frictionVector2 * deltaLambda;
- linearImpulseBody1 = -linearImpulseBody2;
- angularImpulseBody1 = -mFrictionConstraints[i].r1CrossT2 * deltaLambda;
- angularImpulseBody2 = mFrictionConstraints[i].r2CrossT2 * deltaLambda;
-
- // Update the velocities of the body 1 by applying the impulse P
- v1 += mFrictionConstraints[i].massInverseBody1 * linearImpulseBody1;
- w1 += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
-
- // Update the velocities of the body 1 by applying the impulse P
- v2 += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
- w2 += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
-
- // ------ Twist friction constraint at the center of the contact manifol ------ //
-
- // Compute J*v
- deltaV = w2 - w1;
- Jv = deltaV.dot(mFrictionConstraints[i].normal);
-
- deltaLambda = -Jv * (mFrictionConstraints[i].inverseTwistFrictionMass);
- frictionLimit = mFrictionConstraints[i].frictionCoefficient * mFrictionConstraints[i].totalPenetrationImpulse;
- lambdaTemp = mFrictionConstraints[i].frictionTwistImpulse;
- mFrictionConstraints[i].frictionTwistImpulse = std::max(-frictionLimit,
- std::min(mFrictionConstraints[i].frictionTwistImpulse
- + deltaLambda, frictionLimit));
- deltaLambda = mFrictionConstraints[i].frictionTwistImpulse - lambdaTemp;
-
- // Compute the impulse P=J^T * lambda
- linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
- linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);
- angularImpulseBody2 = mFrictionConstraints[i].normal * deltaLambda;
- angularImpulseBody1 = -angularImpulseBody2;
-
- // Update the velocities of the body 1 by applying the impulse P
- v1 += mFrictionConstraints[i].massInverseBody1 * linearImpulseBody1;
- w1 += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
-
- // Update the velocities of the body 1 by applying the impulse P
- v2 += mFrictionConstraints[i].massInverseBody2 * linearImpulseBody2;
- w2 += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
-
- // --------- Rolling resistance constraint at the center of the contact manifold --------- //
-
- if (mFrictionConstraints[i].rollingResistanceFactor > 0) {
+ // --------- Penetration --------- //
// Compute J*v
- const Vector3 JvRolling = w2 - w1;
+ Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+ decimal deltaVDotN = deltaV.dot(contactPoint.normal);
+ decimal Jv = deltaVDotN;
+
+ // Compute the bias "b" of the constraint
+ decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
+ decimal biasPenetrationDepth = 0.0;
+ if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
+ max(0.0f, float(contactPoint.penetrationDepth - SLOP));
+ decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the Lagrange multiplier lambda
- Vector3 deltaLambdaRolling = mFrictionConstraints[i].inverseRollingResistance * (-JvRolling);
- decimal rollingLimit = mFrictionConstraints[i].rollingResistanceFactor * mFrictionConstraints[i].totalPenetrationImpulse;
- Vector3 lambdaTempRolling = mFrictionConstraints[i].rollingResistanceImpulse;
- mFrictionConstraints[i].rollingResistanceImpulse = clamp(mFrictionConstraints[i].rollingResistanceImpulse +
- deltaLambdaRolling, rollingLimit);
- deltaLambdaRolling = mFrictionConstraints[i].rollingResistanceImpulse - lambdaTempRolling;
+ if (mIsSplitImpulseActive) {
+ deltaLambda = - (Jv + contactPoint.restitutionBias) *
+ contactPoint.inversePenetrationMass;
+ }
+ else {
+ deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
+ }
+ lambdaTemp = contactPoint.penetrationImpulse;
+ contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
+ deltaLambda, decimal(0.0));
+ deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
- angularImpulseBody1 = -deltaLambdaRolling;
- angularImpulseBody2 = deltaLambdaRolling;
+ const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
+ contactPoint);
- // Update the velocities of the body 1 by applying the impulse P
- w1 += mFrictionConstraints[i].inverseInertiaTensorBody1 * angularImpulseBody1;
+ // Apply the impulse to the bodies of the constraint
+ applyImpulse(impulsePenetration, contactManifold);
- // Update the velocities of the body 1 by applying the impulse P
- w2 += mFrictionConstraints[i].inverseInertiaTensorBody2 * angularImpulseBody2;
+ sumPenetrationImpulse += contactPoint.penetrationImpulse;
+
+ // If the split impulse position correction is active
+ if (mIsSplitImpulseActive) {
+
+ // Split impulse (position correction)
+ const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
+ const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
+ const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
+ const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
+ Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
+ v1Split - w1Split.cross(contactPoint.r1);
+ decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
+ decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
+ contactPoint.inversePenetrationMass;
+ decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
+ contactPoint.penetrationSplitImpulse = std::max(
+ contactPoint.penetrationSplitImpulse +
+ deltaLambdaSplit, decimal(0.0));
+ deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
+
+ // Compute the impulse P=J^T * lambda
+ const Impulse splitImpulsePenetration = computePenetrationImpulse(
+ deltaLambdaSplit, contactPoint);
+
+ applySplitImpulse(splitImpulsePenetration, contactManifold);
+ }
+
+ // If we do not solve the friction constraints at the center of the contact manifold
+ if (!mIsSolveFrictionAtContactManifoldCenterActive) {
+
+ // --------- Friction 1 --------- //
+
+ // Compute J*v
+ deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+ Jv = deltaV.dot(contactPoint.frictionVector1);
+
+ // Compute the Lagrange multiplier lambda
+ deltaLambda = -Jv;
+ deltaLambda *= contactPoint.inverseFriction1Mass;
+ decimal frictionLimit = contactManifold.frictionCoefficient *
+ contactPoint.penetrationImpulse;
+ lambdaTemp = contactPoint.friction1Impulse;
+ contactPoint.friction1Impulse = std::max(-frictionLimit,
+ std::min(contactPoint.friction1Impulse
+ + deltaLambda, frictionLimit));
+ deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
+
+ // Compute the impulse P=J^T * lambda
+ const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
+ contactPoint);
+
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseFriction1, contactManifold);
+
+ // --------- Friction 2 --------- //
+
+ // Compute J*v
+ deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+ Jv = deltaV.dot(contactPoint.frictionVector2);
+
+ // Compute the Lagrange multiplier lambda
+ deltaLambda = -Jv;
+ deltaLambda *= contactPoint.inverseFriction2Mass;
+ frictionLimit = contactManifold.frictionCoefficient *
+ contactPoint.penetrationImpulse;
+ lambdaTemp = contactPoint.friction2Impulse;
+ contactPoint.friction2Impulse = std::max(-frictionLimit,
+ std::min(contactPoint.friction2Impulse
+ + deltaLambda, frictionLimit));
+ deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
+
+ // Compute the impulse P=J^T * lambda
+ const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
+ contactPoint);
+
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseFriction2, contactManifold);
+
+ // --------- Rolling resistance constraint --------- //
+
+ if (contactManifold.rollingResistanceFactor > 0) {
+
+ // Compute J*v
+ const Vector3 JvRolling = w2 - w1;
+
+ // Compute the Lagrange multiplier lambda
+ Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
+ decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
+ Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse;
+ contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse +
+ deltaLambdaRolling, rollingLimit);
+ deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling;
+
+ // Compute the impulse P=J^T * lambda
+ const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling,
+ Vector3::zero(), deltaLambdaRolling);
+
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseRolling, contactManifold);
+ }
+ }
+ }
+
+ // If we solve the friction constraints at the center of the contact manifold
+ if (mIsSolveFrictionAtContactManifoldCenterActive) {
+
+ // ------ First friction constraint at the center of the contact manifol ------ //
+
+ // Compute J*v
+ Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
+ - v1 - w1.cross(contactManifold.r1Friction);
+ decimal Jv = deltaV.dot(contactManifold.frictionVector1);
+
+ // Compute the Lagrange multiplier lambda
+ decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
+ decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+ lambdaTemp = contactManifold.friction1Impulse;
+ contactManifold.friction1Impulse = std::max(-frictionLimit,
+ std::min(contactManifold.friction1Impulse +
+ deltaLambda, frictionLimit));
+ deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
+
+ // Compute the impulse P=J^T * lambda
+ Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
+ Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
+ Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
+ Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
+ const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
+ linearImpulseBody2, angularImpulseBody2);
+
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseFriction1, contactManifold);
+
+ // ------ Second friction constraint at the center of the contact manifol ----- //
+
+ // Compute J*v
+ deltaV = v2 + w2.cross(contactManifold.r2Friction)
+ - v1 - w1.cross(contactManifold.r1Friction);
+ Jv = deltaV.dot(contactManifold.frictionVector2);
+
+ // Compute the Lagrange multiplier lambda
+ deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
+ frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+ lambdaTemp = contactManifold.friction2Impulse;
+ contactManifold.friction2Impulse = std::max(-frictionLimit,
+ std::min(contactManifold.friction2Impulse +
+ deltaLambda, frictionLimit));
+ deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
+
+ // Compute the impulse P=J^T * lambda
+ linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
+ angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
+ linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
+ angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
+ const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
+ linearImpulseBody2, angularImpulseBody2);
+
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseFriction2, contactManifold);
+
+ // ------ Twist friction constraint at the center of the contact manifol ------ //
+
+ // Compute J*v
+ deltaV = w2 - w1;
+ Jv = deltaV.dot(contactManifold.normal);
+
+ deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
+ frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+ lambdaTemp = contactManifold.frictionTwistImpulse;
+ contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
+ std::min(contactManifold.frictionTwistImpulse
+ + deltaLambda, frictionLimit));
+ deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
+
+ // Compute the impulse P=J^T * lambda
+ linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
+ angularImpulseBody1 = -contactManifold.normal * deltaLambda;
+ linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
+ angularImpulseBody2 = contactManifold.normal * deltaLambda;
+ const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
+ linearImpulseBody2, angularImpulseBody2);
+
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseTwistFriction, contactManifold);
+
+ // --------- Rolling resistance constraint at the center of the contact manifold --------- //
+
+ if (contactManifold.rollingResistanceFactor > 0) {
+
+ // Compute J*v
+ const Vector3 JvRolling = w2 - w1;
+
+ // Compute the Lagrange multiplier lambda
+ Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
+ decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
+ Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
+ contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse +
+ deltaLambdaRolling, rollingLimit);
+ deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
+
+ // Compute the impulse P=J^T * lambda
+ angularImpulseBody1 = -deltaLambdaRolling;
+ angularImpulseBody2 = deltaLambdaRolling;
+ const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1,
+ Vector3::zero(), angularImpulseBody2);
+
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseRolling, contactManifold);
+ }
}
}
}
@@ -649,32 +779,76 @@ void ContactSolver::solveFrictionConstraints() {
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
- // Penetration constraints
- for (uint i=0; i<mNbPenetrationConstraints; i++) {
- mPenetrationConstraints[i].contactPoint->setPenetrationImpulse(mPenetrationConstraints[i].penetrationImpulse);
- }
+ // For each contact manifold
+ for (uint c=0; c<mNbContactManifolds; c++) {
- // Friction constraints
- for (uint i=0; i<mNbFrictionConstraints; i++) {
+ ContactManifoldSolver& manifold = mContactConstraints[c];
- mFrictionConstraints[i].contactManifold->setFrictionImpulse1(mFrictionConstraints[i].friction1Impulse);
- mFrictionConstraints[i].contactManifold->setFrictionImpulse2(mFrictionConstraints[i].friction2Impulse);
- mFrictionConstraints[i].contactManifold->setFrictionTwistImpulse(mFrictionConstraints[i].frictionTwistImpulse);
- mFrictionConstraints[i].contactManifold->setRollingResistanceImpulse(mFrictionConstraints[i].rollingResistanceImpulse);
- mFrictionConstraints[i].contactManifold->setFrictionVector1(mFrictionConstraints[i].frictionVector1);
- mFrictionConstraints[i].contactManifold->setFrictionVector2(mFrictionConstraints[i].frictionVector2);
+ for (uint i=0; i<manifold.nbContacts; i++) {
+
+ ContactPointSolver& contactPoint = manifold.contacts[i];
+
+ contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse);
+ contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse);
+ contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse);
+ contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse);
+
+ contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1);
+ contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2);
+ }
+
+ manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse);
+ manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse);
+ manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse);
+ manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse);
+ manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1);
+ manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2);
}
}
-// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
-// for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
-void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
- FrictionConstraint& frictionConstraint) const {
+// Apply an impulse to the two bodies of a constraint
+void ContactSolver::applyImpulse(const Impulse& impulse,
+ const ContactManifoldSolver& manifold) {
- assert(frictionConstraint.normal.length() > MACHINE_EPSILON);
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
+ impulse.linearImpulseBody1;
+ mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
+ impulse.angularImpulseBody1;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
+ impulse.linearImpulseBody2;
+ mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
+ impulse.angularImpulseBody2;
+}
+
+// Apply an impulse to the two bodies of a constraint
+void ContactSolver::applySplitImpulse(const Impulse& impulse,
+ const ContactManifoldSolver& manifold) {
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
+ impulse.linearImpulseBody1;
+ mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
+ impulse.angularImpulseBody1;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
+ impulse.linearImpulseBody2;
+ mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
+ impulse.angularImpulseBody2;
+}
+
+// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
+// for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal.
+void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
+ ContactPointSolver& contactPoint) const {
+
+ assert(contactPoint.normal.length() > 0.0);
// Compute the velocity difference vector in the tangential plane
- Vector3 normalVelocity = deltaVelocity.dot(frictionConstraint.normal) * frictionConstraint.normal;
+ Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
Vector3 tangentVelocity = deltaVelocity - normalVelocity;
// If the velocty difference in the tangential plane is not zero
@@ -683,15 +857,54 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
// Compute the first friction vector in the direction of the tangent
// velocity difference
- frictionConstraint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
+ contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
}
else {
// Get any orthogonal vector to the normal as the first friction vector
- frictionConstraint.frictionVector1 = frictionConstraint.normal.getOneUnitOrthogonalVector();
+ contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector();
}
// The second friction vector is computed by the cross product of the firs
// friction vector and the contact normal
- frictionConstraint.frictionVector2 = frictionConstraint.normal.cross(frictionConstraint.frictionVector1).getUnit();
+ contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit();
+}
+
+// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
+// for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
+void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
+ ContactManifoldSolver& contact) const {
+
+ assert(contact.normal.length() > 0.0);
+
+ // Compute the velocity difference vector in the tangential plane
+ Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal;
+ Vector3 tangentVelocity = deltaVelocity - normalVelocity;
+
+ // If the velocty difference in the tangential plane is not zero
+ decimal lengthTangenVelocity = tangentVelocity.length();
+ if (lengthTangenVelocity > MACHINE_EPSILON) {
+
+ // Compute the first friction vector in the direction of the tangent
+ // velocity difference
+ contact.frictionVector1 = tangentVelocity / lengthTangenVelocity;
+ }
+ else {
+
+ // Get any orthogonal vector to the normal as the first friction vector
+ contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector();
+ }
+
+ // The second friction vector is computed by the cross product of the firs
+ // friction vector and the contact normal
+ contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit();
+}
+
+// Clean up the constraint solver
+void ContactSolver::cleanup() {
+
+ if (mContactConstraints != nullptr) {
+ delete[] mContactConstraints;
+ mContactConstraints = nullptr;
+ }
}
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index 0e710c6f..7fd4442f 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -31,7 +31,6 @@
#include "configuration.h"
#include "constraint/Joint.h"
#include "collision/ContactManifold.h"
-#include "memory/SingleFrameAllocator.h"
#include "Island.h"
#include "Impulse.h"
#include <map>
@@ -40,6 +39,7 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
+
// Class Contact Solver
/**
* This class represents the contact solver that is used to solve rigid bodies contacts.
@@ -113,100 +113,30 @@ class ContactSolver {
private:
- struct PenetrationConstraint {
-
- /// Index of body 1 in the constraint solver
- uint indexBody1;
-
- /// Index of body 2 in the constraint solver
- uint indexBody2;
-
- /// Normal vector of the contact
- Vector3 normal;
-
- /// Vector from the body 1 center to the contact point
- Vector3 r1;
-
- /// Vector from the body 2 center to the contact point
- Vector3 r2;
-
- /// Cross product of r1 with the contact normal
- Vector3 r1CrossN;
-
- /// Cross product of r2 with the contact normal
- Vector3 r2CrossN;
-
- /// Penetration depth
- decimal penetrationDepth;
-
- /// Velocity restitution bias
- decimal restitutionBias;
+ // Structure ContactPointSolver
+ /**
+ * Contact solver internal data structure that to store all the
+ * information relative to a contact point
+ */
+ struct ContactPointSolver {
/// Accumulated normal impulse
decimal penetrationImpulse;
- /// Accumulated split impulse for penetration correction
- decimal penetrationSplitImpulse;
-
- /// Inverse of the mass of body 1
- decimal massInverseBody1;
-
- /// Inverse of the mass of body 2
- decimal massInverseBody2;
-
- /// Inverse of the matrix K for the penenetration
- decimal inversePenetrationMass;
-
- /// Inverse inertia tensor of body 1
- Matrix3x3 inverseInertiaTensorBody1;
-
- /// Inverse inertia tensor of body 2
- Matrix3x3 inverseInertiaTensorBody2;
-
- /// Index of the corresponding friction constraint
- uint indexFrictionConstraint;
-
- /// Pointer to the corresponding contact point
- ContactPoint* contactPoint;
-
- /// True if this constraint is for a resting contact
- bool isRestingContact;
- };
-
- struct FrictionConstraint {
-
- /// Index of body 1 in the constraint solver
- uint indexBody1;
-
- /// Index of body 2 in the constraint solver
- uint indexBody2;
-
- /// R1 vector for the friction constraints
- Vector3 r1Friction;
-
- /// R2 vector for the friction constraints
- Vector3 r2Friction;
-
- /// Average normal vector of the contact manifold
- Vector3 normal;
-
/// Accumulated impulse in the 1st friction direction
decimal friction1Impulse;
/// Accumulated impulse in the 2nd friction direction
decimal friction2Impulse;
- /// Twist friction impulse at contact manifold center
- decimal frictionTwistImpulse;
+ /// Accumulated split impulse for penetration correction
+ decimal penetrationSplitImpulse;
/// Accumulated rolling resistance impulse
Vector3 rollingResistanceImpulse;
- /// Rolling resistance factor between the two bodies
- decimal rollingResistanceFactor;
-
- /// Mix friction coefficient for the two bodies
- decimal frictionCoefficient;
+ /// Normal vector of the contact
+ Vector3 normal;
/// First friction vector in the tangent plane
Vector3 frictionVector1;
@@ -214,12 +144,18 @@ class ContactSolver {
/// Second friction vector in the tangent plane
Vector3 frictionVector2;
- /// Old 1st friction direction at contact manifold center
+ /// Old first friction vector in the tangent plane
Vector3 oldFrictionVector1;
- /// Old 2nd friction direction at contact manifold center
+ /// Old second friction vector in the tangent plane
Vector3 oldFrictionVector2;
+ /// Vector from the body 1 center to the contact point
+ Vector3 r1;
+
+ /// Vector from the body 2 center to the contact point
+ Vector3 r2;
+
/// Cross product of r1 with 1st friction vector
Vector3 r1CrossT1;
@@ -232,8 +168,20 @@ class ContactSolver {
/// Cross product of r2 with 2nd friction vector
Vector3 r2CrossT2;
- /// Total of the all the corresponding penetration impulses
- decimal totalPenetrationImpulse;
+ /// Cross product of r1 with the contact normal
+ Vector3 r1CrossN;
+
+ /// Cross product of r2 with the contact normal
+ Vector3 r2CrossN;
+
+ /// Penetration depth
+ decimal penetrationDepth;
+
+ /// Velocity restitution bias
+ decimal restitutionBias;
+
+ /// Inverse of the matrix K for the penenetration
+ decimal inversePenetrationMass;
/// Inverse of the matrix K for the 1st friction
decimal inverseFriction1Mass;
@@ -241,16 +189,30 @@ class ContactSolver {
/// Inverse of the matrix K for the 2nd friction
decimal inverseFriction2Mass;
- /// Matrix K for the twist friction constraint
- decimal inverseTwistFrictionMass;
+ /// True if the contact was existing last time step
+ bool isRestingContact;
- /// Matrix K for the rolling resistance constraint
- Matrix3x3 inverseRollingResistance;
+ /// Pointer to the external contact
+ ContactPoint* externalContact;
+ };
+
+ // Structure ContactManifoldSolver
+ /**
+ * Contact solver internal data structure to store all the
+ * information relative to a contact manifold.
+ */
+ struct ContactManifoldSolver {
+
+ /// Index of body 1 in the constraint solver
+ uint indexBody1;
+
+ /// Index of body 2 in the constraint solver
+ uint indexBody2;
/// Inverse of the mass of body 1
decimal massInverseBody1;
- /// Inverse of the mass of body 2
+ // Inverse of the mass of body 2
decimal massInverseBody2;
/// Inverse inertia tensor of body 1
@@ -259,11 +221,94 @@ class ContactSolver {
/// Inverse inertia tensor of body 2
Matrix3x3 inverseInertiaTensorBody2;
- /// Pointer to the corresponding contact manifold
- ContactManifold* contactManifold;
+ /// Contact point constraints
+ ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
- /// True if the original contact manifold has at least one resting contact
- bool hasAtLeastOneRestingContactPoint;
+ /// Number of contact points
+ uint nbContacts;
+
+ /// True if the body 1 is of type dynamic
+ bool isBody1DynamicType;
+
+ /// True if the body 2 is of type dynamic
+ bool isBody2DynamicType;
+
+ /// Mix of the restitution factor for two bodies
+ decimal restitutionFactor;
+
+ /// Mix friction coefficient for the two bodies
+ decimal frictionCoefficient;
+
+ /// Rolling resistance factor between the two bodies
+ decimal rollingResistanceFactor;
+
+ /// Pointer to the external contact manifold
+ ContactManifold* externalContactManifold;
+
+ // - Variables used when friction constraints are apply at the center of the manifold-//
+
+ /// Average normal vector of the contact manifold
+ Vector3 normal;
+
+ /// Point on body 1 where to apply the friction constraints
+ Vector3 frictionPointBody1;
+
+ /// Point on body 2 where to apply the friction constraints
+ Vector3 frictionPointBody2;
+
+ /// R1 vector for the friction constraints
+ Vector3 r1Friction;
+
+ /// R2 vector for the friction constraints
+ Vector3 r2Friction;
+
+ /// Cross product of r1 with 1st friction vector
+ Vector3 r1CrossT1;
+
+ /// Cross product of r1 with 2nd friction vector
+ Vector3 r1CrossT2;
+
+ /// Cross product of r2 with 1st friction vector
+ Vector3 r2CrossT1;
+
+ /// Cross product of r2 with 2nd friction vector
+ Vector3 r2CrossT2;
+
+ /// Matrix K for the first friction constraint
+ decimal inverseFriction1Mass;
+
+ /// Matrix K for the second friction constraint
+ decimal inverseFriction2Mass;
+
+ /// Matrix K for the twist friction constraint
+ decimal inverseTwistFrictionMass;
+
+ /// Matrix K for the rolling resistance constraint
+ Matrix3x3 inverseRollingResistance;
+
+ /// First friction direction at contact manifold center
+ Vector3 frictionVector1;
+
+ /// Second friction direction at contact manifold center
+ Vector3 frictionVector2;
+
+ /// Old 1st friction direction at contact manifold center
+ Vector3 oldFrictionVector1;
+
+ /// Old 2nd friction direction at contact manifold center
+ Vector3 oldFrictionVector2;
+
+ /// First friction direction impulse at manifold center
+ decimal friction1Impulse;
+
+ /// Second friction direction impulse at manifold center
+ decimal friction2Impulse;
+
+ /// Twist friction impulse at contact manifold center
+ decimal frictionTwistImpulse;
+
+ /// Rolling resistance impulse
+ Vector3 rollingResistanceImpulse;
};
// -------------------- Constants --------------------- //
@@ -285,19 +330,17 @@ class ContactSolver {
/// Split angular velocities for the position contact solver (split impulse)
Vector3* mSplitAngularVelocities;
- /// Reference to the single frame memory allocator
- SingleFrameAllocator& mSingleFrameAllocator;
-
/// Current time step
decimal mTimeStep;
- PenetrationConstraint* mPenetrationConstraints;
+ /// Contact constraints
+ ContactManifoldSolver* mContactConstraints;
- FrictionConstraint* mFrictionConstraints;
+ /// Number of contact constraints
+ uint mNbContactManifolds;
- uint mNbPenetrationConstraints;
-
- uint mNbFrictionConstraints;
+ /// Single frame memory allocator
+ SingleFrameAllocator& mSingleFrameAllocator;
/// Array of linear velocities
Vector3* mLinearVelocities;
@@ -320,15 +363,15 @@ class ContactSolver {
// -------------------- Methods -------------------- //
- /// Initialize the constraint solver for a given island
- void initializeForIsland(Island* island);
+ /// Initialize the contact constraints before solving the system
+ void initializeContactConstraints();
/// Apply an impulse to the two bodies of a constraint
- //void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
+ void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
/// Apply an impulse to the two bodies of a constraint
- //void applySplitImpulse(const Impulse& impulse,
- // const ContactManifoldSolver& manifold);
+ void applySplitImpulse(const Impulse& impulse,
+ const ContactManifoldSolver& manifold);
/// Compute the collision restitution factor from the restitution factor of each body
decimal computeMixedRestitutionFactor(RigidBody *body1,
@@ -341,30 +384,29 @@ class ContactSolver {
/// Compute th mixed rolling resistance factor between two bodies
decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const;
- // TODO : Delete this
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
/// plane for a contact point. The two vectors have to be
/// such that : t1 x t2 = contactNormal.
-// void computeFrictionVectors(const Vector3& deltaVelocity,
-// ContactPointSolver &contactPoint) const;
+ void computeFrictionVectors(const Vector3& deltaVelocity,
+ ContactPointSolver &contactPoint) const;
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
/// plane for a contact manifold. The two vectors have to be
/// such that : t1 x t2 = contactNormal.
void computeFrictionVectors(const Vector3& deltaVelocity,
- FrictionConstraint& frictionConstraint) const;
+ ContactManifoldSolver& contactPoint) const;
/// Compute a penetration constraint impulse
-// const Impulse computePenetrationImpulse(decimal deltaLambda,
-// const PenetrationConstraint& constraint) const;
+ const Impulse computePenetrationImpulse(decimal deltaLambda,
+ const ContactPointSolver& contactPoint) const;
/// Compute the first friction constraint impulse
const Impulse computeFriction1Impulse(decimal deltaLambda,
- const FrictionConstraint& contactPoint) const;
+ const ContactPointSolver& contactPoint) const;
/// Compute the second friction constraint impulse
const Impulse computeFriction2Impulse(decimal deltaLambda,
- const FrictionConstraint& contactPoint) const;
+ const ContactPointSolver& contactPoint) const;
public:
@@ -372,16 +414,13 @@ class ContactSolver {
/// Constructor
ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
- SingleFrameAllocator& singleFrameAllocator);
+ SingleFrameAllocator& allocator);
/// Destructor
~ContactSolver() = default;
- /// Initialize the contact constraints
- void init(Island** islands, uint nbIslands, decimal timeStep);
-
- /// Solve the contact constraints of one iteration of the solve
- void solve();
+ /// Initialize the constraint solver for a given island
+ void initializeForIsland(decimal dt, Island* island);
/// Set the split velocities arrays
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
@@ -399,16 +438,7 @@ class ContactSolver {
void storeImpulses();
/// Solve the contacts
- //void solve();
-
- /// Reset the total penetration impulse of friction constraints
- void resetTotalPenetrationImpulse();
-
- /// Solve the penetration constraints
- void solvePenetrationConstraints();
-
- /// Solve the friction constraints
- void solveFrictionConstraints();
+ void solve();
/// Return true if the split impulses position correction technique is used for contacts
bool isSplitImpulseActive() const;
@@ -420,8 +450,8 @@ class ContactSolver {
/// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
- /// Return true if warmstarting is active
- bool IsWarmStartingActive() const;
+ /// Clean up the constraint solver
+ void cleanup();
};
// Set the split velocities arrays
@@ -476,7 +506,7 @@ inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
RigidBody *body2) const {
// Use the geometric mean to compute the mixed friction coefficient
- return std::sqrt(body1->getMaterial().getFrictionCoefficient() *
+ return sqrt(body1->getMaterial().getFrictionCoefficient() *
body2->getMaterial().getFrictionCoefficient());
}
@@ -487,16 +517,16 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1,
}
// Compute a penetration constraint impulse
-//inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
-// const PenetrationConstraint& constraint)
-// const {
-// return Impulse(-constraint.normal * deltaLambda, -constraint.r1CrossN * deltaLambda,
-// constraint.normal * deltaLambda, constraint.r2CrossN * deltaLambda);
-//}
+inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
+ const ContactPointSolver& contactPoint)
+ const {
+ return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda,
+ contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda);
+}
// Compute the first friction constraint impulse
inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
- const FrictionConstraint& contactPoint)
+ const ContactPointSolver& contactPoint)
const {
return Impulse(-contactPoint.frictionVector1 * deltaLambda,
-contactPoint.r1CrossT1 * deltaLambda,
@@ -506,7 +536,7 @@ inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
// Compute the second friction constraint impulse
inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
- const FrictionConstraint& contactPoint)
+ const ContactPointSolver& contactPoint)
const {
return Impulse(-contactPoint.frictionVector2 * deltaLambda,
-contactPoint.r1CrossT2 * deltaLambda,
@@ -514,11 +544,6 @@ inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
contactPoint.r2CrossT2 * deltaLambda);
}
-// Return true if warmstarting is active
-inline bool ContactSolver::IsWarmStartingActive() const {
- return mIsWarmStartingActive;
-}
-
}
#endif
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index b8a48ddc..c40d5ec5 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -331,8 +331,6 @@ void DynamicsWorld::solveContactsAndConstraints() {
PROFILE("DynamicsWorld::solveContactsAndConstraints()");
- // TODO : Do not solve per island but solve every constraints at once
-
// Set the velocities arrays
mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
@@ -342,28 +340,25 @@ void DynamicsWorld::solveContactsAndConstraints() {
mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
mConstrainedOrientations);
- // Initialize the contact solver
- mContactSolver.init(mIslands, mNbIslands, mTimeStep);
+ // ---------- Solve velocity constraints for joints and contacts ---------- //
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Check if there are contacts and constraints to solve
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
- //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
- //if (!isConstraintsToSolve && !isContactsToSolve) continue;
+ bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
+ if (!isConstraintsToSolve && !isContactsToSolve) continue;
// If there are contacts in the current island
-// if (isContactsToSolve) {
+ if (isContactsToSolve) {
-// // Initialize the solver
-// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
+ // Initialize the solver
+ mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
-// // Warm start the contact solver
-// if (mContactSolver.IsWarmStartingActive()) {
-// mContactSolver.warmStart();
-// }
-// }
+ // Warm start the contact solver
+ mContactSolver.warmStart();
+ }
// If there are constraints
if (isConstraintsToSolve) {
@@ -371,40 +366,26 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Initialize the constraint solver
mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
}
- }
// For each iteration of the velocity solver
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
- for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
- // Solve the constraints
- bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
- if (isConstraintsToSolve) {
- mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
- }
+ // Solve the constraints
+ if (isConstraintsToSolve) {
+ mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
}
- mContactSolver.solve();
-
// Solve the contacts
-// if (isContactsToSolve) {
-
-// mContactSolver.resetTotalPenetrationImpulse();
-
-// mContactSolver.solvePenetrationConstraints();
-// mContactSolver.solveFrictionConstraints();
-// }
- }
+ if (isContactsToSolve) mContactSolver.solve();
+ }
// Cache the lambda values in order to use them in the next
// step and cleanup the contact solver
-// if (isContactsToSolve) {
-// mContactSolver.storeImpulses();
-// mContactSolver.cleanup();
-// }
- //}
-
- mContactSolver.storeImpulses();
+ if (isContactsToSolve) {
+ mContactSolver.storeImpulses();
+ mContactSolver.cleanup();
+ }
+ }
}
// Solve the position error correction of the constraints
From 3ab2b8608c99a9f40737f9104114628920d624a2 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 8 Oct 2016 16:58:28 +0200
Subject: [PATCH 011/133] Always solve friction at the center of the manifold
and always use warmstarting
---
src/engine/ContactSolver.cpp | 477 +++++++++++------------------------
src/engine/ContactSolver.h | 17 --
src/engine/DynamicsWorld.h | 14 -
3 files changed, 142 insertions(+), 366 deletions(-)
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index d8c3a644..77abee5d 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -45,8 +45,7 @@ ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocity
mContactConstraints(nullptr), mSingleFrameAllocator(allocator),
mLinearVelocities(nullptr), mAngularVelocities(nullptr),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
- mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
- mIsSolveFrictionAtContactManifoldCenterActive(true) {
+ mIsSplitImpulseActive(true) {
}
@@ -105,11 +104,8 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
- // If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
- internalManifold.frictionPointBody1 = Vector3::zero();
- internalManifold.frictionPointBody2 = Vector3::zero();
- }
+ internalManifold.frictionPointBody1 = Vector3::zero();
+ internalManifold.frictionPointBody2 = Vector3::zero();
// For each contact point of the contact manifold
for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
@@ -137,40 +133,22 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
contactPoint.friction2Impulse = 0.0;
contactPoint.rollingResistanceImpulse = Vector3::zero();
- // If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
- internalManifold.frictionPointBody1 += p1;
- internalManifold.frictionPointBody2 += p2;
- }
+ internalManifold.frictionPointBody1 += p1;
+ internalManifold.frictionPointBody2 += p2;
}
- // If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
- internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts);
- internalManifold.frictionPointBody2 /=static_cast<decimal>(internalManifold.nbContacts);
- internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1;
- internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2;
- internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
- internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
+ internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts);
+ internalManifold.frictionPointBody2 /=static_cast<decimal>(internalManifold.nbContacts);
+ internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1;
+ internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2;
+ internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
+ internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
- // If warm starting is active
- if (mIsWarmStartingActive) {
-
- // Initialize the accumulated impulses with the previous step accumulated impulses
- internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
- internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
- internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
- }
- else {
-
- // Initialize the accumulated impulses to zero
- internalManifold.friction1Impulse = 0.0;
- internalManifold.friction2Impulse = 0.0;
- internalManifold.frictionTwistImpulse = 0.0;
- internalManifold.rollingResistanceImpulse = Vector3(0, 0, 0);
- }
- }
+ // Initialize the accumulated impulses with the previous step accumulated impulses
+ internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
+ internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
+ internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
}
// Fill-in all the matrices needed to solve the LCP problem
@@ -190,9 +168,7 @@ void ContactSolver::initializeContactConstraints() {
Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
// If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
- manifold.normal = Vector3(0.0, 0.0, 0.0);
- }
+ manifold.normal.setToZero();
// Get the velocities of the bodies
const Vector3& v1 = mLinearVelocities[manifold.indexBody1];
@@ -220,37 +196,6 @@ void ContactSolver::initializeContactConstraints() {
massPenetration :
decimal(0.0);
- // If we do not solve the friction constraints at the center of the contact manifold
- if (!mIsSolveFrictionAtContactManifoldCenterActive) {
-
- // Compute the friction vectors
- computeFrictionVectors(deltaV, contactPoint);
-
- contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
- contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2);
- contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1);
- contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2);
-
- // Compute the inverse mass matrix K for the friction
- // constraints at each contact point
- decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(
- contactPoint.frictionVector1) +
- ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(
- contactPoint.frictionVector1);
- decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(
- contactPoint.frictionVector2) +
- ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(
- contactPoint.frictionVector2);
- friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) /
- friction1Mass :
- decimal(0.0);
- friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) /
- friction2Mass :
- decimal(0.0);
- }
-
// Compute the restitution velocity bias "b". We compute this here instead
// of inside the solve() method because we need to use the velocity difference
// at the beginning of the contact. Note that if it is a resting contact (normal
@@ -261,23 +206,16 @@ void ContactSolver::initializeContactConstraints() {
contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
}
- // If the warm starting of the contact solver is active
- if (mIsWarmStartingActive) {
-
- // Get the cached accumulated impulses from the previous step
- contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
- contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
- contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
- contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
- }
+ // Get the cached accumulated impulses from the previous step
+ contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
+ contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
+ contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
+ contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
// Initialize the split impulses to zero
contactPoint.penetrationSplitImpulse = 0.0;
- // If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
- manifold.normal += contactPoint.normal;
- }
+ manifold.normal += contactPoint.normal;
}
// Compute the inverse K matrix for the rolling resistance constraint
@@ -287,45 +225,41 @@ void ContactSolver::initializeContactConstraints() {
manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse();
}
- // If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ manifold.normal.normalize();
- manifold.normal.normalize();
+ Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
+ v1 - w1.cross(manifold.r1Friction);
- Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
- v1 - w1.cross(manifold.r1Friction);
+ // Compute the friction vectors
+ computeFrictionVectors(deltaVFrictionPoint, manifold);
- // Compute the friction vectors
- computeFrictionVectors(deltaVFrictionPoint, manifold);
-
- // Compute the inverse mass matrix K for the friction constraints at the center of
- // the contact manifold
- manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
- manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
- manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
- manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2);
- decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
- manifold.frictionVector1) +
- ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
- manifold.frictionVector1);
- decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
- manifold.frictionVector2) +
- ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
- manifold.frictionVector2);
- decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
- manifold.normal) +
- manifold.normal.dot(manifold.inverseInertiaTensorBody2 *
- manifold.normal);
- friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass
- : decimal(0.0);
- friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass
- : decimal(0.0);
- frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) /
- frictionTwistMass :
- decimal(0.0);
- }
+ // Compute the inverse mass matrix K for the friction constraints at the center of
+ // the contact manifold
+ manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
+ manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
+ manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
+ manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2);
+ decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
+ ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
+ manifold.frictionVector1) +
+ ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
+ manifold.frictionVector1);
+ decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
+ ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
+ manifold.frictionVector2) +
+ ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
+ manifold.frictionVector2);
+ decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
+ manifold.normal) +
+ manifold.normal.dot(manifold.inverseInertiaTensorBody2 *
+ manifold.normal);
+ friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass
+ : decimal(0.0);
+ friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass
+ : decimal(0.0);
+ frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) /
+ frictionTwistMass :
+ decimal(0.0);
}
}
@@ -335,9 +269,6 @@ void ContactSolver::initializeContactConstraints() {
/// the solution of the linear system
void ContactSolver::warmStart() {
- // Check that warm starting is active
- if (!mIsWarmStartingActive) return;
-
// For each constraint
for (uint c=0; c<mNbContactManifolds; c++) {
@@ -362,51 +293,6 @@ void ContactSolver::warmStart() {
// Apply the impulse to the bodies of the constraint
applyImpulse(impulsePenetration, contactManifold);
-
- // If we do not solve the friction constraints at the center of the contact manifold
- if (!mIsSolveFrictionAtContactManifoldCenterActive) {
-
- // Project the old friction impulses (with old friction vectors) into
- // the new friction vectors to get the new friction impulses
- Vector3 oldFrictionImpulse = contactPoint.friction1Impulse *
- contactPoint.oldFrictionVector1 +
- contactPoint.friction2Impulse *
- contactPoint.oldFrictionVector2;
- contactPoint.friction1Impulse = oldFrictionImpulse.dot(
- contactPoint.frictionVector1);
- contactPoint.friction2Impulse = oldFrictionImpulse.dot(
- contactPoint.frictionVector2);
-
- // --------- Friction 1 --------- //
-
- // Compute the impulse P = J^T * lambda
- const Impulse impulseFriction1 = computeFriction1Impulse(
- contactPoint.friction1Impulse, contactPoint);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction1, contactManifold);
-
- // --------- Friction 2 --------- //
-
- // Compute the impulse P=J^T * lambda
- const Impulse impulseFriction2 = computeFriction2Impulse(
- contactPoint.friction2Impulse, contactPoint);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction2, contactManifold);
-
- // ------ Rolling resistance------ //
-
- if (contactManifold.rollingResistanceFactor > 0) {
-
- // Compute the impulse P = J^T * lambda
- const Impulse impulseRollingResistance(Vector3::zero(), -contactPoint.rollingResistanceImpulse,
- Vector3::zero(), contactPoint.rollingResistanceImpulse);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseRollingResistance, contactManifold);
- }
- }
}
else { // If it is a new contact point
@@ -420,7 +306,7 @@ void ContactSolver::warmStart() {
// If we solve the friction constraints at the center of the contact manifold and there is
// at least one resting contact point in the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) {
+ if (atLeastOneRestingContactPoint) {
// Project the old friction impulses (with old friction vectors) into the new friction
// vectors to get the new friction impulses
@@ -588,189 +474,110 @@ void ContactSolver::solve() {
applySplitImpulse(splitImpulsePenetration, contactManifold);
}
-
- // If we do not solve the friction constraints at the center of the contact manifold
- if (!mIsSolveFrictionAtContactManifoldCenterActive) {
-
- // --------- Friction 1 --------- //
-
- // Compute J*v
- deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
- Jv = deltaV.dot(contactPoint.frictionVector1);
-
- // Compute the Lagrange multiplier lambda
- deltaLambda = -Jv;
- deltaLambda *= contactPoint.inverseFriction1Mass;
- decimal frictionLimit = contactManifold.frictionCoefficient *
- contactPoint.penetrationImpulse;
- lambdaTemp = contactPoint.friction1Impulse;
- contactPoint.friction1Impulse = std::max(-frictionLimit,
- std::min(contactPoint.friction1Impulse
- + deltaLambda, frictionLimit));
- deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
-
- // Compute the impulse P=J^T * lambda
- const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda,
- contactPoint);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction1, contactManifold);
-
- // --------- Friction 2 --------- //
-
- // Compute J*v
- deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
- Jv = deltaV.dot(contactPoint.frictionVector2);
-
- // Compute the Lagrange multiplier lambda
- deltaLambda = -Jv;
- deltaLambda *= contactPoint.inverseFriction2Mass;
- frictionLimit = contactManifold.frictionCoefficient *
- contactPoint.penetrationImpulse;
- lambdaTemp = contactPoint.friction2Impulse;
- contactPoint.friction2Impulse = std::max(-frictionLimit,
- std::min(contactPoint.friction2Impulse
- + deltaLambda, frictionLimit));
- deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
-
- // Compute the impulse P=J^T * lambda
- const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda,
- contactPoint);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction2, contactManifold);
-
- // --------- Rolling resistance constraint --------- //
-
- if (contactManifold.rollingResistanceFactor > 0) {
-
- // Compute J*v
- const Vector3 JvRolling = w2 - w1;
-
- // Compute the Lagrange multiplier lambda
- Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
- decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
- Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse;
- contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse +
- deltaLambdaRolling, rollingLimit);
- deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling;
-
- // Compute the impulse P=J^T * lambda
- const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling,
- Vector3::zero(), deltaLambdaRolling);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseRolling, contactManifold);
- }
- }
}
- // If we solve the friction constraints at the center of the contact manifold
- if (mIsSolveFrictionAtContactManifoldCenterActive) {
+ // ------ First friction constraint at the center of the contact manifol ------ //
- // ------ First friction constraint at the center of the contact manifol ------ //
+ // Compute J*v
+ Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
+ - v1 - w1.cross(contactManifold.r1Friction);
+ decimal Jv = deltaV.dot(contactManifold.frictionVector1);
- // Compute J*v
- Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
- - v1 - w1.cross(contactManifold.r1Friction);
- decimal Jv = deltaV.dot(contactManifold.frictionVector1);
+ // Compute the Lagrange multiplier lambda
+ decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
+ decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+ lambdaTemp = contactManifold.friction1Impulse;
+ contactManifold.friction1Impulse = std::max(-frictionLimit,
+ std::min(contactManifold.friction1Impulse +
+ deltaLambda, frictionLimit));
+ deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
- // Compute the Lagrange multiplier lambda
- decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
- decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
- lambdaTemp = contactManifold.friction1Impulse;
- contactManifold.friction1Impulse = std::max(-frictionLimit,
- std::min(contactManifold.friction1Impulse +
- deltaLambda, frictionLimit));
- deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
+ // Compute the impulse P=J^T * lambda
+ Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
+ Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
+ Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
+ Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
+ const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
+ linearImpulseBody2, angularImpulseBody2);
- // Compute the impulse P=J^T * lambda
- Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
- Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
- Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
- Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
- const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseFriction1, contactManifold);
+
+ // ------ Second friction constraint at the center of the contact manifol ----- //
+
+ // Compute J*v
+ deltaV = v2 + w2.cross(contactManifold.r2Friction)
+ - v1 - w1.cross(contactManifold.r1Friction);
+ Jv = deltaV.dot(contactManifold.frictionVector2);
+
+ // Compute the Lagrange multiplier lambda
+ deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
+ frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+ lambdaTemp = contactManifold.friction2Impulse;
+ contactManifold.friction2Impulse = std::max(-frictionLimit,
+ std::min(contactManifold.friction2Impulse +
+ deltaLambda, frictionLimit));
+ deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
+
+ // Compute the impulse P=J^T * lambda
+ linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
+ angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
+ linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
+ angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
+ const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
+ linearImpulseBody2, angularImpulseBody2);
+
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseFriction2, contactManifold);
+
+ // ------ Twist friction constraint at the center of the contact manifol ------ //
+
+ // Compute J*v
+ deltaV = w2 - w1;
+ Jv = deltaV.dot(contactManifold.normal);
+
+ deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
+ frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
+ lambdaTemp = contactManifold.frictionTwistImpulse;
+ contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
+ std::min(contactManifold.frictionTwistImpulse
+ + deltaLambda, frictionLimit));
+ deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
+
+ // Compute the impulse P=J^T * lambda
+ linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
+ angularImpulseBody1 = -contactManifold.normal * deltaLambda;
+ linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
+ angularImpulseBody2 = contactManifold.normal * deltaLambda;
+ const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
linearImpulseBody2, angularImpulseBody2);
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction1, contactManifold);
+ // Apply the impulses to the bodies of the constraint
+ applyImpulse(impulseTwistFriction, contactManifold);
- // ------ Second friction constraint at the center of the contact manifol ----- //
+ // --------- Rolling resistance constraint at the center of the contact manifold --------- //
+
+ if (contactManifold.rollingResistanceFactor > 0) {
// Compute J*v
- deltaV = v2 + w2.cross(contactManifold.r2Friction)
- - v1 - w1.cross(contactManifold.r1Friction);
- Jv = deltaV.dot(contactManifold.frictionVector2);
+ const Vector3 JvRolling = w2 - w1;
// Compute the Lagrange multiplier lambda
- deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
- frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
- lambdaTemp = contactManifold.friction2Impulse;
- contactManifold.friction2Impulse = std::max(-frictionLimit,
- std::min(contactManifold.friction2Impulse +
- deltaLambda, frictionLimit));
- deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
+ Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
+ decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
+ Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
+ contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse +
+ deltaLambdaRolling, rollingLimit);
+ deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
// Compute the impulse P=J^T * lambda
- linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
- angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
- linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
- angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
- const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
+ angularImpulseBody1 = -deltaLambdaRolling;
+ angularImpulseBody2 = deltaLambdaRolling;
+ const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1,
+ Vector3::zero(), angularImpulseBody2);
// Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction2, contactManifold);
-
- // ------ Twist friction constraint at the center of the contact manifol ------ //
-
- // Compute J*v
- deltaV = w2 - w1;
- Jv = deltaV.dot(contactManifold.normal);
-
- deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
- frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
- lambdaTemp = contactManifold.frictionTwistImpulse;
- contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
- std::min(contactManifold.frictionTwistImpulse
- + deltaLambda, frictionLimit));
- deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
-
- // Compute the impulse P=J^T * lambda
- linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
- angularImpulseBody1 = -contactManifold.normal * deltaLambda;
- linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
- angularImpulseBody2 = contactManifold.normal * deltaLambda;
- const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseTwistFriction, contactManifold);
-
- // --------- Rolling resistance constraint at the center of the contact manifold --------- //
-
- if (contactManifold.rollingResistanceFactor > 0) {
-
- // Compute J*v
- const Vector3 JvRolling = w2 - w1;
-
- // Compute the Lagrange multiplier lambda
- Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
- decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
- Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
- contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse +
- deltaLambdaRolling, rollingLimit);
- deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
-
- // Compute the impulse P=J^T * lambda
- angularImpulseBody1 = -deltaLambdaRolling;
- angularImpulseBody2 = deltaLambdaRolling;
- const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1,
- Vector3::zero(), angularImpulseBody2);
-
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseRolling, contactManifold);
- }
+ applyImpulse(impulseRolling, contactManifold);
}
}
}
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index 7fd4442f..5af9433a 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -351,16 +351,9 @@ class ContactSolver {
/// Reference to the map of rigid body to their index in the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
- /// True if the warm starting of the solver is active
- bool mIsWarmStartingActive;
-
/// True if the split impulse position correction is active
bool mIsSplitImpulseActive;
- /// True if we solve 3 friction constraints at the contact manifold center only
- /// instead of 2 friction constraints at each contact point
- bool mIsSolveFrictionAtContactManifoldCenterActive;
-
// -------------------- Methods -------------------- //
/// Initialize the contact constraints before solving the system
@@ -446,10 +439,6 @@ class ContactSolver {
/// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive);
- /// Activate or deactivate the solving of friction constraints at the center of
- /// the contact manifold instead of solving them at each contact point
- void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
-
/// Clean up the constraint solver
void cleanup();
};
@@ -486,12 +475,6 @@ inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
mIsSplitImpulseActive = isActive;
}
-// Activate or deactivate the solving of friction constraints at the center of
-// the contact manifold instead of solving them at each contact point
-inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
- mIsSolveFrictionAtContactManifoldCenterActive = isActive;
-}
-
// Compute the collision restitution factor from the restitution factor of each body
inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
RigidBody* body2) const {
diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h
index dec383a1..746e8255 100644
--- a/src/engine/DynamicsWorld.h
+++ b/src/engine/DynamicsWorld.h
@@ -204,10 +204,6 @@ class DynamicsWorld : public CollisionWorld {
/// Set the position correction technique used for joints
void setJointsPositionCorrectionTechnique(JointsPositionCorrectionTechnique technique);
- /// Activate or deactivate the solving of friction constraints at the center of
- /// the contact manifold instead of solving them at each contact point
- void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
-
/// Create a rigid body into the physics world.
RigidBody* createRigidBody(const Transform& transform);
@@ -367,16 +363,6 @@ inline void DynamicsWorld::setJointsPositionCorrectionTechnique(
}
}
-// Activate or deactivate the solving of friction constraints at the center of
-// the contact manifold instead of solving them at each contact point
-/**
- * @param isActive True if you want the friction to be solved at the center of
- * the contact manifold and false otherwise
- */
-inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) {
- mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
-}
-
// Return the gravity vector of the world
/**
* @return The current gravity vector (in meter per seconds squared)
From a4a141483b31d46049a9165e9e9a93021b36a7ec Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 8 Oct 2016 23:04:22 +0200
Subject: [PATCH 012/133] Remove init contact constraint method
---
src/engine/ContactSolver.cpp | 190 ++++++++++++++---------------------
src/engine/ContactSolver.h | 3 -
2 files changed, 76 insertions(+), 117 deletions(-)
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 77abee5d..0a09f5d8 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -103,10 +103,16 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
internalManifold.externalContactManifold = externalManifold;
internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
-
+ internalManifold.normal.setToZero();
internalManifold.frictionPointBody1 = Vector3::zero();
internalManifold.frictionPointBody2 = Vector3::zero();
+ // Get the velocities of the bodies
+ const Vector3& v1 = mLinearVelocities[internalManifold.indexBody1];
+ const Vector3& w1 = mAngularVelocities[internalManifold.indexBody1];
+ const Vector3& v2 = mLinearVelocities[internalManifold.indexBody2];
+ const Vector3& w2 = mAngularVelocities[internalManifold.indexBody2];
+
// For each contact point of the contact manifold
for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
@@ -128,13 +134,38 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
externalContact->setIsRestingContact(true);
contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1();
contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2();
- contactPoint.penetrationImpulse = 0.0;
- contactPoint.friction1Impulse = 0.0;
- contactPoint.friction2Impulse = 0.0;
- contactPoint.rollingResistanceImpulse = Vector3::zero();
+ contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
+ contactPoint.penetrationSplitImpulse = 0.0;
+ contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
+ contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
+ contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
internalManifold.frictionPointBody1 += p1;
internalManifold.frictionPointBody2 += p2;
+
+ // Compute the velocity difference
+ Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+
+ contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
+ contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
+
+ // Compute the inverse mass matrix K for the penetration constraint
+ decimal massPenetration = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 +
+ ((internalManifold.inverseInertiaTensorBody1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
+ ((internalManifold.inverseInertiaTensorBody2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
+ contactPoint.inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0);
+
+ // Compute the restitution velocity bias "b". We compute this here instead
+ // of inside the solve() method because we need to use the velocity difference
+ // at the beginning of the contact. Note that if it is a resting contact (normal
+ // velocity bellow a given threshold), we do not add a restitution velocity bias
+ contactPoint.restitutionBias = 0.0;
+ decimal deltaVDotN = deltaV.dot(contactPoint.normal);
+ if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
+ contactPoint.restitutionBias = internalManifold.restitutionFactor * deltaVDotN;
+ }
+
+ internalManifold.normal += contactPoint.normal;
}
@@ -149,120 +180,51 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
+
+ // Compute the inverse K matrix for the rolling resistance constraint
+ internalManifold.inverseRollingResistance.setToZero();
+ if (internalManifold.rollingResistanceFactor > 0 && (internalManifold.isBody1DynamicType || internalManifold.isBody2DynamicType)) {
+ internalManifold.inverseRollingResistance = internalManifold.inverseInertiaTensorBody1 + internalManifold.inverseInertiaTensorBody2;
+ internalManifold.inverseRollingResistance = internalManifold.inverseRollingResistance.getInverse();
+ }
+
+ internalManifold.normal.normalize();
+
+ Vector3 deltaVFrictionPoint = v2 + w2.cross(internalManifold.r2Friction) -
+ v1 - w1.cross(internalManifold.r1Friction);
+
+ // Compute the friction vectors
+ computeFrictionVectors(deltaVFrictionPoint, internalManifold);
+
+ // Compute the inverse mass matrix K for the friction constraints at the center of
+ // the contact manifold
+ internalManifold.r1CrossT1 = internalManifold.r1Friction.cross(internalManifold.frictionVector1);
+ internalManifold.r1CrossT2 = internalManifold.r1Friction.cross(internalManifold.frictionVector2);
+ internalManifold.r2CrossT1 = internalManifold.r2Friction.cross(internalManifold.frictionVector1);
+ internalManifold.r2CrossT2 = internalManifold.r2Friction.cross(internalManifold.frictionVector2);
+ decimal friction1Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 +
+ ((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT1).cross(internalManifold.r1Friction)).dot(
+ internalManifold.frictionVector1) +
+ ((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT1).cross(internalManifold.r2Friction)).dot(
+ internalManifold.frictionVector1);
+ decimal friction2Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 +
+ ((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT2).cross(internalManifold.r1Friction)).dot(
+ internalManifold.frictionVector2) +
+ ((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT2).cross(internalManifold.r2Friction)).dot(
+ internalManifold.frictionVector2);
+ decimal frictionTwistMass = internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody1 *
+ internalManifold.normal) +
+ internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody2 *
+ internalManifold.normal);
+ internalManifold.inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0);
+ internalManifold.inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0);
+ internalManifold.inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0);
}
// Fill-in all the matrices needed to solve the LCP problem
initializeContactConstraints();
}
-// Initialize the contact constraints before solving the system
-void ContactSolver::initializeContactConstraints() {
-
- // For each contact constraint
- for (uint c=0; c<mNbContactManifolds; c++) {
-
- ContactManifoldSolver& manifold = mContactConstraints[c];
-
- // Get the inertia tensors of both bodies
- Matrix3x3& I1 = manifold.inverseInertiaTensorBody1;
- Matrix3x3& I2 = manifold.inverseInertiaTensorBody2;
-
- // If we solve the friction constraints at the center of the contact manifold
- manifold.normal.setToZero();
-
- // Get the velocities of the bodies
- const Vector3& v1 = mLinearVelocities[manifold.indexBody1];
- const Vector3& w1 = mAngularVelocities[manifold.indexBody1];
- const Vector3& v2 = mLinearVelocities[manifold.indexBody2];
- const Vector3& w2 = mAngularVelocities[manifold.indexBody2];
-
- // For each contact point constraint
- for (uint i=0; i<manifold.nbContacts; i++) {
-
- ContactPointSolver& contactPoint = manifold.contacts[i];
- ContactPoint* externalContact = contactPoint.externalContact;
-
- // Compute the velocity difference
- Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
-
- contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
- contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
-
- // Compute the inverse mass matrix K for the penetration constraint
- decimal massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
- ((I2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
- massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) /
- massPenetration :
- decimal(0.0);
-
- // Compute the restitution velocity bias "b". We compute this here instead
- // of inside the solve() method because we need to use the velocity difference
- // at the beginning of the contact. Note that if it is a resting contact (normal
- // velocity bellow a given threshold), we do not add a restitution velocity bias
- contactPoint.restitutionBias = 0.0;
- decimal deltaVDotN = deltaV.dot(contactPoint.normal);
- if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
- contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
- }
-
- // Get the cached accumulated impulses from the previous step
- contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
- contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
- contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
- contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
-
- // Initialize the split impulses to zero
- contactPoint.penetrationSplitImpulse = 0.0;
-
- manifold.normal += contactPoint.normal;
- }
-
- // Compute the inverse K matrix for the rolling resistance constraint
- manifold.inverseRollingResistance.setToZero();
- if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) {
- manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2;
- manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse();
- }
-
- manifold.normal.normalize();
-
- Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) -
- v1 - w1.cross(manifold.r1Friction);
-
- // Compute the friction vectors
- computeFrictionVectors(deltaVFrictionPoint, manifold);
-
- // Compute the inverse mass matrix K for the friction constraints at the center of
- // the contact manifold
- manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1);
- manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2);
- manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
- manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2);
- decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot(
- manifold.frictionVector1) +
- ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot(
- manifold.frictionVector1);
- decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 +
- ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot(
- manifold.frictionVector2) +
- ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot(
- manifold.frictionVector2);
- decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 *
- manifold.normal) +
- manifold.normal.dot(manifold.inverseInertiaTensorBody2 *
- manifold.normal);
- friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass
- : decimal(0.0);
- friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass
- : decimal(0.0);
- frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) /
- frictionTwistMass :
- decimal(0.0);
- }
-}
-
// Warm start the solver.
/// For each constraint, we apply the previous impulse (from the previous step)
/// at the beginning. With this technique, we will converge faster towards
@@ -682,7 +644,7 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
ContactManifoldSolver& contact) const {
- assert(contact.normal.length() > 0.0);
+ assert(contact.normal.length() > decimal(0.0));
// Compute the velocity difference vector in the tangential plane
Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal;
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index 5af9433a..a465fa5a 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -356,9 +356,6 @@ class ContactSolver {
// -------------------- Methods -------------------- //
- /// Initialize the contact constraints before solving the system
- void initializeContactConstraints();
-
/// Apply an impulse to the two bodies of a constraint
void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
From 7b5dce927e006d2ab17a8dff3d4f853bcffe1c4d Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 10 Oct 2016 23:30:32 +0200
Subject: [PATCH 013/133] Fix issue with split impulse and refactor contact
solver
---
src/engine/ContactSolver.cpp | 196 +++++++++++++++--------------------
src/engine/ContactSolver.h | 49 +--------
2 files changed, 82 insertions(+), 163 deletions(-)
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 0a09f5d8..d02c773d 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -220,9 +220,6 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
internalManifold.inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0);
internalManifold.inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0);
}
-
- // Fill-in all the matrices needed to solve the LCP problem
- initializeContactConstraints();
}
// Warm start the solver.
@@ -249,12 +246,14 @@ void ContactSolver::warmStart() {
// --------- Penetration --------- //
- // Compute the impulse P = J^T * lambda
- const Impulse impulsePenetration = computePenetrationImpulse(
- contactPoint.penetrationImpulse, contactPoint);
+ // Update the velocities of the body 1 by applying the impulse P
+ Vector3 impulsePenetration = contactPoint.normal * contactPoint.penetrationImpulse;
+ mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-impulsePenetration);
+ mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * contactPoint.penetrationImpulse);
- // Apply the impulse to the bodies of the constraint
- applyImpulse(impulsePenetration, contactManifold);
+ // Update the velocities of the body 2 by applying the impulse P
+ mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * impulsePenetration;
+ mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * contactPoint.penetrationImpulse);
}
else { // If it is a new contact point
@@ -272,72 +271,66 @@ void ContactSolver::warmStart() {
// Project the old friction impulses (with old friction vectors) into the new friction
// vectors to get the new friction impulses
- Vector3 oldFrictionImpulse = contactManifold.friction1Impulse *
- contactManifold.oldFrictionVector1 +
- contactManifold.friction2Impulse *
- contactManifold.oldFrictionVector2;
- contactManifold.friction1Impulse = oldFrictionImpulse.dot(
- contactManifold.frictionVector1);
- contactManifold.friction2Impulse = oldFrictionImpulse.dot(
- contactManifold.frictionVector2);
+ Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * contactManifold.oldFrictionVector1 +
+ contactManifold.friction2Impulse * contactManifold.oldFrictionVector2;
+ contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1);
+ contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector2);
// ------ First friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
- Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 *
- contactManifold.friction1Impulse;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 *
contactManifold.friction1Impulse;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 *
contactManifold.friction1Impulse;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 *
contactManifold.friction1Impulse;
- const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction1, contactManifold);
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
+ mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
+ mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifold ----- //
// Compute the impulse P = J^T * lambda
- linearImpulseBody1 = -contactManifold.frictionVector2 *
- contactManifold.friction2Impulse;
- angularImpulseBody1 = -contactManifold.r1CrossT2 *
- contactManifold.friction2Impulse;
- linearImpulseBody2 = contactManifold.frictionVector2 *
- contactManifold.friction2Impulse;
- angularImpulseBody2 = contactManifold.r2CrossT2 *
- contactManifold.friction2Impulse;
- const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
+ angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse;
+ linearImpulseBody2 = contactManifold.frictionVector2 * contactManifold.friction2Impulse;
+ angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse;
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction2, contactManifold);
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
+ mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
+
+ // Update the velocities of the body 2 by applying the impulse P
+ mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
+ mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
- linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse;
- linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);
angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse;
- const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseTwistFriction, contactManifold);
+ // Update the velocities of the body 1 by applying the impulse P
+ mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
+
+ // Update the velocities of the body 2 by applying the impulse P
+ mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Rolling resistance at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
- angularImpulseBody1 = -contactManifold.rollingResistanceImpulse;
angularImpulseBody2 = contactManifold.rollingResistanceImpulse;
- const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1,
- Vector3::zero(), angularImpulseBody2);
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseRollingResistance, contactManifold);
+ // Update the velocities of the body 1 by applying the impulse P
+ mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-angularImpulseBody2);
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
}
else { // If it is a new contact manifold
@@ -402,12 +395,15 @@ void ContactSolver::solve() {
deltaLambda, decimal(0.0));
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
- // Compute the impulse P=J^T * lambda
- const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda,
- contactPoint);
+ Vector3 linearImpulse = contactPoint.normal * deltaLambda;
- // Apply the impulse to the bodies of the constraint
- applyImpulse(impulsePenetration, contactManifold);
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse);
+ mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * deltaLambda);
+
+ // Update the velocities of the body 2 by applying the impulse P
+ mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse;
+ mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * deltaLambda);
sumPenetrationImpulse += contactPoint.penetrationImpulse;
@@ -428,13 +424,19 @@ void ContactSolver::solve() {
contactPoint.penetrationSplitImpulse = std::max(
contactPoint.penetrationSplitImpulse +
deltaLambdaSplit, decimal(0.0));
- deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
+ deltaLambdaSplit = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
- // Compute the impulse P=J^T * lambda
- const Impulse splitImpulsePenetration = computePenetrationImpulse(
- deltaLambdaSplit, contactPoint);
+ Vector3 linearImpulse = contactPoint.normal * deltaLambdaSplit;
- applySplitImpulse(splitImpulsePenetration, contactManifold);
+ // Update the velocities of the body 1 by applying the impulse P
+ mSplitLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse);
+ mSplitAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 *
+ (-contactPoint.r1CrossN * deltaLambdaSplit);
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mSplitLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse;
+ mSplitAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 *
+ contactPoint.r2CrossN * deltaLambdaSplit;
}
}
@@ -455,21 +457,22 @@ void ContactSolver::solve() {
deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
- Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
- const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction1, contactManifold);
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
+ mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
+
+ // Update the velocities of the body 2 by applying the impulse P
+ mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
+ mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifol ----- //
// Compute J*v
- deltaV = v2 + w2.cross(contactManifold.r2Friction)
- - v1 - w1.cross(contactManifold.r1Friction);
+ deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction);
Jv = deltaV.dot(contactManifold.frictionVector2);
// Compute the Lagrange multiplier lambda
@@ -482,15 +485,17 @@ void ContactSolver::solve() {
deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
- linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
- const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseFriction2, contactManifold);
+ // Update the velocities of the body 1 by applying the impulse P
+ mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
+ mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
+
+ // Update the velocities of the body 2 by applying the impulse P
+ mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
+ mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifol ------ //
@@ -507,15 +512,13 @@ void ContactSolver::solve() {
deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
- linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
- angularImpulseBody1 = -contactManifold.normal * deltaLambda;
- linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);;
angularImpulseBody2 = contactManifold.normal * deltaLambda;
- const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
- linearImpulseBody2, angularImpulseBody2);
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseTwistFriction, contactManifold);
+ // Update the velocities of the body 1 by applying the impulse P
+ mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-angularImpulseBody2);
+
+ // Update the velocities of the body 1 by applying the impulse P
+ mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
// --------- Rolling resistance constraint at the center of the contact manifold --------- //
@@ -532,14 +535,11 @@ void ContactSolver::solve() {
deltaLambdaRolling, rollingLimit);
deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
- // Compute the impulse P=J^T * lambda
- angularImpulseBody1 = -deltaLambdaRolling;
- angularImpulseBody2 = deltaLambdaRolling;
- const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1,
- Vector3::zero(), angularImpulseBody2);
+ // Update the velocities of the body 1 by applying the impulse P
+ mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-deltaLambdaRolling);
- // Apply the impulses to the bodies of the constraint
- applyImpulse(impulseRolling, contactManifold);
+ // Update the velocities of the body 2 by applying the impulse P
+ mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * deltaLambdaRolling;
}
}
}
@@ -575,40 +575,6 @@ void ContactSolver::storeImpulses() {
}
}
-// Apply an impulse to the two bodies of a constraint
-void ContactSolver::applyImpulse(const Impulse& impulse,
- const ContactManifoldSolver& manifold) {
-
- // Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
- impulse.linearImpulseBody1;
- mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
- impulse.angularImpulseBody1;
-
- // Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
- impulse.linearImpulseBody2;
- mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
- impulse.angularImpulseBody2;
-}
-
-// Apply an impulse to the two bodies of a constraint
-void ContactSolver::applySplitImpulse(const Impulse& impulse,
- const ContactManifoldSolver& manifold) {
-
- // Update the velocities of the body 1 by applying the impulse P
- mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 *
- impulse.linearImpulseBody1;
- mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 *
- impulse.angularImpulseBody1;
-
- // Update the velocities of the body 1 by applying the impulse P
- mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 *
- impulse.linearImpulseBody2;
- mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 *
- impulse.angularImpulseBody2;
-}
-
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
// for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal.
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index a465fa5a..802f7024 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -356,13 +356,6 @@ class ContactSolver {
// -------------------- Methods -------------------- //
- /// Apply an impulse to the two bodies of a constraint
- void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold);
-
- /// Apply an impulse to the two bodies of a constraint
- void applySplitImpulse(const Impulse& impulse,
- const ContactManifoldSolver& manifold);
-
/// Compute the collision restitution factor from the restitution factor of each body
decimal computeMixedRestitutionFactor(RigidBody *body1,
RigidBody *body2) const;
@@ -386,18 +379,6 @@ class ContactSolver {
void computeFrictionVectors(const Vector3& deltaVelocity,
ContactManifoldSolver& contactPoint) const;
- /// Compute a penetration constraint impulse
- const Impulse computePenetrationImpulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint) const;
-
- /// Compute the first friction constraint impulse
- const Impulse computeFriction1Impulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint) const;
-
- /// Compute the second friction constraint impulse
- const Impulse computeFriction2Impulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint) const;
-
public:
// -------------------- Methods -------------------- //
@@ -486,7 +467,7 @@ inline decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
inline decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
RigidBody *body2) const {
// Use the geometric mean to compute the mixed friction coefficient
- return sqrt(body1->getMaterial().getFrictionCoefficient() *
+ return std::sqrt(body1->getMaterial().getFrictionCoefficient() *
body2->getMaterial().getFrictionCoefficient());
}
@@ -496,34 +477,6 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1,
return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance());
}
-// Compute a penetration constraint impulse
-inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint)
- const {
- return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda,
- contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda);
-}
-
-// Compute the first friction constraint impulse
-inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint)
- const {
- return Impulse(-contactPoint.frictionVector1 * deltaLambda,
- -contactPoint.r1CrossT1 * deltaLambda,
- contactPoint.frictionVector1 * deltaLambda,
- contactPoint.r2CrossT1 * deltaLambda);
-}
-
-// Compute the second friction constraint impulse
-inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
- const ContactPointSolver& contactPoint)
- const {
- return Impulse(-contactPoint.frictionVector2 * deltaLambda,
- -contactPoint.r1CrossT2 * deltaLambda,
- contactPoint.frictionVector2 * deltaLambda,
- contactPoint.r2CrossT2 * deltaLambda);
-}
-
}
#endif
From 58ae61d6aaa0c72fad376a8d5fd2e16c10fc4d56 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 11 Oct 2016 20:08:47 +0200
Subject: [PATCH 014/133] Remove Impulse class
---
CMakeLists.txt | 1 -
src/engine/ContactSolver.h | 1 -
src/engine/Impulse.h | 87 --------------------------------------
3 files changed, 89 deletions(-)
delete mode 100644 src/engine/Impulse.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 69f20522..87a02105 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -147,7 +147,6 @@ SET (REACTPHYSICS3D_SOURCES
"src/engine/DynamicsWorld.h"
"src/engine/DynamicsWorld.cpp"
"src/engine/EventListener.h"
- "src/engine/Impulse.h"
"src/engine/Island.h"
"src/engine/Island.cpp"
"src/engine/Material.h"
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index 802f7024..02a3c390 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -32,7 +32,6 @@
#include "constraint/Joint.h"
#include "collision/ContactManifold.h"
#include "Island.h"
-#include "Impulse.h"
#include <map>
#include <set>
diff --git a/src/engine/Impulse.h b/src/engine/Impulse.h
deleted file mode 100644
index 89e36429..00000000
--- a/src/engine/Impulse.h
+++ /dev/null
@@ -1,87 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_IMPULSE_H
-#define REACTPHYSICS3D_IMPULSE_H
-
-// Libraries
-#include "mathematics/mathematics.h"
-
-namespace reactphysics3d {
-
-// Structure Impulse
-/**
- * Represents an impulse that we can apply to bodies in the contact or constraint solver.
- */
-struct Impulse {
-
- private:
-
- // -------------------- Methods -------------------- //
-
- public:
-
- // -------------------- Attributes -------------------- //
-
- /// Linear impulse applied to the first body
- const Vector3 linearImpulseBody1;
-
- /// Angular impulse applied to the first body
- const Vector3 angularImpulseBody1;
-
- /// Linear impulse applied to the second body
- const Vector3 linearImpulseBody2;
-
- /// Angular impulse applied to the second body
- const Vector3 angularImpulseBody2;
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- Impulse(const Vector3& initLinearImpulseBody1, const Vector3& initAngularImpulseBody1,
- const Vector3& initLinearImpulseBody2, const Vector3& initAngularImpulseBody2)
- : linearImpulseBody1(initLinearImpulseBody1),
- angularImpulseBody1(initAngularImpulseBody1),
- linearImpulseBody2(initLinearImpulseBody2),
- angularImpulseBody2(initAngularImpulseBody2) {
-
- }
-
- /// Copy-constructor
- Impulse(const Impulse& impulse)
- : linearImpulseBody1(impulse.linearImpulseBody1),
- angularImpulseBody1(impulse.angularImpulseBody1),
- linearImpulseBody2(impulse.linearImpulseBody2),
- angularImpulseBody2(impulse.angularImpulseBody2) {
-
- }
-
- /// Deleted assignment operator
- Impulse& operator=(const Impulse& impulse) = delete;
-};
-
-}
-
-#endif
From d04cee7d0ae1e8b00da8d700bd11712211f2b17f Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 16 Oct 2016 15:40:38 +0200
Subject: [PATCH 015/133] Change the way to iterate over contacts
---
src/engine/ContactSolver.cpp | 561 ++++++++++++++++++-----------------
src/engine/ContactSolver.h | 25 +-
src/engine/DynamicsWorld.cpp | 49 +--
3 files changed, 339 insertions(+), 296 deletions(-)
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index d02c773d..b4e28aaa 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -36,7 +36,7 @@ using namespace std;
// Constants initialization
const decimal ContactSolver::BETA = decimal(0.2);
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
-const decimal ContactSolver::SLOP= decimal(0.01);
+const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
@@ -49,8 +49,54 @@ ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocity
}
+// Initialize the contact constraints
+void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
+
+ PROFILE("ContactSolver::init()");
+
+ mTimeStep = timeStep;
+
+ // TODO : Try not to count manifolds and contact points here
+ uint nbContactManifolds = 0;
+ uint nbContactPoints = 0;
+ for (uint i = 0; i < nbIslands; i++) {
+ uint nbManifoldsInIsland = islands[i]->getNbContactManifolds();
+ nbContactManifolds += nbManifoldsInIsland;
+
+ for (uint j=0; j < nbManifoldsInIsland; j++) {
+ nbContactPoints += islands[i]->getContactManifolds()[j]->getNbContactPoints();
+ }
+ }
+
+ mNbContactManifolds = 0;
+ mNbContactPoints = 0;
+
+ mContactConstraints = nullptr;
+ mContactPoints = nullptr;
+
+ if (nbContactManifolds == 0 || nbContactPoints == 0) return;
+
+ // TODO : Count exactly the number of constraints to allocate here
+ mContactPoints = static_cast<ContactPointSolver*>(mSingleFrameAllocator.allocate(sizeof(ContactPointSolver) * nbContactPoints));
+ assert(mContactPoints != nullptr);
+
+ mContactConstraints = static_cast<ContactManifoldSolver*>(mSingleFrameAllocator.allocate(sizeof(ContactManifoldSolver) * nbContactManifolds));
+ assert(mContactConstraints != nullptr);
+
+ // For each island of the world
+ for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) {
+
+ if (islands[islandIndex]->getNbContactManifolds() > 0) {
+ initializeForIsland(islands[islandIndex]);
+ }
+ }
+
+ // Warmstarting
+ warmStart();
+}
+
// Initialize the constraint solver for a given island
-void ContactSolver::initializeForIsland(decimal dt, Island* island) {
+void ContactSolver::initializeForIsland(Island* island) {
PROFILE("ContactSolver::initializeForIsland()");
@@ -60,22 +106,12 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
assert(mSplitLinearVelocities != nullptr);
assert(mSplitAngularVelocities != nullptr);
- // Set the current time step
- mTimeStep = dt;
-
- mNbContactManifolds = island->getNbContactManifolds();
-
- mContactConstraints = new ContactManifoldSolver[mNbContactManifolds];
- assert(mContactConstraints != nullptr);
-
// For each contact manifold of the island
ContactManifold** contactManifolds = island->getContactManifolds();
- for (uint i=0; i<mNbContactManifolds; i++) {
+ for (uint i=0; i<island->getNbContactManifolds(); i++) {
ContactManifold* externalManifold = contactManifolds[i];
- ContactManifoldSolver& internalManifold = mContactConstraints[i];
-
assert(externalManifold->getNbContactPoints() > 0);
// Get the two bodies of the contact
@@ -90,34 +126,33 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
// Initialize the internal contact manifold structure using the external
// contact manifold
- internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
- internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
- internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
- internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
- internalManifold.massInverseBody1 = body1->mMassInverse;
- internalManifold.massInverseBody2 = body2->mMassInverse;
- internalManifold.nbContacts = externalManifold->getNbContactPoints();
- internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2);
- internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
- internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
- internalManifold.externalContactManifold = externalManifold;
- internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
- internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
- internalManifold.normal.setToZero();
- internalManifold.frictionPointBody1 = Vector3::zero();
- internalManifold.frictionPointBody2 = Vector3::zero();
+ new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
+ mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
+ mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
+ mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
+ mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
+ mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;
+ mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse;
+ mContactConstraints[mNbContactManifolds].nbContacts = externalManifold->getNbContactPoints();
+ mContactConstraints[mNbContactManifolds].restitutionFactor = computeMixedRestitutionFactor(body1, body2);
+ mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
+ mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
+ mContactConstraints[mNbContactManifolds].externalContactManifold = externalManifold;
+ mContactConstraints[mNbContactManifolds].isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
+ mContactConstraints[mNbContactManifolds].isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
+ mContactConstraints[mNbContactManifolds].normal.setToZero();
+ mContactConstraints[mNbContactManifolds].frictionPointBody1 = Vector3::zero();
+ mContactConstraints[mNbContactManifolds].frictionPointBody2 = Vector3::zero();
// Get the velocities of the bodies
- const Vector3& v1 = mLinearVelocities[internalManifold.indexBody1];
- const Vector3& w1 = mAngularVelocities[internalManifold.indexBody1];
- const Vector3& v2 = mLinearVelocities[internalManifold.indexBody2];
- const Vector3& w2 = mAngularVelocities[internalManifold.indexBody2];
+ const Vector3& v1 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody1];
+ const Vector3& w1 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody1];
+ const Vector3& v2 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
+ const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
// For each contact point of the contact manifold
for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
- ContactPointSolver& contactPoint = internalManifold.contacts[c];
-
// Get a contact point
ContactPoint* externalContact = externalManifold->getContactPoint(c);
@@ -125,100 +160,104 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
Vector3 p1 = externalContact->getWorldPointOnBody1();
Vector3 p2 = externalContact->getWorldPointOnBody2();
- contactPoint.externalContact = externalContact;
- contactPoint.normal = externalContact->getNormal();
- contactPoint.r1 = p1 - x1;
- contactPoint.r2 = p2 - x2;
- contactPoint.penetrationDepth = externalContact->getPenetrationDepth();
- contactPoint.isRestingContact = externalContact->getIsRestingContact();
+ new (mContactPoints + mNbContactPoints) ContactPointSolver();
+ mContactPoints[mNbContactPoints].externalContact = externalContact;
+ mContactPoints[mNbContactPoints].normal = externalContact->getNormal();
+ mContactPoints[mNbContactPoints].r1 = p1 - x1;
+ mContactPoints[mNbContactPoints].r2 = p2 - x2;
+ mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth();
+ mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact();
externalContact->setIsRestingContact(true);
- contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1();
- contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2();
- contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
- contactPoint.penetrationSplitImpulse = 0.0;
- contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
- contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
- contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
+ mContactPoints[mNbContactPoints].oldFrictionVector1 = externalContact->getFrictionVector1();
+ mContactPoints[mNbContactPoints].oldFrictionVector2 = externalContact->getFrictionVector2();
+ mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse();
+ mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0;
+ mContactPoints[mNbContactPoints].friction1Impulse = externalContact->getFrictionImpulse1();
+ mContactPoints[mNbContactPoints].friction2Impulse = externalContact->getFrictionImpulse2();
+ mContactPoints[mNbContactPoints].rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
- internalManifold.frictionPointBody1 += p1;
- internalManifold.frictionPointBody2 += p2;
+ mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1;
+ mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2;
// Compute the velocity difference
- Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
+ Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1);
- contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
- contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
+ mContactPoints[mNbContactPoints].r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
+ mContactPoints[mNbContactPoints].r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
// Compute the inverse mass matrix K for the penetration constraint
- decimal massPenetration = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 +
- ((internalManifold.inverseInertiaTensorBody1 * contactPoint.r1CrossN).cross(contactPoint.r1)).dot(contactPoint.normal) +
- ((internalManifold.inverseInertiaTensorBody2 * contactPoint.r2CrossN).cross(contactPoint.r2)).dot(contactPoint.normal);
- contactPoint.inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0);
+ decimal massPenetration = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
+ ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactPoints[mNbContactPoints].r1CrossN).cross(mContactPoints[mNbContactPoints].r1)).dot(mContactPoints[mNbContactPoints].normal) +
+ ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactPoints[mNbContactPoints].r2CrossN).cross(mContactPoints[mNbContactPoints].r2)).dot(mContactPoints[mNbContactPoints].normal);
+ mContactPoints[mNbContactPoints].inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0);
// Compute the restitution velocity bias "b". We compute this here instead
// of inside the solve() method because we need to use the velocity difference
// at the beginning of the contact. Note that if it is a resting contact (normal
// velocity bellow a given threshold), we do not add a restitution velocity bias
- contactPoint.restitutionBias = 0.0;
- decimal deltaVDotN = deltaV.dot(contactPoint.normal);
+ mContactPoints[mNbContactPoints].restitutionBias = 0.0;
+ decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal);
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
- contactPoint.restitutionBias = internalManifold.restitutionFactor * deltaVDotN;
+ mContactPoints[mNbContactPoints].restitutionBias = mContactConstraints[mNbContactManifolds].restitutionFactor * deltaVDotN;
}
- internalManifold.normal += contactPoint.normal;
+ mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal;
+
+ mNbContactPoints++;
}
-
- internalManifold.frictionPointBody1 /=static_cast<decimal>(internalManifold.nbContacts);
- internalManifold.frictionPointBody2 /=static_cast<decimal>(internalManifold.nbContacts);
- internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1;
- internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2;
- internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1();
- internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2();
+ mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
+ mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
+ mContactConstraints[mNbContactManifolds].r1Friction = mContactConstraints[mNbContactManifolds].frictionPointBody1 - x1;
+ mContactConstraints[mNbContactManifolds].r2Friction = mContactConstraints[mNbContactManifolds].frictionPointBody2 - x2;
+ mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1();
+ mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2();
// Initialize the accumulated impulses with the previous step accumulated impulses
- internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1();
- internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2();
- internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
+ mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold->getFrictionImpulse1();
+ mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold->getFrictionImpulse2();
+ mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
// Compute the inverse K matrix for the rolling resistance constraint
- internalManifold.inverseRollingResistance.setToZero();
- if (internalManifold.rollingResistanceFactor > 0 && (internalManifold.isBody1DynamicType || internalManifold.isBody2DynamicType)) {
- internalManifold.inverseRollingResistance = internalManifold.inverseInertiaTensorBody1 + internalManifold.inverseInertiaTensorBody2;
- internalManifold.inverseRollingResistance = internalManifold.inverseRollingResistance.getInverse();
+ mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
+ if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (mContactConstraints[mNbContactManifolds].isBody1DynamicType || mContactConstraints[mNbContactManifolds].isBody2DynamicType)) {
+ mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2;
+ mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse();
}
- internalManifold.normal.normalize();
+ mContactConstraints[mNbContactManifolds].normal.normalize();
- Vector3 deltaVFrictionPoint = v2 + w2.cross(internalManifold.r2Friction) -
- v1 - w1.cross(internalManifold.r1Friction);
+ Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) -
+ v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction);
// Compute the friction vectors
- computeFrictionVectors(deltaVFrictionPoint, internalManifold);
+ computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]);
// Compute the inverse mass matrix K for the friction constraints at the center of
// the contact manifold
- internalManifold.r1CrossT1 = internalManifold.r1Friction.cross(internalManifold.frictionVector1);
- internalManifold.r1CrossT2 = internalManifold.r1Friction.cross(internalManifold.frictionVector2);
- internalManifold.r2CrossT1 = internalManifold.r2Friction.cross(internalManifold.frictionVector1);
- internalManifold.r2CrossT2 = internalManifold.r2Friction.cross(internalManifold.frictionVector2);
- decimal friction1Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 +
- ((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT1).cross(internalManifold.r1Friction)).dot(
- internalManifold.frictionVector1) +
- ((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT1).cross(internalManifold.r2Friction)).dot(
- internalManifold.frictionVector1);
- decimal friction2Mass = internalManifold.massInverseBody1 + internalManifold.massInverseBody2 +
- ((internalManifold.inverseInertiaTensorBody1 * internalManifold.r1CrossT2).cross(internalManifold.r1Friction)).dot(
- internalManifold.frictionVector2) +
- ((internalManifold.inverseInertiaTensorBody2 * internalManifold.r2CrossT2).cross(internalManifold.r2Friction)).dot(
- internalManifold.frictionVector2);
- decimal frictionTwistMass = internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody1 *
- internalManifold.normal) +
- internalManifold.normal.dot(internalManifold.inverseInertiaTensorBody2 *
- internalManifold.normal);
- internalManifold.inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0);
- internalManifold.inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0);
- internalManifold.inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0);
+ mContactConstraints[mNbContactManifolds].r1CrossT1 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1);
+ mContactConstraints[mNbContactManifolds].r1CrossT2 = mContactConstraints[mNbContactManifolds].r1Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2);
+ mContactConstraints[mNbContactManifolds].r2CrossT1 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector1);
+ mContactConstraints[mNbContactManifolds].r2CrossT2 = mContactConstraints[mNbContactManifolds].r2Friction.cross(mContactConstraints[mNbContactManifolds].frictionVector2);
+ decimal friction1Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
+ ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT1).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot(
+ mContactConstraints[mNbContactManifolds].frictionVector1) +
+ ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT1).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot(
+ mContactConstraints[mNbContactManifolds].frictionVector1);
+ decimal friction2Mass = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
+ ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactConstraints[mNbContactManifolds].r1CrossT2).cross(mContactConstraints[mNbContactManifolds].r1Friction)).dot(
+ mContactConstraints[mNbContactManifolds].frictionVector2) +
+ ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactConstraints[mNbContactManifolds].r2CrossT2).cross(mContactConstraints[mNbContactManifolds].r2Friction)).dot(
+ mContactConstraints[mNbContactManifolds].frictionVector2);
+ decimal frictionTwistMass = mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 *
+ mContactConstraints[mNbContactManifolds].normal) +
+ mContactConstraints[mNbContactManifolds].normal.dot(mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 *
+ mContactConstraints[mNbContactManifolds].normal);
+ mContactConstraints[mNbContactManifolds].inverseFriction1Mass = friction1Mass > decimal(0.0) ? decimal(1.0) / friction1Mass : decimal(0.0);
+ mContactConstraints[mNbContactManifolds].inverseFriction2Mass = friction2Mass > decimal(0.0) ? decimal(1.0) / friction2Mass : decimal(0.0);
+ mContactConstraints[mNbContactManifolds].inverseTwistFrictionMass = frictionTwistMass > decimal(0.0) ? decimal(1.0) / frictionTwistMass : decimal(0.0);
+
+ mNbContactManifolds++;
}
}
@@ -228,41 +267,41 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
/// the solution of the linear system
void ContactSolver::warmStart() {
+ uint contactPointIndex = 0;
+
// For each constraint
for (uint c=0; c<mNbContactManifolds; c++) {
- ContactManifoldSolver& contactManifold = mContactConstraints[c];
-
bool atLeastOneRestingContactPoint = false;
- for (uint i=0; i<contactManifold.nbContacts; i++) {
-
- ContactPointSolver& contactPoint = contactManifold.contacts[i];
+ for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
// If it is not a new contact (this contact was already existing at last time step)
- if (contactPoint.isRestingContact) {
+ if (mContactPoints[contactPointIndex].isRestingContact) {
atLeastOneRestingContactPoint = true;
// --------- Penetration --------- //
// Update the velocities of the body 1 by applying the impulse P
- Vector3 impulsePenetration = contactPoint.normal * contactPoint.penetrationImpulse;
- mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-impulsePenetration);
- mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * contactPoint.penetrationImpulse);
+ Vector3 impulsePenetration = mContactPoints[contactPointIndex].normal * mContactPoints[contactPointIndex].penetrationImpulse;
+ mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-impulsePenetration);
+ mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-mContactPoints[contactPointIndex].r1CrossN * mContactPoints[contactPointIndex].penetrationImpulse);
// Update the velocities of the body 2 by applying the impulse P
- mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * impulsePenetration;
- mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * contactPoint.penetrationImpulse);
+ mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * impulsePenetration;
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * (mContactPoints[contactPointIndex].r2CrossN * mContactPoints[contactPointIndex].penetrationImpulse);
}
else { // If it is a new contact point
// Initialize the accumulated impulses to zero
- contactPoint.penetrationImpulse = 0.0;
- contactPoint.friction1Impulse = 0.0;
- contactPoint.friction2Impulse = 0.0;
- contactPoint.rollingResistanceImpulse = Vector3::zero();
+ mContactPoints[contactPointIndex].penetrationImpulse = 0.0;
+ mContactPoints[contactPointIndex].friction1Impulse = 0.0;
+ mContactPoints[contactPointIndex].friction2Impulse = 0.0;
+ mContactPoints[contactPointIndex].rollingResistanceImpulse = Vector3::zero();
}
+
+ contactPointIndex++;
}
// If we solve the friction constraints at the center of the contact manifold and there is
@@ -271,74 +310,74 @@ void ContactSolver::warmStart() {
// Project the old friction impulses (with old friction vectors) into the new friction
// vectors to get the new friction impulses
- Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * contactManifold.oldFrictionVector1 +
- contactManifold.friction2Impulse * contactManifold.oldFrictionVector2;
- contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1);
- contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector2);
+ Vector3 oldFrictionImpulse = mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1 +
+ mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2;
+ mContactConstraints[c].friction1Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector1);
+ mContactConstraints[c].friction2Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector2);
// ------ First friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
- Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 *
- contactManifold.friction1Impulse;
- Vector3 linearImpulseBody2 = contactManifold.frictionVector1 *
- contactManifold.friction1Impulse;
- Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 *
- contactManifold.friction1Impulse;
+ Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 *
+ mContactConstraints[c].friction1Impulse;
+ Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 *
+ mContactConstraints[c].friction1Impulse;
+ Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 *
+ mContactConstraints[c].friction1Impulse;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
- mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
+ mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
+ mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
- mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
+ mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifold ----- //
// Compute the impulse P = J^T * lambda
- angularImpulseBody1 = -contactManifold.r1CrossT2 * contactManifold.friction2Impulse;
- linearImpulseBody2 = contactManifold.frictionVector2 * contactManifold.friction2Impulse;
- angularImpulseBody2 = contactManifold.r2CrossT2 * contactManifold.friction2Impulse;
+ angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * mContactConstraints[c].friction2Impulse;
+ linearImpulseBody2 = mContactConstraints[c].frictionVector2 * mContactConstraints[c].friction2Impulse;
+ angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
- mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
+ mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
+ mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
- mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
- mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
+ mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
- angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse;
- angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse;
+ angularImpulseBody1 = -mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
+ angularImpulseBody2 = mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
// Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
+ mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
- mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Rolling resistance at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
- angularImpulseBody2 = contactManifold.rollingResistanceImpulse;
+ angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse;
// Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-angularImpulseBody2);
+ mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-angularImpulseBody2);
// Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
}
else { // If it is a new contact manifold
// Initialize the accumulated impulses to zero
- contactManifold.friction1Impulse = 0.0;
- contactManifold.friction2Impulse = 0.0;
- contactManifold.frictionTwistImpulse = 0.0;
- contactManifold.rollingResistanceImpulse = Vector3::zero();
+ mContactConstraints[c].friction1Impulse = 0.0;
+ mContactConstraints[c].friction2Impulse = 0.0;
+ mContactConstraints[c].frictionTwistImpulse = 0.0;
+ mContactConstraints[c].rollingResistanceImpulse.setToZero();
}
}
}
@@ -350,196 +389,195 @@ void ContactSolver::solve() {
decimal deltaLambda;
decimal lambdaTemp;
+ uint contactPointIndex = 0;
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
- ContactManifoldSolver& contactManifold = mContactConstraints[c];
-
decimal sumPenetrationImpulse = 0.0;
// Get the constrained velocities
- const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
- const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
- const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
- const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
+ const Vector3& v1 = mLinearVelocities[mContactConstraints[c].indexBody1];
+ const Vector3& w1 = mAngularVelocities[mContactConstraints[c].indexBody1];
+ const Vector3& v2 = mLinearVelocities[mContactConstraints[c].indexBody2];
+ const Vector3& w2 = mAngularVelocities[mContactConstraints[c].indexBody2];
- for (uint i=0; i<contactManifold.nbContacts; i++) {
-
- ContactPointSolver& contactPoint = contactManifold.contacts[i];
+ for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
// --------- Penetration --------- //
// Compute J*v
- Vector3 deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1);
- decimal deltaVDotN = deltaV.dot(contactPoint.normal);
+ Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1);
+ decimal deltaVDotN = deltaV.dot(mContactPoints[contactPointIndex].normal);
decimal Jv = deltaVDotN;
// Compute the bias "b" of the constraint
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
decimal biasPenetrationDepth = 0.0;
- if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
- max(0.0f, float(contactPoint.penetrationDepth - SLOP));
- decimal b = biasPenetrationDepth + contactPoint.restitutionBias;
+ if (mContactPoints[contactPointIndex].penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) *
+ max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP));
+ decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias;
// Compute the Lagrange multiplier lambda
if (mIsSplitImpulseActive) {
- deltaLambda = - (Jv + contactPoint.restitutionBias) *
- contactPoint.inversePenetrationMass;
+ deltaLambda = - (Jv + mContactPoints[contactPointIndex].restitutionBias) *
+ mContactPoints[contactPointIndex].inversePenetrationMass;
}
else {
- deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass;
+ deltaLambda = - (Jv + b) * mContactPoints[contactPointIndex].inversePenetrationMass;
}
- lambdaTemp = contactPoint.penetrationImpulse;
- contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse +
+ lambdaTemp = mContactPoints[contactPointIndex].penetrationImpulse;
+ mContactPoints[contactPointIndex].penetrationImpulse = std::max(mContactPoints[contactPointIndex].penetrationImpulse +
deltaLambda, decimal(0.0));
- deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
+ deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp;
- Vector3 linearImpulse = contactPoint.normal * deltaLambda;
+ Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse);
- mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-contactPoint.r1CrossN * deltaLambda);
+ mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse);
+ mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-mContactPoints[contactPointIndex].r1CrossN * deltaLambda);
// Update the velocities of the body 2 by applying the impulse P
- mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse;
- mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * (contactPoint.r2CrossN * deltaLambda);
+ mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * (mContactPoints[contactPointIndex].r2CrossN * deltaLambda);
- sumPenetrationImpulse += contactPoint.penetrationImpulse;
+ sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
// If the split impulse position correction is active
if (mIsSplitImpulseActive) {
// Split impulse (position correction)
- const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1];
- const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1];
- const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2];
- const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2];
- Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) -
- v1Split - w1Split.cross(contactPoint.r1);
- decimal JvSplit = deltaVSplit.dot(contactPoint.normal);
+ const Vector3& v1Split = mSplitLinearVelocities[mContactConstraints[c].indexBody1];
+ const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1];
+ const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2];
+ const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2];
+ Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) -
+ v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
+ decimal JvSplit = deltaVSplit.dot(mContactPoints[contactPointIndex].normal);
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
- contactPoint.inversePenetrationMass;
- decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse;
- contactPoint.penetrationSplitImpulse = std::max(
- contactPoint.penetrationSplitImpulse +
+ mContactPoints[contactPointIndex].inversePenetrationMass;
+ decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse;
+ mContactPoints[contactPointIndex].penetrationSplitImpulse = std::max(
+ mContactPoints[contactPointIndex].penetrationSplitImpulse +
deltaLambdaSplit, decimal(0.0));
- deltaLambdaSplit = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
+ deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit;
- Vector3 linearImpulse = contactPoint.normal * deltaLambdaSplit;
+ Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit;
// Update the velocities of the body 1 by applying the impulse P
- mSplitLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulse);
- mSplitAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 *
- (-contactPoint.r1CrossN * deltaLambdaSplit);
+ mSplitLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse);
+ mSplitAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 *
+ (-mContactPoints[contactPointIndex].r1CrossN * deltaLambdaSplit);
// Update the velocities of the body 1 by applying the impulse P
- mSplitLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulse;
- mSplitAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 *
- contactPoint.r2CrossN * deltaLambdaSplit;
+ mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
+ mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 *
+ mContactPoints[contactPointIndex].r2CrossN * deltaLambdaSplit;
}
+
+ contactPointIndex++;
}
// ------ First friction constraint at the center of the contact manifol ------ //
// Compute J*v
- Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction)
- - v1 - w1.cross(contactManifold.r1Friction);
- decimal Jv = deltaV.dot(contactManifold.frictionVector1);
+ Vector3 deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction)
+ - v1 - w1.cross(mContactConstraints[c].r1Friction);
+ decimal Jv = deltaV.dot(mContactConstraints[c].frictionVector1);
// Compute the Lagrange multiplier lambda
- decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
- decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
- lambdaTemp = contactManifold.friction1Impulse;
- contactManifold.friction1Impulse = std::max(-frictionLimit,
- std::min(contactManifold.friction1Impulse +
+ decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass;
+ decimal frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
+ lambdaTemp = mContactConstraints[c].friction1Impulse;
+ mContactConstraints[c].friction1Impulse = std::max(-frictionLimit,
+ std::min(mContactConstraints[c].friction1Impulse +
deltaLambda, frictionLimit));
- deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
+ deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
- Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda;
- Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda;
- Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda;
+ Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * deltaLambda;
+ Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * deltaLambda;
+ Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
- mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
+ mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
+ mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
- mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
- mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
+ mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifol ----- //
// Compute J*v
- deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction);
- Jv = deltaV.dot(contactManifold.frictionVector2);
+ deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
+ Jv = deltaV.dot(mContactConstraints[c].frictionVector2);
// Compute the Lagrange multiplier lambda
- deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
- frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
- lambdaTemp = contactManifold.friction2Impulse;
- contactManifold.friction2Impulse = std::max(-frictionLimit,
- std::min(contactManifold.friction2Impulse +
+ deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass;
+ frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
+ lambdaTemp = mContactConstraints[c].friction2Impulse;
+ mContactConstraints[c].friction2Impulse = std::max(-frictionLimit,
+ std::min(mContactConstraints[c].friction2Impulse +
deltaLambda, frictionLimit));
- deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
+ deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
- angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda;
- linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda;
- angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda;
+ angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * deltaLambda;
+ linearImpulseBody2 = mContactConstraints[c].frictionVector2 * deltaLambda;
+ angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * (-linearImpulseBody2);
- mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * angularImpulseBody1;
+ mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
+ mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
- mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * linearImpulseBody2;
- mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
+ mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifol ------ //
// Compute J*v
deltaV = w2 - w1;
- Jv = deltaV.dot(contactManifold.normal);
+ Jv = deltaV.dot(mContactConstraints[c].normal);
- deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
- frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
- lambdaTemp = contactManifold.frictionTwistImpulse;
- contactManifold.frictionTwistImpulse = std::max(-frictionLimit,
- std::min(contactManifold.frictionTwistImpulse
+ deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass);
+ frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
+ lambdaTemp = mContactConstraints[c].frictionTwistImpulse;
+ mContactConstraints[c].frictionTwistImpulse = std::max(-frictionLimit,
+ std::min(mContactConstraints[c].frictionTwistImpulse
+ deltaLambda, frictionLimit));
- deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
+ deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
- angularImpulseBody2 = contactManifold.normal * deltaLambda;
+ angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-angularImpulseBody2);
+ mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-angularImpulseBody2);
// Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * angularImpulseBody2;
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// --------- Rolling resistance constraint at the center of the contact manifold --------- //
- if (contactManifold.rollingResistanceFactor > 0) {
+ if (mContactConstraints[c].rollingResistanceFactor > 0) {
// Compute J*v
const Vector3 JvRolling = w2 - w1;
// Compute the Lagrange multiplier lambda
- Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling);
- decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
- Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse;
- contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse +
+ Vector3 deltaLambdaRolling = mContactConstraints[c].inverseRollingResistance * (-JvRolling);
+ decimal rollingLimit = mContactConstraints[c].rollingResistanceFactor * sumPenetrationImpulse;
+ Vector3 lambdaTempRolling = mContactConstraints[c].rollingResistanceImpulse;
+ mContactConstraints[c].rollingResistanceImpulse = clamp(mContactConstraints[c].rollingResistanceImpulse +
deltaLambdaRolling, rollingLimit);
- deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling;
+ deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling;
// Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * (-deltaLambdaRolling);
+ mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-deltaLambdaRolling);
// Update the velocities of the body 2 by applying the impulse P
- mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * deltaLambdaRolling;
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
}
}
}
@@ -548,30 +586,30 @@ void ContactSolver::solve() {
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
+ uint contactPointIndex = 0;
+
// For each contact manifold
for (uint c=0; c<mNbContactManifolds; c++) {
- ContactManifoldSolver& manifold = mContactConstraints[c];
+ for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
- for (uint i=0; i<manifold.nbContacts; i++) {
+ mContactPoints[contactPointIndex].externalContact->setPenetrationImpulse(mContactPoints[contactPointIndex].penetrationImpulse);
+ mContactPoints[contactPointIndex].externalContact->setFrictionImpulse1(mContactPoints[contactPointIndex].friction1Impulse);
+ mContactPoints[contactPointIndex].externalContact->setFrictionImpulse2(mContactPoints[contactPointIndex].friction2Impulse);
+ mContactPoints[contactPointIndex].externalContact->setRollingResistanceImpulse(mContactPoints[contactPointIndex].rollingResistanceImpulse);
- ContactPointSolver& contactPoint = manifold.contacts[i];
+ mContactPoints[contactPointIndex].externalContact->setFrictionVector1(mContactPoints[contactPointIndex].frictionVector1);
+ mContactPoints[contactPointIndex].externalContact->setFrictionVector2(mContactPoints[contactPointIndex].frictionVector2);
- contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse);
- contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse);
- contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse);
- contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse);
-
- contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1);
- contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2);
+ contactPointIndex++;
}
- manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse);
- manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse);
- manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse);
- manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse);
- manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1);
- manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2);
+ mContactConstraints[c].externalContactManifold->setFrictionImpulse1(mContactConstraints[c].friction1Impulse);
+ mContactConstraints[c].externalContactManifold->setFrictionImpulse2(mContactConstraints[c].friction2Impulse);
+ mContactConstraints[c].externalContactManifold->setFrictionTwistImpulse(mContactConstraints[c].frictionTwistImpulse);
+ mContactConstraints[c].externalContactManifold->setRollingResistanceImpulse(mContactConstraints[c].rollingResistanceImpulse);
+ mContactConstraints[c].externalContactManifold->setFrictionVector1(mContactConstraints[c].frictionVector1);
+ mContactConstraints[c].externalContactManifold->setFrictionVector2(mContactConstraints[c].frictionVector2);
}
}
@@ -634,12 +672,3 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
// friction vector and the contact normal
contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit();
}
-
-// Clean up the constraint solver
-void ContactSolver::cleanup() {
-
- if (mContactConstraints != nullptr) {
- delete[] mContactConstraints;
- mContactConstraints = nullptr;
- }
-}
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index 02a3c390..e4e2710e 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -220,11 +220,8 @@ class ContactSolver {
/// Inverse inertia tensor of body 2
Matrix3x3 inverseInertiaTensorBody2;
- /// Contact point constraints
- ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];
-
/// Number of contact points
- uint nbContacts;
+ short int nbContacts;
/// True if the body 1 is of type dynamic
bool isBody1DynamicType;
@@ -335,6 +332,12 @@ class ContactSolver {
/// Contact constraints
ContactManifoldSolver* mContactConstraints;
+ /// Contact points
+ ContactPointSolver* mContactPoints;
+
+ /// Number of contact point constraints
+ uint mNbContactPoints;
+
/// Number of contact constraints
uint mNbContactManifolds;
@@ -378,6 +381,9 @@ class ContactSolver {
void computeFrictionVectors(const Vector3& deltaVelocity,
ContactManifoldSolver& contactPoint) const;
+ /// Warm start the solver.
+ void warmStart();
+
public:
// -------------------- Methods -------------------- //
@@ -389,8 +395,11 @@ class ContactSolver {
/// Destructor
~ContactSolver() = default;
+ /// Initialize the contact constraints
+ void init(Island** islands, uint nbIslands, decimal timeStep);
+
/// Initialize the constraint solver for a given island
- void initializeForIsland(decimal dt, Island* island);
+ void initializeForIsland(Island* island);
/// Set the split velocities arrays
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
@@ -400,9 +409,6 @@ class ContactSolver {
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities);
- /// Warm start the solver.
- void warmStart();
-
/// Store the computed impulses to use them to
/// warm start the solver at the next iteration
void storeImpulses();
@@ -415,9 +421,6 @@ class ContactSolver {
/// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive);
-
- /// Clean up the constraint solver
- void cleanup();
};
// Set the split velocities arrays
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index c40d5ec5..d52348f8 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -342,23 +342,28 @@ void DynamicsWorld::solveContactsAndConstraints() {
// ---------- Solve velocity constraints for joints and contacts ---------- //
+ // Initialize the contact solver
+ mContactSolver.init(mIslands, mNbIslands, mTimeStep);
+
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Check if there are contacts and constraints to solve
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
- bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
- if (!isConstraintsToSolve && !isContactsToSolve) continue;
+ //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
+ //if (!isConstraintsToSolve && !isContactsToSolve) continue;
// If there are contacts in the current island
- if (isContactsToSolve) {
+// if (isContactsToSolve) {
- // Initialize the solver
- mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
+// // Initialize the solver
+// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
- // Warm start the contact solver
- mContactSolver.warmStart();
- }
+// // Warm start the contact solver
+// if (mContactSolver.IsWarmStartingActive()) {
+// mContactSolver.warmStart();
+// }
+// }
// If there are constraints
if (isConstraintsToSolve) {
@@ -366,26 +371,32 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Initialize the constraint solver
mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
}
+ }
- // For each iteration of the velocity solver
- for (uint i=0; i<mNbVelocitySolverIterations; i++) {
+ // For each iteration of the velocity solver
+ for (uint i=0; i<mNbVelocitySolverIterations; i++) {
+ for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Solve the constraints
+ bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
if (isConstraintsToSolve) {
mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
}
-
- // Solve the contacts
- if (isContactsToSolve) mContactSolver.solve();
}
- // Cache the lambda values in order to use them in the next
- // step and cleanup the contact solver
- if (isContactsToSolve) {
- mContactSolver.storeImpulses();
- mContactSolver.cleanup();
- }
+ mContactSolver.solve();
+
+ // Solve the contacts
+// if (isContactsToSolve) {
+
+// mContactSolver.resetTotalPenetrationImpulse();
+
+// mContactSolver.solvePenetrationConstraints();
+// mContactSolver.solveFrictionConstraints();
+// }
}
+
+ mContactSolver.storeImpulses();
}
// Solve the position error correction of the constraints
From 81426293e02d441f0bbefde7712955ceb8c93003 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 16 Oct 2016 23:18:42 +0200
Subject: [PATCH 016/133] Remove unused variables in contact solver
---
src/constraint/ContactPoint.cpp | 5 +-
src/constraint/ContactPoint.h | 92 ---------------------------------
src/engine/ContactSolver.cpp | 58 +++------------------
src/engine/ContactSolver.h | 42 ++-------------
4 files changed, 11 insertions(+), 186 deletions(-)
diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp
index 3be8476a..c0d567ea 100644
--- a/src/constraint/ContactPoint.cpp
+++ b/src/constraint/ContactPoint.cpp
@@ -45,9 +45,6 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
contactInfo.localPoint2),
mIsRestingContact(false) {
- mFrictionVectors[0] = Vector3(0, 0, 0);
- mFrictionVectors[1] = Vector3(0, 0, 0);
-
- assert(mPenetrationDepth > 0.0);
+ assert(mPenetrationDepth > decimal(0.0));
}
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index d2e8f189..18be17b0 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -128,21 +128,9 @@ class ContactPoint {
/// True if the contact is a resting contact (exists for more than one time step)
bool mIsRestingContact;
- /// Two orthogonal vectors that span the tangential friction plane
- Vector3 mFrictionVectors[2];
-
/// Cached penetration impulse
decimal mPenetrationImpulse;
- /// Cached first friction impulse
- decimal mFrictionImpulse1;
-
- /// Cached second friction impulse
- decimal mFrictionImpulse2;
-
- /// Cached rolling resistance impulse
- Vector3 mRollingResistanceImpulse;
-
public :
// -------------------- Methods -------------------- //
@@ -186,27 +174,9 @@ class ContactPoint {
/// Return the cached penetration impulse
decimal getPenetrationImpulse() const;
- /// Return the cached first friction impulse
- decimal getFrictionImpulse1() const;
-
- /// Return the cached second friction impulse
- decimal getFrictionImpulse2() const;
-
- /// Return the cached rolling resistance impulse
- Vector3 getRollingResistanceImpulse() const;
-
/// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse);
- /// Set the first cached friction impulse
- void setFrictionImpulse1(decimal impulse);
-
- /// Set the second cached friction impulse
- void setFrictionImpulse2(decimal impulse);
-
- /// Set the cached rolling resistance impulse
- void setRollingResistanceImpulse(const Vector3& impulse);
-
/// Set the contact world point on body 1
void setWorldPointOnBody1(const Vector3& worldPoint);
@@ -219,18 +189,6 @@ class ContactPoint {
/// Set the mIsRestingContact variable
void setIsRestingContact(bool isRestingContact);
- /// Get the first friction vector
- Vector3 getFrictionVector1() const;
-
- /// Set the first friction vector
- void setFrictionVector1(const Vector3& frictionVector1);
-
- /// Get the second friction vector
- Vector3 getFrictionVector2() const;
-
- /// Set the second friction vector
- void setFrictionVector2(const Vector3& frictionVector2);
-
/// Return the penetration depth
decimal getPenetrationDepth() const;
@@ -283,41 +241,11 @@ inline decimal ContactPoint::getPenetrationImpulse() const {
return mPenetrationImpulse;
}
-// Return the cached first friction impulse
-inline decimal ContactPoint::getFrictionImpulse1() const {
- return mFrictionImpulse1;
-}
-
-// Return the cached second friction impulse
-inline decimal ContactPoint::getFrictionImpulse2() const {
- return mFrictionImpulse2;
-}
-
-// Return the cached rolling resistance impulse
-inline Vector3 ContactPoint::getRollingResistanceImpulse() const {
- return mRollingResistanceImpulse;
-}
-
// Set the cached penetration impulse
inline void ContactPoint::setPenetrationImpulse(decimal impulse) {
mPenetrationImpulse = impulse;
}
-// Set the first cached friction impulse
-inline void ContactPoint::setFrictionImpulse1(decimal impulse) {
- mFrictionImpulse1 = impulse;
-}
-
-// Set the second cached friction impulse
-inline void ContactPoint::setFrictionImpulse2(decimal impulse) {
- mFrictionImpulse2 = impulse;
-}
-
-// Set the cached rolling resistance impulse
-inline void ContactPoint::setRollingResistanceImpulse(const Vector3& impulse) {
- mRollingResistanceImpulse = impulse;
-}
-
// Set the contact world point on body 1
inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
mWorldPointOnBody1 = worldPoint;
@@ -338,26 +266,6 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
mIsRestingContact = isRestingContact;
}
-// Get the first friction vector
-inline Vector3 ContactPoint::getFrictionVector1() const {
- return mFrictionVectors[0];
-}
-
-// Set the first friction vector
-inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) {
- mFrictionVectors[0] = frictionVector1;
-}
-
-// Get the second friction vector
-inline Vector3 ContactPoint::getFrictionVector2() const {
- return mFrictionVectors[1];
-}
-
-// Set the second friction vector
-inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) {
- mFrictionVectors[1] = frictionVector2;
-}
-
// Return the penetration depth of the contact
inline decimal ContactPoint::getPenetrationDepth() const {
return mPenetrationDepth;
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index b4e28aaa..34e86472 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -134,15 +134,12 @@ void ContactSolver::initializeForIsland(Island* island) {
mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;
mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse;
mContactConstraints[mNbContactManifolds].nbContacts = externalManifold->getNbContactPoints();
- mContactConstraints[mNbContactManifolds].restitutionFactor = computeMixedRestitutionFactor(body1, body2);
mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
mContactConstraints[mNbContactManifolds].externalContactManifold = externalManifold;
- mContactConstraints[mNbContactManifolds].isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
- mContactConstraints[mNbContactManifolds].isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
mContactConstraints[mNbContactManifolds].normal.setToZero();
- mContactConstraints[mNbContactManifolds].frictionPointBody1 = Vector3::zero();
- mContactConstraints[mNbContactManifolds].frictionPointBody2 = Vector3::zero();
+ mContactConstraints[mNbContactManifolds].frictionPointBody1.setToZero();
+ mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero();
// Get the velocities of the bodies
const Vector3& v1 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody1];
@@ -168,13 +165,8 @@ void ContactSolver::initializeForIsland(Island* island) {
mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth();
mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact();
externalContact->setIsRestingContact(true);
- mContactPoints[mNbContactPoints].oldFrictionVector1 = externalContact->getFrictionVector1();
- mContactPoints[mNbContactPoints].oldFrictionVector2 = externalContact->getFrictionVector2();
mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse();
mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0;
- mContactPoints[mNbContactPoints].friction1Impulse = externalContact->getFrictionImpulse1();
- mContactPoints[mNbContactPoints].friction2Impulse = externalContact->getFrictionImpulse2();
- mContactPoints[mNbContactPoints].rollingResistanceImpulse = externalContact->getRollingResistanceImpulse();
mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1;
mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2;
@@ -197,8 +189,9 @@ void ContactSolver::initializeForIsland(Island* island) {
// velocity bellow a given threshold), we do not add a restitution velocity bias
mContactPoints[mNbContactPoints].restitutionBias = 0.0;
decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal);
+ const decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2);
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
- mContactPoints[mNbContactPoints].restitutionBias = mContactConstraints[mNbContactManifolds].restitutionFactor * deltaVDotN;
+ mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN;
}
mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal;
@@ -219,8 +212,10 @@ void ContactSolver::initializeForIsland(Island* island) {
mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
// Compute the inverse K matrix for the rolling resistance constraint
+ bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
+ bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
- if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (mContactConstraints[mNbContactManifolds].isBody1DynamicType || mContactConstraints[mNbContactManifolds].isBody2DynamicType)) {
+ if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) {
mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2;
mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse();
}
@@ -296,9 +291,6 @@ void ContactSolver::warmStart() {
// Initialize the accumulated impulses to zero
mContactPoints[contactPointIndex].penetrationImpulse = 0.0;
- mContactPoints[contactPointIndex].friction1Impulse = 0.0;
- mContactPoints[contactPointIndex].friction2Impulse = 0.0;
- mContactPoints[contactPointIndex].rollingResistanceImpulse = Vector3::zero();
}
contactPointIndex++;
@@ -594,12 +586,6 @@ void ContactSolver::storeImpulses() {
for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
mContactPoints[contactPointIndex].externalContact->setPenetrationImpulse(mContactPoints[contactPointIndex].penetrationImpulse);
- mContactPoints[contactPointIndex].externalContact->setFrictionImpulse1(mContactPoints[contactPointIndex].friction1Impulse);
- mContactPoints[contactPointIndex].externalContact->setFrictionImpulse2(mContactPoints[contactPointIndex].friction2Impulse);
- mContactPoints[contactPointIndex].externalContact->setRollingResistanceImpulse(mContactPoints[contactPointIndex].rollingResistanceImpulse);
-
- mContactPoints[contactPointIndex].externalContact->setFrictionVector1(mContactPoints[contactPointIndex].frictionVector1);
- mContactPoints[contactPointIndex].externalContact->setFrictionVector2(mContactPoints[contactPointIndex].frictionVector2);
contactPointIndex++;
}
@@ -613,36 +599,6 @@ void ContactSolver::storeImpulses() {
}
}
-// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
-// for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal.
-void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
- ContactPointSolver& contactPoint) const {
-
- assert(contactPoint.normal.length() > 0.0);
-
- // Compute the velocity difference vector in the tangential plane
- Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal;
- Vector3 tangentVelocity = deltaVelocity - normalVelocity;
-
- // If the velocty difference in the tangential plane is not zero
- decimal lengthTangenVelocity = tangentVelocity.length();
- if (lengthTangenVelocity > MACHINE_EPSILON) {
-
- // Compute the first friction vector in the direction of the tangent
- // velocity difference
- contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity;
- }
- else {
-
- // Get any orthogonal vector to the normal as the first friction vector
- contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector();
- }
-
- // The second friction vector is computed by the cross product of the firs
- // friction vector and the contact normal
- contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit();
-}
-
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
// for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index e4e2710e..becd5529 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -122,33 +122,12 @@ class ContactSolver {
/// Accumulated normal impulse
decimal penetrationImpulse;
- /// Accumulated impulse in the 1st friction direction
- decimal friction1Impulse;
-
- /// Accumulated impulse in the 2nd friction direction
- decimal friction2Impulse;
-
/// Accumulated split impulse for penetration correction
decimal penetrationSplitImpulse;
- /// Accumulated rolling resistance impulse
- Vector3 rollingResistanceImpulse;
-
/// Normal vector of the contact
Vector3 normal;
- /// First friction vector in the tangent plane
- Vector3 frictionVector1;
-
- /// Second friction vector in the tangent plane
- Vector3 frictionVector2;
-
- /// Old first friction vector in the tangent plane
- Vector3 oldFrictionVector1;
-
- /// Old second friction vector in the tangent plane
- Vector3 oldFrictionVector2;
-
/// Vector from the body 1 center to the contact point
Vector3 r1;
@@ -188,11 +167,11 @@ class ContactSolver {
/// Inverse of the matrix K for the 2nd friction
decimal inverseFriction2Mass;
- /// True if the contact was existing last time step
- bool isRestingContact;
-
/// Pointer to the external contact
ContactPoint* externalContact;
+
+ /// True if the contact was existing last time step
+ bool isRestingContact;
};
// Structure ContactManifoldSolver
@@ -223,15 +202,6 @@ class ContactSolver {
/// Number of contact points
short int nbContacts;
- /// True if the body 1 is of type dynamic
- bool isBody1DynamicType;
-
- /// True if the body 2 is of type dynamic
- bool isBody2DynamicType;
-
- /// Mix of the restitution factor for two bodies
- decimal restitutionFactor;
-
/// Mix friction coefficient for the two bodies
decimal frictionCoefficient;
@@ -369,12 +339,6 @@ class ContactSolver {
/// Compute th mixed rolling resistance factor between two bodies
decimal computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) const;
- /// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
- /// plane for a contact point. The two vectors have to be
- /// such that : t1 x t2 = contactNormal.
- void computeFrictionVectors(const Vector3& deltaVelocity,
- ContactPointSolver &contactPoint) const;
-
/// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
/// plane for a contact manifold. The two vectors have to be
/// such that : t1 x t2 = contactNormal.
From 14bfb0aca4160d8323760ccb8d77f0bbc37fc0a3 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 17 Oct 2016 22:41:33 +0200
Subject: [PATCH 017/133] Some optimizations in contact solver
---
src/collision/ContactManifold.h | 6 ++--
src/engine/ContactSolver.h | 58 ++++++++++++---------------------
2 files changed, 23 insertions(+), 41 deletions(-)
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index 2f77e9b2..c36c1476 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -102,7 +102,7 @@ class ContactManifold {
short int mNormalDirectionId;
/// Number of contacts in the cache
- uint mNbContactPoints;
+ int8 mNbContactPoints;
/// First friction vector of the contact manifold
Vector3 mFrictionVector1;
@@ -187,7 +187,7 @@ class ContactManifold {
void clear();
/// Return the number of contact points in the manifold
- uint getNbContactPoints() const;
+ int8 getNbContactPoints() const;
/// Return the first friction vector at the center of the contact manifold
const Vector3& getFrictionVector1() const;
@@ -264,7 +264,7 @@ inline short int ContactManifold::getNormalDirectionId() const {
}
// Return the number of contact points in the manifold
-inline uint ContactManifold::getNbContactPoints() const {
+inline int8 ContactManifold::getNbContactPoints() const {
return mNbContactPoints;
}
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index becd5529..089d8b53 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -119,11 +119,8 @@ class ContactSolver {
*/
struct ContactPointSolver {
- /// Accumulated normal impulse
- decimal penetrationImpulse;
-
- /// Accumulated split impulse for penetration correction
- decimal penetrationSplitImpulse;
+ /// Pointer to the external contact
+ ContactPoint* externalContact;
/// Normal vector of the contact
Vector3 normal;
@@ -134,41 +131,26 @@ class ContactSolver {
/// Vector from the body 2 center to the contact point
Vector3 r2;
- /// Cross product of r1 with 1st friction vector
- Vector3 r1CrossT1;
-
- /// Cross product of r1 with 2nd friction vector
- Vector3 r1CrossT2;
-
- /// Cross product of r2 with 1st friction vector
- Vector3 r2CrossT1;
-
- /// Cross product of r2 with 2nd friction vector
- Vector3 r2CrossT2;
-
- /// Cross product of r1 with the contact normal
- Vector3 r1CrossN;
-
- /// Cross product of r2 with the contact normal
- Vector3 r2CrossN;
-
/// Penetration depth
decimal penetrationDepth;
/// Velocity restitution bias
decimal restitutionBias;
+ /// Accumulated normal impulse
+ decimal penetrationImpulse;
+
+ /// Accumulated split impulse for penetration correction
+ decimal penetrationSplitImpulse;
+
/// Inverse of the matrix K for the penenetration
decimal inversePenetrationMass;
- /// Inverse of the matrix K for the 1st friction
- decimal inverseFriction1Mass;
+ /// Cross product of r1 with the contact normal
+ Vector3 r1CrossN;
- /// Inverse of the matrix K for the 2nd friction
- decimal inverseFriction2Mass;
-
- /// Pointer to the external contact
- ContactPoint* externalContact;
+ /// Cross product of r2 with the contact normal
+ Vector3 r2CrossN;
/// True if the contact was existing last time step
bool isRestingContact;
@@ -181,11 +163,14 @@ class ContactSolver {
*/
struct ContactManifoldSolver {
+ /// Pointer to the external contact manifold
+ ContactManifold* externalContactManifold;
+
/// Index of body 1 in the constraint solver
- uint indexBody1;
+ int32 indexBody1;
/// Index of body 2 in the constraint solver
- uint indexBody2;
+ int32 indexBody2;
/// Inverse of the mass of body 1
decimal massInverseBody1;
@@ -199,18 +184,12 @@ class ContactSolver {
/// Inverse inertia tensor of body 2
Matrix3x3 inverseInertiaTensorBody2;
- /// Number of contact points
- short int nbContacts;
-
/// Mix friction coefficient for the two bodies
decimal frictionCoefficient;
/// Rolling resistance factor between the two bodies
decimal rollingResistanceFactor;
- /// Pointer to the external contact manifold
- ContactManifold* externalContactManifold;
-
// - Variables used when friction constraints are apply at the center of the manifold-//
/// Average normal vector of the contact manifold
@@ -275,6 +254,9 @@ class ContactSolver {
/// Rolling resistance impulse
Vector3 rollingResistanceImpulse;
+
+ /// Number of contact points
+ int8 nbContacts;
};
// -------------------- Constants --------------------- //
From ce06a4b935e15c7fa4376c549572c8153cfcf02e Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 17 Oct 2016 22:41:58 +0200
Subject: [PATCH 018/133] Change fixed size data types
---
src/configuration.h | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/src/configuration.h b/src/configuration.h
index c14783d8..04cb0956 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -30,6 +30,7 @@
#include <limits>
#include <cfloat>
#include <utility>
+#include <cstdint>
#include "decimal.h"
// Windows platform
@@ -51,10 +52,9 @@ using luint = long unsigned int;
using bodyindex = luint;
using bodyindexpair = std::pair<bodyindex, bodyindex>;
-using int16 = signed short;
-using int32 = signed int;
-using uint16 = unsigned short;
-using uint32 = unsigned int;
+using int8 = int8_t;
+using int16 = int16_t;
+using int32 = int32_t;
// ------------------- Enumerations ------------------- //
From cc6d3d621d4ef3b976ba091ce953da49f5bd464d Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 20 Oct 2016 19:16:55 +0200
Subject: [PATCH 019/133] Add profiling data
---
src/engine/ContactSolver.cpp | 6 ++++++
1 file changed, 6 insertions(+)
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 34e86472..837fdf38 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -262,6 +262,8 @@ void ContactSolver::initializeForIsland(Island* island) {
/// the solution of the linear system
void ContactSolver::warmStart() {
+ PROFILE("ContactSolver::warmStart()");
+
uint contactPointIndex = 0;
// For each constraint
@@ -578,6 +580,8 @@ void ContactSolver::solve() {
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
+ PROFILE("ContactSolver::storeImpulses()");
+
uint contactPointIndex = 0;
// For each contact manifold
@@ -604,6 +608,8 @@ void ContactSolver::storeImpulses() {
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
ContactManifoldSolver& contact) const {
+ PROFILE("ContactSolver::computeFrictionVectors()");
+
assert(contact.normal.length() > decimal(0.0));
// Compute the velocity difference vector in the tangential plane
From b3d24e4299a6503bcf1635475ae2da7694bfcb42 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 23 Oct 2016 20:04:52 +0200
Subject: [PATCH 020/133] Cache some calculation in contact solver
---
src/engine/ContactSolver.cpp | 45 ++++++++++++++++++------------------
src/engine/ContactSolver.h | 4 ++--
src/mathematics/Vector3.h | 2 +-
3 files changed, 26 insertions(+), 25 deletions(-)
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 837fdf38..5d61243a 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -174,13 +174,16 @@ void ContactSolver::initializeForIsland(Island* island) {
// Compute the velocity difference
Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1);
- mContactPoints[mNbContactPoints].r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
- mContactPoints[mNbContactPoints].r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
+ Vector3 r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
+ Vector3 r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
+
+ mContactPoints[mNbContactPoints].i1TimesR1CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * r1CrossN;
+ mContactPoints[mNbContactPoints].i2TimesR2CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * r2CrossN;
// Compute the inverse mass matrix K for the penetration constraint
decimal massPenetration = mContactConstraints[mNbContactManifolds].massInverseBody1 + mContactConstraints[mNbContactManifolds].massInverseBody2 +
- ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * mContactPoints[mNbContactPoints].r1CrossN).cross(mContactPoints[mNbContactPoints].r1)).dot(mContactPoints[mNbContactPoints].normal) +
- ((mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * mContactPoints[mNbContactPoints].r2CrossN).cross(mContactPoints[mNbContactPoints].r2)).dot(mContactPoints[mNbContactPoints].normal);
+ ((mContactPoints[mNbContactPoints].i1TimesR1CrossN).cross(mContactPoints[mNbContactPoints].r1)).dot(mContactPoints[mNbContactPoints].normal) +
+ ((mContactPoints[mNbContactPoints].i2TimesR2CrossN).cross(mContactPoints[mNbContactPoints].r2)).dot(mContactPoints[mNbContactPoints].normal);
mContactPoints[mNbContactPoints].inversePenetrationMass = massPenetration > decimal(0.0) ? decimal(1.0) / massPenetration : decimal(0.0);
// Compute the restitution velocity bias "b". We compute this here instead
@@ -282,12 +285,12 @@ void ContactSolver::warmStart() {
// Update the velocities of the body 1 by applying the impulse P
Vector3 impulsePenetration = mContactPoints[contactPointIndex].normal * mContactPoints[contactPointIndex].penetrationImpulse;
- mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-impulsePenetration);
- mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-mContactPoints[contactPointIndex].r1CrossN * mContactPoints[contactPointIndex].penetrationImpulse);
+ mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * impulsePenetration;
+ mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * mContactPoints[contactPointIndex].penetrationImpulse;
// Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * impulsePenetration;
- mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * (mContactPoints[contactPointIndex].r2CrossN * mContactPoints[contactPointIndex].penetrationImpulse);
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * mContactPoints[contactPointIndex].penetrationImpulse;
}
else { // If it is a new contact point
@@ -320,7 +323,7 @@ void ContactSolver::warmStart() {
mContactConstraints[c].friction1Impulse;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
+ mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 1 by applying the impulse P
@@ -335,7 +338,7 @@ void ContactSolver::warmStart() {
angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
+ mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
@@ -360,7 +363,7 @@ void ContactSolver::warmStart() {
angularImpulseBody2 = mContactConstraints[c].rollingResistanceImpulse;
// Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-angularImpulseBody2);
+ mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
// Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
@@ -428,12 +431,12 @@ void ContactSolver::solve() {
Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse);
- mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-mContactPoints[contactPointIndex].r1CrossN * deltaLambda);
+ mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse;
+ mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambda;
// Update the velocities of the body 2 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
- mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * (mContactPoints[contactPointIndex].r2CrossN * deltaLambda);
+ mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambda;
sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
@@ -459,14 +462,12 @@ void ContactSolver::solve() {
Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit;
// Update the velocities of the body 1 by applying the impulse P
- mSplitLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulse);
- mSplitAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 *
- (-mContactPoints[contactPointIndex].r1CrossN * deltaLambdaSplit);
+ mSplitLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse;
+ mSplitAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambdaSplit;
// Update the velocities of the body 1 by applying the impulse P
mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
- mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 *
- mContactPoints[contactPointIndex].r2CrossN * deltaLambdaSplit;
+ mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambdaSplit;
}
contactPointIndex++;
@@ -494,7 +495,7 @@ void ContactSolver::solve() {
Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
+ mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
@@ -522,7 +523,7 @@ void ContactSolver::solve() {
angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].massInverseBody1 * (-linearImpulseBody2);
+ mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
@@ -547,7 +548,7 @@ void ContactSolver::solve() {
angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-angularImpulseBody2);
+ mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
// Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
@@ -568,7 +569,7 @@ void ContactSolver::solve() {
deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling;
// Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * (-deltaLambdaRolling);
+ mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
// Update the velocities of the body 2 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index 089d8b53..40e0721d 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -147,10 +147,10 @@ class ContactSolver {
decimal inversePenetrationMass;
/// Cross product of r1 with the contact normal
- Vector3 r1CrossN;
+ Vector3 i1TimesR1CrossN;
/// Cross product of r2 with the contact normal
- Vector3 r2CrossN;
+ Vector3 i2TimesR2CrossN;
/// True if the contact was existing last time step
bool isRestingContact;
diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h
index 4f30391d..1ca1adfd 100644
--- a/src/mathematics/Vector3.h
+++ b/src/mathematics/Vector3.h
@@ -184,7 +184,7 @@ inline void Vector3::setAllValues(decimal newX, decimal newY, decimal newZ) {
// Return the length of the vector
inline decimal Vector3::length() const {
- return sqrt(x*x + y*y + z*z);
+ return std::sqrt(x*x + y*y + z*z);
}
// Return the square of the length of the vector
From 16d27f40b9369737fbb8bf3b0893f1e6d10a9433 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 3 Nov 2016 18:06:45 +0100
Subject: [PATCH 021/133] Remove bodies pointer from ContactPoint
---
src/collision/CollisionDetection.cpp | 2 ++
src/constraint/ContactPoint.cpp | 3 +--
src/constraint/ContactPoint.h | 22 ----------------------
src/engine/ContactSolver.cpp | 4 ++--
4 files changed, 5 insertions(+), 26 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index c3caa97a..2fada228 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -433,6 +433,8 @@ void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const C
void CollisionDetection::createContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo) {
+ PROFILE("CollisionDetection::createContact()");
+
// Create a new contact
ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(contactInfo);
diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp
index c0d567ea..7756d68b 100644
--- a/src/constraint/ContactPoint.cpp
+++ b/src/constraint/ContactPoint.cpp
@@ -32,8 +32,7 @@ using namespace std;
// Constructor
ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
- : mBody1(contactInfo.shape1->getBody()), mBody2(contactInfo.shape2->getBody()),
- mNormal(contactInfo.normal),
+ : mNormal(contactInfo.normal),
mPenetrationDepth(contactInfo.penetrationDepth),
mLocalPointOnBody1(contactInfo.localPoint1),
mLocalPointOnBody2(contactInfo.localPoint2),
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index 18be17b0..c15dbafa 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -101,12 +101,6 @@ class ContactPoint {
// -------------------- Attributes -------------------- //
- /// First rigid body of the contact
- CollisionBody* mBody1;
-
- /// Second rigid body of the contact
- CollisionBody* mBody2;
-
/// Normalized normal vector of the contact (from body1 toward body2) in world space
const Vector3 mNormal;
@@ -147,12 +141,6 @@ class ContactPoint {
/// Deleted assignment operator
ContactPoint& operator=(const ContactPoint& contact) = delete;
- /// Return the reference to the body 1
- CollisionBody* getBody1() const;
-
- /// Return the reference to the body 2
- CollisionBody* getBody2() const;
-
/// Return the normal vector of the contact
Vector3 getNormal() const;
@@ -196,16 +184,6 @@ class ContactPoint {
size_t getSizeInBytes() const;
};
-// Return the reference to the body 1
-inline CollisionBody* ContactPoint::getBody1() const {
- return mBody1;
-}
-
-// Return the reference to the body 2
-inline CollisionBody* ContactPoint::getBody2() const {
- return mBody2;
-}
-
// Return the normal vector of the contact
inline Vector3 ContactPoint::getNormal() const {
return mNormal;
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 5d61243a..d5825d5c 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -115,8 +115,8 @@ void ContactSolver::initializeForIsland(Island* island) {
assert(externalManifold->getNbContactPoints() > 0);
// Get the two bodies of the contact
- RigidBody* body1 = static_cast<RigidBody*>(externalManifold->getContactPoint(0)->getBody1());
- RigidBody* body2 = static_cast<RigidBody*>(externalManifold->getContactPoint(0)->getBody2());
+ RigidBody* body1 = static_cast<RigidBody*>(externalManifold->getBody1());
+ RigidBody* body2 = static_cast<RigidBody*>(externalManifold->getBody2());
assert(body1 != nullptr);
assert(body2 != nullptr);
From f82777bd3b50021dc77b5cb9bfe9398b4a795e2c Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 5 Nov 2016 19:20:54 +0100
Subject: [PATCH 022/133] Refactor some methods in ContactPoint
---
src/collision/CollisionDetection.cpp | 3 ++
src/collision/ContactManifold.cpp | 9 ++----
src/constraint/ContactPoint.cpp | 6 ----
src/constraint/ContactPoint.h | 43 ++++++++++++----------------
4 files changed, 25 insertions(+), 36 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 2fada228..e2599fca 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -439,6 +439,9 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair,
ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(contactInfo);
+ contact->updateWorldContactPoints(overlappingPair->getShape1()->getLocalToWorldTransform(),
+ overlappingPair->getShape2()->getLocalToWorldTransform());
+
// Add the contact to the contact manifold set of the corresponding overlapping pair
overlappingPair->addContact(contact);
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index ff15bce6..dc40774b 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -110,12 +110,9 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans
// Update the world coordinates and penetration depth of the contact points in the manifold
for (uint i=0; i<mNbContactPoints; i++) {
- mContactPoints[i]->setWorldPointOnBody1(transform1 *
- mContactPoints[i]->getLocalPointOnBody1());
- mContactPoints[i]->setWorldPointOnBody2(transform2 *
- mContactPoints[i]->getLocalPointOnBody2());
- mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() -
- mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
+
+ mContactPoints[i]->updateWorldContactPoints(transform1, transform2);
+ mContactPoints[i]->updatePenetrationDepth();
}
const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp
index 7756d68b..00ed60d7 100644
--- a/src/constraint/ContactPoint.cpp
+++ b/src/constraint/ContactPoint.cpp
@@ -36,12 +36,6 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
mPenetrationDepth(contactInfo.penetrationDepth),
mLocalPointOnBody1(contactInfo.localPoint1),
mLocalPointOnBody2(contactInfo.localPoint2),
- mWorldPointOnBody1(contactInfo.shape1->getBody()->getTransform() *
- contactInfo.shape1->getLocalToBodyTransform() *
- contactInfo.localPoint1),
- mWorldPointOnBody2(contactInfo.shape2->getBody()->getTransform() *
- contactInfo.shape2->getLocalToBodyTransform() *
- contactInfo.localPoint2),
mIsRestingContact(false) {
assert(mPenetrationDepth > decimal(0.0));
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index c15dbafa..994f5426 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -53,6 +53,8 @@ struct ContactPointInfo {
// -------------------- Attributes -------------------- //
+ // TODO : Check if we really need the shape1, shape2, collisionShape1 and collisionShape2 fields
+
/// First proxy shape of the contact
ProxyShape* shape1;
@@ -141,12 +143,15 @@ class ContactPoint {
/// Deleted assignment operator
ContactPoint& operator=(const ContactPoint& contact) = delete;
+ /// Update the world contact points
+ void updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform);
+
+ /// Update the penetration depth
+ void updatePenetrationDepth();
+
/// Return the normal vector of the contact
Vector3 getNormal() const;
- /// Set the penetration depth of the contact
- void setPenetrationDepth(decimal penetrationDepth);
-
/// Return the contact local point on body 1
Vector3 getLocalPointOnBody1() const;
@@ -165,12 +170,6 @@ class ContactPoint {
/// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse);
- /// Set the contact world point on body 1
- void setWorldPointOnBody1(const Vector3& worldPoint);
-
- /// Set the contact world point on body 2
- void setWorldPointOnBody2(const Vector3& worldPoint);
-
/// Return true if the contact is a resting contact
bool getIsRestingContact() const;
@@ -184,16 +183,22 @@ class ContactPoint {
size_t getSizeInBytes() const;
};
+// Update the world contact points
+inline void ContactPoint::updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform) {
+ mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
+ mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
+}
+
+// Update the penetration depth
+inline void ContactPoint::updatePenetrationDepth() {
+ mPenetrationDepth = (mWorldPointOnBody1 - mWorldPointOnBody2).dot(mNormal);
+}
+
// Return the normal vector of the contact
inline Vector3 ContactPoint::getNormal() const {
return mNormal;
}
-// Set the penetration depth of the contact
-inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) {
- this->mPenetrationDepth = penetrationDepth;
-}
-
// Return the contact point on body 1
inline Vector3 ContactPoint::getLocalPointOnBody1() const {
return mLocalPointOnBody1;
@@ -224,16 +229,6 @@ inline void ContactPoint::setPenetrationImpulse(decimal impulse) {
mPenetrationImpulse = impulse;
}
-// Set the contact world point on body 1
-inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
- mWorldPointOnBody1 = worldPoint;
-}
-
-// Set the contact world point on body 2
-inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) {
- mWorldPointOnBody2 = worldPoint;
-}
-
// Return true if the contact is a resting contact
inline bool ContactPoint::getIsRestingContact() const {
return mIsRestingContact;
From 4a97c2ca9732526df3d0dcebd04b241bc604cc91 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 8 Jan 2017 19:56:59 +0100
Subject: [PATCH 023/133] Refactor collision detection
---
CMakeLists.txt | 7 +-
src/body/CollisionBody.h | 12 +
src/collision/CollisionDetection.cpp | 714 +++++++++++++++---
src/collision/CollisionDetection.h | 107 ++-
src/collision/ContactManifoldSet.cpp | 2 +-
...CollisionShapeInfo.h => NarrowPhaseInfo.h} | 45 +-
src/collision/ProxyShape.h | 27 +-
.../broadphase/BroadPhaseAlgorithm.cpp | 76 +-
.../broadphase/BroadPhaseAlgorithm.h | 36 +-
src/collision/broadphase/DynamicAABBTree.cpp | 2 +-
src/collision/narrowphase/CollisionDispatch.h | 5 -
.../narrowphase/ConcaveVsConvexAlgorithm.cpp | 236 +++---
.../narrowphase/ConcaveVsConvexAlgorithm.h | 99 +--
.../narrowphase/DefaultCollisionDispatch.cpp | 25 +-
.../narrowphase/DefaultCollisionDispatch.h | 7 -
.../narrowphase/EPA/EPAAlgorithm.cpp | 32 +-
src/collision/narrowphase/EPA/EPAAlgorithm.h | 20 +-
.../narrowphase/GJK/GJKAlgorithm.cpp | 86 +--
src/collision/narrowphase/GJK/GJKAlgorithm.h | 25 +-
.../narrowphase/NarrowPhaseAlgorithm.h | 28 +-
.../narrowphase/SphereVsSphereAlgorithm.cpp | 23 +-
.../narrowphase/SphereVsSphereAlgorithm.h | 5 +-
src/constraint/ContactPoint.h | 33 +-
src/containers/LinkedList.h | 124 +++
src/{memory => containers}/Stack.h | 0
src/engine/CollisionWorld.cpp | 122 +--
src/engine/CollisionWorld.h | 127 +++-
src/engine/DynamicsWorld.cpp | 112 ---
src/engine/DynamicsWorld.h | 26 -
.../Allocator.h} | 36 +-
src/memory/PoolAllocator.h | 10 +-
src/memory/SingleFrameAllocator.h | 15 +-
32 files changed, 1331 insertions(+), 893 deletions(-)
rename src/collision/{CollisionShapeInfo.h => NarrowPhaseInfo.h} (59%)
create mode 100644 src/containers/LinkedList.h
rename src/{memory => containers}/Stack.h (100%)
rename src/{collision/narrowphase/NarrowPhaseAlgorithm.cpp => memory/Allocator.h} (75%)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 87a02105..b3d37ed1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -80,7 +80,6 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/narrowphase/GJK/GJKAlgorithm.h"
"src/collision/narrowphase/GJK/GJKAlgorithm.cpp"
"src/collision/narrowphase/NarrowPhaseAlgorithm.h"
- "src/collision/narrowphase/NarrowPhaseAlgorithm.cpp"
"src/collision/narrowphase/SphereVsSphereAlgorithm.h"
"src/collision/narrowphase/SphereVsSphereAlgorithm.cpp"
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
@@ -121,7 +120,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/TriangleMesh.cpp"
"src/collision/CollisionDetection.h"
"src/collision/CollisionDetection.cpp"
- "src/collision/CollisionShapeInfo.h"
+ "src/collision/NarrowPhaseInfo.h"
"src/collision/ContactManifold.h"
"src/collision/ContactManifold.cpp"
"src/collision/ContactManifoldSet.h"
@@ -173,11 +172,13 @@ SET (REACTPHYSICS3D_SOURCES
"src/mathematics/Vector3.h"
"src/mathematics/Ray.h"
"src/mathematics/Vector3.cpp"
+ "src/memory/Allocator.h"
"src/memory/PoolAllocator.h"
"src/memory/PoolAllocator.cpp"
"src/memory/SingleFrameAllocator.h"
"src/memory/SingleFrameAllocator.cpp"
- "src/memory/Stack.h"
+ "src/containers/Stack.h"
+ "src/containers/LinkedList.h"
)
# Create the library
diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h
index bd5c7818..2d5101b9 100644
--- a/src/body/CollisionBody.h
+++ b/src/body/CollisionBody.h
@@ -153,6 +153,9 @@ class CollisionBody : public Body {
/// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
+ /// Test if the collision body overlaps with a given AABB
+ bool testAABBOverlap(const AABB& worldAABB) const;
+
/// Compute and return the AABB of the body by merging all proxy shapes AABBs
AABB getAABB() const;
@@ -301,6 +304,15 @@ inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
return mTransform.getOrientation().getInverse() * worldVector;
}
+/// Test if the collision body overlaps with a given AABB
+/**
+* @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap
+* @return True if the given AABB overlaps with the AABB of the collision body
+*/
+inline bool CollisionBody::testAABBOverlap(const AABB& worldAABB) const {
+ return worldAABB.testCollision(getAABB());
+}
+
}
#endif
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index e2599fca..c199859c 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -35,15 +35,16 @@
#include <set>
#include <utility>
#include <utility>
+#include <unordered_set>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
using namespace std;
// Constructor
-CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator)
- : mMemoryAllocator(memoryAllocator),
- mWorld(world), mBroadPhaseAlgorithm(*this),
+CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator)
+ : mMemoryAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator),
+ mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this),
mIsCollisionShapesAdded(false) {
// Set the default collision dispatch configuration
@@ -60,27 +61,20 @@ void CollisionDetection::computeCollisionDetection() {
// Compute the broad-phase collision detection
computeBroadPhase();
+
+ // Compute the middle-phase collision detection
+ computeMiddlePhase();
// Compute the narrow-phase collision detection
- computeNarrowPhase();
-}
-
-// Compute the collision detection
-void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
- const std::set<uint>& shapes1,
- const std::set<uint>& shapes2) {
-
- // Compute the broad-phase collision detection
- computeBroadPhase();
-
- // Delete all the contact points in the currently overlapping pairs
- clearContactPoints();
-
- // Compute the narrow-phase collision detection among given sets of shapes
- computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2);
+ computeNarrowPhase();
+
+ // Reset the linked list of narrow-phase info
+ mNarrowPhaseInfoList = nullptr;
}
+// TODO : Remove this method
// Report collision between two sets of shapes
+/*
void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2) {
@@ -126,13 +120,9 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac
ContactPoint* contactPoint = manifold->getContactPoint(i);
// Create the contact info object for the contact
- ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(),
- manifold->getShape1()->getCollisionShape(),
- manifold->getShape2()->getCollisionShape(),
- contactPoint->getNormal(),
- contactPoint->getPenetrationDepth(),
- contactPoint->getLocalPointOnBody1(),
- contactPoint->getLocalPointOnBody2());
+ ContactPointInfo contactInfo;
+ contactInfo.init(contactPoint->getNormal(), contactPoint->getPenetrationDepth(),
+ contactPoint->getLocalPointOnBody1(), contactPoint->getLocalPointOnBody2());
// Notify the collision callback about this new contact
if (callback != nullptr) callback->notifyContact(contactInfo);
@@ -140,6 +130,7 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac
}
}
}
+*/
// Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() {
@@ -152,18 +143,18 @@ void CollisionDetection::computeBroadPhase() {
// Ask the broad-phase to recompute the overlapping pairs of collision
// shapes. This call can only add new overlapping pairs in the collision
// detection.
- mBroadPhaseAlgorithm.computeOverlappingPairs();
+ mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryAllocator);
}
}
-// Compute the narrow-phase collision detection
-void CollisionDetection::computeNarrowPhase() {
+// Compute the middle-phase collision detection
+void CollisionDetection::computeMiddlePhase() {
- PROFILE("CollisionDetection::computeNarrowPhase()");
+ PROFILE("CollisionDetection::computeMiddlePhase()");
// Clear the set of overlapping pairs in narrow-phase contact
mContactOverlappingPairs.clear();
-
+
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
@@ -199,7 +190,7 @@ void CollisionDetection::computeNarrowPhase() {
CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody();
-
+
// Update the contact cache of the overlapping pair
pair->update();
@@ -211,37 +202,154 @@ void CollisionDetection::computeNarrowPhase() {
// Check if the bodies are in the set of bodies that cannot collide between each other
bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
-
- // Select the narrow phase algorithm to use according to the two collision shapes
+
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+
+ // If both shapes are convex
+ if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) {
+
+ // No middle-phase is necessary, simply create a narrow phase info
+ // for the narrow-phase collision detection
+ NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList;
+ mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo)))
+ NarrowPhaseInfo(pair, shape1->getCollisionShape(),
+ shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
+ shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
+ shape2->getCachedCollisionData());
+ mNarrowPhaseInfoList->next = firstNarrowPhaseInfo;
+
+ }
+ // Concave vs Convex algorithm
+ else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
+ (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
+
+ NarrowPhaseInfo* narrowPhaseInfo = nullptr;
+ computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo);
+
+ // Add all the narrow-phase info object reported by the callback into the
+ // list of all the narrow-phase info object
+ while (narrowPhaseInfo != nullptr) {
+ NarrowPhaseInfo* head = mNarrowPhaseInfoList;
+ mNarrowPhaseInfoList = narrowPhaseInfo;
+ mNarrowPhaseInfoList->next = head;
+
+ narrowPhaseInfo = narrowPhaseInfo->next;
+ }
+ }
+ // Concave vs Concave shape
+ else {
+ // Not handled
+ continue;
+ }
+ }
+}
+
+// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
+void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator,
+ NarrowPhaseInfo** firstNarrowPhaseInfo) {
+
+ ProxyShape* shape1 = pair->getShape1();
+ ProxyShape* shape2 = pair->getShape2();
+
+ ProxyShape* convexProxyShape;
+ ProxyShape* concaveProxyShape;
+ const ConvexShape* convexShape;
+ const ConcaveShape* concaveShape;
+
+ // Collision shape 1 is convex, collision shape 2 is concave
+ if (shape1->getCollisionShape()->isConvex()) {
+ convexProxyShape = shape1;
+ convexShape = static_cast<const ConvexShape*>(shape1->getCollisionShape());
+ concaveProxyShape = shape2;
+ concaveShape = static_cast<const ConcaveShape*>(shape2->getCollisionShape());
+ }
+ else { // Collision shape 2 is convex, collision shape 1 is concave
+ convexProxyShape = shape2;
+ convexShape = static_cast<const ConvexShape*>(shape2->getCollisionShape());
+ concaveProxyShape = shape1;
+ concaveShape = static_cast<const ConcaveShape*>(shape1->getCollisionShape());
+ }
+
+ // Set the parameters of the callback object
+ MiddlePhaseTriangleCallback middlePhaseCallback(pair, concaveProxyShape, convexProxyShape,
+ concaveShape, allocator);
+
+ // Compute the convex shape AABB in the local-space of the convex shape
+ AABB aabb;
+ convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
+
+ // TODO : Implement smooth concave mesh collision somewhere
+
+ // Call the convex vs triangle callback for each triangle of the concave shape
+ concaveShape->testAllTriangles(middlePhaseCallback, aabb);
+
+ // Add all the narrow-phase info object reported by the callback into the
+ // list of all the narrow-phase info object
+ *firstNarrowPhaseInfo = middlePhaseCallback.narrowPhaseInfoList;
+}
+
+// Compute the narrow-phase collision detection
+void CollisionDetection::computeNarrowPhase() {
+
+ PROFILE("CollisionDetection::computeNarrowPhase()");
+
+ const NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList;
+ while (currentNarrowPhaseInfo != nullptr) {
+
+ // Select the narrow phase algorithm to use according to the two collision shapes
+ const CollisionShapeType shape1Type = currentNarrowPhaseInfo->collisionShape1->getType();
+ const CollisionShapeType shape2Type = currentNarrowPhaseInfo->collisionShape2->getType();
const int shape1Index = static_cast<int>(shape1Type);
const int shape2Index = static_cast<int>(shape2Type);
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
- // If there is no collision algorithm between those two kinds of shapes
- if (narrowPhaseAlgorithm == nullptr) continue;
+ // If there is no collision algorithm between those two kinds of shapes, skip it
+ if (narrowPhaseAlgorithm != nullptr) {
- // Notify the narrow-phase algorithm about the overlapping pair we are going to test
- narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
+ // Use the narrow-phase collision detection algorithm to check
+ // if there really is a collision. If a collision occurs, the
+ // notifyContact() callback method will be called.
+ ContactPointInfo contactPointInfo;
+ if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactPointInfo)) {
- // Create the CollisionShapeInfo objects
- CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
- pair, shape1->getCachedCollisionData());
- CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
- pair, shape2->getCachedCollisionData());
-
- // Use the narrow-phase collision detection algorithm to check
- // if there really is a collision. If a collision occurs, the
- // notifyContact() callback method will be called.
- narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this);
+ // If it is the first contact since the pairs are overlapping
+ if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) {
+
+ // Trigger a callback event
+ if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactPointInfo);
+ }
+
+ // Create a new contact
+ ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint)))
+ ContactPoint(contactPointInfo);
+
+ contact->updateWorldContactPoints(currentNarrowPhaseInfo->shape1ToWorldTransform,
+ currentNarrowPhaseInfo->shape2ToWorldTransform);
+
+ // Add the contact to the contact manifold set of the corresponding overlapping pair
+ currentNarrowPhaseInfo->overlappingPair->addContact(contact);
+
+ // Add the overlapping pair into the set of pairs in contact during narrow-phase
+ overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(),
+ currentNarrowPhaseInfo->overlappingPair->getShape2());
+ mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair;
+
+ // Trigger a callback event for the new contact
+ if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactPointInfo);
+ }
+ }
+
+ currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
}
// Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies();
}
+// TODO : Remove this method
// Compute the narrow-phase collision detection
+/*
void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2) {
@@ -326,9 +434,6 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
// If there is no collision algorithm between those two kinds of shapes
if (narrowPhaseAlgorithm == nullptr) continue;
- // Notify the narrow-phase algorithm about the overlapping pair we are going to test
- narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
-
// Create the CollisionShapeInfo objects
CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
pair, shape1->getCachedCollisionData());
@@ -345,6 +450,7 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
// Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies();
}
+*/
// Allow the broadphase to notify the collision detection about an overlapping pair.
/// This method is called by the broad-phase collision detection algorithm
@@ -352,9 +458,6 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
- // If the two proxy collision shapes are from the same body, skip it
- if (shape1->getBody()->getID() == shape2->getBody()->getID()) return;
-
// Check if the collision filtering allows collision between the two shapes
if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return;
@@ -412,45 +515,6 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
}
-// Called by a narrow-phase collision algorithm when a new contact has been found
-void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) {
-
- // If it is the first contact since the pairs are overlapping
- if (overlappingPair->getNbContactPoints() == 0) {
-
- // Trigger a callback event
- if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactInfo);
- }
-
- // Create a new contact
- createContact(overlappingPair, contactInfo);
-
- // Trigger a callback event for the new contact
- if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactInfo);
-}
-
-// Create a new contact
-void CollisionDetection::createContact(OverlappingPair* overlappingPair,
- const ContactPointInfo& contactInfo) {
-
- PROFILE("CollisionDetection::createContact()");
-
- // Create a new contact
- ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint)))
- ContactPoint(contactInfo);
-
- contact->updateWorldContactPoints(overlappingPair->getShape1()->getLocalToWorldTransform(),
- overlappingPair->getShape2()->getLocalToWorldTransform());
-
- // Add the contact to the contact manifold set of the corresponding overlapping pair
- overlappingPair->addContact(contact);
-
- // Add the overlapping pair into the set of pairs in contact during narrow-phase
- overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(),
- overlappingPair->getShape2());
- mContactOverlappingPairs[pairId] = overlappingPair;
-}
-
void CollisionDetection::addAllContactManifoldsToBodies() {
// For each overlapping pairs in contact during the narrow-phase
@@ -508,6 +572,464 @@ void CollisionDetection::clearContactPoints() {
}
}
+// Compute the middle-phase collision detection between two proxy shapes
+NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape* shape1, ProxyShape* shape2) {
+
+ // Create a temporary overlapping pair
+ OverlappingPair pair(shape1, shape2, 0, mMemoryAllocator);
+
+ // -------------------------------------------------------
+
+ const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
+ const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+
+ NarrowPhaseInfo* narrowPhaseInfo = nullptr;
+
+ // If both shapes are convex
+ if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) {
+
+ // No middle-phase is necessary, simply create a narrow phase info
+ // for the narrow-phase collision detection
+ narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(&pair, shape1->getCollisionShape(),
+ shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
+ shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
+ shape2->getCachedCollisionData());
+
+ }
+ // Concave vs Convex algorithm
+ else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
+ (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
+
+ // Run the middle-phase collision detection algorithm to find the triangles of the concave
+ // shape we need to use during the narrow-phase collision detection
+ computeConvexVsConcaveMiddlePhase(&pair, mMemoryAllocator, &narrowPhaseInfo);
+ }
+
+ return nullptr;
+}
+
+// Report all the bodies that overlap with the aabb in parameter
+void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback,
+ unsigned short categoryMaskBits) {
+ assert(overlapCallback != nullptr);
+
+ std::unordered_set<bodyindex> reportedBodies;
+
+ // Ask the broad-phase to get all the overlapping shapes
+ LinkedList<int> overlappingNodes(mMemoryAllocator);
+ mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes);
+
+ // For each overlaping proxy shape
+ LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
+ while (element != nullptr) {
+
+ // Get the overlapping proxy shape
+ int broadPhaseId = element->data;
+ ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId);
+
+ CollisionBody* overlapBody = proxyShape->getBody();
+
+ // If the proxy shape is from a body that we have not already reported collision
+ if (reportedBodies.find(overlapBody->getID()) == reportedBodies.end()) {
+
+ // Check if the collision filtering allows collision between the two shapes
+ if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
+
+ // Add the body into the set of reported bodies
+ reportedBodies.insert(overlapBody->getID());
+
+ // Notify the overlap to the user
+ overlapCallback->notifyOverlap(overlapBody);
+ }
+ }
+
+ // Go to the next overlapping proxy shape
+ element = element->next;
+ }
+}
+
+// Return true if two bodies overlap
+bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) {
+
+ // For each proxy shape proxy shape of the first body
+ ProxyShape* body1ProxyShape = body1->getProxyShapesList();
+ while (body1ProxyShape != nullptr) {
+
+ AABB aabb1 = body1ProxyShape->getWorldAABB();
+
+ // For each proxy shape of the second body
+ ProxyShape* body2ProxyShape = body2->getProxyShapesList();
+ while (body2ProxyShape != nullptr) {
+
+ AABB aabb2 = body2ProxyShape->getWorldAABB();
+
+ // Test if the AABBs of the two proxy shapes overlap
+ if (aabb1.testCollision(aabb2)) {
+
+ const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType();
+ const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType();
+
+ // Compute the middle-phase collision detection between the two shapes
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape);
+
+ bool isColliding = false;
+
+ // For each narrow-phase info object
+ while (narrowPhaseInfo != nullptr) {
+
+ // If we have not found a collision yet
+ if (!isColliding) {
+
+ // Select the narrow phase algorithm to use according to the two collision shapes
+ const int shape1Index = static_cast<int>(shape1Type);
+ const int shape2Index = static_cast<int>(shape2Type);
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+
+ // If there is a collision algorithm for those two kinds of shapes
+ if (narrowPhaseAlgorithm != nullptr) {
+
+ // Use the narrow-phase collision detection algorithm to check
+ // if there really is a collision. If a collision occurs, the
+ // notifyContact() callback method will be called.
+ ContactPointInfo contactPointInfo;
+ isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo);
+ }
+ }
+
+ NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+ narrowPhaseInfo = narrowPhaseInfo->next;
+
+ // Release the allocated memory
+ mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ }
+
+ // Return if we have found a narrow-phase collision
+ if (isColliding) return true;
+ }
+
+ // Go to the next proxy shape
+ body2ProxyShape = body2ProxyShape->getNext();
+ }
+
+ // Go to the next proxy shape
+ body1ProxyShape = body1ProxyShape->getNext();
+ }
+
+ // No overlap has been found
+ return false;
+}
+
+// Report all the bodies that overlap with the body in parameter
+void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overlapCallback,
+ unsigned short categoryMaskBits) {
+
+ assert(overlapCallback != nullptr);
+
+ std::unordered_set<bodyindex> reportedBodies;
+
+ // For each proxy shape proxy shape of the body
+ ProxyShape* bodyProxyShape = body->getProxyShapesList();
+ while (bodyProxyShape != nullptr) {
+
+ // Get the AABB of the shape
+ const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
+
+ // Ask the broad-phase to get all the overlapping shapes
+ LinkedList<int> overlappingNodes(mMemoryAllocator);
+ mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
+
+ const bodyindex bodyId = body->getID();
+
+ // For each overlaping proxy shape
+ LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
+ while (element != nullptr) {
+
+ // Get the overlapping proxy shape
+ int broadPhaseId = element->data;
+ ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId);
+
+ // If the proxy shape is from a body that we have not already reported collision and the
+ // two proxy collision shapes are not from the same body
+ if (reportedBodies.find(proxyShape->getBody()->getID()) == reportedBodies.end() &&
+ proxyShape->getBody()->getID() != bodyId) {
+
+ // Check if the collision filtering allows collision between the two shapes
+ if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
+
+ const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType();
+ const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
+
+ // Compute the middle-phase collision detection between the two shapes
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape);
+
+ bool isColliding = false;
+
+ // For each narrow-phase info object
+ while (narrowPhaseInfo != nullptr) {
+
+ // If we have not found a collision yet
+ if (!isColliding) {
+
+ // Select the narrow phase algorithm to use according to the two collision shapes
+ const int shape1Index = static_cast<int>(shape1Type);
+ const int shape2Index = static_cast<int>(shape2Type);
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+
+ // If there is a collision algorithm for those two kinds of shapes
+ if (narrowPhaseAlgorithm != nullptr) {
+
+ // Use the narrow-phase collision detection algorithm to check
+ // if there really is a collision. If a collision occurs, the
+ // notifyContact() callback method will be called.
+ ContactPointInfo contactPointInfo;
+ isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo);
+ }
+ }
+
+ NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+ narrowPhaseInfo = narrowPhaseInfo->next;
+
+ // Release the allocated memory
+ mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ }
+
+ // Return if we have found a narrow-phase collision
+ if (isColliding) {
+
+ CollisionBody* overlapBody = proxyShape->getBody();
+
+ // Add the body into the set of reported bodies
+ reportedBodies.insert(overlapBody->getID());
+
+ // Notify the overlap to the user
+ overlapCallback->notifyOverlap(overlapBody);
+ }
+ }
+ }
+
+ // Go to the next overlapping proxy shape
+ element = element->next;
+ }
+
+ // Go to the next proxy shape
+ bodyProxyShape = bodyProxyShape->getNext();
+ }
+}
+
+// Test and report collisions between two bodies
+void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* collisionCallback) {
+
+ assert(collisionCallback != nullptr);
+
+ // For each proxy shape proxy shape of the first body
+ ProxyShape* body1ProxyShape = body1->getProxyShapesList();
+ while (body1ProxyShape != nullptr) {
+
+ AABB aabb1 = body1ProxyShape->getWorldAABB();
+
+ // For each proxy shape of the second body
+ ProxyShape* body2ProxyShape = body2->getProxyShapesList();
+ while (body2ProxyShape != nullptr) {
+
+ AABB aabb2 = body2ProxyShape->getWorldAABB();
+
+ // Test if the AABBs of the two proxy shapes overlap
+ if (aabb1.testCollision(aabb2)) {
+
+ // Compute the middle-phase collision detection between the two shapes
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape);
+
+ const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType();
+ const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType();
+
+ // For each narrow-phase info object
+ while (narrowPhaseInfo != nullptr) {
+
+ // Select the narrow phase algorithm to use according to the two collision shapes
+ const int shape1Index = static_cast<int>(shape1Type);
+ const int shape2Index = static_cast<int>(shape2Type);
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+
+ // If there is a collision algorithm for those two kinds of shapes
+ if (narrowPhaseAlgorithm != nullptr) {
+
+ // Use the narrow-phase collision detection algorithm to check
+ // if there really is a collision. If a collision occurs, the
+ // notifyContact() callback method will be called.
+ ContactPointInfo contactPointInfo;
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
+
+ CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body1, body2,
+ body1ProxyShape, body2ProxyShape);
+
+ // Report the contact to the user
+ collisionCallback->notifyContact(collisionInfo);
+ }
+ }
+
+ NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+ narrowPhaseInfo = narrowPhaseInfo->next;
+
+ // Release the allocated memory
+ mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ }
+ }
+
+ // Go to the next proxy shape
+ body2ProxyShape = body2ProxyShape->getNext();
+ }
+
+ // Go to the next proxy shape
+ body1ProxyShape = body1ProxyShape->getNext();
+ }
+}
+
+// Test and report collisions between a body and all the others bodies of the world
+void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits) {
+
+ assert(callback != nullptr);
+
+ // For each proxy shape proxy shape of the body
+ ProxyShape* bodyProxyShape = body->getProxyShapesList();
+ while (bodyProxyShape != nullptr) {
+
+ // Get the AABB of the shape
+ const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
+
+ // Ask the broad-phase to get all the overlapping shapes
+ LinkedList<int> overlappingNodes(mMemoryAllocator);
+ mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
+
+ const bodyindex bodyId = body->getID();
+
+ // For each overlaping proxy shape
+ LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
+ while (element != nullptr) {
+
+ // Get the overlapping proxy shape
+ int broadPhaseId = element->data;
+ ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId);
+
+ // If the two proxy collision shapes are not from the same body
+ if (proxyShape->getBody()->getID() != bodyId) {
+
+ // Check if the collision filtering allows collision between the two shapes
+ if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
+
+ const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType();
+ const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
+
+ // Compute the middle-phase collision detection between the two shapes
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape);
+
+ // For each narrow-phase info object
+ while (narrowPhaseInfo != nullptr) {
+
+ // Select the narrow phase algorithm to use according to the two collision shapes
+ const int shape1Index = static_cast<int>(shape1Type);
+ const int shape2Index = static_cast<int>(shape2Type);
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+
+ // If there is a collision algorithm for those two kinds of shapes
+ if (narrowPhaseAlgorithm != nullptr) {
+
+ // Use the narrow-phase collision detection algorithm to check
+ // if there really is a collision. If a collision occurs, the
+ // notifyContact() callback method will be called.
+ ContactPointInfo contactPointInfo;
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
+
+ CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body,
+ proxyShape->getBody(), bodyProxyShape,
+ proxyShape);
+
+ // Report the contact to the user
+ callback->notifyContact(collisionInfo);
+ }
+ }
+
+ NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+ narrowPhaseInfo = narrowPhaseInfo->next;
+
+ // Release the allocated memory
+ mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ }
+ }
+ }
+
+ // Go to the next overlapping proxy shape
+ element = element->next;
+ }
+
+ // Go to the next proxy shape
+ bodyProxyShape = bodyProxyShape->getNext();
+ }
+}
+
+// Test and report collisions between all shapes of the world
+void CollisionDetection::testCollision(CollisionCallback* callback) {
+
+ assert(callback != nullptr);
+
+ // Compute the broad-phase collision detection
+ computeBroadPhase();
+
+ // For each possible collision pair of bodies
+ map<overlappingpairid, OverlappingPair*>::iterator it;
+ for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+
+ OverlappingPair* pair = it->second;
+
+ ProxyShape* shape1 = pair->getShape1();
+ ProxyShape* shape2 = pair->getShape2();
+
+ // Check if the collision filtering allows collision between the two shapes and
+ // that the two shapes are still overlapping.
+ if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 &&
+ (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0) &&
+ mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
+
+ // Compute the middle-phase collision detection between the two shapes
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(shape1, shape2);
+
+ const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
+ const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+
+ // For each narrow-phase info object
+ while (narrowPhaseInfo != nullptr) {
+
+ // Select the narrow phase algorithm to use according to the two collision shapes
+ const int shape1Index = static_cast<int>(shape1Type);
+ const int shape2Index = static_cast<int>(shape2Type);
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+
+ // If there is a collision algorithm for those two kinds of shapes
+ if (narrowPhaseAlgorithm != nullptr) {
+
+ // Use the narrow-phase collision detection algorithm to check
+ // if there really is a collision. If a collision occurs, the
+ // notifyContact() callback method will be called.
+ ContactPointInfo contactPointInfo;
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
+
+ CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, shape1->getBody(),
+ shape2->getBody(), shape1, shape2);
+
+ // Report the contact to the user
+ callback->notifyContact(collisionInfo);
+ }
+ }
+
+ NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+ narrowPhaseInfo = narrowPhaseInfo->next;
+
+ // Release the allocated memory
+ mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ }
+ }
+ }
+}
+
// Fill-in the collision detection matrix
void CollisionDetection::fillInCollisionMatrix() {
@@ -528,9 +1050,3 @@ EventListener* CollisionDetection::getWorldEventListener() {
PoolAllocator& CollisionDetection::getWorldMemoryAllocator() {
return mWorld->mPoolAllocator;
}
-
-// Called by a narrow-phase collision algorithm when a new contact has been found
-void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair,
- const ContactPointInfo& contactInfo) {
- mCollisionCallback->notifyContact(contactInfo);
-}
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 0fe486c4..3bb27366 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -33,6 +33,7 @@
#include "engine/EventListener.h"
#include "narrowphase/DefaultCollisionDispatch.h"
#include "memory/PoolAllocator.h"
+#include "memory/SingleFrameAllocator.h"
#include "constraint/ContactPoint.h"
#include <vector>
#include <set>
@@ -46,29 +47,7 @@ namespace reactphysics3d {
class BroadPhaseAlgorithm;
class CollisionWorld;
class CollisionCallback;
-
-// Class TestCollisionBetweenShapesCallback
-class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
-
- private:
-
- CollisionCallback* mCollisionCallback;
-
- public:
-
- // Constructor
- TestCollisionBetweenShapesCallback(CollisionCallback* callback)
- : mCollisionCallback(callback) {
-
- }
-
- // Destructor
- virtual ~TestCollisionBetweenShapesCallback() { }
-
- // Called by a narrow-phase collision algorithm when a new contact has been found
- virtual void notifyContact(OverlappingPair* overlappingPair,
- const ContactPointInfo& contactInfo) override;
-};
+class OverlapCallback;
// Class CollisionDetection
/**
@@ -77,7 +56,7 @@ class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
* collide and then we run a narrow-phase algorithm to compute the
* collision contacts between bodies.
*/
-class CollisionDetection : public NarrowPhaseCallback {
+class CollisionDetection {
private :
@@ -95,9 +74,15 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Reference to the memory allocator
PoolAllocator& mMemoryAllocator;
+ /// Reference to the single frame memory allocator
+ SingleFrameAllocator& mSingleFrameAllocator;
+
/// Pointer to the physics world
CollisionWorld* mWorld;
+ /// Pointer to the first narrow-phase info of the linked list
+ NarrowPhaseInfo* mNarrowPhaseInfoList;
+
/// Broad-phase overlapping pairs
std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
@@ -111,6 +96,7 @@ class CollisionDetection : public NarrowPhaseCallback {
// TODO : Delete this
GJKAlgorithm mNarrowPhaseGJKAlgorithm;
+ // TODO : Maybe delete this set (what is the purpose ?)
/// Set of pair of bodies that cannot collide between each other
std::set<bodyindexpair> mNoCollisionPairs;
@@ -122,6 +108,9 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Compute the broad-phase collision detection
void computeBroadPhase();
+ /// Compute the middle-phase collision detection
+ void computeMiddlePhase();
+
/// Compute the narrow-phase collision detection
void computeNarrowPhase();
@@ -137,13 +126,20 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Add all the contact manifold of colliding pairs to their bodies
void addAllContactManifoldsToBodies();
+
+ /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
+ void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator,
+ NarrowPhaseInfo** firstNarrowPhaseInfo);
+
+ /// Compute the middle-phase collision detection between two proxy shapes
+ NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(ProxyShape* shape1, ProxyShape* shape2);
public :
// -------------------- Methods -------------------- //
/// Constructor
- CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator);
+ CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator);
/// Destructor
~CollisionDetection() = default;
@@ -183,35 +179,42 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Compute the collision detection
void computeCollisionDetection();
- /// Compute the collision detection
- void testCollisionBetweenShapes(CollisionCallback* callback,
- const std::set<uint>& shapes1,
- const std::set<uint>& shapes2);
-
+ // TODO : Remove this method
/// Report collision between two sets of shapes
- void reportCollisionBetweenShapes(CollisionCallback* callback,
- const std::set<uint>& shapes1,
- const std::set<uint>& shapes2) ;
+ //void reportCollisionBetweenShapes(CollisionCallback* callback,
+ // const std::set<uint>& shapes1,
+ // const std::set<uint>& shapes2) ;
/// Ray casting method
void raycast(RaycastCallback* raycastCallback, const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const;
- /// Test if the AABBs of two bodies overlap
- bool testAABBOverlap(const CollisionBody* body1,
- const CollisionBody* body2) const;
+ /// Report all the bodies that overlap with the aabb in parameter
+ void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
- /// Test if the AABBs of two proxy shapes overlap
- bool testAABBOverlap(const ProxyShape* shape1,
- const ProxyShape* shape2) const;
+ /// Return true if two bodies overlap
+ bool testOverlap(CollisionBody* body1, CollisionBody* body2);
+
+ /// Report all the bodies that overlap with the body in parameter
+ void testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
+
+ /// Test and report collisions between two bodies
+ void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback);
+
+ /// Test and report collisions between a body and all the others bodies of the world
+ void testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits = 0xFFFF);
+
+ /// Test and report collisions between all shapes of the world
+ void testCollision(CollisionCallback* callback);
/// Allow the broadphase to notify the collision detection about an overlapping pair.
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
+ // TODO : Remove this method
/// Compute the narrow-phase collision detection
- void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
- const std::set<uint>& shapes1,
- const std::set<uint>& shapes2);
+ //void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
+ // const std::set<uint>& shapes1,
+ // const std::set<uint>& shapes2);
/// Return a pointer to the world
CollisionWorld* getWorld();
@@ -222,12 +225,6 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Return a reference to the world memory allocator
PoolAllocator& getWorldMemoryAllocator();
- /// Called by a narrow-phase collision algorithm when a new contact has been found
- virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) override;
-
- /// Create a new contact
- void createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo);
-
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
@@ -244,8 +241,6 @@ inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(Collision
inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
mCollisionDispatch = collisionDispatch;
- mCollisionDispatch->init(this, &mMemoryAllocator);
-
// Fill-in the collision matrix with the new algorithms to use
fillInCollisionMatrix();
}
@@ -299,18 +294,6 @@ inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits);
}
-// Test if the AABBs of two proxy shapes overlap
-inline bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1,
- const ProxyShape* shape2) const {
-
- // If one of the shape's body is not active, we return no overlap
- if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) {
- return false;
- }
-
- return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
-}
-
// Return a pointer to the world
inline CollisionWorld* CollisionDetection::getWorld() {
return mWorld;
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index 26f3a66f..a7dc9d2e 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -33,7 +33,7 @@ ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
PoolAllocator& memoryAllocator, int nbMaxManifolds)
: mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator) {
- assert(nbMaxManifolds >= 1);
+
}
// Destructor
diff --git a/src/collision/CollisionShapeInfo.h b/src/collision/NarrowPhaseInfo.h
similarity index 59%
rename from src/collision/CollisionShapeInfo.h
rename to src/collision/NarrowPhaseInfo.h
index 2e1d9504..b97a71be 100644
--- a/src/collision/CollisionShapeInfo.h
+++ b/src/collision/NarrowPhaseInfo.h
@@ -23,8 +23,8 @@
* *
********************************************************************************/
-#ifndef REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
-#define REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
+#ifndef REACTPHYSICS3D_NARROW_PHASE_INFO_H
+#define REACTPHYSICS3D_NARROW_PHASE_INFO_H
// Libraries
#include "shapes/CollisionShape.h"
@@ -34,37 +34,48 @@ namespace reactphysics3d {
class OverlappingPair;
-// Class CollisionShapeInfo
+// Class NarrowPhaseInfo
/**
* This structure regroups different things about a collision shape. This is
* used to pass information about a collision shape to a collision algorithm.
*/
-struct CollisionShapeInfo {
+struct NarrowPhaseInfo {
public:
/// Broadphase overlapping pair
OverlappingPair* overlappingPair;
- /// Proxy shape
- ProxyShape* proxyShape;
+ /// Pointer to the first collision shape to test collision with
+ const CollisionShape* collisionShape1;
- /// Pointer to the collision shape
- const CollisionShape* collisionShape;
+ /// Pointer to the second collision shape to test collision with
+ const CollisionShape* collisionShape2;
- /// Transform that maps from collision shape local-space to world-space
- Transform shapeToWorldTransform;
+ /// Transform that maps from collision shape 1 local-space to world-space
+ Transform shape1ToWorldTransform;
+
+ /// Transform that maps from collision shape 2 local-space to world-space
+ Transform shape2ToWorldTransform;
/// Cached collision data of the proxy shape
- void** cachedCollisionData;
+ // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
+ void** cachedCollisionData1;
+
+ /// Cached collision data of the proxy shape
+ // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
+ void** cachedCollisionData2;
+
+ /// Pointer to the next element in the linked list
+ NarrowPhaseInfo* next;
/// Constructor
- CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape,
- const Transform& shapeLocalToWorldTransform, OverlappingPair* pair,
- void** cachedData)
- : overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape),
- shapeToWorldTransform(shapeLocalToWorldTransform),
- cachedCollisionData(cachedData) {
+ NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1,
+ const CollisionShape* shape2, const Transform& shape1Transform,
+ const Transform& shape2Transform, void** cachedData1, void** cachedData2)
+ : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
+ shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
+ cachedCollisionData1(cachedData1), cachedCollisionData2(cachedData2), next(nullptr) {
}
};
diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h
index d7c81621..45c0add2 100644
--- a/src/collision/ProxyShape.h
+++ b/src/collision/ProxyShape.h
@@ -126,6 +126,12 @@ class ProxyShape {
/// Return the local to world transform
const Transform getLocalToWorldTransform() const;
+ /// Return the AABB of the proxy shape in world-space
+ const AABB getWorldAABB() const;
+
+ /// Test if the proxy shape overlaps with a given AABB
+ bool testAABBOverlap(const AABB& worldAABB) const;
+
/// Return true if a point is inside the collision shape
bool testPointInside(const Vector3& worldPoint);
@@ -176,7 +182,7 @@ class ProxyShape {
};
// Return the pointer to the cached collision data
-inline void** ProxyShape::getCachedCollisionData() {
+inline void** ProxyShape::getCachedCollisionData() {
return &mCachedCollisionData;
}
@@ -249,6 +255,16 @@ inline const Transform ProxyShape::getLocalToWorldTransform() const {
return mBody->mTransform * mLocalToBodyTransform;
}
+// Return the AABB of the proxy shape in world-space
+/**
+ * @return The AABB of the proxy shape in world-space
+ */
+inline const AABB ProxyShape::getWorldAABB() const {
+ AABB aabb;
+ mCollisionShape->computeAABB(aabb, getLocalToWorldTransform());
+ return aabb;
+}
+
// Return the next proxy shape in the linked list of proxy shapes
/**
* @return Pointer to the next proxy shape in the linked list of proxy shapes
@@ -320,6 +336,15 @@ inline void ProxyShape::setLocalScaling(const Vector3& scaling) {
mBody->updateProxyShapeInBroadPhase(this, true);
}
+/// Test if the proxy shape overlaps with a given AABB
+/**
+* @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap
+* @return True if the given AABB overlaps with the AABB of the collision body
+*/
+inline bool ProxyShape::testAABBOverlap(const AABB& worldAABB) const {
+ return worldAABB.testCollision(getWorldAABB());
+}
+
}
#endif
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp
index 781273e5..bc03f44d 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp
@@ -162,12 +162,25 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, cons
}
}
+void BroadPhaseAlgorithm::reportAllShapesOverlappingWithAABB(const AABB& aabb,
+ LinkedList<int>& overlappingNodes) const {
+
+ AABBOverlapCallback callback(overlappingNodes);
+
+ // Ask the dynamic AABB tree to report all collision shapes that overlap with this AABB
+ mDynamicAABBTree.reportAllShapesOverlappingWithAABB(aabb, callback);
+}
+
// Compute all the overlapping pairs of collision shapes
-void BroadPhaseAlgorithm::computeOverlappingPairs() {
+void BroadPhaseAlgorithm::computeOverlappingPairs(Allocator& allocator) {
+
+ // TODO : Try to see if we can allocate potential pairs in single frame allocator
// Reset the potential overlapping pairs
mNbPotentialPairs = 0;
+ LinkedList<int> overlappingNodes(allocator);
+
// For all collision shapes that have moved (or have been created) during the
// last simulation step
for (uint i=0; i<mNbMovedShapes; i++) {
@@ -175,7 +188,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
if (shapeID == -1) continue;
- AABBOverlapCallback callback(*this, shapeID);
+ AABBOverlapCallback callback(overlappingNodes);
// Get the AABB of the shape
const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);
@@ -184,6 +197,12 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair.
mDynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback);
+
+ // Add the potential overlapping pairs
+ addOverlappingNodes(shapeID, overlappingNodes);
+
+ // Remove all the elements of the linked list of overlapping nodes
+ overlappingNodes.reset();
}
// Reset the array of collision shapes that have move (or have been created) during the
@@ -208,8 +227,12 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID));
ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID));
- // Notify the collision detection about the overlapping pair
- mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
+ // If the two proxy collision shapes are from the same body, skip it
+ if (shape1->getBody()->getID() != shape2->getBody()->getID()) {
+
+ // Notify the collision detection about the overlapping pair
+ mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
+ }
// Skip the duplicate overlapping pairs
while (i < mNbPotentialPairs) {
@@ -241,34 +264,41 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
}
// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
-void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
+void BroadPhaseAlgorithm::addOverlappingNodes(int referenceNodeId, const LinkedList<int>& overlappingNodes) {
- // If both the nodes are the same, we do not create store the overlapping pair
- if (node1ID == node2ID) return;
+ // For each overlapping node in the linked list
+ LinkedList<int>::ListElement* elem = overlappingNodes.getListHead();
+ while (elem != nullptr) {
- // If we need to allocate more memory for the array of potential overlapping pairs
- if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
+ // If both the nodes are the same, we do not create store the overlapping pair
+ if (referenceNodeId != elem->data) {
- // Allocate more memory for the array of potential pairs
- BroadPhasePair* oldPairs = mPotentialPairs;
- mNbAllocatedPotentialPairs *= 2;
- mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
- assert(mPotentialPairs);
- memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
- free(oldPairs);
+ // If we need to allocate more memory for the array of potential overlapping pairs
+ if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
+
+ // Allocate more memory for the array of potential pairs
+ BroadPhasePair* oldPairs = mPotentialPairs;
+ mNbAllocatedPotentialPairs *= 2;
+ mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
+ assert(mPotentialPairs);
+ memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
+ free(oldPairs);
+ }
+
+ // Add the new potential pair into the array of potential overlapping pairs
+ mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(referenceNodeId, elem->data);
+ mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(referenceNodeId, elem->data);
+ mNbPotentialPairs++;
+ }
+
+ elem = elem->next;
}
-
- // Add the new potential pair into the array of potential overlapping pairs
- mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
- mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
- mNbPotentialPairs++;
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
void AABBOverlapCallback::notifyOverlappingNode(int nodeId) {
-
- mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId);
+ mOverlappingNodes.insert(nodeId);
}
// Called for a broad-phase shape that has to be tested for raycast
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h
index 45c8acfb..8d2cda35 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.h
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.h
@@ -32,6 +32,7 @@
#include "collision/ProxyShape.h"
#include "DynamicAABBTree.h"
#include "engine/Profiler.h"
+#include "containers/LinkedList.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@@ -66,15 +67,13 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private:
- BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
-
- int mReferenceNodeId;
-
public:
+ LinkedList<int>& mOverlappingNodes;
+
// Constructor
- AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId)
- : mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) {
+ AABBOverlapCallback(LinkedList<int>& overlappingNodes)
+ : mOverlappingNodes(overlappingNodes) {
}
@@ -197,15 +196,24 @@ class BroadPhaseAlgorithm {
/// step and that need to be tested again for broad-phase overlapping.
void removeMovedCollisionShape(int broadPhaseID);
- /// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
- void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2);
+ /// Add potential overlapping pairs in the dynamic AABB tree
+ void addOverlappingNodes(int broadPhaseId1, const LinkedList<int>& overlappingNodes);
+
+ /// Report all the shapes that are overlapping with a given AABB
+ void reportAllShapesOverlappingWithAABB(const AABB& aabb, LinkedList<int>& overlappingNodes) const;
/// Compute all the overlapping pairs of collision shapes
- void computeOverlappingPairs();
+ void computeOverlappingPairs(Allocator& allocator);
+
+ /// Return the proxy shape corresponding to the broad-phase node id in parameter
+ ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const;
/// Return true if the two broad-phase collision shapes are overlapping
bool testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const;
+ /// Return the fat AABB of a given broad-phase shape
+ const AABB& getFatAABB(int broadPhaseId) const;
+
/// Ray casting method
void raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const;
@@ -232,6 +240,11 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
return aabb1.testCollision(aabb2);
}
+// Return the fat AABB of a given broad-phase shape
+inline const AABB& BroadPhaseAlgorithm::getFatAABB(int broadPhaseId) const {
+ return mDynamicAABBTree.getFatAABB(broadPhaseId);
+}
+
// Ray casting method
inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const {
@@ -243,6 +256,11 @@ inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTes
mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
}
+// Return the proxy shape corresponding to the broad-phase node id in parameter
+inline ProxyShape* BroadPhaseAlgorithm::getProxyShapeForBroadPhaseId(int broadPhaseId) const {
+ return static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(broadPhaseId));
+}
+
}
#endif
diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp
index 7efd86ac..f829c000 100644
--- a/src/collision/broadphase/DynamicAABBTree.cpp
+++ b/src/collision/broadphase/DynamicAABBTree.cpp
@@ -26,7 +26,7 @@
// Libraries
#include "DynamicAABBTree.h"
#include "BroadPhaseAlgorithm.h"
-#include "memory/Stack.h"
+#include "containers/Stack.h"
#include "engine/Profiler.h"
using namespace reactphysics3d;
diff --git a/src/collision/narrowphase/CollisionDispatch.h b/src/collision/narrowphase/CollisionDispatch.h
index 6e027721..230ebbb3 100644
--- a/src/collision/narrowphase/CollisionDispatch.h
+++ b/src/collision/narrowphase/CollisionDispatch.h
@@ -49,11 +49,6 @@ class CollisionDispatch {
/// Destructor
virtual ~CollisionDispatch() = default;
- /// Initialize the collision dispatch configuration
- virtual void init(CollisionDetection* collisionDetection,
- PoolAllocator* memoryAllocator) {
-
- }
/// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes.
diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
index 1e3c86c6..c0da7611 100644
--- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
+++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
@@ -33,94 +33,111 @@
using namespace reactphysics3d;
-// Return true and compute a contact info if the two bounding volumes collide
-void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
- const CollisionShapeInfo& shape2Info,
- NarrowPhaseCallback* narrowPhaseCallback) {
-
- ProxyShape* convexProxyShape;
- ProxyShape* concaveProxyShape;
- const ConvexShape* convexShape;
- const ConcaveShape* concaveShape;
-
- // Collision shape 1 is convex, collision shape 2 is concave
- if (shape1Info.collisionShape->isConvex()) {
- convexProxyShape = shape1Info.proxyShape;
- convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
- concaveProxyShape = shape2Info.proxyShape;
- concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
- }
- else { // Collision shape 2 is convex, collision shape 1 is concave
- convexProxyShape = shape2Info.proxyShape;
- convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
- concaveProxyShape = shape1Info.proxyShape;
- concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
- }
-
- // Set the parameters of the callback object
- ConvexVsTriangleCallback convexVsTriangleCallback;
- convexVsTriangleCallback.setCollisionDetection(mCollisionDetection);
- convexVsTriangleCallback.setConvexShape(convexShape);
- convexVsTriangleCallback.setConcaveShape(concaveShape);
- convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
- convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
-
- // Compute the convex shape AABB in the local-space of the convex shape
- AABB aabb;
- convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
-
- // If smooth mesh collision is enabled for the concave mesh
- if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
-
- std::vector<SmoothMeshContactInfo> contactPoints;
-
- SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
-
- convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback);
-
- // Call the convex vs triangle callback for each triangle of the concave shape
- concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
-
- // Run the smooth mesh collision algorithm
- processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
- }
- else {
-
- convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
-
- // Call the convex vs triangle callback for each triangle of the concave shape
- concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
- }
-}
-
-// Test collision between a triangle and the convex mesh shape
-void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
+// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase)
+void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints) {
// Create a triangle collision shape
decimal margin = mConcaveShape->getTriangleMargin();
- TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+ TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
+ TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
- // Select the collision algorithm to use between the triangle and the convex shape
- NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(),
- mConvexShape->getType());
-
- // If there is no collision algorithm between those two kinds of shapes
- if (algo == nullptr) return;
-
- // Notify the narrow-phase algorithm about the overlapping pair we are going to test
- algo->setCurrentOverlappingPair(mOverlappingPair);
-
- // Create the CollisionShapeInfo objects
- CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(),
- mOverlappingPair, mConvexProxyShape->getCachedCollisionData());
- CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape,
- mConcaveProxyShape->getLocalToWorldTransform(),
- mOverlappingPair, mConcaveProxyShape->getCachedCollisionData());
-
- // Use the collision algorithm to test collision between the triangle and the other convex shape
- algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback);
+ // Create a narrow phase info for the narrow-phase collision detection
+ NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList;
+ narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo)))
+ NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(),
+ triangleShape, mConvexProxyShape->getLocalToWorldTransform(),
+ mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(),
+ mConcaveProxyShape->getCachedCollisionData());
+ narrowPhaseInfoList->next = firstNarrowPhaseInfo;
}
+// Return true and compute a contact info if the two bounding volumes collide
+void ConcaveVsConvexAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ NarrowPhaseCallback* narrowPhaseCallback) {
+
+// ProxyShape* convexProxyShape;
+// ProxyShape* concaveProxyShape;
+// const ConvexShape* convexShape;
+// const ConcaveShape* concaveShape;
+
+// // Collision shape 1 is convex, collision shape 2 is concave
+// if (shape1Info.collisionShape->isConvex()) {
+// convexProxyShape = shape1Info.proxyShape;
+// convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
+// concaveProxyShape = shape2Info.proxyShape;
+// concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
+// }
+// else { // Collision shape 2 is convex, collision shape 1 is concave
+// convexProxyShape = shape2Info.proxyShape;
+// convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+// concaveProxyShape = shape1Info.proxyShape;
+// concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
+// }
+
+// // Set the parameters of the callback object
+// ConvexVsTriangleCallback convexVsTriangleCallback;
+// convexVsTriangleCallback.setCollisionDetection(mCollisionDetection);
+// convexVsTriangleCallback.setConvexShape(convexShape);
+// convexVsTriangleCallback.setConcaveShape(concaveShape);
+// convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
+// convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
+
+// // Compute the convex shape AABB in the local-space of the convex shape
+// AABB aabb;
+// convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
+
+// // If smooth mesh collision is enabled for the concave mesh
+// if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
+
+// std::vector<SmoothMeshContactInfo> contactPoints;
+
+// SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
+
+// convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback);
+
+// // Call the convex vs triangle callback for each triangle of the concave shape
+// concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
+
+// // Run the smooth mesh collision algorithm
+// processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
+// }
+// else {
+
+// convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
+
+// // Call the convex vs triangle callback for each triangle of the concave shape
+// concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
+// }
+}
+
+//// Test collision between a triangle and the convex mesh shape
+//void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
+
+// // Create a triangle collision shape
+// decimal margin = mConcaveShape->getTriangleMargin();
+// TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+
+// // Select the collision algorithm to use between the triangle and the convex shape
+// NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(),
+// mConvexShape->getType());
+
+// // If there is no collision algorithm between those two kinds of shapes
+// if (algo == nullptr) return;
+
+// // Notify the narrow-phase algorithm about the overlapping pair we are going to test
+// algo->setCurrentOverlappingPair(mOverlappingPair);
+
+// // Create the CollisionShapeInfo objects
+// CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(),
+// mOverlappingPair, mConvexProxyShape->getCachedCollisionData());
+// CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape,
+// mConcaveProxyShape->getLocalToWorldTransform(),
+// mOverlappingPair, mConcaveProxyShape->getCachedCollisionData());
+
+// // Use the collision algorithm to test collision between the triangle and the other convex shape
+// algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback);
+//}
+
// Process the concave triangle mesh collision using the smooth mesh collision algorithm described
// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
// issue with some internal edges.
@@ -246,36 +263,37 @@ bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multi
return false;
}
-// Called by a narrow-phase collision algorithm when a new contact has been found
-void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
- const ContactPointInfo& contactInfo) {
- Vector3 triangleVertices[3];
- bool isFirstShapeTriangle;
- // If the collision shape 1 is the triangle
- if (contactInfo.collisionShape1->getType() == CollisionShapeType::TRIANGLE) {
- assert(contactInfo.collisionShape2->getType() != CollisionShapeType::TRIANGLE);
+//// Called by a narrow-phase collision algorithm when a new contact has been found
+//void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
+// const ContactPointInfo& contactInfo) {
+// Vector3 triangleVertices[3];
+// bool isFirstShapeTriangle;
- const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1);
- triangleVertices[0] = triangleShape->getVertex(0);
- triangleVertices[1] = triangleShape->getVertex(1);
- triangleVertices[2] = triangleShape->getVertex(2);
+// // If the collision shape 1 is the triangle
+// if (contactInfo.collisionShape1->getType() == CollisionShapeType::TRIANGLE) {
+// assert(contactInfo.collisionShape2->getType() != CollisionShapeType::TRIANGLE);
- isFirstShapeTriangle = true;
- }
- else { // If the collision shape 2 is the triangle
- assert(contactInfo.collisionShape2->getType() == CollisionShapeType::TRIANGLE);
+// const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1);
+// triangleVertices[0] = triangleShape->getVertex(0);
+// triangleVertices[1] = triangleShape->getVertex(1);
+// triangleVertices[2] = triangleShape->getVertex(2);
- const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2);
- triangleVertices[0] = triangleShape->getVertex(0);
- triangleVertices[1] = triangleShape->getVertex(1);
- triangleVertices[2] = triangleShape->getVertex(2);
+// isFirstShapeTriangle = true;
+// }
+// else { // If the collision shape 2 is the triangle
+// assert(contactInfo.collisionShape2->getType() == CollisionShapeType::TRIANGLE);
- isFirstShapeTriangle = false;
- }
- SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
+// const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2);
+// triangleVertices[0] = triangleShape->getVertex(0);
+// triangleVertices[1] = triangleShape->getVertex(1);
+// triangleVertices[2] = triangleShape->getVertex(2);
- // Add the narrow-phase contact into the list of contact to process for
- // smooth mesh collision
- mContactPoints.push_back(smoothContactInfo);
-}
+// isFirstShapeTriangle = false;
+// }
+// SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
+
+// // Add the narrow-phase contact into the list of contact to process for
+// // smooth mesh collision
+// mContactPoints.push_back(smoothContactInfo);
+//}
diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
index bcd4ef8f..531615da 100644
--- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
+++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
@@ -30,6 +30,7 @@
#include "NarrowPhaseAlgorithm.h"
#include "collision/shapes/ConvexShape.h"
#include "collision/shapes/ConcaveShape.h"
+#include "memory/SingleFrameAllocator.h"
#include <unordered_map>
/// Namespace ReactPhysics3D
@@ -37,73 +38,43 @@ namespace reactphysics3d {
// Class ConvexVsTriangleCallback
/**
- * This class is used to encapsulate a callback method for
- * collision detection between the triangle of a concave mesh shape
- * and a convex shape.
+ * This class is used to report a collision between the triangle
+ * of a concave mesh shape and a convex shape during the
+ * middle-phase algorithm
*/
-class ConvexVsTriangleCallback : public TriangleCallback {
+class MiddlePhaseTriangleCallback : public TriangleCallback {
protected:
- /// Pointer to the collision detection object
- CollisionDetection* mCollisionDetection;
-
- /// Narrow-phase collision callback
- NarrowPhaseCallback* mNarrowPhaseCallback;
-
- /// Convex collision shape to test collision with
- const ConvexShape* mConvexShape;
-
- /// Concave collision shape
- const ConcaveShape* mConcaveShape;
-
- /// Proxy shape of the convex collision shape
- ProxyShape* mConvexProxyShape;
-
- /// Proxy shape of the concave collision shape
- ProxyShape* mConcaveProxyShape;
-
/// Broadphase overlapping pair
OverlappingPair* mOverlappingPair;
- /// Used to sort ContactPointInfos according to their penetration depth
- static bool contactsDepthCompare(const ContactPointInfo& contact1,
- const ContactPointInfo& contact2);
+ /// Pointer to the concave proxy shape
+ ProxyShape* mConcaveProxyShape;
+
+ /// Pointer to the convex proxy shape
+ ProxyShape* mConvexProxyShape;
+
+ /// Pointer to the concave collision shape
+ const ConcaveShape* mConcaveShape;
+
+ /// Reference to the single-frame memory allocator
+ Allocator& mAllocator;
public:
- /// Destructor
- virtual ~ConvexVsTriangleCallback() override = default;
+ /// Pointer to the first element of the linked-list of narrow-phase info
+ NarrowPhaseInfo* narrowPhaseInfoList;
- /// Set the collision detection pointer
- void setCollisionDetection(CollisionDetection* collisionDetection) {
- mCollisionDetection = collisionDetection;
- }
+ /// Constructor
+ MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair,
+ ProxyShape* concaveProxyShape,
+ ProxyShape* convexProxyShape, const ConcaveShape* concaveShape,
+ Allocator& allocator)
+ :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape),
+ mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape),
+ mAllocator(allocator), narrowPhaseInfoList(nullptr) {
- /// Set the narrow-phase collision callback
- void setNarrowPhaseCallback(NarrowPhaseCallback* callback) {
- mNarrowPhaseCallback = callback;
- }
-
- /// Set the convex collision shape to test collision with
- void setConvexShape(const ConvexShape* convexShape) {
- mConvexShape = convexShape;
- }
-
- /// Set the concave collision shape
- void setConcaveShape(const ConcaveShape* concaveShape) {
- mConcaveShape = concaveShape;
- }
-
- /// Set the broadphase overlapping pair
- void setOverlappingPair(OverlappingPair* overlappingPair) {
- mOverlappingPair = overlappingPair;
- }
-
- /// Set the proxy shapes of the two collision shapes
- void setProxyShapes(ProxyShape* convexProxyShape, ProxyShape* concaveProxyShape) {
- mConvexProxyShape = convexProxyShape;
- mConcaveProxyShape = concaveProxyShape;
}
/// Test collision between a triangle and the convex mesh shape
@@ -148,13 +119,14 @@ struct ContactsDepthCompare {
// return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
//}
+// TODO : Delete this
// Class SmoothCollisionNarrowPhaseCallback
/**
* This class is used as a narrow-phase callback to get narrow-phase contacts
* of the concave triangle mesh to temporary store them in order to be used in
* the smooth mesh collision algorithm if this one is enabled.
*/
-class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
+class SmoothCollisionNarrowPhaseCallback {
private:
@@ -169,13 +141,9 @@ class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
}
-
- /// Called by a narrow-phase collision algorithm when a new contact has been found
- virtual void notifyContact(OverlappingPair* overlappingPair,
- const ContactPointInfo& contactInfo) override;
-
};
+// TODO : Delete this
// Class ConcaveVsConvexAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
@@ -183,7 +151,7 @@ class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
* to use the GJK collision detection algorithm to compute the collision between
* the convex shape and each of the triangles in the concave shape.
*/
-class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
+class ConcaveVsConvexAlgorithm {
protected :
@@ -212,7 +180,7 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
ConcaveVsConvexAlgorithm() = default;
/// Destructor
- virtual ~ConcaveVsConvexAlgorithm() override = default;
+ ~ConcaveVsConvexAlgorithm() = default;
/// Private copy-constructor
ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm) = delete;
@@ -221,9 +189,8 @@ class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
- virtual void testCollision(const CollisionShapeInfo& shape1Info,
- const CollisionShapeInfo& shape2Info,
- NarrowPhaseCallback* narrowPhaseCallback) override;
+ void testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ NarrowPhaseCallback* narrowPhaseCallback);
};
// Add a triangle vertex into the set of processed triangles
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
index f6812d74..35b1e50b 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
@@ -29,16 +29,6 @@
using namespace reactphysics3d;
-/// Initialize the collision dispatch configuration
-void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection,
- PoolAllocator* memoryAllocator) {
-
- // Initialize the collision algorithms
- mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator);
- mGJKAlgorithm.init(collisionDetection, memoryAllocator);
- mConcaveVsConvexAlgorithm.init(collisionDetection, memoryAllocator);
-}
-
// Select and return the narrow-phase collision detection algorithm to
// use between two types of collision shapes.
NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) {
@@ -46,19 +36,14 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
- // Sphere vs Sphere algorithm
- if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) {
- return &mSphereVsSphereAlgorithm;
- }
- // Concave vs Convex algorithm
- else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
- (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
- return &mConcaveVsConvexAlgorithm;
- }
// Convex vs Convex algorithm (GJK algorithm)
- else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
+ if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
return &mGJKAlgorithm;
}
+ // Sphere vs Sphere algorithm
+ else if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) {
+ return &mSphereVsSphereAlgorithm;
+ }
else {
return nullptr;
}
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h
index 5dd07bf2..f62a6564 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.h
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.h
@@ -47,9 +47,6 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Sphere vs Sphere collision algorithm
SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
- /// Concave vs Convex collision algorithm
- ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm;
-
/// GJK Algorithm
GJKAlgorithm mGJKAlgorithm;
@@ -61,10 +58,6 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Destructor
virtual ~DefaultCollisionDispatch() override = default;
- /// Initialize the collision dispatch configuration
- virtual void init(CollisionDetection* collisionDetection,
- PoolAllocator* memoryAllocator) override;
-
/// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes.
virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) override;
diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
index 6fc19680..8779c33e 100644
--- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
+++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
@@ -74,25 +74,22 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
/// the correct penetration depth. This method returns true if the EPA penetration
/// depth computation has succeeded and false it has failed.
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
- CollisionShapeInfo shape1Info,
- const Transform& transform1,
- CollisionShapeInfo shape2Info,
- const Transform& transform2,
+ const NarrowPhaseInfo* narrowPhaseInfo,
Vector3& v,
- NarrowPhaseCallback* narrowPhaseCallback) {
+ ContactPointInfo& contactPointInfo) {
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
decimal gjkPenDepthSquare = v.lengthSquare();
- assert(shape1Info.collisionShape->isConvex());
- assert(shape2Info.collisionShape->isConvex());
+ assert(narrowPhaseInfo->collisionShape1->isConvex());
+ assert(narrowPhaseInfo->collisionShape2->isConvex());
- const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
- const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+ const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
+ const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
- void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
- void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+ void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1;
+ void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2;
Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
@@ -103,12 +100,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex&
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
- Transform body2Tobody1 = transform1.getInverse() * transform2;
+ Transform body2Tobody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform;
// Matrix that transform a direction from local
// space of body 1 into local space of body 2
- Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
- transform1.getOrientation();
+ Quaternion rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getInverse() *
+ narrowPhaseInfo->shape1ToWorldTransform.getOrientation();
// Get the simplex computed previously by the GJK algorithm
int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
@@ -416,7 +413,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex&
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info
- v = transform1.getOrientation() * triangle->getClosestPoint();
+ v = narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * triangle->getClosestPoint();
Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
@@ -430,10 +427,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex&
if (penetrationDepth * penetrationDepth > gjkPenDepthSquare && penetrationDepth > 0) {
// Create the contact info object
- ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
- shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
-
- narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+ contactPointInfo.init(normal, penetrationDepth, pALocal, pBLocal);
return true;
}
diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h
index 05a78eea..1d241540 100644
--- a/src/collision/narrowphase/EPA/EPAAlgorithm.h
+++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h
@@ -29,7 +29,7 @@
// Libraries
#include "collision/narrowphase/GJK/VoronoiSimplex.h"
#include "collision/shapes/CollisionShape.h"
-#include "collision/CollisionShapeInfo.h"
+#include "collision/NarrowPhaseInfo.h"
#include "constraint/ContactPoint.h"
#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
#include "mathematics/mathematics.h"
@@ -87,9 +87,6 @@ class EPAAlgorithm {
// -------------------- Attributes -------------------- //
- /// Reference to the memory allocator
- PoolAllocator* mMemoryAllocator;
-
/// Triangle comparison operator
TriangleComparison mTriangleComparison;
@@ -119,17 +116,11 @@ class EPAAlgorithm {
/// Deleted assignment operator
EPAAlgorithm& operator=(const EPAAlgorithm& algorithm) = delete;
- /// Initalize the algorithm
- void init(PoolAllocator* memoryAllocator);
-
/// Compute the penetration depth with EPA algorithm.
bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
- CollisionShapeInfo shape1Info,
- const Transform& transform1,
- CollisionShapeInfo shape2Info,
- const Transform& transform2,
+ const NarrowPhaseInfo* narrowPhaseInfo,
Vector3& v,
- NarrowPhaseCallback* narrowPhaseCallback);
+ ContactPointInfo &contactPointInfo);
};
// Add a triangle face in the candidate triangle heap in the EPA algorithm
@@ -150,11 +141,6 @@ inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA**
}
}
-// Initalize the algorithm
-inline void EPAAlgorithm::init(PoolAllocator* memoryAllocator) {
- mMemoryAllocator = memoryAllocator;
-}
-
}
#endif
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index b7916a53..dec43865 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -36,11 +36,6 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-// Constructor
-GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() {
-
-}
-
// Compute a contact info if the two collision shapes collide.
/// This method implements the Hybrid Technique for computing the penetration depth by
/// running the GJK algorithm on original objects (without margin). If the shapes intersect
@@ -50,9 +45,8 @@ GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() {
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects.
-void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
- const CollisionShapeInfo& shape2Info,
- NarrowPhaseCallback* narrowPhaseCallback) {
+bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) {
PROFILE("GJKAlgorithm::testCollision()");
@@ -65,20 +59,20 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
decimal prevDistSquare;
bool contactFound = false;
- assert(shape1Info.collisionShape->isConvex());
- assert(shape2Info.collisionShape->isConvex());
+ assert(narrowPhaseInfo->collisionShape1->isConvex());
+ assert(narrowPhaseInfo->collisionShape2->isConvex());
- const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
- const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+ const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
+ const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
- void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
- void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+ void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1;
+ void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2;
bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
// Get the local-space to world-space transforms
- const Transform transform1 = shape1Info.shapeToWorldTransform;
- const Transform transform2 = shape2Info.shapeToWorldTransform;
+ const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
+ const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
@@ -92,13 +86,13 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// Initialize the margin (sum of margins of both objects)
decimal margin = shape1->getMargin() + shape2->getMargin();
decimal marginSquare = margin * margin;
- assert(margin > 0.0);
+ assert(margin > decimal(0.0));
// Create a simplex set
VoronoiSimplex simplex;
// Get the previous point V (last cached separating axis)
- Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
+ Vector3 v = narrowPhaseInfo->overlappingPair->getCachedSeparatingAxis();
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
@@ -116,13 +110,13 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
vDotw = v.dot(w);
// If the enlarge objects (with margins) do not intersect
- if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
+ if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence
- mCurrentOverlappingPair->setCachedSeparatingAxis(v);
+ narrowPhaseInfo->overlappingPair->setCachedSeparatingAxis(v);
// No intersection, we return
- return;
+ return false;
}
// If the objects intersect only in the margins
@@ -188,8 +182,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// 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.
- isEPAResultValid = computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info,
- transform2, narrowPhaseCallback, v);
+ isEPAResultValid = computePenetrationDepthForEnlargedObjects(narrowPhaseInfo, contactPointInfo, v);
}
if ((contactFound || !isEPAResultValid) && distSquare > MACHINE_EPSILON) {
@@ -200,7 +193,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = std::sqrt(distSquare);
- assert(dist > 0.0);
+ assert(dist > decimal(0.0));
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
@@ -209,21 +202,22 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
decimal penetrationDepth = margin - dist;
// If the penetration depth is negative (due too numerical errors), there is no contact
- if (penetrationDepth <= 0.0) {
- return;
+ if (penetrationDepth <= decimal(0.0)) {
+ return false;
}
// Do not generate a contact point with zero normal length
if (normal.lengthSquare() < MACHINE_EPSILON) {
- return;
+ return false;
}
// Create the contact info object
- ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
- shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
+ contactPointInfo.init(normal, penetrationDepth, pA, pB);
- narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+ return true;
}
+
+ return false;
}
/// This method runs the GJK algorithm on the two enlarged objects (with margin)
@@ -231,11 +225,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
/// assumed to intersect in the original objects (without margin). Therefore such
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to
/// compute the correct penetration depth and contact points of the enlarged objects.
-bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
- const Transform& transform1,
- const CollisionShapeInfo& shape2Info,
- const Transform& transform2,
- NarrowPhaseCallback* narrowPhaseCallback,
+bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo,
Vector3& v) {
PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
@@ -247,24 +238,24 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
decimal distSquare = DECIMAL_LARGEST;
decimal prevDistSquare;
- assert(shape1Info.collisionShape->isConvex());
- assert(shape2Info.collisionShape->isConvex());
+ assert(narrowPhaseInfo->collisionShape1->isConvex());
+ assert(narrowPhaseInfo->collisionShape2->isConvex());
- const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
- const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
+ const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
+ const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
- void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
- void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
+ void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1;
+ void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2;
// Transform a point from local space of body 2 to local space
// of body 1 (the GJK algorithm is done in local space of body 1)
- Transform body2ToBody1 = transform1.getInverse() * transform2;
+ Transform body2ToBody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform;
// Matrix that transform a direction from local space of body 1 into local space of body 2
- Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
- transform1.getOrientation().getMatrix();
+ Matrix3x3 rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getMatrix().getTranspose() *
+ narrowPhaseInfo->shape1ToWorldTransform.getOrientation().getMatrix();
do {
// Compute the support points for the enlarged object A and B
@@ -277,7 +268,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
vDotw = v.dot(w);
// If the enlarge objects do not intersect
- if (vDotw > 0.0) {
+ if (vDotw > decimal(0.0)) {
// No intersection, we return
return false;
@@ -308,9 +299,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
// Give the simplex computed with GJK algorithm to the EPA algorithm
// which will compute the correct penetration depth and contact points
// between the two enlarged objects
- return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info,
- transform1, shape2Info, transform2,
- v, narrowPhaseCallback);
+ return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, narrowPhaseInfo, v, contactPointInfo);
}
// Use the GJK Algorithm to find if a point is inside a convex collision shape
@@ -378,7 +367,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
return true;
}
-
// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h
index b6cce5c2..e0dad891 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.h
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h
@@ -69,11 +69,8 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
// -------------------- Methods -------------------- //
/// Compute the penetration depth for enlarged objects.
- bool computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
- const Transform& transform1,
- const CollisionShapeInfo& shape2Info,
- const Transform& transform2,
- NarrowPhaseCallback* narrowPhaseCallback,
+ bool computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo,
Vector3& v);
public :
@@ -81,7 +78,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
// -------------------- Methods -------------------- //
/// Constructor
- GJKAlgorithm();
+ GJKAlgorithm() = default;
/// Destructor
~GJKAlgorithm() = default;
@@ -92,14 +89,9 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete;
- /// Initalize the algorithm
- virtual void init(CollisionDetection* collisionDetection,
- PoolAllocator* memoryAllocator) override;
-
/// Compute a contact info if the two bounding volumes collide.
- virtual void testCollision(const CollisionShapeInfo& shape1Info,
- const CollisionShapeInfo& shape2Info,
- NarrowPhaseCallback* narrowPhaseCallback) override;
+ virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) override;
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
@@ -108,13 +100,6 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
};
-// Initalize the algorithm
-inline void GJKAlgorithm::init(CollisionDetection* collisionDetection,
- PoolAllocator* memoryAllocator) {
- NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator);
- mAlgoEPA.init(memoryAllocator);
-}
-
}
#endif
diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
index 0759d138..f7f68907 100644
--- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h
+++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
@@ -31,7 +31,7 @@
#include "constraint/ContactPoint.h"
#include "memory/PoolAllocator.h"
#include "engine/OverlappingPair.h"
-#include "collision/CollisionShapeInfo.h"
+#include "collision/NarrowPhaseInfo.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@@ -67,21 +67,12 @@ class NarrowPhaseAlgorithm {
// -------------------- Attributes -------------------- //
- /// Pointer to the collision detection object
- CollisionDetection* mCollisionDetection;
-
- /// Pointer to the memory allocator
- PoolAllocator* mMemoryAllocator;
-
- /// Overlapping pair of the bodies currently tested for collision
- OverlappingPair* mCurrentOverlappingPair;
-
public :
// -------------------- Methods -------------------- //
/// Constructor
- NarrowPhaseAlgorithm();
+ NarrowPhaseAlgorithm() = default;
/// Destructor
virtual ~NarrowPhaseAlgorithm() = default;
@@ -92,23 +83,10 @@ class NarrowPhaseAlgorithm {
/// Deleted assignment operator
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
- /// Initalize the algorithm
- virtual void init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator);
-
- /// Set the current overlapping pair of bodies
- void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
-
/// Compute a contact info if the two bounding volume collide
- virtual void testCollision(const CollisionShapeInfo& shape1Info,
- const CollisionShapeInfo& shape2Info,
- NarrowPhaseCallback* narrowPhaseCallback)=0;
+ virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo)=0;
};
-// Set the current overlapping pair of bodies
-inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) {
- mCurrentOverlappingPair = overlappingPair;
-}
-
}
#endif
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
index 9d2c4baa..6b28372d 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
@@ -30,17 +30,16 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
- const CollisionShapeInfo& shape2Info,
- NarrowPhaseCallback* narrowPhaseCallback) {
+bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) {
// Get the sphere collision shapes
- const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape);
- const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape);
+ const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
+ const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape2);
// Get the local-space to world-space transforms
- const Transform& transform1 = shape1Info.shapeToWorldTransform;
- const Transform& transform2 = shape2Info.shapeToWorldTransform;
+ const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
+ const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
@@ -60,11 +59,11 @@ void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
- ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
- shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth,
- intersectionOnBody1, intersectionOnBody2);
+ contactPointInfo.init(vectorBetweenCenters.getUnit(), penetrationDepth,
+ intersectionOnBody1, intersectionOnBody2);
- // Notify about the new contact
- narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
+ return true;
}
+
+ return false;
}
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
index 525c55d5..24886f7f 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
@@ -61,9 +61,8 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
- virtual void testCollision(const CollisionShapeInfo& shape1Info,
- const CollisionShapeInfo& shape2Info,
- NarrowPhaseCallback* narrowPhaseCallback) override;
+ virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) override;
};
}
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index 994f5426..4028ed8f 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -28,7 +28,7 @@
// Libraries
#include "body/CollisionBody.h"
-#include "collision/CollisionShapeInfo.h"
+#include "collision/NarrowPhaseInfo.h"
#include "configuration.h"
#include "mathematics/mathematics.h"
#include "configuration.h"
@@ -53,20 +53,6 @@ struct ContactPointInfo {
// -------------------- Attributes -------------------- //
- // TODO : Check if we really need the shape1, shape2, collisionShape1 and collisionShape2 fields
-
- /// First proxy shape of the contact
- ProxyShape* shape1;
-
- /// Second proxy shape of the contact
- ProxyShape* shape2;
-
- /// First collision shape
- const CollisionShape* collisionShape1;
-
- /// Second collision shape
- const CollisionShape* collisionShape2;
-
/// Normalized normal vector of the collision contact in world space
Vector3 normal;
@@ -82,13 +68,18 @@ struct ContactPointInfo {
// -------------------- Methods -------------------- //
/// Constructor
- ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1,
- const CollisionShape* collShape2, const Vector3& normal, decimal penetrationDepth,
- const Vector3& localPoint1, const Vector3& localPoint2)
- : shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2),
- normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1),
- localPoint2(localPoint2) {
+ ContactPointInfo() = default;
+ /// Destructor
+ ~ContactPointInfo() = default;
+
+ /// Initialize the contact point info
+ void init(const Vector3& contactNormal, decimal penDepth,
+ const Vector3& localPt1, const Vector3& localPt2) {
+ normal = contactNormal;
+ penetrationDepth = penDepth;
+ localPoint1 = localPt1;
+ localPoint2 = localPt2;
}
};
diff --git a/src/containers/LinkedList.h b/src/containers/LinkedList.h
new file mode 100644
index 00000000..281c134c
--- /dev/null
+++ b/src/containers/LinkedList.h
@@ -0,0 +1,124 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_LINKED_LIST_H
+#define REACTPHYSICS3D_LINKED_LIST_H
+
+// Libraries
+#include "memory/Allocator.h"
+
+namespace reactphysics3d {
+
+// Class LinkedList
+/**
+ * This class represents a simple generic linked list.
+ */
+template<typename T>
+class LinkedList {
+
+ public:
+
+ /// Element of the linked list
+ struct ListElement {
+
+ /// Data of the list element
+ T data;
+
+ /// Pointer to the next element of the list
+ ListElement* next;
+
+ /// Constructor
+ ListElement(T elementData, ListElement* nextElement)
+ : data(elementData), next(nextElement) {
+
+ }
+ };
+
+ private:
+
+ // -------------------- Attributes -------------------- //
+
+ /// Pointer to the first element of the list
+ ListElement* mListHead;
+
+ /// Memory allocator used to allocate the list elements
+ Allocator& mAllocator;
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ LinkedList(Allocator& allocator) : mListHead(nullptr), mAllocator(allocator) {
+
+ }
+
+ /// Destructor
+ ~LinkedList() {
+ reset();
+ }
+
+ /// Return the first element of the list
+ ListElement* getListHead() const;
+
+ /// Insert an element at the beginning of the linked list
+ void insert(const T& data);
+
+ /// Remove all the elements of the list
+ void reset();
+
+};
+
+// Return the first element of the list
+template<typename T>
+inline typename LinkedList<T>::ListElement* LinkedList<T>::getListHead() const {
+ return mListHead;
+}
+
+// Insert an element at the beginning of the linked list
+template<typename T>
+inline void LinkedList<T>::insert(const T& data) {
+ ListElement* element = new (mAllocator.allocate(sizeof(ListElement))) ListElement(data, mListHead);
+ mListHead = element;
+}
+
+// Remove all the elements of the list
+template<typename T>
+inline void LinkedList<T>::reset() {
+
+ // Release all the list elements
+ ListElement* element = mListHead;
+ while (element != nullptr) {
+ ListElement* nextElement = element->next;
+ mAllocator.release(element, sizeof(ListElement));
+ element = nextElement;
+ }
+
+ mListHead = nullptr;
+}
+
+}
+
+#endif
diff --git a/src/memory/Stack.h b/src/containers/Stack.h
similarity index 100%
rename from src/memory/Stack.h
rename to src/containers/Stack.h
diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp
index b1b004ec..d160021c 100644
--- a/src/engine/CollisionWorld.cpp
+++ b/src/engine/CollisionWorld.cpp
@@ -33,7 +33,8 @@ using namespace std;
// Constructor
CollisionWorld::CollisionWorld()
- : mCollisionDetection(this, mPoolAllocator), mCurrentBodyID(0),
+ : mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES),
+ mCollisionDetection(this, mPoolAllocator, mSingleFrameAllocator), mCurrentBodyID(0),
mEventListener(nullptr) {
}
@@ -148,119 +149,18 @@ bool CollisionWorld::testAABBOverlap(const CollisionBody* body1,
return body1AABB.testCollision(body2AABB);
}
-// Test and report collisions between a given shape and all the others
-// shapes of the world.
+// Report all the bodies that overlap with the aabb in parameter
/**
- * @param shape Pointer to the proxy shape to test
- * @param callback Pointer to the object with the callback method
+ * @param aabb AABB used to test for overlap
+ * @param overlapCallback Pointer to the callback class to report overlap
+ * @param categoryMaskBits bits mask used to filter the bodies to test overlap with
*/
-void CollisionWorld::testCollision(const ProxyShape* shape,
- CollisionCallback* callback) {
-
- // Reset all the contact manifolds lists of each body
- resetContactManifoldListsOfBodies();
-
- // Create the sets of shapes
- std::set<uint> shapes;
- shapes.insert(shape->mBroadPhaseID);
- std::set<uint> emptySet;
-
- // Perform the collision detection and report contacts
- mCollisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
+inline void CollisionWorld::testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits) {
+ mCollisionDetection.testAABBOverlap(aabb, overlapCallback, categoryMaskBits);
}
-// Test and report collisions between two given shapes
-/**
- * @param shape1 Pointer to the first proxy shape to test
- * @param shape2 Pointer to the second proxy shape to test
- * @param callback Pointer to the object with the callback method
- */
-void CollisionWorld::testCollision(const ProxyShape* shape1,
- const ProxyShape* shape2,
- CollisionCallback* callback) {
-
- // Reset all the contact manifolds lists of each body
- resetContactManifoldListsOfBodies();
-
- // Create the sets of shapes
- std::set<uint> shapes1;
- shapes1.insert(shape1->mBroadPhaseID);
- std::set<uint> shapes2;
- shapes2.insert(shape2->mBroadPhaseID);
-
- // Perform the collision detection and report contacts
- mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
-}
-
-// Test and report collisions between a body and all the others bodies of the
-// world
-/**
- * @param body Pointer to the first body to test
- * @param callback Pointer to the object with the callback method
- */
-void CollisionWorld::testCollision(const CollisionBody* body,
- CollisionCallback* callback) {
-
- // Reset all the contact manifolds lists of each body
- resetContactManifoldListsOfBodies();
-
- // Create the sets of shapes
- std::set<uint> shapes1;
-
- // For each shape of the body
- for (const ProxyShape* shape=body->getProxyShapesList(); shape != nullptr;
- shape = shape->getNext()) {
- shapes1.insert(shape->mBroadPhaseID);
- }
-
- std::set<uint> emptySet;
-
- // Perform the collision detection and report contacts
- mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
-}
-
-// Test and report collisions between two bodies
-/**
- * @param body1 Pointer to the first body to test
- * @param body2 Pointer to the second body to test
- * @param callback Pointer to the object with the callback method
- */
-void CollisionWorld::testCollision(const CollisionBody* body1,
- const CollisionBody* body2,
- CollisionCallback* callback) {
-
- // Reset all the contact manifolds lists of each body
- resetContactManifoldListsOfBodies();
-
- // Create the sets of shapes
- std::set<uint> shapes1;
- for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr;
- shape = shape->getNext()) {
- shapes1.insert(shape->mBroadPhaseID);
- }
-
- std::set<uint> shapes2;
- for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr;
- shape = shape->getNext()) {
- shapes2.insert(shape->mBroadPhaseID);
- }
-
- // Perform the collision detection and report contacts
- mCollisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
-}
-
-// Test and report collisions between all shapes of the world
-/**
- * @param callback Pointer to the object with the callback method
- */
-void CollisionWorld::testCollision(CollisionCallback* callback) {
-
- // Reset all the contact manifolds lists of each body
- resetContactManifoldListsOfBodies();
-
- std::set<uint> emptySet;
-
- // Perform the collision detection and report contacts
- mCollisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);
+// Return true if two bodies overlap
+bool CollisionWorld::testOverlap(CollisionBody* body1, CollisionBody* body2) {
+ return mCollisionDetection.testOverlap(body1, body2);
}
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index d04acb4e..dc162716 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -47,6 +47,7 @@ namespace reactphysics3d {
// Declarations
class CollisionCallback;
+class OverlapCallback;
// Class CollisionWorld
/**
@@ -60,6 +61,12 @@ class CollisionWorld {
// -------------------- Attributes -------------------- //
+ /// Pool Memory allocator
+ PoolAllocator mPoolAllocator;
+
+ /// Single frame Memory allocator
+ SingleFrameAllocator mSingleFrameAllocator;
+
/// Reference to the collision detection
CollisionDetection mCollisionDetection;
@@ -72,9 +79,6 @@ class CollisionWorld {
/// List of free ID for rigid bodies
std::vector<luint> mFreeBodiesIDs;
- /// Pool Memory allocator
- PoolAllocator mPoolAllocator;
-
/// Pointer to an event listener object
EventListener* mEventListener;
@@ -125,32 +129,23 @@ class CollisionWorld {
bool testAABBOverlap(const CollisionBody* body1,
const CollisionBody* body2) const;
- /// Test if the AABBs of two proxy shapes overlap
- bool testAABBOverlap(const ProxyShape* shape1,
- const ProxyShape* shape2) const;
+ /// Report all the bodies that overlap with the AABB in parameter
+ void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
- /// Test and report collisions between a given shape and all the others
- /// shapes of the world
- virtual void testCollision(const ProxyShape* shape,
- CollisionCallback* callback);
+ /// Return true if two bodies overlap
+ bool testOverlap(CollisionBody* body1, CollisionBody* body2);
- /// Test and report collisions between two given shapes
- virtual void testCollision(const ProxyShape* shape1,
- const ProxyShape* shape2,
- CollisionCallback* callback);
-
- /// Test and report collisions between a body and all the others bodies of the
- /// world
- virtual void testCollision(const CollisionBody* body,
- CollisionCallback* callback);
+ /// Report all the bodies that overlap with the body in parameter
+ void testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
/// Test and report collisions between two bodies
- virtual void testCollision(const CollisionBody* body1,
- const CollisionBody* body2,
- CollisionCallback* callback);
+ void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback);
+
+ /// Test and report collisions between a body and all the others bodies of the world
+ void testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits = 0xFFFF);
/// Test and report collisions between all shapes of the world
- virtual void testCollision(CollisionCallback* callback);
+ void testCollision(CollisionCallback* callback);
// -------------------- Friendship -------------------- //
@@ -200,36 +195,100 @@ inline void CollisionWorld::raycast(const Ray& ray,
mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits);
}
-// Test if the AABBs of two proxy shapes overlap
+// Test and report collisions between two bodies
/**
- * @param shape1 Pointer to the first proxy shape to test
- * @param shape2 Pointer to the second proxy shape to test
- * @return
+ * @param body1 Pointer to the first body to test
+ * @param body2 Pointer to the second body to test
+ * @param callback Pointer to the object with the callback method
*/
-inline bool CollisionWorld::testAABBOverlap(const ProxyShape* shape1,
- const ProxyShape* shape2) const {
+inline void CollisionWorld::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback) {
+ mCollisionDetection.testCollision(body1, body2, callback);
+}
- return mCollisionDetection.testAABBOverlap(shape1, shape2);
+// Test and report collisions between a body and all the others bodies of the world
+/**
+ * @param body Pointer to the body against which we need to test collision
+ * @param callback Pointer to the object with the callback method to report contacts
+ * @param categoryMaskBits Bits mask corresponding to the category of bodies we need to test collision with
+ */
+inline void CollisionWorld::testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits) {
+ mCollisionDetection.testCollision(body, callback, categoryMaskBits);
+}
+
+// Test and report collisions between all bodies of the world
+/**
+ * @param callback Pointer to the object with the callback method to report contacts
+ */
+inline void CollisionWorld::testCollision(CollisionCallback* callback) {
+ mCollisionDetection.testCollision(callback);
+}
+
+// Report all the bodies that overlap with the body in parameter
+/**
+ * @param body Pointer to the collision body to test overlap with
+ * @param overlapCallback Pointer to the callback class to report overlap
+ * @param categoryMaskBits bits mask used to filter the bodies to test overlap with
+ */
+inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits) {
+ mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits);
}
// Class CollisionCallback
/**
* This class can be used to register a callback for collision test queries.
* You should implement your own class inherited from this one and implement
- * the notifyRaycastHit() method. This method will be called for each ProxyShape
- * that is hit by the ray.
+ * the notifyContact() method. This method will called each time a contact
+ * point is reported.
*/
class CollisionCallback {
public:
+ struct CollisionCallbackInfo {
+
+ public:
+ const ContactPointInfo& contactPoint;
+ CollisionBody* body1;
+ CollisionBody* body2;
+ const ProxyShape* proxyShape1;
+ const ProxyShape* proxyShape2;
+
+ // Constructor
+ CollisionCallbackInfo(const ContactPointInfo& point, CollisionBody* b1, CollisionBody* b2,
+ const ProxyShape* proxShape1, const ProxyShape* proxShape2) :
+ contactPoint(point), body1(b1), body2(b2),
+ proxyShape1(proxShape1), proxyShape2(proxShape2) {
+
+ }
+ };
+
/// Destructor
virtual ~CollisionCallback() {
}
- /// This method will be called for contact
- virtual void notifyContact(const ContactPointInfo& contactPointInfo)=0;
+ /// This method will be called for each reported contact point
+ virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
+};
+
+// Class OverlapCallback
+/**
+ * This class can be used to register a callback for collision overlap queries.
+ * You should implement your own class inherited from this one and implement
+ * the notifyOverlap() method. This method will called each time a contact
+ * point is reported.
+ */
+class OverlapCallback {
+
+ public:
+
+ /// Destructor
+ virtual ~OverlapCallback() {
+
+ }
+
+ /// This method will be called for each reported overlapping bodies
+ virtual void notifyOverlap(CollisionBody* collisionBody)=0;
};
}
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index d52348f8..4f7bd15a 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -40,7 +40,6 @@ using namespace std;
*/
DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
: CollisionWorld(),
- mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES),
mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator),
mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
@@ -829,117 +828,6 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
}
}
-// Test and report collisions between a given shape and all the others
-// shapes of the world.
-/// This method should be called after calling the
-/// DynamicsWorld::update() method that will compute the collisions.
-/**
- * @param shape Pointer to the proxy shape to test
- * @param callback Pointer to the object with the callback method
- */
-void DynamicsWorld::testCollision(const ProxyShape* shape,
- CollisionCallback* callback) {
-
- // Create the sets of shapes
- std::set<uint> shapes;
- shapes.insert(shape->mBroadPhaseID);
- std::set<uint> emptySet;
-
- // Perform the collision detection and report contacts
- mCollisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet);
-}
-
-// Test and report collisions between two given shapes.
-/// This method should be called after calling the
-/// DynamicsWorld::update() method that will compute the collisions.
-/**
- * @param shape1 Pointer to the first proxy shape to test
- * @param shape2 Pointer to the second proxy shape to test
- * @param callback Pointer to the object with the callback method
- */
-void DynamicsWorld::testCollision(const ProxyShape* shape1,
- const ProxyShape* shape2,
- CollisionCallback* callback) {
-
- // Create the sets of shapes
- std::set<uint> shapes1;
- shapes1.insert(shape1->mBroadPhaseID);
- std::set<uint> shapes2;
- shapes2.insert(shape2->mBroadPhaseID);
-
- // Perform the collision detection and report contacts
- mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
-}
-
-// Test and report collisions between a body and all the others bodies of the
-// world.
-/// This method should be called after calling the
-/// DynamicsWorld::update() method that will compute the collisions.
-/**
- * @param body Pointer to the first body to test
- * @param callback Pointer to the object with the callback method
- */
-void DynamicsWorld::testCollision(const CollisionBody* body,
- CollisionCallback* callback) {
-
- // Create the sets of shapes
- std::set<uint> shapes1;
-
- // For each shape of the body
- for (const ProxyShape* shape=body->getProxyShapesList(); shape != nullptr;
- shape = shape->getNext()) {
- shapes1.insert(shape->mBroadPhaseID);
- }
-
- std::set<uint> emptySet;
-
- // Perform the collision detection and report contacts
- mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet);
-}
-
-// Test and report collisions between two bodies.
-/// This method should be called after calling the
-/// DynamicsWorld::update() method that will compute the collisions.
-/**
- * @param body1 Pointer to the first body to test
- * @param body2 Pointer to the second body to test
- * @param callback Pointer to the object with the callback method
- */
-void DynamicsWorld::testCollision(const CollisionBody* body1,
- const CollisionBody* body2,
- CollisionCallback* callback) {
-
- // Create the sets of shapes
- std::set<uint> shapes1;
- for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr;
- shape = shape->getNext()) {
- shapes1.insert(shape->mBroadPhaseID);
- }
-
- std::set<uint> shapes2;
- for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr;
- shape = shape->getNext()) {
- shapes2.insert(shape->mBroadPhaseID);
- }
-
- // Perform the collision detection and report contacts
- mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
-}
-
-// Test and report collisions between all shapes of the world.
-/// This method should be called after calling the
-/// DynamicsWorld::update() method that will compute the collisions.
-/**
- * @param callback Pointer to the object with the callback method
- */
-void DynamicsWorld::testCollision(CollisionCallback* callback) {
-
- std::set<uint> emptySet;
-
- // Perform the collision detection and report contacts
- mCollisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
-}
-
/// Return the list of all contacts of the world
std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h
index 746e8255..b632dcf2 100644
--- a/src/engine/DynamicsWorld.h
+++ b/src/engine/DynamicsWorld.h
@@ -50,9 +50,6 @@ class DynamicsWorld : public CollisionWorld {
// -------------------- Attributes -------------------- //
- /// Single frame Memory allocator
- SingleFrameAllocator mSingleFrameAllocator;
-
/// Contact solver
ContactSolver mContactSolver;
@@ -267,29 +264,6 @@ class DynamicsWorld : public CollisionWorld {
/// Set an event listener object to receive events callbacks.
void setEventListener(EventListener* eventListener);
- /// Test and report collisions between a given shape and all the others
- /// shapes of the world
- virtual void testCollision(const ProxyShape* shape,
- CollisionCallback* callback) override;
-
- /// Test and report collisions between two given shapes
- virtual void testCollision(const ProxyShape* shape1,
- const ProxyShape* shape2,
- CollisionCallback* callback) override;
-
- /// Test and report collisions between a body and all
- /// the others bodies of the world
- virtual void testCollision(const CollisionBody* body,
- CollisionCallback* callback) override;
-
- /// Test and report collisions between two bodies
- virtual void testCollision(const CollisionBody* body1,
- const CollisionBody* body2,
- CollisionCallback* callback) override;
-
- /// Test and report collisions between all shapes of the world
- virtual void testCollision(CollisionCallback* callback) override;
-
/// Return the list of all contacts of the world
std::vector<const ContactManifold*> getContactsList() const;
diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp b/src/memory/Allocator.h
similarity index 75%
rename from src/collision/narrowphase/NarrowPhaseAlgorithm.cpp
rename to src/memory/Allocator.h
index 782f8d6f..f7b9ac73 100644
--- a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp
+++ b/src/memory/Allocator.h
@@ -23,20 +23,34 @@
* *
********************************************************************************/
+#ifndef REACTPHYSICS3D_ALLOCATOR_H
+#define REACTPHYSICS3D_ALLOCATOR_H
+
// Libraries
-#include "NarrowPhaseAlgorithm.h"
+#include <cstring>
-// We want to use the ReactPhysics3D namespace
-using namespace reactphysics3d;
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
-// Constructor
-NarrowPhaseAlgorithm::NarrowPhaseAlgorithm()
- : mMemoryAllocator(nullptr), mCurrentOverlappingPair(nullptr) {
+// Class Allocator
+/**
+ * Abstract class with the basic interface of all the derived memory allocators
+ */
+class Allocator {
+
+ public:
+
+ /// Destructor
+ virtual ~Allocator() = default;
+
+ /// Allocate memory of a given size (in bytes) and return a pointer to the
+ /// allocated memory.
+ virtual void* allocate(size_t size)=0;
+
+ /// Release previously allocated memory.
+ virtual void release(void* pointer, size_t size)=0;
+};
}
-// Initalize the algorithm
-void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator) {
- mCollisionDetection = collisionDetection;
- mMemoryAllocator = memoryAllocator;
-}
+#endif
diff --git a/src/memory/PoolAllocator.h b/src/memory/PoolAllocator.h
index 2af762ca..71e11714 100644
--- a/src/memory/PoolAllocator.h
+++ b/src/memory/PoolAllocator.h
@@ -27,8 +27,8 @@
#define REACTPHYSICS3D_POOL_ALLOCATOR_H
// Libraries
-#include <cstring>
#include "configuration.h"
+#include "Allocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -40,7 +40,7 @@ namespace reactphysics3d {
* efficiently. This implementation is inspired by the small block allocator
* described here : http://www.codeproject.com/useritems/Small_Block_Allocator.asp
*/
-class PoolAllocator {
+class PoolAllocator : public Allocator {
private :
@@ -129,14 +129,14 @@ class PoolAllocator {
PoolAllocator();
/// Destructor
- ~PoolAllocator();
+ virtual ~PoolAllocator() override;
/// Allocate memory of a given size (in bytes) and return a pointer to the
/// allocated memory.
- void* allocate(size_t size);
+ virtual void* allocate(size_t size) override;
/// Release previously allocated memory.
- void release(void* pointer, size_t size);
+ virtual void release(void* pointer, size_t size) override;
};
diff --git a/src/memory/SingleFrameAllocator.h b/src/memory/SingleFrameAllocator.h
index e3151f0a..cc030daa 100644
--- a/src/memory/SingleFrameAllocator.h
+++ b/src/memory/SingleFrameAllocator.h
@@ -27,7 +27,7 @@
#define REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H
// Libraries
-#include <cstring>
+#include "Allocator.h"
#include "configuration.h"
/// ReactPhysics3D namespace
@@ -38,7 +38,7 @@ namespace reactphysics3d {
* This class represent a memory allocator used to efficiently allocate
* memory on the heap that is used during a single frame.
*/
-class SingleFrameAllocator {
+class SingleFrameAllocator : public Allocator {
private :
@@ -61,13 +61,18 @@ class SingleFrameAllocator {
SingleFrameAllocator(size_t totalSizeBytes);
/// Destructor
- ~SingleFrameAllocator();
+ virtual ~SingleFrameAllocator() override;
/// Allocate memory of a given size (in bytes)
- void* allocate(size_t size);
+ virtual void* allocate(size_t size) override;
+
+ /// Release previously allocated memory.
+ virtual void release(void* pointer, size_t size) override { }
/// Reset the marker of the current allocated memory
- void reset();
+ virtual void reset();
+
+
};
From c7e977250d1eab8d64ee35d7811f346a1b7f197d Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 9 Jan 2017 21:34:31 +0100
Subject: [PATCH 024/133] Update tests of collision world according to changes
in collision detection
---
test/tests/collision/TestCollisionWorld.h | 51 ++++++++--------------
test/tests/collision/TestDynamicAABBTree.h | 6 +--
2 files changed, 21 insertions(+), 36 deletions(-)
diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h
index d778bcd5..94d44a98 100644
--- a/test/tests/collision/TestCollisionWorld.h
+++ b/test/tests/collision/TestCollisionWorld.h
@@ -28,6 +28,7 @@
// Libraries
#include "reactphysics3d.h"
+#include "Test.h"
/// Reactphysics3D namespace
namespace reactphysics3d {
@@ -70,28 +71,28 @@ class WorldCollisionCallback : public CollisionCallback
}
// This method will be called for contact
- virtual void notifyContact(const ContactPointInfo& contactPointInfo) override {
+ virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
- if (isContactBetweenBodies(boxBody, sphere1Body, contactPointInfo)) {
+ if (isContactBetweenBodies(boxBody, sphere1Body, collisionCallbackInfo)) {
boxCollideWithSphere1 = true;
}
- else if (isContactBetweenBodies(boxBody, cylinderBody, contactPointInfo)) {
+ else if (isContactBetweenBodies(boxBody, cylinderBody, collisionCallbackInfo)) {
boxCollideWithCylinder = true;
}
- else if (isContactBetweenBodies(sphere1Body, cylinderBody, contactPointInfo)) {
+ else if (isContactBetweenBodies(sphere1Body, cylinderBody, collisionCallbackInfo)) {
sphere1CollideWithCylinder = true;
}
- else if (isContactBetweenBodies(sphere1Body, sphere2Body, contactPointInfo)) {
+ else if (isContactBetweenBodies(sphere1Body, sphere2Body, collisionCallbackInfo)) {
sphere1CollideWithSphere2 = true;
}
}
bool isContactBetweenBodies(const CollisionBody* body1, const CollisionBody* body2,
- const ContactPointInfo& contactPointInfo) {
- return (contactPointInfo.shape1->getBody()->getID() == body1->getID() &&
- contactPointInfo.shape2->getBody()->getID() == body2->getID()) ||
- (contactPointInfo.shape2->getBody()->getID() == body1->getID() &&
- contactPointInfo.shape1->getBody()->getID() == body2->getID());
+ const CollisionCallbackInfo& collisionCallbackInfo) {
+ return (collisionCallbackInfo.body1->getID() == body1->getID() &&
+ collisionCallbackInfo.body2->getID() == body2->getID()) ||
+ (collisionCallbackInfo.body2->getID() == body1->getID() &&
+ collisionCallbackInfo.body1->getID() == body2->getID());
}
};
@@ -197,10 +198,10 @@ class TestCollisionWorld : public Test {
test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody));
test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
- test(mWorld->testAABBOverlap(mBoxProxyShape, mSphere1ProxyShape));
- test(mWorld->testAABBOverlap(mBoxProxyShape, mCylinderProxyShape));
- test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mCylinderProxyShape));
- test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mSphere2ProxyShape));
+ test(mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB()));
+ test(mBoxProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
+ test(!mSphere1ProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
+ test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB()));
mCollisionCallback.reset();
mWorld->testCollision(mCylinderBody, &mCollisionCallback);
@@ -223,20 +224,6 @@ class TestCollisionWorld : public Test {
test(!mCollisionCallback.sphere1CollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithSphere2);
- mCollisionCallback.reset();
- mWorld->testCollision(mCylinderProxyShape, &mCollisionCallback);
- test(!mCollisionCallback.boxCollideWithSphere1);
- test(mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithSphere2);
-
- mCollisionCallback.reset();
- mWorld->testCollision(mBoxProxyShape, mCylinderProxyShape, &mCollisionCallback);
- test(!mCollisionCallback.boxCollideWithSphere1);
- test(mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithSphere2);
-
// Move sphere 1 to collide with sphere 2
mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
@@ -282,10 +269,10 @@ class TestCollisionWorld : public Test {
test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody));
test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
- test(!mWorld->testAABBOverlap(mBoxProxyShape, mSphere1ProxyShape));
- test(!mWorld->testAABBOverlap(mBoxProxyShape, mCylinderProxyShape));
- test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mCylinderProxyShape));
- test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mSphere2ProxyShape));
+ test(!mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB()));
+ test(!mBoxProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
+ test(!mSphere1ProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
+ test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB()));
mBoxBody->setIsActive(true);
mCylinderBody->setIsActive(true);
diff --git a/test/tests/collision/TestDynamicAABBTree.h b/test/tests/collision/TestDynamicAABBTree.h
index cb407738..7ab3202b 100644
--- a/test/tests/collision/TestDynamicAABBTree.h
+++ b/test/tests/collision/TestDynamicAABBTree.h
@@ -34,7 +34,7 @@
/// Reactphysics3D namespace
namespace reactphysics3d {
-class OverlapCallback : public DynamicAABBTreeOverlapCallback {
+class TestOverlapCallback : public DynamicAABBTreeOverlapCallback {
public :
@@ -86,11 +86,9 @@ class TestDynamicAABBTree : public Test {
// ---------- Atributes ---------- //
- OverlapCallback mOverlapCallback;
+ TestOverlapCallback mOverlapCallback;
DynamicTreeRaycastCallback mRaycastCallback;
-
-
public :
// ---------- Methods ---------- //
From f2a6dde9132b3f860d7e110eb95a4313be7b5110 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 18 Jan 2017 23:05:43 +0100
Subject: [PATCH 025/133] Fix return value in EPA Algorithm
---
src/collision/narrowphase/EPA/EPAAlgorithm.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
index 8779c33e..6afa7a10 100644
--- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
+++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
@@ -127,7 +127,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex&
// Only one point in the simplex (which should be the origin).
// We have a touching contact with zero penetration depth.
// We drop that kind of contact. Therefore, we return false
- return true;
+ return false;
case 2: {
// The simplex returned by GJK is a line segment d containing the origin.
From a50ae736637960d427d4286e7e327b0041660dda Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 19 Jan 2017 20:29:40 +0100
Subject: [PATCH 026/133] Fix issue in GJK algorithm
---
src/collision/narrowphase/GJK/GJKAlgorithm.cpp | 10 ++++++----
1 file changed, 6 insertions(+), 4 deletions(-)
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index dec43865..e1649af0 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -173,8 +173,6 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
} while((isPolytopeShape && !simplex.isFull()) || (!isPolytopeShape && !simplex.isFull() &&
distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()));
- bool isEPAResultValid = false;
-
// If no contact has been found (penetration case)
if (!contactFound) {
@@ -182,10 +180,14 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
// 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.
- isEPAResultValid = computePenetrationDepthForEnlargedObjects(narrowPhaseInfo, contactPointInfo, v);
+ if(computePenetrationDepthForEnlargedObjects(narrowPhaseInfo, contactPointInfo, v)) {
+
+ // A contact has been found with EPA algorithm, we return true
+ return true;
+ }
}
- if ((contactFound || !isEPAResultValid) && distSquare > MACHINE_EPSILON) {
+ if (contactFound && distSquare > MACHINE_EPSILON) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
From 99eb7cf82cc247b187c7ab051a08ec094553536e Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 19 Jan 2017 21:27:58 +0100
Subject: [PATCH 027/133] Fix issue in collision detection
---
src/collision/CollisionDetection.cpp | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index c199859c..345c46bb 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -230,11 +230,11 @@ void CollisionDetection::computeMiddlePhase() {
// Add all the narrow-phase info object reported by the callback into the
// list of all the narrow-phase info object
while (narrowPhaseInfo != nullptr) {
- NarrowPhaseInfo* head = mNarrowPhaseInfoList;
+ NarrowPhaseInfo* next = narrowPhaseInfo->next;
+ narrowPhaseInfo->next = mNarrowPhaseInfoList;
mNarrowPhaseInfoList = narrowPhaseInfo;
- mNarrowPhaseInfoList->next = head;
- narrowPhaseInfo = narrowPhaseInfo->next;
+ narrowPhaseInfo = next;
}
}
// Concave vs Concave shape
From e491e3814630d4371e411271812340653c3c18d6 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 27 Jan 2017 20:26:56 +0100
Subject: [PATCH 028/133] Fix issue in collision detection
---
src/collision/CollisionDetection.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 345c46bb..be17e4f7 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -605,7 +605,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape
computeConvexVsConcaveMiddlePhase(&pair, mMemoryAllocator, &narrowPhaseInfo);
}
- return nullptr;
+ return narrowPhaseInfo;
}
// Report all the bodies that overlap with the aabb in parameter
@@ -976,7 +976,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
- for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+ for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
OverlappingPair* pair = it->second;
From bccdfa9b5803adfc81fccf725acf0b693d49239a Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 27 Jan 2017 20:26:56 +0100
Subject: [PATCH 029/133] Fix issue in collision detection
---
src/collision/CollisionDetection.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 345c46bb..be17e4f7 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -605,7 +605,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape
computeConvexVsConcaveMiddlePhase(&pair, mMemoryAllocator, &narrowPhaseInfo);
}
- return nullptr;
+ return narrowPhaseInfo;
}
// Report all the bodies that overlap with the aabb in parameter
@@ -976,7 +976,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
- for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+ for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
OverlappingPair* pair = it->second;
From d1cb0d92756963afcb8b27fd8c39658c95cc03cb Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 28 Jan 2017 00:22:22 +0100
Subject: [PATCH 030/133] Refactor testbed application, Add collision detection
scene
---
testbed/CMakeLists.txt | 2 +
testbed/common/Box.cpp | 161 +++----
testbed/common/Box.h | 30 +-
testbed/common/Capsule.cpp | 10 +-
testbed/common/Capsule.h | 2 +-
testbed/common/ConcaveMesh.cpp | 10 +-
testbed/common/ConcaveMesh.h | 2 +-
testbed/common/Cone.cpp | 10 +-
testbed/common/Cone.h | 2 +-
testbed/common/ConvexMesh.cpp | 10 +-
testbed/common/ConvexMesh.h | 2 +-
testbed/common/Cylinder.cpp | 10 +-
testbed/common/Cylinder.h | 2 +-
testbed/common/Dumbbell.cpp | 10 +-
testbed/common/Dumbbell.h | 2 +-
testbed/common/HeightField.cpp | 10 +-
testbed/common/HeightField.h | 2 +-
testbed/common/Sphere.cpp | 11 +-
testbed/common/Sphere.h | 2 +-
testbed/meshes/cube.obj | 17 +
.../CollisionDetectionScene.cpp | 359 +++++++++++++++
.../CollisionDetectionScene.h | 235 ++++++++++
.../collisionshapes/CollisionShapesScene.cpp | 20 +-
.../scenes/concavemesh/ConcaveMeshScene.cpp | 6 +-
testbed/scenes/cubes/CubesScene.cpp | 408 +++++++++---------
testbed/scenes/cubes/CubesScene.h | 192 ++++-----
.../scenes/heightfield/HeightFieldScene.cpp | 6 +-
testbed/scenes/joints/JointsScene.cpp | 28 +-
testbed/scenes/raycast/RaycastScene.cpp | 20 +-
testbed/src/Gui.cpp | 7 +
testbed/src/Scene.cpp | 2 +-
testbed/src/Scene.h | 19 +
testbed/src/TestbedApplication.cpp | 12 +-
testbed/src/TestbedApplication.h | 3 +
34 files changed, 1166 insertions(+), 458 deletions(-)
create mode 100644 testbed/meshes/cube.obj
create mode 100644 testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
create mode 100644 testbed/scenes/collisiondetection/CollisionDetectionScene.h
diff --git a/testbed/CMakeLists.txt b/testbed/CMakeLists.txt
index 808ded2f..8d402ad3 100644
--- a/testbed/CMakeLists.txt
+++ b/testbed/CMakeLists.txt
@@ -110,6 +110,8 @@ SET(SCENES_SOURCES
scenes/raycast/RaycastScene.cpp
scenes/collisionshapes/CollisionShapesScene.h
scenes/collisionshapes/CollisionShapesScene.cpp
+ scenes/collisiondetection/CollisionDetectionScene.h
+ scenes/collisiondetection/CollisionDetectionScene.cpp
scenes/concavemesh/ConcaveMeshScene.h
scenes/concavemesh/ConcaveMeshScene.cpp
scenes/heightfield/HeightFieldScene.h
diff --git a/testbed/common/Box.cpp b/testbed/common/Box.cpp
index 549b86bb..6c8f052d 100644
--- a/testbed/common/Box.cpp
+++ b/testbed/common/Box.cpp
@@ -32,88 +32,21 @@
// Initialize static variables
openglframework::VertexBufferObject Box::mVBOVertices(GL_ARRAY_BUFFER);
openglframework::VertexBufferObject Box::mVBONormals(GL_ARRAY_BUFFER);
+openglframework::VertexBufferObject Box::mVBOTextureCoords(GL_ARRAY_BUFFER);
+openglframework::VertexBufferObject Box::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER);
openglframework::VertexArrayObject Box::mVAO;
int Box::totalNbBoxes = 0;
-GLfloat Box::mCubeVertices[108] = {
- -1.0f,-1.0f,-1.0f, // triangle 1 : begin
- -1.0f,-1.0f, 1.0f,
- -1.0f, 1.0f, 1.0f, // triangle 1 : end
- 1.0f, 1.0f,-1.0f, // triangle 2 : begin
- -1.0f,-1.0f,-1.0f,
- -1.0f, 1.0f,-1.0f, // triangle 2 : end
- 1.0f,-1.0f, 1.0f,
- -1.0f,-1.0f,-1.0f,
- 1.0f,-1.0f,-1.0f,
- 1.0f, 1.0f,-1.0f,
- 1.0f,-1.0f,-1.0f,
- -1.0f,-1.0f,-1.0f,
- -1.0f,-1.0f,-1.0f,
- -1.0f, 1.0f, 1.0f,
- -1.0f, 1.0f,-1.0f,
- 1.0f,-1.0f, 1.0f,
- -1.0f,-1.0f, 1.0f,
- -1.0f,-1.0f,-1.0f,
- -1.0f, 1.0f, 1.0f,
- -1.0f,-1.0f, 1.0f,
- 1.0f,-1.0f, 1.0f,
- 1.0f, 1.0f, 1.0f,
- 1.0f,-1.0f,-1.0f,
- 1.0f, 1.0f,-1.0f,
- 1.0f,-1.0f,-1.0f,
- 1.0f, 1.0f, 1.0f,
- 1.0f,-1.0f, 1.0f,
- 1.0f, 1.0f, 1.0f,
- 1.0f, 1.0f,-1.0f,
- -1.0f, 1.0f,-1.0f,
- 1.0f, 1.0f, 1.0f,
- -1.0f, 1.0f,-1.0f,
- -1.0f, 1.0f, 1.0f,
- 1.0f, 1.0f, 1.0f,
- -1.0f, 1.0f, 1.0f,
- 1.0f,-1.0f, 1.0f
-};
-GLfloat Box::mCubeNormals[108] = {
- -1.0f, 0.0f, 0.0f, // triangle 1 : begin
- -1.0f, 0.0f, 0.0f,
- -1.0f, 0.0f, 0.0f, // triangle 1 : end
- 0.0f, 0.0f,-1.0f, // triangle 2 : begin
- 0.0f, 0.0f,-1.0f,
- 0.0f, 0.0f,-1.0f, // triangle 2 : end
- 0.0f,-1.0f, 0.0f,
- 0.0f,-1.0f, 0.0f,
- 0.0f,-1.0f, 0.0f,//
- 0.0f, 0.0f,-1.0f,
- 0.0f, 0.0f,-1.0f,
- 0.0f, 0.0f,-1.0f,//
- -1.0f, 0.0f, 0.0f,
- -1.0f, 0.0f, 0.0f,
- -1.0f, 0.0f,0.0f,//
- 0.0f,-1.0f, 0.0f,
- 0.0f,-1.0f, 0.0f,
- 0.0f,-1.0f, 0.0f,//
- 0.0f, 0.0f, 1.0f,
- 0.0f, 0.0f, 1.0f,
- 0.0f, 0.0f, 1.0f,//
- 1.0f, 0.0f, 0.0f,
- 1.0f, 0.0f, 0.0f,
- 1.0f, 0.0f, 0.0f,//
- 1.0f, 0.0f, 0.0f,
- 1.0f, 0.0f, 0.0f,
- 1.0f, 0.0f, 0.0f,//
- 0.0f, 1.0f, 0.0f,
- 0.0f, 1.0f, 0.0f,
- 0.0f, 1.0f, 0.0f,//
- 0.0f, 1.0f, 0.0f,
- 0.0f, 1.0f, 0.0f,
- 0.0f, 1.0f, 0.0f,//
- 0.0f, 0.0f, 1.0f,
- 0.0f, 0.0f, 1.0f,
- 0.0f, 0.0f, 1.0f//
-};
+
// Constructor
Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world)
- : openglframework::Object3D() {
+ reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath)
+ : openglframework::Mesh() {
+
+ // Load the mesh from a file
+ openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this);
+
+ // Calculate the normals of the mesh
+ calculateNormals();
// Initialize the size of the box
mSize[0] = size.x * 0.5f;
@@ -161,8 +94,14 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
// Constructor
Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& position,
- float mass, reactphysics3d::DynamicsWorld* world)
- : openglframework::Object3D() {
+ float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath)
+ : openglframework::Mesh() {
+
+ // Load the mesh from a file
+ openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this);
+
+ // Calculate the normals of the mesh
+ calculateNormals();
// Initialize the size of the box
mSize[0] = size.x * 0.5f;
@@ -226,16 +165,15 @@ Box::~Box() {
// Render the cube at the correct position and with the correct orientation
void Box::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
- // Bind the VAO
- mVAO.bind();
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+ }
// Bind the shader
shader.bind();
- mVBOVertices.bind();
-
// Set the model to camera matrix
shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix);
shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
@@ -252,20 +190,27 @@ void Box::render(openglframework::Shader& shader,
openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a);
shader.setVector4Uniform("vertexColor", color, false);
+ // Bind the VAO
+ mVAO.bind();
+
+ mVBOVertices.bind();
+
// Get the location of shader attribute variables
GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false);
glEnableVertexAttribArray(vertexPositionLoc);
- glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, nullptr);
+ glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr);
mVBONormals.bind();
+ if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr);
if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc);
- if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, nullptr);
- // Draw the geometry of the box
- glDrawArrays(GL_TRIANGLES, 0, 36);
+ // For each part of the mesh
+ for (unsigned int i=0; i<getNbParts(); i++) {
+ glDrawElements(GL_TRIANGLES, getNbFaces(i) * 3, GL_UNSIGNED_INT, (char*)nullptr);
+ }
glDisableVertexAttribArray(vertexPositionLoc);
if (vertexNormalLoc != -1) glDisableVertexAttribArray(vertexNormalLoc);
@@ -278,6 +223,10 @@ void Box::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+ }
}
// Create the Vertex Buffer Objects used to render to box with OpenGL.
@@ -288,15 +237,33 @@ void Box::createVBOAndVAO() {
// Create the VBO for the vertices data
mVBOVertices.create();
mVBOVertices.bind();
- mVBOVertices.copyDataIntoVBO(sizeof(mCubeVertices), mCubeVertices, GL_STATIC_DRAW);
+ size_t sizeVertices = mVertices.size() * sizeof(openglframework::Vector3);
+ mVBOVertices.copyDataIntoVBO(sizeVertices, getVerticesPointer(), GL_STATIC_DRAW);
mVBOVertices.unbind();
- // Create th VBO for the normals data
+ // Create the VBO for the normals data
mVBONormals.create();
mVBONormals.bind();
- mVBONormals.copyDataIntoVBO(sizeof(mCubeNormals), mCubeNormals, GL_STATIC_DRAW);
+ size_t sizeNormals = mNormals.size() * sizeof(openglframework::Vector3);
+ mVBONormals.copyDataIntoVBO(sizeNormals, getNormalsPointer(), GL_STATIC_DRAW);
mVBONormals.unbind();
+ if (hasTexture()) {
+ // Create the VBO for the texture co data
+ mVBOTextureCoords.create();
+ mVBOTextureCoords.bind();
+ size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::Vector2);
+ mVBOTextureCoords.copyDataIntoVBO(sizeTextureCoords, getUVTextureCoordinatesPointer(), GL_STATIC_DRAW);
+ mVBOTextureCoords.unbind();
+ }
+
+ // Create th VBO for the indices data
+ mVBOIndices.create();
+ mVBOIndices.bind();
+ size_t sizeIndices = mIndices[0].size() * sizeof(unsigned int);
+ mVBOIndices.copyDataIntoVBO(sizeIndices, getIndicesPointer(), GL_STATIC_DRAW);
+ mVBOIndices.unbind();
+
// Create the VAO for both VBOs
mVAO.create();
mVAO.bind();
@@ -304,9 +271,17 @@ void Box::createVBOAndVAO() {
// Bind the VBO of vertices
mVBOVertices.bind();
- // Bind the VBO of indices
+ // Bind the VBO of normals
mVBONormals.bind();
+ if (hasTexture()) {
+ // Bind the VBO of texture coords
+ mVBOTextureCoords.bind();
+ }
+
+ // Bind the VBO of indices
+ mVBOIndices.bind();
+
// Unbind the VAO
mVAO.unbind();
}
diff --git a/testbed/common/Box.h b/testbed/common/Box.h
index 56f8daf5..53da204e 100644
--- a/testbed/common/Box.h
+++ b/testbed/common/Box.h
@@ -32,7 +32,7 @@
#include "PhysicsObject.h"
// Class Box
-class Box : public openglframework::Object3D, public PhysicsObject {
+class Box : public openglframework::Mesh, public PhysicsObject {
private :
@@ -47,20 +47,20 @@ class Box : public openglframework::Object3D, public PhysicsObject {
/// Scaling matrix (applied to a cube to obtain the correct box dimensions)
openglframework::Matrix4 mScalingMatrix;
- /// Vertex Buffer Object for the vertices data used to render the box with OpenGL
- static openglframework::VertexBufferObject mVBOVertices;
+ /// Vertex Buffer Object for the vertices data
+ static openglframework::VertexBufferObject mVBOVertices;
- /// Vertex Buffer Object for the normales used to render the box with OpenGL
- static openglframework::VertexBufferObject mVBONormals;
+ /// Vertex Buffer Object for the normals data
+ static openglframework::VertexBufferObject mVBONormals;
- /// Vertex Array Object for the vertex data
- static openglframework::VertexArrayObject mVAO;
+ /// Vertex Buffer Object for the texture coords
+ static openglframework::VertexBufferObject mVBOTextureCoords;
- /// Vertices coordinates of the triangles of the box
- static GLfloat mCubeVertices[108];
+ /// Vertex Buffer Object for the indices
+ static openglframework::VertexBufferObject mVBOIndices;
- /// Vertices normals of the triangles of the box
- static GLfloat mCubeNormals[108];
+ /// Vertex Array Object for the vertex data
+ static openglframework::VertexArrayObject mVAO;
/// Total number of boxes created
static int totalNbBoxes;
@@ -68,7 +68,7 @@ class Box : public openglframework::Object3D, public PhysicsObject {
// -------------------- Methods -------------------- //
/// Create a the VAO and VBOs to render to box with OpenGL
- static void createVBOAndVAO();
+ void createVBOAndVAO();
public :
@@ -76,17 +76,17 @@ class Box : public openglframework::Object3D, public PhysicsObject {
/// Constructor
Box(const openglframework::Vector3& size, const openglframework::Vector3& position,
- reactphysics3d::CollisionWorld* world);
+ reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath);
/// Constructor
Box(const openglframework::Vector3& size, const openglframework::Vector3& position,
- float mass, reactphysics3d::DynamicsWorld *world);
+ float mass, reactphysics3d::DynamicsWorld *world, const std::string& meshFolderPath);
/// Destructor
~Box();
/// Render the cube at the correct position and with the correct orientation
- void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix);
+ void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
diff --git a/testbed/common/Capsule.cpp b/testbed/common/Capsule.cpp
index 7b83f6b3..ba6a45b4 100644
--- a/testbed/common/Capsule.cpp
+++ b/testbed/common/Capsule.cpp
@@ -152,7 +152,11 @@ Capsule::~Capsule() {
// Render the sphere at the correct position and with the correct orientation
void Capsule::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+ }
// Bind the shader
shader.bind();
@@ -206,6 +210,10 @@ void Capsule::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+ }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/Capsule.h b/testbed/common/Capsule.h
index ee380914..748fa46b 100644
--- a/testbed/common/Capsule.h
+++ b/testbed/common/Capsule.h
@@ -95,7 +95,7 @@ class Capsule : public openglframework::Mesh, public PhysicsObject {
/// Render the sphere at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix);
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp
index 68f7a56e..c21732f8 100644
--- a/testbed/common/ConcaveMesh.cpp
+++ b/testbed/common/ConcaveMesh.cpp
@@ -165,7 +165,11 @@ ConcaveMesh::~ConcaveMesh() {
// Render the sphere at the correct position and with the correct orientation
void ConcaveMesh::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+ }
// Bind the shader
shader.bind();
@@ -219,6 +223,10 @@ void ConcaveMesh::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+ }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/ConcaveMesh.h b/testbed/common/ConcaveMesh.h
index 087c030c..3c645ec8 100644
--- a/testbed/common/ConcaveMesh.h
+++ b/testbed/common/ConcaveMesh.h
@@ -88,7 +88,7 @@ class ConcaveMesh : public openglframework::Mesh, public PhysicsObject {
/// Render the mesh at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix);
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
diff --git a/testbed/common/Cone.cpp b/testbed/common/Cone.cpp
index 1a971922..07a825a8 100644
--- a/testbed/common/Cone.cpp
+++ b/testbed/common/Cone.cpp
@@ -149,7 +149,11 @@ Cone::~Cone() {
// Render the cone at the correct position and with the correct orientation
void Cone::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+ }
// Bind the shader
shader.bind();
@@ -203,6 +207,10 @@ void Cone::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+ }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/Cone.h b/testbed/common/Cone.h
index e886c66b..e4f7face 100644
--- a/testbed/common/Cone.h
+++ b/testbed/common/Cone.h
@@ -94,7 +94,7 @@ class Cone : public openglframework::Mesh, public PhysicsObject {
/// Render the cone at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix);
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp
index 1ded48a2..ca372a1f 100644
--- a/testbed/common/ConvexMesh.cpp
+++ b/testbed/common/ConvexMesh.cpp
@@ -146,7 +146,11 @@ ConvexMesh::~ConvexMesh() {
// Render the sphere at the correct position and with the correct orientation
void ConvexMesh::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+ }
// Bind the shader
shader.bind();
@@ -200,6 +204,10 @@ void ConvexMesh::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+ }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h
index 842d187f..a4a2ffab 100644
--- a/testbed/common/ConvexMesh.h
+++ b/testbed/common/ConvexMesh.h
@@ -87,7 +87,7 @@ class ConvexMesh : public openglframework::Mesh, public PhysicsObject {
/// Render the mesh at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix);
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
diff --git a/testbed/common/Cylinder.cpp b/testbed/common/Cylinder.cpp
index f791317b..d5ec1964 100644
--- a/testbed/common/Cylinder.cpp
+++ b/testbed/common/Cylinder.cpp
@@ -150,7 +150,11 @@ Cylinder::~Cylinder() {
// Render the cylinder at the correct position and with the correct orientation
void Cylinder::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+ }
// Bind the shader
shader.bind();
@@ -204,6 +208,10 @@ void Cylinder::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+ }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/Cylinder.h b/testbed/common/Cylinder.h
index 2e75084c..eb7ba72d 100644
--- a/testbed/common/Cylinder.h
+++ b/testbed/common/Cylinder.h
@@ -94,7 +94,7 @@ class Cylinder : public openglframework::Mesh, public PhysicsObject {
/// Render the cylinder at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix);
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
diff --git a/testbed/common/Dumbbell.cpp b/testbed/common/Dumbbell.cpp
index 92f50223..e590d033 100644
--- a/testbed/common/Dumbbell.cpp
+++ b/testbed/common/Dumbbell.cpp
@@ -191,7 +191,11 @@ Dumbbell::~Dumbbell() {
// Render the sphere at the correct position and with the correct orientation
void Dumbbell::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+ }
// Bind the shader
shader.bind();
@@ -245,6 +249,10 @@ void Dumbbell::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+ }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h
index 7982819d..4d44a38b 100644
--- a/testbed/common/Dumbbell.h
+++ b/testbed/common/Dumbbell.h
@@ -95,7 +95,7 @@ class Dumbbell : public openglframework::Mesh, public PhysicsObject {
/// Render the sphere at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix);
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
diff --git a/testbed/common/HeightField.cpp b/testbed/common/HeightField.cpp
index f65123ee..38eda261 100644
--- a/testbed/common/HeightField.cpp
+++ b/testbed/common/HeightField.cpp
@@ -131,7 +131,11 @@ HeightField::~HeightField() {
// Render the sphere at the correct position and with the correct orientation
void HeightField::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+ }
// Bind the shader
shader.bind();
@@ -185,6 +189,10 @@ void HeightField::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
+
+ if (wireframe) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+ }
}
// Compute the heights of the height field
diff --git a/testbed/common/HeightField.h b/testbed/common/HeightField.h
index bf1cb148..d4a4895d 100644
--- a/testbed/common/HeightField.h
+++ b/testbed/common/HeightField.h
@@ -101,7 +101,7 @@ class HeightField : public openglframework::Mesh, public PhysicsObject {
/// Render the mesh at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix);
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
diff --git a/testbed/common/Sphere.cpp b/testbed/common/Sphere.cpp
index e7f880ea..236a81ad 100644
--- a/testbed/common/Sphere.cpp
+++ b/testbed/common/Sphere.cpp
@@ -150,8 +150,11 @@ Sphere::~Sphere() {
}
// Render the sphere at the correct position and with the correct orientation
-void Sphere::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+void Sphere::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireFrame) {
+
+ if (wireFrame) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+ }
// Bind the shader
shader.bind();
@@ -205,6 +208,10 @@ void Sphere::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
+
+ if (wireFrame) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+ }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/Sphere.h b/testbed/common/Sphere.h
index 70270026..5fdfcfe9 100644
--- a/testbed/common/Sphere.h
+++ b/testbed/common/Sphere.h
@@ -91,7 +91,7 @@ class Sphere : public openglframework::Mesh, public PhysicsObject {
/// Render the sphere at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix);
+ const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
diff --git a/testbed/meshes/cube.obj b/testbed/meshes/cube.obj
new file mode 100644
index 00000000..167925a5
--- /dev/null
+++ b/testbed/meshes/cube.obj
@@ -0,0 +1,17 @@
+# Blender v2.72 (sub 0) OBJ File: ''
+# www.blender.org
+v -1.000000 -1.000000 1.000000
+v -1.000000 -1.000000 -1.000000
+v 1.000000 -1.000000 -1.000000
+v 1.000000 -1.000000 1.000000
+v -1.000000 1.000000 1.000000
+v -1.000000 1.000000 -1.000000
+v 1.000000 1.000000 -1.000000
+v 1.000000 1.000000 1.000000
+s off
+f 5 6 2 1
+f 6 7 3 2
+f 7 8 4 3
+f 8 5 1 4
+f 1 2 3 4
+f 8 7 6 5
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
new file mode 100644
index 00000000..09544540
--- /dev/null
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -0,0 +1,359 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "CollisionDetectionScene.h"
+
+// Namespaces
+using namespace openglframework;
+using namespace collisiondetectionscene;
+
+// Constructor
+CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
+ : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
+ mContactManager(mPhongShader, mMeshFolderPath),
+ mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) {
+
+ mSelectedShapeIndex = 0;
+ mIsContactPointsDisplayed = true;
+ mIsWireframeEnabled = true;
+
+ // Compute the radius and the center of the scene
+ openglframework::Vector3 center(0, 0, 0);
+
+ // Set the center of the scene
+ setScenePosition(center, SCENE_RADIUS);
+
+ // Create the dynamics world for the physics simulation
+ mCollisionWorld = new rp3d::CollisionWorld();
+
+ // ---------- Sphere 1 ---------- //
+ openglframework::Vector3 position1(0, 0, 0);
+
+ // Create a sphere and a corresponding collision body in the dynamics world
+ mSphere1 = new Sphere(6, position1, mCollisionWorld, mMeshFolderPath);
+ mAllShapesObjects.push_back(mSphere1);
+ mAllShapesPhysicsObjects.push_back(mSphere1);
+
+ // Set the color
+ mSphere1->setColor(mGreyColorDemo);
+ mSphere1->setSleepingColor(mRedColorDemo);
+
+ // ---------- Sphere 2 ---------- //
+ openglframework::Vector3 position2(4, 0, 0);
+
+ // Create a sphere and a corresponding collision body in the dynamics world
+ mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath);
+ mAllShapesObjects.push_back(mSphere2);
+ mAllShapesPhysicsObjects.push_back(mSphere2);
+
+ // Set the color
+ mSphere2->setColor(mGreyColorDemo);
+ mSphere2->setSleepingColor(mRedColorDemo);
+
+ // ---------- Cone ---------- //
+ //openglframework::Vector3 position4(0, 0, 0);
+
+ // Create a cone and a corresponding collision body in the dynamics world
+ //mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld,
+ // mMeshFolderPath);
+
+ // Set the color
+ //mCone->setColor(mGreyColorDemo);
+ //mCone->setSleepingColor(mRedColorDemo);
+
+ // ---------- Cylinder ---------- //
+ //openglframework::Vector3 position5(0, 0, 0);
+
+ // Create a cylinder and a corresponding collision body in the dynamics world
+ //mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5,
+ // mCollisionWorld, mMeshFolderPath);
+
+ // Set the color
+ //mCylinder->setColor(mGreyColorDemo);
+ //mCylinder->setSleepingColor(mRedColorDemo);
+
+ // ---------- Capsule ---------- //
+ //openglframework::Vector3 position6(0, 0, 0);
+
+ // Create a cylinder and a corresponding collision body in the dynamics world
+ //mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 ,
+ // mCollisionWorld, mMeshFolderPath);
+
+ // Set the color
+ //mCapsule->setColor(mGreyColorDemo);
+ //mCapsule->setSleepingColor(mRedColorDemo);
+
+ // ---------- Convex Mesh ---------- //
+ //openglframework::Vector3 position7(0, 0, 0);
+
+ // Create a convex mesh and a corresponding collision body in the dynamics world
+ //mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj");
+
+ // Set the color
+ //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 + "city.obj");
+
+ // Set the color
+ //mConcaveMesh->setColor(mGreyColorDemo);
+ //mConcaveMesh->setSleepingColor(mRedColorDemo);
+
+ // ---------- Heightfield ---------- //
+ //openglframework::Vector3 position9(0, 0, 0);
+
+ // Create a convex mesh and a corresponding collision body in the dynamics world
+ //mHeightField = new HeightField(position9, mCollisionWorld);
+
+ // Set the color
+ //mHeightField->setColor(mGreyColorDemo);
+ //mHeightField->setSleepingColor(mRedColorDemo);
+
+ // Create the VBO and VAO to render the lines
+ createVBOAndVAO(mPhongShader);
+
+ mAllShapesPhysicsObjects[mSelectedShapeIndex]->setColor(mBlueColorDemo);
+}
+
+// Reset the scene
+void CollisionDetectionScene::reset() {
+
+}
+
+// Destructor
+CollisionDetectionScene::~CollisionDetectionScene() {
+
+ // Destroy the shader
+ mPhongShader.destroy();
+
+ // Destroy the box rigid body from the dynamics world
+ //mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody());
+ //delete mBox;
+
+ // Destroy the spheres
+ mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody());
+ delete mSphere1;
+
+ mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody());
+ delete mSphere2;
+
+ /*
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody());
+ delete mCone;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody());
+
+ // Destroy the sphere
+ delete mCylinder;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody());
+
+ // Destroy the sphere
+ delete mCapsule;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody());
+
+ // Destroy the convex mesh
+ delete mConvexMesh;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody());
+
+ // Destroy the dumbbell
+ delete mDumbbell;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody());
+
+ // Destroy the convex mesh
+ delete mConcaveMesh;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody());
+
+ // Destroy the convex mesh
+ delete mHeightField;
+ */
+
+ mContactManager.resetPoints();
+
+ // Destroy the static data for the visual contact points
+ VisualContactPoint::destroyStaticData();
+
+ // Destroy the collision world
+ delete mCollisionWorld;
+
+ // Destroy the VBOs and VAO
+ mVBOVertices.destroy();
+ mVAO.destroy();
+}
+
+// Update the physics world (take a simulation step)
+void CollisionDetectionScene::updatePhysics() {
+
+
+}
+
+// Take a step for the simulation
+void CollisionDetectionScene::update() {
+
+ mContactManager.resetPoints();
+
+ mCollisionWorld->testCollision(&mContactManager);
+
+ SceneDemo::update();
+}
+
+// Render the scene
+void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
+ const openglframework::Matrix4& worldToCameraMatrix) {
+
+ /*
+ // Bind the VAO
+ mVAO.bind();
+
+ // Bind the shader
+ shader.bind();
+
+ mVBOVertices.bind();
+
+ // Set the model to camera matrix
+ const Matrix4 localToCameraMatrix = Matrix4::identity();
+ shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix);
+ shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
+
+ // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
+ // model-view matrix)
+ const openglframework::Matrix3 normalMatrix =
+ localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
+ shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
+
+ // Set the vertex color
+ openglframework::Vector4 color(1, 0, 0, 1);
+ shader.setVector4Uniform("vertexColor", color, false);
+
+ // Get the location of shader attribute variables
+ GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
+
+ glEnableVertexAttribArray(vertexPositionLoc);
+ glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL);
+
+ // Draw the lines
+ glDrawArrays(GL_LINES, 0, NB_RAYS);
+
+ glDisableVertexAttribArray(vertexPositionLoc);
+
+ mVBOVertices.unbind();
+
+ // Unbind the VAO
+ mVAO.unbind();
+
+ shader.unbind();
+ */
+
+ // Render the shapes
+ if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+
+ /*
+ if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
+ if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix);
+ if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix);
+ 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);
+ if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix);
+ */
+
+ shader.unbind();
+}
+
+// Create the Vertex Buffer Objects used to render with OpenGL.
+/// We create two VBOs (one for vertices and one for indices)
+void CollisionDetectionScene::createVBOAndVAO(openglframework::Shader& shader) {
+
+ // Bind the shader
+ shader.bind();
+
+ // Create the VBO for the vertices data
+ mVBOVertices.create();
+ mVBOVertices.bind();
+ size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::Vector3);
+ mVBOVertices.copyDataIntoVBO(sizeVertices, &mLinePoints[0], GL_STATIC_DRAW);
+ mVBOVertices.unbind();
+
+ // Create the VAO for both VBOs
+ mVAO.create();
+ mVAO.bind();
+
+ // Bind the VBO of vertices
+ mVBOVertices.bind();
+
+ // Unbind the VAO
+ mVAO.unbind();
+
+ // Unbind the shader
+ shader.unbind();
+}
+
+void CollisionDetectionScene::selectNextShape() {
+
+ int previousIndex = mSelectedShapeIndex;
+ mSelectedShapeIndex++;
+ if (mSelectedShapeIndex >= mAllShapesPhysicsObjects.size()) {
+ mSelectedShapeIndex = 0;
+ }
+
+ mAllShapesPhysicsObjects[previousIndex]->setColor(mGreyColorDemo);
+ mAllShapesPhysicsObjects[mSelectedShapeIndex]->setColor(mBlueColorDemo);
+}
+
+// Called when a keyboard event occurs
+bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, int mods) {
+
+ // If the space key has been pressed
+ if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) {
+ selectNextShape();
+ return true;
+ }
+
+ if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapesPhysicsObjects[mSelectedShapeIndex]->getCollisionBody()->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(1, 0, 0));
+ mAllShapesObjects[mSelectedShapeIndex]->translateWorld(openglframework::Vector3(1, 0, 0));
+ }
+
+ return false;
+}
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
new file mode 100644
index 00000000..d77cee7f
--- /dev/null
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -0,0 +1,235 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef COLLISION_DETECTION_SCENE_H
+#define COLLISION_DETECTION_SCENE_H
+
+// Libraries
+#define _USE_MATH_DEFINES
+#include <cmath>
+#include "openglframework.h"
+#include "reactphysics3d.h"
+#include "SceneDemo.h"
+#include "Sphere.h"
+#include "Box.h"
+#include "Cone.h"
+#include "Cylinder.h"
+#include "Capsule.h"
+#include "Line.h"
+#include "ConvexMesh.h"
+#include "ConcaveMesh.h"
+#include "HeightField.h"
+#include "Dumbbell.h"
+#include "VisualContactPoint.h"
+
+namespace collisiondetectionscene {
+
+// Constants
+const float SCENE_RADIUS = 30.0f;
+const openglframework::Vector3 BOX_SIZE(4, 2, 1);
+const float SPHERE_RADIUS = 3.0f;
+const float CONE_RADIUS = 3.0f;
+const float CONE_HEIGHT = 5.0f;
+const float CYLINDER_RADIUS = 3.0f;
+const float CYLINDER_HEIGHT = 5.0f;
+const float CAPSULE_RADIUS = 3.0f;
+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 = 9;
+
+// Raycast manager
+class ContactManager : public rp3d::CollisionCallback {
+
+ private:
+
+ /// All the visual contact points
+ std::vector<ContactPoint> mContactPoints;
+
+ /// All the normals at contact points
+ std::vector<Line*> mNormals;
+
+ /// Contact point mesh folder path
+ std::string mMeshFolderPath;
+
+ public:
+
+ ContactManager(openglframework::Shader& shader,
+ const std::string& meshFolderPath)
+ : mMeshFolderPath(meshFolderPath) {
+
+ }
+
+ /// This method will be called for each reported contact point
+ virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
+
+ rp3d::Vector3 point1 = collisionCallbackInfo.contactPoint.localPoint1;
+ point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
+ openglframework::Vector3 position1(point1.x, point1.y, point1.z);
+ mContactPoints.push_back(ContactPoint(position1));
+
+ rp3d::Vector3 point2 = collisionCallbackInfo.contactPoint.localPoint2;
+ point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
+ openglframework::Vector3 position2(point2.x, point2.y, point2.z);
+ mContactPoints.push_back(ContactPoint(position2));
+
+ // Create a line to display the normal at hit point
+ rp3d::Vector3 n = collisionCallbackInfo.contactPoint.normal;
+ openglframework::Vector3 normal(n.x, n.y, n.z);
+ Line* normalLine = new Line(position1, position1 + normal);
+ mNormals.push_back(normalLine);
+ }
+
+ void resetPoints() {
+
+ mContactPoints.clear();
+
+ // Destroy all the normals
+ for (std::vector<Line*>::iterator it = mNormals.begin();
+ it != mNormals.end(); ++it) {
+ delete (*it);
+ }
+ mNormals.clear();
+ }
+
+ std::vector<ContactPoint> getContactPoints() const {
+ return mContactPoints;
+ }
+};
+
+// Class CollisionDetectionScene
+class CollisionDetectionScene : public SceneDemo {
+
+ private :
+
+ // -------------------- Attributes -------------------- //
+
+ /// Contact point mesh folder path
+ std::string mMeshFolderPath;
+
+ /// Contact manager
+ ContactManager mContactManager;
+
+ bool mAreNormalsDisplayed;
+
+ /// All objects on the scene
+ //Box* mBox;
+ Sphere* mSphere1;
+ Sphere* mSphere2;
+ //Cone* mCone;
+ //Cylinder* mCylinder;
+ //Capsule* mCapsule;
+ //ConvexMesh* mConvexMesh;
+ //Dumbbell* mDumbbell;
+ //ConcaveMesh* mConcaveMesh;
+ //HeightField* mHeightField;
+
+ std::vector<openglframework::Object3D*> mAllShapesObjects;
+ std::vector<PhysicsObject*> mAllShapesPhysicsObjects;
+
+ int mSelectedShapeIndex;
+
+ /// Collision world used for the physics simulation
+ rp3d::CollisionWorld* mCollisionWorld;
+
+ /// All the points to render the lines
+ std::vector<openglframework::Vector3> mLinePoints;
+
+ /// Vertex Buffer Object for the vertices data
+ openglframework::VertexBufferObject mVBOVertices;
+
+ /// Vertex Array Object for the vertex data
+ openglframework::VertexArrayObject mVAO;
+
+ /// Create the Vertex Buffer Objects used to render with OpenGL.
+ void createVBOAndVAO(openglframework::Shader& shader);
+
+ /// Select the next shape
+ void selectNextShape();
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ CollisionDetectionScene(const std::string& name);
+
+ /// Destructor
+ virtual ~CollisionDetectionScene() override;
+
+ /// Update the physics world (take a simulation step)
+ /// Can be called several times per frame
+ virtual void updatePhysics() override;
+
+ /// Take a step for the simulation
+ virtual void update() override;
+
+ /// Render the scene in a single pass
+ virtual void renderSinglePass(openglframework::Shader& shader,
+ const openglframework::Matrix4& worldToCameraMatrix) override;
+
+ /// Reset the scene
+ virtual void reset() override;
+
+ /// Display or not the surface normals at hit points
+ void showHideNormals();
+
+ /// Called when a keyboard event occurs
+ virtual bool keyboardEvent(int key, int scancode, int action, int mods) override;
+
+ /// Enabled/Disable the shadow mapping
+ virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override;
+
+ /// Display/Hide the contact points
+ virtual void setIsContactPointsDisplayed(bool display) override;
+
+ /// Return all the contact points of the scene
+ virtual std::vector<ContactPoint> getContactPoints() const override;
+};
+
+// Display or not the surface normals at hit points
+inline void CollisionDetectionScene::showHideNormals() {
+ mAreNormalsDisplayed = !mAreNormalsDisplayed;
+}
+
+// Enabled/Disable the shadow mapping
+inline void CollisionDetectionScene::setIsShadowMappingEnabled(bool isShadowMappingEnabled) {
+ SceneDemo::setIsShadowMappingEnabled(false);
+}
+
+// Display/Hide the contact points
+inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) {
+ SceneDemo::setIsContactPointsDisplayed(true);
+}
+
+// Return all the contact points of the scene
+inline std::vector<ContactPoint> CollisionDetectionScene::getContactPoints() const {
+ return mContactManager.getContactPoints();
+}
+
+}
+
+#endif
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
index 5078cd14..e146bd6b 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
@@ -83,7 +83,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
radius * sin(angle));
// Create a sphere and a corresponding rigid in the dynamics world
- Box* box = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld);
+ Box* box = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld, mMeshFolderPath);
// Set the box color
box->setColor(mDemoColors[i % mNbDemoColors]);
@@ -235,7 +235,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// ---------- Create the floor ---------
openglframework::Vector3 floorPosition(0, 0, 0);
- mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
+ mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath);
// Set the box color
mFloor->setColor(mGreyColorDemo);
@@ -461,43 +461,43 @@ void CollisionShapesScene::renderSinglePass(openglframework::Shader& shader,
// Render all the boxes of the scene
for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix);
+ (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
}
// Render all the sphere of the scene
for (std::vector<Sphere*>::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix);
+ (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
}
// Render all the cones of the scene
for (std::vector<Cone*>::iterator it = mCones.begin(); it != mCones.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix);
+ (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
}
// Render all the cylinders of the scene
for (std::vector<Cylinder*>::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix);
+ (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
}
// Render all the capsules of the scene
for (std::vector<Capsule*>::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix);
+ (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
}
// Render all the convex meshes of the scene
for (std::vector<ConvexMesh*>::iterator it = mConvexMeshes.begin();
it != mConvexMeshes.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix);
+ (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
}
// Render all the dumbbells of the scene
for (std::vector<Dumbbell*>::iterator it = mDumbbells.begin();
it != mDumbbells.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix);
+ (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
}
// Render the floor
- mFloor->render(shader, worldToCameraMatrix);
+ mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
//mConcaveMesh->render(shader, worldToCameraMatrix);
diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
index 41234f3f..9b65893e 100644
--- a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
+++ b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
@@ -58,7 +58,7 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name)
openglframework::Vector3 boxPosition(-NB_BOXES_X * BOX_SIZE * BOXES_SPACE / 2 + i * BOX_SIZE * BOXES_SPACE, 30, -NB_BOXES_Z * BOX_SIZE * BOXES_SPACE / 2 + j * BOX_SIZE * BOXES_SPACE);
// Create a sphere and a corresponding rigid in the dynamics world
- mBoxes[i * NB_BOXES_Z + j] = new Box(Vector3(BOX_SIZE, BOX_SIZE, BOX_SIZE) * 0.5f, boxPosition, 80.1, mDynamicsWorld);
+ mBoxes[i * NB_BOXES_Z + j] = new Box(Vector3(BOX_SIZE, BOX_SIZE, BOX_SIZE) * 0.5f, boxPosition, 80.1, mDynamicsWorld, mMeshFolderPath);
// Set the sphere color
mBoxes[i * NB_BOXES_Z + j]->setColor(mDemoColors[0]);
@@ -160,10 +160,10 @@ void ConcaveMeshScene::renderSinglePass(Shader& shader, const openglframework::M
// Bind the shader
shader.bind();
- mConcaveMesh->render(shader, worldToCameraMatrix);
+ mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
for (int i=0; i<NB_BOXES_X * NB_BOXES_Z; i++) {
- mBoxes[i]->render(shader, worldToCameraMatrix);
+ mBoxes[i]->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
}
// Unbind the shader
diff --git a/testbed/scenes/cubes/CubesScene.cpp b/testbed/scenes/cubes/CubesScene.cpp
index f9c8ad77..17af45c1 100644
--- a/testbed/scenes/cubes/CubesScene.cpp
+++ b/testbed/scenes/cubes/CubesScene.cpp
@@ -1,195 +1,213 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-// Libraries
-#include "CubesScene.h"
-
-// Namespaces
-using namespace openglframework;
-using namespace cubesscene;
-
-// Constructor
-CubesScene::CubesScene(const std::string& name)
- : SceneDemo(name, SCENE_RADIUS) {
-
- // Compute the radius and the center of the scene
- openglframework::Vector3 center(0, 5, 0);
-
- // Set the center of the scene
- setScenePosition(center, SCENE_RADIUS);
-
- // Gravity vector in the dynamics world
- rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0);
-
- // Create the dynamics world for the physics simulation
- mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
-
- float radius = 2.0f;
-
- // Create all the cubes of the scene
- for (int i=0; i<NB_CUBES; i++) {
-
- // Position of the cubes
- float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 30 + i * (BOX_SIZE.y + 0.3f),
- 0);
-
- // Create a cube and a corresponding rigid in the dynamics world
- Box* cube = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld);
-
- // Set the box color
- cube->setColor(mDemoColors[i % mNbDemoColors]);
- cube->setSleepingColor(mRedColorDemo);
-
- // Change the material properties of the rigid body
- rp3d::Material& material = cube->getRigidBody()->getMaterial();
- material.setBounciness(rp3d::decimal(0.4));
-
- // Add the box the list of box in the scene
- mBoxes.push_back(cube);
- }
-
- // Create the floor
- openglframework::Vector3 floorPosition(0, 0, 0);
- mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
- mFloor->setColor(mGreyColorDemo);
- mFloor->setSleepingColor(mGreyColorDemo);
-
- // The floor must be a static rigid body
- mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC);
-
- // Change the material properties of the floor rigid body
- rp3d::Material& material = mFloor->getRigidBody()->getMaterial();
- material.setBounciness(rp3d::decimal(0.3));
-
- // Get the physics engine parameters
- mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled();
- rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity();
- mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z);
- mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled();
- mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity();
- mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity();
- mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver();
- mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver();
- mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep();
-}
-
-// Destructor
-CubesScene::~CubesScene() {
-
- // Destroy all the cubes of the scene
- for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
-
- // Destroy the corresponding rigid body from the dynamics world
- mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
-
- // Destroy the cube
- delete (*it);
- }
-
- // Destroy the rigid body of the floor
- mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody());
-
- // Destroy the floor
- delete mFloor;
-
- // Destroy the dynamics world
- delete mDynamicsWorld;
-}
-
-// Update the physics world (take a simulation step)
-void CubesScene::updatePhysics() {
-
- // Update the physics engine parameters
- mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled);
- rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y,
- mEngineSettings.gravity.z);
- mDynamicsWorld->setGravity(gravity);
- mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled);
- mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity);
- mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity);
- mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations);
- mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations);
- mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep);
-
- // Take a simulation step
- mDynamicsWorld->update(mEngineSettings.timeStep);
-}
-
-// Update the scene
-void CubesScene::update() {
-
- SceneDemo::update();
-
- // Update the position and orientation of the boxes
- for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
-
- // Update the transform used for the rendering
- (*it)->updateTransform(mInterpolationFactor);
- }
-
- mFloor->updateTransform(mInterpolationFactor);
-}
-
-// Render the scene in a single pass
-void CubesScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
-
- // Bind the shader
- shader.bind();
-
- // Render all the cubes of the scene
- for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix);
- }
-
- // Render the floor
- mFloor->render(shader, worldToCameraMatrix);
-
- // Unbind the shader
- shader.unbind();
-}
-
-// Reset the scene
-void CubesScene::reset() {
-
- float radius = 2.0f;
-
- for (int i=0; i<NB_CUBES; i++) {
-
- // Position of the cubes
- float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 10 + i * (BOX_SIZE.y + 0.3f),
- 0);
-
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- mBoxes[i]->resetTransform(transform);
- }
-}
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "CubesScene.h"
+
+// Namespaces
+using namespace openglframework;
+using namespace cubesscene;
+
+// Constructor
+CubesScene::CubesScene(const std::string& name)
+ : SceneDemo(name, SCENE_RADIUS) {
+
+ // Compute the radius and the center of the scene
+ openglframework::Vector3 center(0, 5, 0);
+
+ // Set the center of the scene
+ setScenePosition(center, SCENE_RADIUS);
+
+ // Gravity vector in the dynamics world
+ rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0);
+
+ // Create the dynamics world for the physics simulation
+ mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
+
+ float radius = 2.0f;
+
+ //// Create all the cubes of the scene
+ //for (int i=0; i<NB_CUBES; i++) {
+
+ // // Position of the cubes
+ // float angle = i * 30.0f;
+ // openglframework::Vector3 position(radius * cos(angle),
+ // 30 + i * (BOX_SIZE.y + 0.3f),
+ // 0);
+
+ // // Create a cube and a corresponding rigid in the dynamics world
+ // Box* cube = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld);
+
+ // // Set the box color
+ // cube->setColor(mDemoColors[i % mNbDemoColors]);
+ // cube->setSleepingColor(mRedColorDemo);
+
+ // // Change the material properties of the rigid body
+ // rp3d::Material& material = cube->getRigidBody()->getMaterial();
+ // material.setBounciness(rp3d::decimal(0.4));
+
+ // // Add the box the list of box in the scene
+ // mBoxes.push_back(cube);
+ //}
+
+ // ------------------------- FLOOR ----------------------- //
+
+ // Create the floor
+ openglframework::Vector3 floorPosition(0, 0, 0);
+ mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath);
+ mFloor->setColor(mGreyColorDemo);
+ mFloor->setSleepingColor(mGreyColorDemo);
+
+ // The floor must be a static rigid body
+ mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC);
+
+ // Change the material properties of the floor rigid body
+ //rp3d::Material& material = mFloor->getRigidBody()->getMaterial();
+ //material.setBounciness(rp3d::decimal(0.3));
+
+ // ------------------------- BOX ----------------------- //
+
+ // Create a cube and a corresponding rigid in the dynamics world
+ Box* cube = new Box(BOX_SIZE, Vector3(0, 10, 0), BOX_MASS, mDynamicsWorld, mMeshFolderPath);
+
+ // Set the box color
+ cube->setColor(mDemoColors[0]);
+ cube->setSleepingColor(mRedColorDemo);
+
+ // Change the material properties of the rigid body
+ //rp3d::Material& material = cube->getRigidBody()->getMaterial();
+ //material.setBounciness(rp3d::decimal(0.4));
+
+ // Add the box the list of box in the scene
+ mBoxes.push_back(cube);
+
+ // Get the physics engine parameters
+ mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled();
+ rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity();
+ mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z);
+ mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled();
+ mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity();
+ mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity();
+ mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver();
+ mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver();
+ mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep();
+}
+
+// Destructor
+CubesScene::~CubesScene() {
+
+ // Destroy all the cubes of the scene
+ for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
+
+ // Destroy the cube
+ delete (*it);
+ }
+
+ // Destroy the rigid body of the floor
+ mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody());
+
+ // Destroy the floor
+ delete mFloor;
+
+ // Destroy the dynamics world
+ delete mDynamicsWorld;
+}
+
+// Update the physics world (take a simulation step)
+void CubesScene::updatePhysics() {
+
+ // Update the physics engine parameters
+ mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled);
+ rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y,
+ mEngineSettings.gravity.z);
+ mDynamicsWorld->setGravity(gravity);
+ mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled);
+ mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity);
+ mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity);
+ mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations);
+ mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations);
+ mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep);
+
+ // Take a simulation step
+ mDynamicsWorld->update(mEngineSettings.timeStep);
+}
+
+// Update the scene
+void CubesScene::update() {
+
+ SceneDemo::update();
+
+ // Update the position and orientation of the boxes
+ for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
+
+ // Update the transform used for the rendering
+ (*it)->updateTransform(mInterpolationFactor);
+ }
+
+ mFloor->updateTransform(mInterpolationFactor);
+}
+
+// Render the scene in a single pass
+void CubesScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
+
+ // Bind the shader
+ shader.bind();
+
+ // Render all the cubes of the scene
+ for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
+ (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ }
+
+ // Render the floor
+ mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+
+ // Unbind the shader
+ shader.unbind();
+}
+
+// Reset the scene
+void CubesScene::reset() {
+
+ float radius = 2.0f;
+
+ for (int i=0; i<NB_CUBES; i++) {
+
+ // Position of the cubes
+ float angle = i * 30.0f;
+ openglframework::Vector3 position(radius * cos(angle),
+ 10 + i * (BOX_SIZE.y + 0.3f),
+ 0);
+
+ // Initial position and orientation of the rigid body
+ rp3d::Vector3 initPosition(position.x, position.y, position.z);
+ rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
+ rp3d::Transform transform(initPosition, initOrientation);
+
+ mBoxes[i]->resetTransform(transform);
+ }
+}
diff --git a/testbed/scenes/cubes/CubesScene.h b/testbed/scenes/cubes/CubesScene.h
index a07c74d2..4ad57f77 100644
--- a/testbed/scenes/cubes/CubesScene.h
+++ b/testbed/scenes/cubes/CubesScene.h
@@ -1,96 +1,96 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef CUBES_SCENE_H
-#define CUBES_SCENE_H
-
-// Libraries
-#include "openglframework.h"
-#include "reactphysics3d.h"
-#include "Box.h"
-#include "SceneDemo.h"
-
-namespace cubesscene {
-
-// Constants
-const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters
-const int NB_CUBES = 30; // Number of boxes in the scene
-const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters
-const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters
-const float BOX_MASS = 1.0f; // Box mass in kilograms
-const float FLOOR_MASS = 100.0f; // Floor mass in kilograms
-
-// Class CubesScene
-class CubesScene : public SceneDemo {
-
- protected :
-
- // -------------------- Attributes -------------------- //
-
- /// All the boxes of the scene
- std::vector<Box*> mBoxes;
-
- /// Box for the floor
- Box* mFloor;
-
- /// Dynamics world used for the physics simulation
- rp3d::DynamicsWorld* mDynamicsWorld;
-
- public:
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- CubesScene(const std::string& name);
-
- /// Destructor
- virtual ~CubesScene() override;
-
- /// Update the physics world (take a simulation step)
- /// Can be called several times per frame
- virtual void updatePhysics() override;
-
- /// Update the scene (take a simulation step)
- virtual void update() override;
-
- /// Render the scene in a single pass
- virtual void renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) override;
-
- /// Reset the scene
- virtual void reset() override;
-
- /// Return all the contact points of the scene
- virtual std::vector<ContactPoint> getContactPoints() const override;
-};
-
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> CubesScene::getContactPoints() const {
- return computeContactPointsOfWorld(mDynamicsWorld);
-}
-
-}
-
-#endif
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef CUBES_SCENE_H
+#define CUBES_SCENE_H
+
+// Libraries
+#include "openglframework.h"
+#include "reactphysics3d.h"
+#include "Box.h"
+#include "SceneDemo.h"
+
+namespace cubesscene {
+
+// Constants
+const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters
+const int NB_CUBES = 30; // Number of boxes in the scene
+const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters
+const openglframework::Vector3 FLOOR_SIZE(5, 5.0, 5); // Floor dimensions in meters
+const float BOX_MASS = 1.0f; // Box mass in kilograms
+const float FLOOR_MASS = 100.0f; // Floor mass in kilograms
+
+// Class CubesScene
+class CubesScene : public SceneDemo {
+
+ protected :
+
+ // -------------------- Attributes -------------------- //
+
+ /// All the boxes of the scene
+ std::vector<Box*> mBoxes;
+
+ /// Box for the floor
+ Box* mFloor;
+
+ /// Dynamics world used for the physics simulation
+ rp3d::DynamicsWorld* mDynamicsWorld;
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ CubesScene(const std::string& name);
+
+ /// Destructor
+ virtual ~CubesScene() override;
+
+ /// Update the physics world (take a simulation step)
+ /// Can be called several times per frame
+ virtual void updatePhysics() override;
+
+ /// Update the scene (take a simulation step)
+ virtual void update() override;
+
+ /// Render the scene in a single pass
+ virtual void renderSinglePass(openglframework::Shader& shader,
+ const openglframework::Matrix4& worldToCameraMatrix) override;
+
+ /// Reset the scene
+ virtual void reset() override;
+
+ /// Return all the contact points of the scene
+ virtual std::vector<ContactPoint> getContactPoints() const override;
+};
+
+// Return all the contact points of the scene
+inline std::vector<ContactPoint> CubesScene::getContactPoints() const {
+ return computeContactPointsOfWorld(mDynamicsWorld);
+}
+
+}
+
+#endif
diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp
index 3395ac17..5b62f614 100644
--- a/testbed/scenes/heightfield/HeightFieldScene.cpp
+++ b/testbed/scenes/heightfield/HeightFieldScene.cpp
@@ -54,7 +54,7 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
openglframework::Vector3 position(15, 10 + 6 * i, 0);
// Create a box and a corresponding rigid in the dynamics world
- mBoxes[i] = new Box(Vector3(3, 3, 3), position, 80.1, mDynamicsWorld);
+ mBoxes[i] = new Box(Vector3(3, 3, 3), position, 80.1, mDynamicsWorld, mMeshFolderPath);
// Set the box color
mBoxes[i]->setColor(mDemoColors[2]);
@@ -156,10 +156,10 @@ void HeightFieldScene::renderSinglePass(Shader& shader, const openglframework::M
// Bind the shader
shader.bind();
- mHeightField->render(shader, worldToCameraMatrix);
+ mHeightField->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
for (int i=0; i<NB_BOXES; i++) {
- mBoxes[i]->render(shader, worldToCameraMatrix);
+ mBoxes[i]->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
}
// Unbind the shader
diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp
index b1891627..34bb1503 100644
--- a/testbed/scenes/joints/JointsScene.cpp
+++ b/testbed/scenes/joints/JointsScene.cpp
@@ -163,17 +163,17 @@ void JointsScene::renderSinglePass(openglframework::Shader& shader,
shader.bind();
// Render all the boxes
- mSliderJointBottomBox->render(shader, worldToCameraMatrix);
- mSliderJointTopBox->render(shader, worldToCameraMatrix);
- mPropellerBox->render(shader, worldToCameraMatrix);
- mFixedJointBox1->render(shader, worldToCameraMatrix);
- mFixedJointBox2->render(shader, worldToCameraMatrix);
+ mSliderJointBottomBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ mSliderJointTopBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ mPropellerBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ mFixedJointBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ mFixedJointBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
for (int i=0; i<NB_BALLSOCKETJOINT_BOXES; i++) {
- mBallAndSocketJointChainBoxes[i]->render(shader, worldToCameraMatrix);
+ mBallAndSocketJointChainBoxes[i]->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
}
// Render the floor
- mFloor->render(shader, worldToCameraMatrix);
+ mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
// Unbind the shader
shader.unbind();
@@ -263,7 +263,7 @@ void JointsScene::createBallAndSocketJoints() {
// Create a box and a corresponding rigid in the dynamics world
mBallAndSocketJointChainBoxes[i] = new Box(boxDimension, positionBox , boxMass,
- mDynamicsWorld);
+ mDynamicsWorld, mMeshFolderPath);
// Set the box color
mBallAndSocketJointChainBoxes[i]->setColor(mDemoColors[i % mNbDemoColors]);
@@ -312,7 +312,7 @@ void JointsScene::createSliderJoint() {
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 box1Dimension(2, 4, 2);
- mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld);
+ mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath);
// Set the box color
mSliderJointBottomBox->setColor(mBlueColorDemo);
@@ -332,7 +332,7 @@ void JointsScene::createSliderJoint() {
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 box2Dimension(1.5f, 4, 1.5f);
- mSliderJointTopBox = new Box(box2Dimension, positionBox2, BOX_MASS, mDynamicsWorld);
+ mSliderJointTopBox = new Box(box2Dimension, positionBox2, BOX_MASS, mDynamicsWorld, mMeshFolderPath);
// Set the box color
mSliderJointTopBox->setColor(mOrangeColorDemo);
@@ -372,7 +372,7 @@ void JointsScene::createPropellerHingeJoint() {
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 boxDimension(10, 1, 1);
- mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld);
+ mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath);
// Set the box color
mPropellerBox->setColor(mYellowColorDemo);
@@ -411,7 +411,7 @@ void JointsScene::createFixedJoints() {
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 boxDimension(1.5, 1.5, 1.5);
- mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld);
+ mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath);
// Set the box color
mFixedJointBox1->setColor(mPinkColorDemo);
@@ -427,7 +427,7 @@ void JointsScene::createFixedJoints() {
openglframework::Vector3 positionBox2(-5, 7, 0);
// Create a box and a corresponding rigid in the dynamics world
- mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld);
+ mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld, mMeshFolderPath);
// Set the box color
mFixedJointBox2->setColor(mBlueColorDemo);
@@ -466,7 +466,7 @@ void JointsScene::createFloor() {
// Create the floor
openglframework::Vector3 floorPosition(0, 0, 0);
- mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld);
+ mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath);
// Set the box color
mFloor->setColor(mGreyColorDemo);
diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp
index 479e0915..58b19055 100644
--- a/testbed/scenes/raycast/RaycastScene.cpp
+++ b/testbed/scenes/raycast/RaycastScene.cpp
@@ -61,7 +61,7 @@ RaycastScene::RaycastScene(const std::string& name)
openglframework::Vector3 position2(0, 0, 0);
// Create a box and a corresponding collision body in the dynamics world
- mBox = new Box(BOX_SIZE, position2, mCollisionWorld);
+ mBox = new Box(BOX_SIZE, position2, mCollisionWorld, mMeshFolderPath);
mBox->getCollisionBody()->setIsActive(false);
// Set the box color
@@ -376,15 +376,15 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader,
shader.unbind();
// Render the shapes
- if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
- if (mSphere->getCollisionBody()->isActive()) mSphere->render(shader, worldToCameraMatrix);
- if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix);
- if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix);
- 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);
- if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix);
+ if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mSphere->getCollisionBody()->isActive()) mSphere->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
shader.unbind();
}
diff --git a/testbed/src/Gui.cpp b/testbed/src/Gui.cpp
index 499f1fb3..cd9f8f4e 100644
--- a/testbed/src/Gui.cpp
+++ b/testbed/src/Gui.cpp
@@ -402,6 +402,13 @@ void Gui::createSettingsPanel() {
mApp->mIsShadowMappingEnabled = value;
});
+ // Enable/Disable wireframe mode
+ CheckBox* checkboxWireframe = new CheckBox(mRenderingPanel, "Wireframe");
+ checkboxWireframe->setChecked(mApp->mIsWireframeEnabled);
+ checkboxWireframe->setCallback([&](bool value) {
+ mApp->mIsWireframeEnabled = value;
+ });
+
mPhysicsPanel->setVisible(true);
mRenderingPanel->setVisible(false);
}
diff --git a/testbed/src/Scene.cpp b/testbed/src/Scene.cpp
index 8031c1f8..e872b3bc 100644
--- a/testbed/src/Scene.cpp
+++ b/testbed/src/Scene.cpp
@@ -33,7 +33,7 @@ using namespace openglframework;
Scene::Scene(const std::string& name, bool isShadowMappingEnabled)
: mName(name), mInterpolationFactor(0.0f), mViewportX(0), mViewportY(0),
mViewportWidth(0), mViewportHeight(0), mIsShadowMappingEnabled(isShadowMappingEnabled),
- mIsContactPointsDisplayed(true) {
+ mIsContactPointsDisplayed(true), mIsWireframeEnabled(false) {
}
diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h
index bfac3ecb..d51ffec1 100644
--- a/testbed/src/Scene.h
+++ b/testbed/src/Scene.h
@@ -108,6 +108,9 @@ class Scene {
/// True if contact points are displayed
bool mIsContactPointsDisplayed;
+ /// True if we render shapes in wireframe mode
+ bool mIsWireframeEnabled;
+
// -------------------- Methods -------------------- //
/// Set the scene position (where the camera needs to look at)
@@ -199,6 +202,12 @@ class Scene {
/// Display/Hide the contact points
void virtual setIsContactPointsDisplayed(bool display);
+ /// Return true if wireframe rendering is enabled
+ bool getIsWireframeEnabled() const;
+
+ /// Enable/disbale wireframe rendering
+ void setIsWireframeEnabled(bool isEnabled);
+
/// Return all the contact points of the scene
std::vector<ContactPoint> virtual getContactPoints() const;
};
@@ -267,6 +276,16 @@ inline void Scene::setIsContactPointsDisplayed(bool display) {
mIsContactPointsDisplayed = display;
}
+// Return true if wireframe rendering is enabled
+inline bool Scene::getIsWireframeEnabled() const {
+ return mIsWireframeEnabled;
+}
+
+// Enable/disbale wireframe rendering
+inline void Scene::setIsWireframeEnabled(bool isEnabled) {
+ mIsWireframeEnabled = isEnabled;
+}
+
// Return all the contact points of the scene
inline std::vector<ContactPoint> Scene::getContactPoints() const {
diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp
index f57aa829..09f464f3 100644
--- a/testbed/src/TestbedApplication.cpp
+++ b/testbed/src/TestbedApplication.cpp
@@ -30,6 +30,7 @@
#include <cstdlib>
#include <sstream>
#include "cubes/CubesScene.h"
+#include "collisiondetection/CollisionDetectionScene.h"
#include "joints/JointsScene.h"
#include "collisionshapes/CollisionShapesScene.h"
#include "heightfield/HeightFieldScene.h"
@@ -43,6 +44,7 @@ using namespace raycastscene;
using namespace collisionshapesscene;
using namespace trianglemeshscene;
using namespace heightfieldscene;
+using namespace collisiondetectionscene;
// Initialization of static variables
const float TestbedApplication::SCROLL_SENSITIVITY = 0.08f;
@@ -63,6 +65,7 @@ TestbedApplication::TestbedApplication(bool isFullscreen)
mIsShadowMappingEnabled = true;
mIsVSyncEnabled = false;
mIsContactPointsDisplayed = false;
+ mIsWireframeEnabled = false;
init();
@@ -113,7 +116,11 @@ void TestbedApplication::createScenes() {
RaycastScene* raycastScene = new RaycastScene("Raycast");
mScenes.push_back(raycastScene);
- // Raycast scene
+ // Collision Detection scene
+ CollisionDetectionScene* collisionDetectionScene = new CollisionDetectionScene("Collision Detection");
+ mScenes.push_back(collisionDetectionScene);
+
+ // Concave Mesh scene
ConcaveMeshScene* concaveMeshScene = new ConcaveMeshScene("Concave Mesh");
mScenes.push_back(concaveMeshScene);
@@ -195,6 +202,9 @@ void TestbedApplication::update() {
// Display/Hide contact points
mCurrentScene->setIsContactPointsDisplayed(mIsContactPointsDisplayed);
+ // Enable/Disable wireframe mode
+ mCurrentScene->setIsWireframeEnabled(mIsWireframeEnabled);
+
// Update the scene
mCurrentScene->update();
}
diff --git a/testbed/src/TestbedApplication.h b/testbed/src/TestbedApplication.h
index dcff0598..93d85ea7 100644
--- a/testbed/src/TestbedApplication.h
+++ b/testbed/src/TestbedApplication.h
@@ -109,6 +109,9 @@ class TestbedApplication : public Screen {
/// True if contact points are displayed
bool mIsContactPointsDisplayed;
+ /// True if the wireframe rendering is enabled
+ bool mIsWireframeEnabled;
+
/// True if vsync is enabled
bool mIsVSyncEnabled;
From a9da0d6d3c7a68136864ebaf88c23e58230c194d Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 28 Jan 2017 00:59:04 +0100
Subject: [PATCH 031/133] Refactor testbed application
---
testbed/common/Box.cpp | 6 ++----
testbed/common/Box.h | 2 +-
testbed/common/Capsule.cpp | 4 ++--
testbed/common/Capsule.h | 2 +-
testbed/common/ConcaveMesh.cpp | 4 ++--
testbed/common/ConcaveMesh.h | 2 +-
testbed/common/Cone.cpp | 4 ++--
testbed/common/Cone.h | 2 +-
testbed/common/ConvexMesh.cpp | 4 ++--
testbed/common/ConvexMesh.h | 2 +-
testbed/common/Cylinder.cpp | 4 ++--
testbed/common/Cylinder.h | 2 +-
testbed/common/Dumbbell.cpp | 6 ++----
testbed/common/Dumbbell.h | 2 +-
testbed/common/HeightField.cpp | 4 ++--
testbed/common/HeightField.h | 2 +-
testbed/common/PhysicsObject.cpp | 2 +-
testbed/common/PhysicsObject.h | 2 +-
testbed/common/Sphere.cpp | 4 ++--
testbed/common/Sphere.h | 2 +-
testbed/scenes/cubes/CubesScene.cpp | 24 ++++++++++++------------
21 files changed, 41 insertions(+), 45 deletions(-)
diff --git a/testbed/common/Box.cpp b/testbed/common/Box.cpp
index 6c8f052d..0720799e 100644
--- a/testbed/common/Box.cpp
+++ b/testbed/common/Box.cpp
@@ -39,8 +39,7 @@ int Box::totalNbBoxes = 0;
// Constructor
Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath)
- : openglframework::Mesh() {
+ reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this);
@@ -94,8 +93,7 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
// Constructor
Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& position,
- float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath)
- : openglframework::Mesh() {
+ float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this);
diff --git a/testbed/common/Box.h b/testbed/common/Box.h
index 53da204e..0c21be36 100644
--- a/testbed/common/Box.h
+++ b/testbed/common/Box.h
@@ -32,7 +32,7 @@
#include "PhysicsObject.h"
// Class Box
-class Box : public openglframework::Mesh, public PhysicsObject {
+class Box : public PhysicsObject {
private :
diff --git a/testbed/common/Capsule.cpp b/testbed/common/Capsule.cpp
index ba6a45b4..e9ae2b40 100644
--- a/testbed/common/Capsule.cpp
+++ b/testbed/common/Capsule.cpp
@@ -37,7 +37,7 @@ int Capsule::totalNbCapsules = 0;
Capsule::Capsule(float radius, float height, const openglframework::Vector3& position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
- : openglframework::Mesh(), mRadius(radius), mHeight(height) {
+ : mRadius(radius), mHeight(height) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this);
@@ -86,7 +86,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
Capsule::Capsule(float radius, float height, const openglframework::Vector3& position,
float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath)
- : openglframework::Mesh(), mRadius(radius), mHeight(height) {
+ : mRadius(radius), mHeight(height) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this);
diff --git a/testbed/common/Capsule.h b/testbed/common/Capsule.h
index 748fa46b..2a97b14b 100644
--- a/testbed/common/Capsule.h
+++ b/testbed/common/Capsule.h
@@ -32,7 +32,7 @@
#include "PhysicsObject.h"
// Class Sphere
-class Capsule : public openglframework::Mesh, public PhysicsObject {
+class Capsule : public PhysicsObject {
private :
diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp
index c21732f8..2f0360ba 100644
--- a/testbed/common/ConcaveMesh.cpp
+++ b/testbed/common/ConcaveMesh.cpp
@@ -30,7 +30,7 @@
ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world,
const std::string& meshPath)
- : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER),
+ : mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
@@ -87,7 +87,7 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position,
ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass,
reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshPath)
- : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER),
+ : mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
diff --git a/testbed/common/ConcaveMesh.h b/testbed/common/ConcaveMesh.h
index 3c645ec8..e4b33308 100644
--- a/testbed/common/ConcaveMesh.h
+++ b/testbed/common/ConcaveMesh.h
@@ -32,7 +32,7 @@
#include "PhysicsObject.h"
// Class ConcaveMesh
-class ConcaveMesh : public openglframework::Mesh, public PhysicsObject {
+class ConcaveMesh : public PhysicsObject {
private :
diff --git a/testbed/common/Cone.cpp b/testbed/common/Cone.cpp
index 07a825a8..96f52c54 100644
--- a/testbed/common/Cone.cpp
+++ b/testbed/common/Cone.cpp
@@ -37,7 +37,7 @@ int Cone::totalNbCones = 0;
Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
- : openglframework::Mesh(), mRadius(radius), mHeight(height) {
+ : mRadius(radius), mHeight(height) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this);
@@ -85,7 +85,7 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath)
- : openglframework::Mesh(), mRadius(radius), mHeight(height) {
+ : mRadius(radius), mHeight(height) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this);
diff --git a/testbed/common/Cone.h b/testbed/common/Cone.h
index e4f7face..6fc35182 100644
--- a/testbed/common/Cone.h
+++ b/testbed/common/Cone.h
@@ -32,7 +32,7 @@
#include "PhysicsObject.h"
// Class Cone
-class Cone : public openglframework::Mesh, public PhysicsObject {
+class Cone : public PhysicsObject {
private :
diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp
index ca372a1f..c107724f 100644
--- a/testbed/common/ConvexMesh.cpp
+++ b/testbed/common/ConvexMesh.cpp
@@ -30,7 +30,7 @@
ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world,
const std::string& meshPath)
- : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER),
+ : mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
@@ -81,7 +81,7 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshPath)
- : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER),
+ : mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h
index a4a2ffab..19b39220 100644
--- a/testbed/common/ConvexMesh.h
+++ b/testbed/common/ConvexMesh.h
@@ -32,7 +32,7 @@
#include "PhysicsObject.h"
// Class ConvexMesh
-class ConvexMesh : public openglframework::Mesh, public PhysicsObject {
+class ConvexMesh : public PhysicsObject {
private :
diff --git a/testbed/common/Cylinder.cpp b/testbed/common/Cylinder.cpp
index d5ec1964..9b520d5c 100644
--- a/testbed/common/Cylinder.cpp
+++ b/testbed/common/Cylinder.cpp
@@ -37,7 +37,7 @@ int Cylinder::totalNbCylinders = 0;
Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
- : openglframework::Mesh(), mRadius(radius), mHeight(height) {
+ : mRadius(radius), mHeight(height) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this);
@@ -85,7 +85,7 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p
Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position,
float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath)
- : openglframework::Mesh(), mRadius(radius), mHeight(height) {
+ : mRadius(radius), mHeight(height) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this);
diff --git a/testbed/common/Cylinder.h b/testbed/common/Cylinder.h
index eb7ba72d..4aab2dc9 100644
--- a/testbed/common/Cylinder.h
+++ b/testbed/common/Cylinder.h
@@ -32,7 +32,7 @@
#include "PhysicsObject.h"
// Class Cylinder
-class Cylinder : public openglframework::Mesh, public PhysicsObject {
+class Cylinder : public PhysicsObject {
private :
diff --git a/testbed/common/Dumbbell.cpp b/testbed/common/Dumbbell.cpp
index e590d033..770f8ec1 100644
--- a/testbed/common/Dumbbell.cpp
+++ b/testbed/common/Dumbbell.cpp
@@ -35,8 +35,7 @@ int Dumbbell::totalNbDumbbells = 0;
// Constructor
Dumbbell::Dumbbell(const openglframework::Vector3 &position,
- reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath)
- : openglframework::Mesh() {
+ reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this);
@@ -106,8 +105,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
// Constructor
Dumbbell::Dumbbell(const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath)
- : openglframework::Mesh() {
+ reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this);
diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h
index 4d44a38b..5ce3ec95 100644
--- a/testbed/common/Dumbbell.h
+++ b/testbed/common/Dumbbell.h
@@ -32,7 +32,7 @@
#include "PhysicsObject.h"
// Class Sphere
-class Dumbbell : public openglframework::Mesh, public PhysicsObject {
+class Dumbbell : public PhysicsObject {
private :
diff --git a/testbed/common/HeightField.cpp b/testbed/common/HeightField.cpp
index 38eda261..55da4e1d 100644
--- a/testbed/common/HeightField.cpp
+++ b/testbed/common/HeightField.cpp
@@ -30,7 +30,7 @@
// Constructor
HeightField::HeightField(const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world)
- : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER),
+ : mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
@@ -73,7 +73,7 @@ HeightField::HeightField(const openglframework::Vector3 &position,
// Constructor
HeightField::HeightField(const openglframework::Vector3 &position, float mass,
reactphysics3d::DynamicsWorld* dynamicsWorld)
- : openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER),
+ : mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
diff --git a/testbed/common/HeightField.h b/testbed/common/HeightField.h
index d4a4895d..d9851992 100644
--- a/testbed/common/HeightField.h
+++ b/testbed/common/HeightField.h
@@ -33,7 +33,7 @@
// Class HeightField
-class HeightField : public openglframework::Mesh, public PhysicsObject {
+class HeightField : public PhysicsObject {
private :
diff --git a/testbed/common/PhysicsObject.cpp b/testbed/common/PhysicsObject.cpp
index c176bb5d..c69f9485 100644
--- a/testbed/common/PhysicsObject.cpp
+++ b/testbed/common/PhysicsObject.cpp
@@ -27,7 +27,7 @@
#include "PhysicsObject.h"
/// Constructor
-PhysicsObject::PhysicsObject() {
+PhysicsObject::PhysicsObject() : openglframework::Mesh() {
mColor = openglframework::Color(1, 1, 1, 1);
mSleepingColor = openglframework::Color(1, 0, 0, 1);
diff --git a/testbed/common/PhysicsObject.h b/testbed/common/PhysicsObject.h
index 5a54d6d0..bc470091 100644
--- a/testbed/common/PhysicsObject.h
+++ b/testbed/common/PhysicsObject.h
@@ -31,7 +31,7 @@
#include "reactphysics3d.h"
// Class PhysicsObject
-class PhysicsObject {
+class PhysicsObject : public openglframework::Mesh {
protected:
diff --git a/testbed/common/Sphere.cpp b/testbed/common/Sphere.cpp
index 236a81ad..93c48af2 100644
--- a/testbed/common/Sphere.cpp
+++ b/testbed/common/Sphere.cpp
@@ -37,7 +37,7 @@ int Sphere::totalNbSpheres = 0;
Sphere::Sphere(float radius, const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
- : openglframework::Mesh(), mRadius(radius) {
+ : mRadius(radius) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this);
@@ -86,7 +86,7 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
Sphere::Sphere(float radius, const openglframework::Vector3 &position,
float mass, reactphysics3d::DynamicsWorld* world,
const std::string& meshFolderPath)
- : openglframework::Mesh(), mRadius(radius) {
+ : mRadius(radius) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this);
diff --git a/testbed/common/Sphere.h b/testbed/common/Sphere.h
index 5fdfcfe9..d941d984 100644
--- a/testbed/common/Sphere.h
+++ b/testbed/common/Sphere.h
@@ -32,7 +32,7 @@
#include "PhysicsObject.h"
// Class Sphere
-class Sphere : public openglframework::Mesh, public PhysicsObject {
+class Sphere : public PhysicsObject {
private :
diff --git a/testbed/scenes/cubes/CubesScene.cpp b/testbed/scenes/cubes/CubesScene.cpp
index 17af45c1..81929e58 100644
--- a/testbed/scenes/cubes/CubesScene.cpp
+++ b/testbed/scenes/cubes/CubesScene.cpp
@@ -195,19 +195,19 @@ void CubesScene::reset() {
float radius = 2.0f;
- for (int i=0; i<NB_CUBES; i++) {
+// for (int i=0; i<NB_CUBES; i++) {
- // Position of the cubes
- float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 10 + i * (BOX_SIZE.y + 0.3f),
- 0);
+// // Position of the cubes
+// float angle = i * 30.0f;
+// openglframework::Vector3 position(radius * cos(angle),
+// 10 + i * (BOX_SIZE.y + 0.3f),
+// 0);
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
+// // Initial position and orientation of the rigid body
+// rp3d::Vector3 initPosition(position.x, position.y, position.z);
+// rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
+// rp3d::Transform transform(initPosition, initOrientation);
- mBoxes[i]->resetTransform(transform);
- }
+// mBoxes[i]->resetTransform(transform);
+// }
}
From 0b0975769b1a408266cf3f23e1175709e48cfe77 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 28 Jan 2017 11:38:48 +0100
Subject: [PATCH 032/133] Refactor testbed application
---
testbed/common/Box.cpp | 30 +++-------------
testbed/common/Box.h | 3 --
testbed/common/Capsule.cpp | 34 ++----------------
testbed/common/Capsule.h | 3 --
testbed/common/ConcaveMesh.cpp | 34 ++----------------
testbed/common/ConcaveMesh.h | 3 --
testbed/common/Cone.cpp | 34 ++----------------
testbed/common/Cone.h | 3 --
testbed/common/ConvexMesh.cpp | 34 ++----------------
testbed/common/ConvexMesh.h | 3 --
testbed/common/Cylinder.cpp | 34 ++----------------
testbed/common/Cylinder.h | 3 --
testbed/common/Dumbbell.cpp | 36 +++----------------
testbed/common/Dumbbell.h | 3 --
testbed/common/HeightField.cpp | 18 ----------
testbed/common/HeightField.h | 3 --
testbed/common/PhysicsObject.cpp | 28 +++++++++++++++
testbed/common/PhysicsObject.h | 6 ++++
testbed/common/Sphere.cpp | 34 ++----------------
testbed/common/Sphere.h | 3 --
.../CollisionDetectionScene.h | 1 -
21 files changed, 54 insertions(+), 296 deletions(-)
diff --git a/testbed/common/Box.cpp b/testbed/common/Box.cpp
index 0720799e..09dc388e 100644
--- a/testbed/common/Box.cpp
+++ b/testbed/common/Box.cpp
@@ -39,13 +39,8 @@ int Box::totalNbBoxes = 0;
// Constructor
Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) {
-
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
+ reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath)
+ : PhysicsObject(meshFolderPath + "cube.obj") {
// Initialize the size of the box
mSize[0] = size.x * 0.5f;
@@ -93,7 +88,8 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
// Constructor
Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& position,
- float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath) {
+ float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath)
+ : PhysicsObject(meshFolderPath + "cube.obj") {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cube.obj", *this);
@@ -284,24 +280,6 @@ void Box::createVBOAndVAO() {
mVAO.unbind();
}
-// Reset the transform
-void Box::resetTransform(const rp3d::Transform& transform) {
-
- // Reset the transform
- mBody->setTransform(transform);
-
- mBody->setIsSleeping(false);
-
- // Reset the velocity of the rigid body
- rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
- if (rigidBody != nullptr) {
- rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0));
- rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0));
- }
-
- updateTransform(1.0f);
-}
-
// Set the scaling of the object
void Box::setScaling(const openglframework::Vector3& scaling) {
diff --git a/testbed/common/Box.h b/testbed/common/Box.h
index 0c21be36..a1f05d5e 100644
--- a/testbed/common/Box.h
+++ b/testbed/common/Box.h
@@ -88,9 +88,6 @@ class Box : public PhysicsObject {
/// Render the cube at the correct position and with the correct orientation
void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
- /// Set the position of the box
- void resetTransform(const rp3d::Transform& transform);
-
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/Capsule.cpp b/testbed/common/Capsule.cpp
index e9ae2b40..413d6eb2 100644
--- a/testbed/common/Capsule.cpp
+++ b/testbed/common/Capsule.cpp
@@ -37,13 +37,7 @@ int Capsule::totalNbCapsules = 0;
Capsule::Capsule(float radius, float height, const openglframework::Vector3& position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
- : mRadius(radius), mHeight(height) {
-
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
+ : PhysicsObject(meshFolderPath + "capsule.obj"), mRadius(radius), mHeight(height) {
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
@@ -86,13 +80,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
Capsule::Capsule(float radius, float height, const openglframework::Vector3& position,
float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath)
- : mRadius(radius), mHeight(height) {
-
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
+ : PhysicsObject(meshFolderPath + "capsule.obj"), mRadius(radius), mHeight(height) {
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
@@ -272,24 +260,6 @@ void Capsule::createVBOAndVAO() {
mVAO.unbind();
}
-// Reset the transform
-void Capsule::resetTransform(const rp3d::Transform& transform) {
-
- // Reset the transform
- mBody->setTransform(transform);
-
- mBody->setIsSleeping(false);
-
- // Reset the velocity of the rigid body
- rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
- if (rigidBody != nullptr) {
- rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0));
- rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0));
- }
-
- updateTransform(1.0f);
-}
-
// Set the scaling of the object
void Capsule::setScaling(const openglframework::Vector3& scaling) {
diff --git a/testbed/common/Capsule.h b/testbed/common/Capsule.h
index 2a97b14b..f117984f 100644
--- a/testbed/common/Capsule.h
+++ b/testbed/common/Capsule.h
@@ -97,9 +97,6 @@ class Capsule : public PhysicsObject {
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
- /// Set the position of the box
- void resetTransform(const rp3d::Transform& transform);
-
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp
index 2f0360ba..608e0020 100644
--- a/testbed/common/ConcaveMesh.cpp
+++ b/testbed/common/ConcaveMesh.cpp
@@ -30,16 +30,10 @@
ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world,
const std::string& meshPath)
- : mVBOVertices(GL_ARRAY_BUFFER),
+ : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
-
// Initialize the position where the sphere will be rendered
translateWorld(position);
@@ -87,16 +81,10 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position,
ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass,
reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshPath)
- : mVBOVertices(GL_ARRAY_BUFFER),
+ : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
-
// Initialize the position where the sphere will be rendered
translateWorld(position);
@@ -285,24 +273,6 @@ void ConcaveMesh::createVBOAndVAO() {
mVAO.unbind();
}
-// Reset the transform
-void ConcaveMesh::resetTransform(const rp3d::Transform& transform) {
-
- // Reset the transform
- mBody->setTransform(transform);
-
- mBody->setIsSleeping(false);
-
- // Reset the velocity of the rigid body
- rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
- if (rigidBody != nullptr) {
- rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0));
- rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0));
- }
-
- updateTransform(1.0f);
-}
-
// Set the scaling of the object
void ConcaveMesh::setScaling(const openglframework::Vector3& scaling) {
diff --git a/testbed/common/ConcaveMesh.h b/testbed/common/ConcaveMesh.h
index e4b33308..afda3d45 100644
--- a/testbed/common/ConcaveMesh.h
+++ b/testbed/common/ConcaveMesh.h
@@ -90,9 +90,6 @@ class ConcaveMesh : public PhysicsObject {
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
- /// Set the position of the box
- void resetTransform(const rp3d::Transform& transform);
-
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/Cone.cpp b/testbed/common/Cone.cpp
index 96f52c54..bbd32ed9 100644
--- a/testbed/common/Cone.cpp
+++ b/testbed/common/Cone.cpp
@@ -37,13 +37,7 @@ int Cone::totalNbCones = 0;
Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
- : mRadius(radius), mHeight(height) {
-
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
+ : PhysicsObject(meshFolderPath + "cone.obj"), mRadius(radius), mHeight(height) {
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
@@ -85,13 +79,7 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath)
- : mRadius(radius), mHeight(height) {
-
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
+ : PhysicsObject(meshFolderPath + "cone.obj"), mRadius(radius), mHeight(height) {
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
@@ -269,24 +257,6 @@ void Cone::createVBOAndVAO() {
mVAO.unbind();
}
-// Reset the transform
-void Cone::resetTransform(const rp3d::Transform& transform) {
-
- // Reset the transform
- mBody->setTransform(transform);
-
- mBody->setIsSleeping(false);
-
- // Reset the velocity of the rigid body
- rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
- if (rigidBody != nullptr) {
- rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0));
- rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0));
- }
-
- updateTransform(1.0f);
-}
-
// Set the scaling of the object
void Cone::setScaling(const openglframework::Vector3& scaling) {
diff --git a/testbed/common/Cone.h b/testbed/common/Cone.h
index 6fc35182..8f71493c 100644
--- a/testbed/common/Cone.h
+++ b/testbed/common/Cone.h
@@ -96,9 +96,6 @@ class Cone : public PhysicsObject {
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
- /// Set the position of the box
- void resetTransform(const rp3d::Transform& transform);
-
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp
index c107724f..7842c529 100644
--- a/testbed/common/ConvexMesh.cpp
+++ b/testbed/common/ConvexMesh.cpp
@@ -30,16 +30,10 @@
ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world,
const std::string& meshPath)
- : mVBOVertices(GL_ARRAY_BUFFER),
+ : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
-
// Initialize the position where the sphere will be rendered
translateWorld(position);
@@ -81,16 +75,10 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshPath)
- : mVBOVertices(GL_ARRAY_BUFFER),
+ : PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
-
// Initialize the position where the sphere will be rendered
translateWorld(position);
@@ -266,24 +254,6 @@ void ConvexMesh::createVBOAndVAO() {
mVAO.unbind();
}
-// Reset the transform
-void ConvexMesh::resetTransform(const rp3d::Transform& transform) {
-
- // Reset the transform
- mBody->setTransform(transform);
-
- mBody->setIsSleeping(false);
-
- // Reset the velocity of the rigid body
- rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
- if (rigidBody != nullptr) {
- rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0));
- rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0));
- }
-
- updateTransform(1.0f);
-}
-
// Set the scaling of the object
void ConvexMesh::setScaling(const openglframework::Vector3& scaling) {
diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h
index 19b39220..b6cb90c1 100644
--- a/testbed/common/ConvexMesh.h
+++ b/testbed/common/ConvexMesh.h
@@ -89,9 +89,6 @@ class ConvexMesh : public PhysicsObject {
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
- /// Set the position of the box
- void resetTransform(const rp3d::Transform& transform);
-
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/Cylinder.cpp b/testbed/common/Cylinder.cpp
index 9b520d5c..2943c3e8 100644
--- a/testbed/common/Cylinder.cpp
+++ b/testbed/common/Cylinder.cpp
@@ -37,13 +37,7 @@ int Cylinder::totalNbCylinders = 0;
Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
- : mRadius(radius), mHeight(height) {
-
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
+ : PhysicsObject(meshFolderPath + "cylinder.obj"), mRadius(radius), mHeight(height) {
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
@@ -85,13 +79,7 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p
Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position,
float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath)
- : mRadius(radius), mHeight(height) {
-
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
+ : PhysicsObject(meshFolderPath + "cylinder.obj"), mRadius(radius), mHeight(height) {
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
@@ -270,24 +258,6 @@ void Cylinder::createVBOAndVAO() {
mVAO.unbind();
}
-// Reset the transform
-void Cylinder::resetTransform(const rp3d::Transform& transform) {
-
- // Reset the transform
- mBody->setTransform(transform);
-
- mBody->setIsSleeping(false);
-
- // Reset the velocity of the rigid body
- rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
- if (rigidBody != nullptr) {
- rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0));
- rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0));
- }
-
- updateTransform(1.0f);
-}
-
// Set the scaling of the object
void Cylinder::setScaling(const openglframework::Vector3& scaling) {
diff --git a/testbed/common/Cylinder.h b/testbed/common/Cylinder.h
index 4aab2dc9..3a94b443 100644
--- a/testbed/common/Cylinder.h
+++ b/testbed/common/Cylinder.h
@@ -96,9 +96,6 @@ class Cylinder : public PhysicsObject {
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
- /// Set the position of the box
- void resetTransform(const rp3d::Transform& transform);
-
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/Dumbbell.cpp b/testbed/common/Dumbbell.cpp
index 770f8ec1..03afd59b 100644
--- a/testbed/common/Dumbbell.cpp
+++ b/testbed/common/Dumbbell.cpp
@@ -35,13 +35,8 @@ int Dumbbell::totalNbDumbbells = 0;
// Constructor
Dumbbell::Dumbbell(const openglframework::Vector3 &position,
- reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath) {
-
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
+ reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath)
+ : PhysicsObject(meshFolderPath + "dumbbell.obj") {
// Identity scaling matrix
mScalingMatrix.setToIdentity();
@@ -105,13 +100,8 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
// Constructor
Dumbbell::Dumbbell(const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath) {
-
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
+ reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath)
+ : PhysicsObject(meshFolderPath + "dumbbell.obj"){
// Identity scaling matrix
mScalingMatrix.setToIdentity();
@@ -309,24 +299,6 @@ void Dumbbell::createVBOAndVAO() {
mVAO.unbind();
}
-// Reset the transform
-void Dumbbell::resetTransform(const rp3d::Transform& transform) {
-
- // Reset the transform
- mBody->setTransform(transform);
-
- mBody->setIsSleeping(false);
-
- // Reset the velocity of the rigid body
- rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
- if (rigidBody != NULL) {
- rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0));
- rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0));
- }
-
- updateTransform(1.0f);
-}
-
// Set the scaling of the object
void Dumbbell::setScaling(const openglframework::Vector3& scaling) {
diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h
index 5ce3ec95..81020935 100644
--- a/testbed/common/Dumbbell.h
+++ b/testbed/common/Dumbbell.h
@@ -97,9 +97,6 @@ class Dumbbell : public PhysicsObject {
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
- /// Set the position of the box
- void resetTransform(const rp3d::Transform& transform);
-
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/HeightField.cpp b/testbed/common/HeightField.cpp
index 55da4e1d..377cab07 100644
--- a/testbed/common/HeightField.cpp
+++ b/testbed/common/HeightField.cpp
@@ -328,24 +328,6 @@ void HeightField::createVBOAndVAO() {
mVAO.unbind();
}
-// Reset the transform
-void HeightField::resetTransform(const rp3d::Transform& transform) {
-
- // Reset the transform
- mBody->setTransform(transform);
-
- mBody->setIsSleeping(false);
-
- // Reset the velocity of the rigid body
- rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
- if (rigidBody != NULL) {
- rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0));
- rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0));
- }
-
- updateTransform(1.0f);
-}
-
// Set the scaling of the object
void HeightField::setScaling(const openglframework::Vector3& scaling) {
diff --git a/testbed/common/HeightField.h b/testbed/common/HeightField.h
index d9851992..acb39163 100644
--- a/testbed/common/HeightField.h
+++ b/testbed/common/HeightField.h
@@ -103,9 +103,6 @@ class HeightField : public PhysicsObject {
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
- /// Set the position of the box
- void resetTransform(const rp3d::Transform& transform);
-
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/PhysicsObject.cpp b/testbed/common/PhysicsObject.cpp
index c69f9485..acba8e67 100644
--- a/testbed/common/PhysicsObject.cpp
+++ b/testbed/common/PhysicsObject.cpp
@@ -33,6 +33,16 @@ PhysicsObject::PhysicsObject() : openglframework::Mesh() {
mSleepingColor = openglframework::Color(1, 0, 0, 1);
}
+/// Constructor
+PhysicsObject::PhysicsObject(const std::string& meshPath) : PhysicsObject() {
+
+ // Load the mesh from a file
+ openglframework::MeshReaderWriter::loadMeshFromFile(meshPath, *this);
+
+ // Calculate the normals of the mesh
+ calculateNormals();
+}
+
// Compute the new transform matrix
openglframework::Matrix4 PhysicsObject::computeTransform(float interpolationFactor,
const openglframework::Matrix4& scalingMatrix) {
@@ -57,3 +67,21 @@ openglframework::Matrix4 PhysicsObject::computeTransform(float interpolationFact
// Apply the scaling matrix to have the correct box dimensions
return newMatrix * scalingMatrix;
}
+
+// Reset the transform
+void PhysicsObject::resetTransform(const rp3d::Transform& transform) {
+
+ // Reset the transform
+ mBody->setTransform(transform);
+
+ mBody->setIsSleeping(false);
+
+ // Reset the velocity of the rigid body
+ rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
+ if (rigidBody != nullptr) {
+ rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0));
+ rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0));
+ }
+
+ updateTransform(1.0f);
+}
diff --git a/testbed/common/PhysicsObject.h b/testbed/common/PhysicsObject.h
index bc470091..15f5cf80 100644
--- a/testbed/common/PhysicsObject.h
+++ b/testbed/common/PhysicsObject.h
@@ -56,6 +56,9 @@ class PhysicsObject : public openglframework::Mesh {
/// Constructor
PhysicsObject();
+ /// Constructor
+ PhysicsObject(const std::string& meshPath);
+
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor)=0;
@@ -65,6 +68,9 @@ class PhysicsObject : public openglframework::Mesh {
/// Set the sleeping color of the box
void setSleepingColor(const openglframework::Color& color);
+ /// Set the position of the box
+ void resetTransform(const rp3d::Transform& transform);
+
/// Return a pointer to the collision body of the box
reactphysics3d::CollisionBody* getCollisionBody();
diff --git a/testbed/common/Sphere.cpp b/testbed/common/Sphere.cpp
index 93c48af2..9eaf62c2 100644
--- a/testbed/common/Sphere.cpp
+++ b/testbed/common/Sphere.cpp
@@ -37,13 +37,7 @@ int Sphere::totalNbSpheres = 0;
Sphere::Sphere(float radius, const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
- : mRadius(radius) {
-
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
+ : PhysicsObject(meshFolderPath + "sphere.obj"), mRadius(radius) {
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
@@ -86,13 +80,7 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
Sphere::Sphere(float radius, const openglframework::Vector3 &position,
float mass, reactphysics3d::DynamicsWorld* world,
const std::string& meshFolderPath)
- : mRadius(radius) {
-
- // Load the mesh from a file
- openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this);
-
- // Calculate the normals of the mesh
- calculateNormals();
+ : PhysicsObject(meshFolderPath + "sphere.obj"), mRadius(radius) {
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
@@ -270,24 +258,6 @@ void Sphere::createVBOAndVAO() {
mVAO.unbind();
}
-// Reset the transform
-void Sphere::resetTransform(const rp3d::Transform& transform) {
-
- // Reset the transform
- mBody->setTransform(transform);
-
- mBody->setIsSleeping(false);
-
- // Reset the velocity of the rigid body
- rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
- if (rigidBody != NULL) {
- rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0));
- rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0));
- }
-
- updateTransform(1.0f);
-}
-
// Set the scaling of the object
void Sphere::setScaling(const openglframework::Vector3& scaling) {
diff --git a/testbed/common/Sphere.h b/testbed/common/Sphere.h
index d941d984..62f1809e 100644
--- a/testbed/common/Sphere.h
+++ b/testbed/common/Sphere.h
@@ -93,9 +93,6 @@ class Sphere : public PhysicsObject {
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
- /// Set the position of the box
- void resetTransform(const rp3d::Transform& transform);
-
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index d77cee7f..06b426fe 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -27,7 +27,6 @@
#define COLLISION_DETECTION_SCENE_H
// Libraries
-#define _USE_MATH_DEFINES
#include <cmath>
#include "openglframework.h"
#include "reactphysics3d.h"
From adeac945061b4b737dc08c3609982b2b554709da Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 28 Jan 2017 15:23:05 +0100
Subject: [PATCH 033/133] Working on collision detection scene
---
testbed/common/PhysicsObject.cpp | 2 +-
testbed/common/PhysicsObject.h | 12 ++-
.../CollisionDetectionScene.cpp | 78 ++++++++++++++++---
.../CollisionDetectionScene.h | 3 +-
.../collisionshapes/CollisionShapesScene.cpp | 14 ++--
.../scenes/concavemesh/ConcaveMeshScene.cpp | 4 +-
.../scenes/heightfield/HeightFieldScene.cpp | 4 +-
testbed/scenes/joints/JointsScene.cpp | 12 +--
testbed/src/TestbedApplication.h | 16 ----
9 files changed, 96 insertions(+), 49 deletions(-)
diff --git a/testbed/common/PhysicsObject.cpp b/testbed/common/PhysicsObject.cpp
index acba8e67..9955725c 100644
--- a/testbed/common/PhysicsObject.cpp
+++ b/testbed/common/PhysicsObject.cpp
@@ -69,7 +69,7 @@ openglframework::Matrix4 PhysicsObject::computeTransform(float interpolationFact
}
// Reset the transform
-void PhysicsObject::resetTransform(const rp3d::Transform& transform) {
+void PhysicsObject::setTransform(const rp3d::Transform& transform) {
// Reset the transform
mBody->setTransform(transform);
diff --git a/testbed/common/PhysicsObject.h b/testbed/common/PhysicsObject.h
index 15f5cf80..0da224c3 100644
--- a/testbed/common/PhysicsObject.h
+++ b/testbed/common/PhysicsObject.h
@@ -68,8 +68,11 @@ class PhysicsObject : public openglframework::Mesh {
/// Set the sleeping color of the box
void setSleepingColor(const openglframework::Color& color);
- /// Set the position of the box
- void resetTransform(const rp3d::Transform& transform);
+ /// Get the transform
+ const rp3d::Transform& getTransform() const;
+
+ /// Set the transform
+ void setTransform(const rp3d::Transform& transform);
/// Return a pointer to the collision body of the box
reactphysics3d::CollisionBody* getCollisionBody();
@@ -91,6 +94,11 @@ inline void PhysicsObject::setSleepingColor(const openglframework::Color& color)
mSleepingColor = color;
}
+// Get the transform
+inline const rp3d::Transform& PhysicsObject::getTransform() const {
+ return mBody->getTransform();
+}
+
// Return a pointer to the collision body of the box
inline rp3d::CollisionBody* PhysicsObject::getCollisionBody() {
return mBody;
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index 09544540..8aae0145 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -54,8 +54,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Create a sphere and a corresponding collision body in the dynamics world
mSphere1 = new Sphere(6, position1, mCollisionWorld, mMeshFolderPath);
- mAllShapesObjects.push_back(mSphere1);
- mAllShapesPhysicsObjects.push_back(mSphere1);
+ mAllShapes.push_back(mSphere1);
// Set the color
mSphere1->setColor(mGreyColorDemo);
@@ -66,8 +65,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Create a sphere and a corresponding collision body in the dynamics world
mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath);
- mAllShapesObjects.push_back(mSphere2);
- mAllShapesPhysicsObjects.push_back(mSphere2);
+ mAllShapes.push_back(mSphere2);
// Set the color
mSphere2->setColor(mGreyColorDemo);
@@ -139,7 +137,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Create the VBO and VAO to render the lines
createVBOAndVAO(mPhongShader);
- mAllShapesPhysicsObjects[mSelectedShapeIndex]->setColor(mBlueColorDemo);
+ mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo);
}
// Reset the scene
@@ -332,12 +330,12 @@ void CollisionDetectionScene::selectNextShape() {
int previousIndex = mSelectedShapeIndex;
mSelectedShapeIndex++;
- if (mSelectedShapeIndex >= mAllShapesPhysicsObjects.size()) {
+ if (mSelectedShapeIndex >= mAllShapes.size()) {
mSelectedShapeIndex = 0;
}
- mAllShapesPhysicsObjects[previousIndex]->setColor(mGreyColorDemo);
- mAllShapesPhysicsObjects[mSelectedShapeIndex]->setColor(mBlueColorDemo);
+ mAllShapes[previousIndex]->setColor(mGreyColorDemo);
+ mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo);
}
// Called when a keyboard event occurs
@@ -349,10 +347,68 @@ bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, i
return true;
}
+ float stepDist = 0.5f;
+ float stepAngle = 20 * (3.14f / 180.0f);
+
if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapesPhysicsObjects[mSelectedShapeIndex]->getCollisionBody()->getTransform();
- transform.setPosition(transform.getPosition() + rp3d::Vector3(1, 0, 0));
- mAllShapesObjects[mSelectedShapeIndex]->translateWorld(openglframework::Vector3(1, 0, 0));
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(stepDist, 0, 0));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_LEFT && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(-stepDist, 0, 0));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_UP && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(0, stepDist, 0));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_DOWN && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(0, -stepDist, 0));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_Z && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, stepDist));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_H && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, -stepDist));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_A && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(0, stepAngle, 0) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_D && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(0, -stepAngle, 0) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_W && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(stepAngle, 0, 0) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_S && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(-stepAngle, 0, 0) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_F && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(0, 0, stepAngle) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_G && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(0, 0, -stepAngle) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
}
return false;
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index 06b426fe..b7f57c1d 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -146,8 +146,7 @@ class CollisionDetectionScene : public SceneDemo {
//ConcaveMesh* mConcaveMesh;
//HeightField* mHeightField;
- std::vector<openglframework::Object3D*> mAllShapesObjects;
- std::vector<PhysicsObject*> mAllShapesPhysicsObjects;
+ std::vector<PhysicsObject*> mAllShapes;
int mSelectedShapeIndex;
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
index e146bd6b..8db4908a 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
@@ -524,7 +524,7 @@ void CollisionShapesScene::reset() {
rp3d::Transform transform(initPosition, initOrientation);
// Reset the transform
- mDumbbells[i]->resetTransform(transform);
+ mDumbbells[i]->setTransform(transform);
}
// Create all the boxes of the scene
@@ -542,7 +542,7 @@ void CollisionShapesScene::reset() {
rp3d::Transform transform(initPosition, initOrientation);
// Reset the transform
- mBoxes[i]->resetTransform(transform);
+ mBoxes[i]->setTransform(transform);
}
// Create all the spheres of the scene
@@ -560,7 +560,7 @@ void CollisionShapesScene::reset() {
rp3d::Transform transform(initPosition, initOrientation);
// Reset the transform
- mSpheres[i]->resetTransform(transform);
+ mSpheres[i]->setTransform(transform);
}
// Create all the cones of the scene
@@ -578,7 +578,7 @@ void CollisionShapesScene::reset() {
rp3d::Transform transform(initPosition, initOrientation);
// Reset the transform
- mCones[i]->resetTransform(transform);
+ mCones[i]->setTransform(transform);
}
// Create all the cylinders of the scene
@@ -596,7 +596,7 @@ void CollisionShapesScene::reset() {
rp3d::Transform transform(initPosition, initOrientation);
// Reset the transform
- mCylinders[i]->resetTransform(transform);
+ mCylinders[i]->setTransform(transform);
}
// Create all the capsules of the scene
@@ -614,7 +614,7 @@ void CollisionShapesScene::reset() {
rp3d::Transform transform(initPosition, initOrientation);
// Reset the transform
- mCapsules[i]->resetTransform(transform);
+ mCapsules[i]->setTransform(transform);
}
// Create all the convex meshes of the scene
@@ -632,6 +632,6 @@ void CollisionShapesScene::reset() {
rp3d::Transform transform(initPosition, initOrientation);
// Reset the transform
- mConvexMeshes[i]->resetTransform(transform);
+ mConvexMeshes[i]->setTransform(transform);
}
}
diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
index 9b65893e..11474819 100644
--- a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
+++ b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
@@ -175,7 +175,7 @@ void ConcaveMeshScene::reset() {
// Reset the transform
rp3d::Transform transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity());
- mConcaveMesh->resetTransform(transform);
+ mConcaveMesh->setTransform(transform);
for (int i=0; i<NB_BOXES_X; i++) {
for (int j=0; j<NB_BOXES_Z; j++) {
@@ -184,7 +184,7 @@ void ConcaveMeshScene::reset() {
rp3d::Vector3 boxPosition(-NB_BOXES_X * BOX_SIZE * BOXES_SPACE / 2 + i * BOX_SIZE * BOXES_SPACE, 30, -NB_BOXES_Z * BOX_SIZE * BOXES_SPACE / 2 + j * BOX_SIZE * BOXES_SPACE);
rp3d::Transform boxTransform(boxPosition, rp3d::Quaternion::identity());
- mBoxes[i * NB_BOXES_Z + j]->resetTransform(boxTransform);
+ mBoxes[i * NB_BOXES_Z + j]->setTransform(boxTransform);
}
}
diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp
index 5b62f614..3e4061c9 100644
--- a/testbed/scenes/heightfield/HeightFieldScene.cpp
+++ b/testbed/scenes/heightfield/HeightFieldScene.cpp
@@ -171,13 +171,13 @@ void HeightFieldScene::reset() {
// Reset the transform
rp3d::Transform transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity());
- mHeightField->resetTransform(transform);
+ mHeightField->setTransform(transform);
float heightFieldWidth = 10.0f;
float stepDist = heightFieldWidth / (NB_BOXES + 1);
for (int i=0; i<NB_BOXES; i++) {
rp3d::Vector3 boxPosition(-heightFieldWidth * 0.5f + i * stepDist , 14 + 6.0f * i, -heightFieldWidth * 0.5f + i * stepDist);
rp3d::Transform boxTransform(boxPosition, rp3d::Quaternion::identity());
- mBoxes[i]->resetTransform(boxTransform);
+ mBoxes[i]->setTransform(boxTransform);
}
}
diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp
index 34bb1503..23bcdb93 100644
--- a/testbed/scenes/joints/JointsScene.cpp
+++ b/testbed/scenes/joints/JointsScene.cpp
@@ -193,7 +193,7 @@ void JointsScene::reset() {
rp3d::Transform transform(initPosition, initOrientation);
// Create a box and a corresponding rigid in the dynamics world
- mBallAndSocketJointChainBoxes[i]->resetTransform(transform);
+ mBallAndSocketJointChainBoxes[i]->setTransform(transform);
positionBox.y -= boxDimension.y + 0.5f;
}
@@ -207,7 +207,7 @@ void JointsScene::reset() {
rp3d::Transform transformBottomBox(initPosition, initOrientation);
// Create a box and a corresponding rigid in the dynamics world
- mSliderJointBottomBox->resetTransform(transformBottomBox);
+ mSliderJointBottomBox->setTransform(transformBottomBox);
// Position of the box
openglframework::Vector3 positionBox2(0, 4.2f, 0);
@@ -216,7 +216,7 @@ void JointsScene::reset() {
rp3d::Transform transformTopBox(initPosition, initOrientation);
// Create a box and a corresponding rigid in the dynamics world
- mSliderJointTopBox->resetTransform(transformTopBox);
+ mSliderJointTopBox->setTransform(transformTopBox);
// --------------- Propeller Hinge joint --------------- //
@@ -227,7 +227,7 @@ void JointsScene::reset() {
rp3d::Transform transformHingeBox(initPosition, initOrientation);
// Create a box and a corresponding rigid in the dynamics world
- mPropellerBox->resetTransform(transformHingeBox);
+ mPropellerBox->setTransform(transformHingeBox);
// --------------- Fixed joint --------------- //
@@ -238,7 +238,7 @@ void JointsScene::reset() {
rp3d::Transform transformFixedBox1(initPosition, initOrientation);
// Create a box and a corresponding rigid in the dynamics world
- mFixedJointBox1->resetTransform(transformFixedBox1);
+ mFixedJointBox1->setTransform(transformFixedBox1);
// Position of the box
positionBox2 = openglframework::Vector3(-5, 7, 0);
@@ -247,7 +247,7 @@ void JointsScene::reset() {
rp3d::Transform transformFixedBox2(initPosition, initOrientation);
// Create a box and a corresponding rigid in the dynamics world
- mFixedJointBox2->resetTransform(transformFixedBox2);
+ mFixedJointBox2->setTransform(transformFixedBox2);
}
// Create the boxes and joints for the Ball-and-Socket joint example
diff --git a/testbed/src/TestbedApplication.h b/testbed/src/TestbedApplication.h
index 93d85ea7..8790a1f7 100644
--- a/testbed/src/TestbedApplication.h
+++ b/testbed/src/TestbedApplication.h
@@ -162,12 +162,6 @@ class TestbedApplication : public Screen {
/// Set the variable to know if we need to take a single physics step
void toggleTakeSinglePhysicsStep();
- /// Enable/Disable shadow mapping
- void enableShadows(bool enable);
-
- /// Display/Hide contact points
- void displayContactPoints(bool display);
-
public :
// -------------------- Methods -------------------- //
@@ -252,16 +246,6 @@ inline void TestbedApplication::toggleTakeSinglePhysicsStep() {
}
}
-// Enable/Disable shadow mapping
-inline void TestbedApplication::enableShadows(bool enable) {
- mIsShadowMappingEnabled = enable;
-}
-
-/// Display/Hide contact points
-inline void TestbedApplication::displayContactPoints(bool display) {
- mIsContactPointsDisplayed = display;
-}
-
// Enable/Disable Vertical synchronization
inline void TestbedApplication::enableVSync(bool enable) {
mIsVSyncEnabled = enable;
From e9f2f94f6455ef366cd5ab33b457d007cd19a580 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 2 Feb 2017 22:58:40 +0100
Subject: [PATCH 034/133] Continue working on SAT, remove Cone and Cylinder
shapes
---
CMakeLists.txt | 22 +-
src/collision/CollisionDetection.cpp | 35 +-
src/collision/CollisionDetection.h | 26 +-
src/collision/HalfEdgeStructure.cpp | 116 ++++
src/collision/HalfEdgeStructure.h | 142 +++++
src/collision/PolyhedronMesh.cpp | 62 +++
src/collision/PolyhedronMesh.h | 86 +++
src/collision/TriangleMesh.h | 2 +-
...tore.cpp => CapsuleVsCapsuleAlgorithm.cpp} | 12 +-
.../narrowphase/CapsuleVsCapsuleAlgorithm.h | 71 +++
.../narrowphase/DefaultCollisionDispatch.cpp | 20 +-
.../narrowphase/DefaultCollisionDispatch.h | 4 +
.../narrowphase/EPA/EPAAlgorithm.cpp | 436 ---------------
src/collision/narrowphase/EPA/EPAAlgorithm.h | 147 -----
src/collision/narrowphase/EPA/EdgeEPA.cpp | 125 -----
src/collision/narrowphase/EPA/TriangleEPA.cpp | 145 -----
src/collision/narrowphase/EPA/TriangleEPA.h | 197 -------
.../narrowphase/EPA/TrianglesStore.h | 139 -----
.../narrowphase/GJK/GJKAlgorithm.cpp | 119 +---
src/collision/narrowphase/GJK/GJKAlgorithm.h | 26 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 43 ++
.../{EPA/EdgeEPA.h => SAT/SATAlgorithm.h} | 89 +--
.../SphereVsConvexMeshAlgorithm.cpp | 62 +++
.../narrowphase/SphereVsConvexMeshAlgorithm.h | 71 +++
src/collision/shapes/CollisionShape.h | 3 +-
src/collision/shapes/ConeShape.cpp | 209 -------
src/collision/shapes/ConeShape.h | 194 -------
src/collision/shapes/CylinderShape.cpp | 231 --------
src/collision/shapes/CylinderShape.h | 190 -------
src/engine/OverlappingPair.cpp | 4 +-
src/mathematics/Quaternion.h | 2 +-
src/reactphysics3d.h | 2 -
test/tests/collision/TestCollisionWorld.h | 39 --
test/tests/collision/TestPointInside.h | 215 +-------
test/tests/collision/TestRaycast.h | 508 +-----------------
testbed/CMakeLists.txt | 4 -
testbed/common/Cone.cpp | 272 ----------
testbed/common/Cone.h | 110 ----
testbed/common/Cylinder.cpp | 272 ----------
testbed/common/Cylinder.h | 111 ----
testbed/common/Dumbbell.cpp | 22 +-
testbed/common/Dumbbell.h | 6 +-
.../CollisionDetectionScene.h | 2 -
.../collisionshapes/CollisionShapesScene.cpp | 136 -----
.../collisionshapes/CollisionShapesScene.h | 8 -
testbed/scenes/raycast/RaycastScene.cpp | 50 +-
testbed/scenes/raycast/RaycastScene.h | 4 -
47 files changed, 792 insertions(+), 3999 deletions(-)
create mode 100644 src/collision/HalfEdgeStructure.cpp
create mode 100644 src/collision/HalfEdgeStructure.h
create mode 100644 src/collision/PolyhedronMesh.cpp
create mode 100644 src/collision/PolyhedronMesh.h
rename src/collision/narrowphase/{EPA/TrianglesStore.cpp => CapsuleVsCapsuleAlgorithm.cpp} (87%)
create mode 100644 src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
delete mode 100644 src/collision/narrowphase/EPA/EPAAlgorithm.cpp
delete mode 100644 src/collision/narrowphase/EPA/EPAAlgorithm.h
delete mode 100644 src/collision/narrowphase/EPA/EdgeEPA.cpp
delete mode 100644 src/collision/narrowphase/EPA/TriangleEPA.cpp
delete mode 100644 src/collision/narrowphase/EPA/TriangleEPA.h
delete mode 100644 src/collision/narrowphase/EPA/TrianglesStore.h
create mode 100644 src/collision/narrowphase/SAT/SATAlgorithm.cpp
rename src/collision/narrowphase/{EPA/EdgeEPA.h => SAT/SATAlgorithm.h} (51%)
create mode 100644 src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp
create mode 100644 src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h
delete mode 100644 src/collision/shapes/ConeShape.cpp
delete mode 100644 src/collision/shapes/ConeShape.h
delete mode 100644 src/collision/shapes/CylinderShape.cpp
delete mode 100644 src/collision/shapes/CylinderShape.h
delete mode 100644 testbed/common/Cone.cpp
delete mode 100644 testbed/common/Cone.h
delete mode 100644 testbed/common/Cylinder.cpp
delete mode 100644 testbed/common/Cylinder.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b3d37ed1..9b6af8c9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -67,23 +67,21 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/narrowphase/CollisionDispatch.h"
"src/collision/narrowphase/DefaultCollisionDispatch.h"
"src/collision/narrowphase/DefaultCollisionDispatch.cpp"
- "src/collision/narrowphase/EPA/EdgeEPA.h"
- "src/collision/narrowphase/EPA/EdgeEPA.cpp"
- "src/collision/narrowphase/EPA/EPAAlgorithm.h"
- "src/collision/narrowphase/EPA/EPAAlgorithm.cpp"
- "src/collision/narrowphase/EPA/TriangleEPA.h"
- "src/collision/narrowphase/EPA/TriangleEPA.cpp"
- "src/collision/narrowphase/EPA/TrianglesStore.h"
- "src/collision/narrowphase/EPA/TrianglesStore.cpp"
"src/collision/narrowphase/GJK/VoronoiSimplex.h"
"src/collision/narrowphase/GJK/VoronoiSimplex.cpp"
"src/collision/narrowphase/GJK/GJKAlgorithm.h"
"src/collision/narrowphase/GJK/GJKAlgorithm.cpp"
+ "src/collision/narrowphase/SAT/SATAlgorithm.h"
+ "src/collision/narrowphase/SAT/SATAlgorithm.cpp"
"src/collision/narrowphase/NarrowPhaseAlgorithm.h"
"src/collision/narrowphase/SphereVsSphereAlgorithm.h"
"src/collision/narrowphase/SphereVsSphereAlgorithm.cpp"
+ "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h"
+ "src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp"
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
+ "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h"
+ "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp"
"src/collision/shapes/AABB.h"
"src/collision/shapes/AABB.cpp"
"src/collision/shapes/ConvexShape.h"
@@ -96,12 +94,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/shapes/CapsuleShape.cpp"
"src/collision/shapes/CollisionShape.h"
"src/collision/shapes/CollisionShape.cpp"
- "src/collision/shapes/ConeShape.h"
- "src/collision/shapes/ConeShape.cpp"
"src/collision/shapes/ConvexMeshShape.h"
"src/collision/shapes/ConvexMeshShape.cpp"
- "src/collision/shapes/CylinderShape.h"
- "src/collision/shapes/CylinderShape.cpp"
"src/collision/shapes/SphereShape.h"
"src/collision/shapes/SphereShape.cpp"
"src/collision/shapes/TriangleShape.h"
@@ -118,6 +112,10 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/TriangleVertexArray.cpp"
"src/collision/TriangleMesh.h"
"src/collision/TriangleMesh.cpp"
+ "src/collision/PolyhedronMesh.h"
+ "src/collision/PolyhedronMesh.cpp"
+ "src/collision/HalfEdgeStructure.h"
+ "src/collision/HalfEdgeStructure.cpp"
"src/collision/CollisionDetection.h"
"src/collision/CollisionDetection.cpp"
"src/collision/NarrowPhaseInfo.h"
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index be17e4f7..161f6328 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -300,9 +300,7 @@ void CollisionDetection::computeNarrowPhase() {
// Select the narrow phase algorithm to use according to the two collision shapes
const CollisionShapeType shape1Type = currentNarrowPhaseInfo->collisionShape1->getType();
const CollisionShapeType shape2Type = currentNarrowPhaseInfo->collisionShape2->getType();
- const int shape1Index = static_cast<int>(shape1Type);
- const int shape2Index = static_cast<int>(shape2Type);
- NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
// If there is no collision algorithm between those two kinds of shapes, skip it
if (narrowPhaseAlgorithm != nullptr) {
@@ -462,6 +460,17 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return;
+ // Make sure the shape with the smallest collision shape type comes first
+ const uint shape1TypeIndex = static_cast<const uint>(shape1->getCollisionShape()->getType());
+ const uint shape2TypeIndex = static_cast<const uint>(shape2->getCollisionShape()->getType());
+ if (shape2TypeIndex > shape1TypeIndex) {
+
+ // Swap the two shapes
+ ProxyShape* temp = shape1;
+ shape1 = shape2;
+ shape2 = temp;
+ }
+
// Compute the overlapping pair ID
overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
@@ -681,9 +690,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
if (!isColliding) {
// Select the narrow phase algorithm to use according to the two collision shapes
- const int shape1Index = static_cast<int>(shape1Type);
- const int shape2Index = static_cast<int>(shape2Type);
- NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
// If there is a collision algorithm for those two kinds of shapes
if (narrowPhaseAlgorithm != nullptr) {
@@ -771,9 +778,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
if (!isColliding) {
// Select the narrow phase algorithm to use according to the two collision shapes
- const int shape1Index = static_cast<int>(shape1Type);
- const int shape2Index = static_cast<int>(shape2Type);
- NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
// If there is a collision algorithm for those two kinds of shapes
if (narrowPhaseAlgorithm != nullptr) {
@@ -846,9 +851,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
while (narrowPhaseInfo != nullptr) {
// Select the narrow phase algorithm to use according to the two collision shapes
- const int shape1Index = static_cast<int>(shape1Type);
- const int shape2Index = static_cast<int>(shape2Type);
- NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
// If there is a collision algorithm for those two kinds of shapes
if (narrowPhaseAlgorithm != nullptr) {
@@ -926,9 +929,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
while (narrowPhaseInfo != nullptr) {
// Select the narrow phase algorithm to use according to the two collision shapes
- const int shape1Index = static_cast<int>(shape1Type);
- const int shape2Index = static_cast<int>(shape2Type);
- NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
// If there is a collision algorithm for those two kinds of shapes
if (narrowPhaseAlgorithm != nullptr) {
@@ -999,9 +1000,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
while (narrowPhaseInfo != nullptr) {
// Select the narrow phase algorithm to use according to the two collision shapes
- const int shape1Index = static_cast<int>(shape1Type);
- const int shape2Index = static_cast<int>(shape2Type);
- NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
// If there is a collision algorithm for those two kinds of shapes
if (narrowPhaseAlgorithm != nullptr) {
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 3bb27366..c06a6357 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -124,6 +124,10 @@ class CollisionDetection {
/// Fill-in the collision detection matrix
void fillInCollisionMatrix();
+ /// Return the corresponding narrow-phase algorithm
+ NarrowPhaseAlgorithm* selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type,
+ const CollisionShapeType& shape2Type) const;
+
/// Add all the contact manifold of colliding pairs to their bodies
void addAllContactManifoldsToBodies();
@@ -153,10 +157,6 @@ class CollisionDetection {
/// Set the collision dispatch configuration
void setCollisionDispatch(CollisionDispatch* collisionDispatch);
- /// Return the Narrow-phase collision detection algorithm to use between two types of shapes
- NarrowPhaseAlgorithm* getCollisionAlgorithm(CollisionShapeType shape1Type,
- CollisionShapeType shape2Type) const;
-
/// Add a proxy collision shape to the collision detection
void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
@@ -231,12 +231,6 @@ class CollisionDetection {
friend class ConvexMeshShape;
};
-// Return the Narrow-phase collision detection algorithm to use between two types of shapes
-inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type,
- CollisionShapeType shape2Type) const {
- return mCollisionMatrix[static_cast<int>(shape1Type)][static_cast<int>(shape2Type)];
-}
-
// Set the collision dispatch configuration
inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
mCollisionDispatch = collisionDispatch;
@@ -280,6 +274,18 @@ inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, con
mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
}
+// Return the corresponding narrow-phase algorithm
+inline NarrowPhaseAlgorithm* CollisionDetection::selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type,
+ const CollisionShapeType& shape2Type) const {
+
+ const unsigned int shape1Index = static_cast<unsigned int>(shape1Type);
+ const unsigned int shape2Index = static_cast<unsigned int>(shape2Type);
+
+ assert(shape1Index <= shape2Index);
+
+ return mCollisionMatrix[shape1Index][shape2Index];
+}
+
// Ray casting method
inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
const Ray& ray,
diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp
new file mode 100644
index 00000000..16a79704
--- /dev/null
+++ b/src/collision/HalfEdgeStructure.cpp
@@ -0,0 +1,116 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "HalfEdgeStructure.h"
+#include <map>
+
+using namespace reactphysics3d;
+
+// Initialize the structure
+void HalfEdgeStructure::init(std::vector<const Vector3> vertices, std::vector<std::vector<uint>> faces) {
+
+ using edgeKey = std::pair<uint, uint>;
+
+ std::map<edgeKey, Edge> edges;
+ std::map<edgeKey, edgeKey> nextEdges;
+ std::map<edgeKey, uint> mapEdgeToStartVertex;
+
+ // For each vertices
+ for (uint v=0; v<vertices.size(); v++) {
+ Vertex vertex(vertices[v]);
+ mVertices.push_back(vertex);
+ }
+
+ // For each face
+ for (uint f=0; f<faces.size(); f++) {
+
+ // Create a new face
+ Face face;
+ mFaces.push_back(face);
+
+ // Vertices of the current face
+ std::vector<uint>& faceVertices = faces[f];
+
+ std::vector<edgeKey> currentFaceEdges;
+
+ edgeKey firstEdgeKey;
+
+ // For each edge of the face
+ for (uint e=0; e < faceVertices.size(); e++) {
+ uint v1Index = faceVertices[e];
+ uint v2Index = faceVertices[e == (faceVertices.size() - 1) ? 0 : e + 1];
+
+ const edgeKey pairV1V2 = std::make_pair(v1Index, v2Index);
+
+ // Create a new half-edge
+ Edge edge;
+ edge.faceIndex = f;
+ edge.vertexIndex = v1Index;
+ if (e == 0) {
+ firstEdgeKey = pairV1V2;
+ }
+ else if (e >= 1) {
+ nextEdges.insert(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2);
+ }
+ if (e == (faceVertices.size() - 1)) {
+ nextEdges.insert(pairV1V2, firstEdgeKey);
+ }
+ edges.insert(pairV1V2, edge);
+
+ const edgeKey pairV2V1 = std::make_pair(v2Index, v1Index);
+
+ mapEdgeToStartVertex.insert(pairV1V2, v1Index);
+ mapEdgeToStartVertex.insert(pairV2V1, v2Index);
+
+ auto itEdge = edges.find(pairV2V1);
+ if (itEdge != edges.end()) {
+
+ const uint edgeIndex = mEdges.size();
+ mEdges.push_back(itEdge->second);
+ mEdges.push_back(edge);
+
+ itEdge->second.twinEdgeIndex = edgeIndex + 1;
+ itEdge->second.nextEdgeIndex = nextEdges[pairV2V1];
+
+ edge.twinEdgeIndex = edgeIndex;
+ edge.nextEdgeIndex = edges[nextEdges[pairV1V2]].;
+
+ mVertices[v1Index].edgeIndex = edgeIndex;
+ mVertices[v2Index].edgeIndex = edgeIndex + 1;
+
+ face.edgeIndex = edgeIndex + 1;
+ }
+
+ currentFaceEdges.push_back(pairV1V2);
+ }
+
+ // For each edge of the face
+ for (uint e=0; e < currentFaceEdges.size(); e++) {
+ Edge& edge = currentFaceEdges[e];
+ edge.nextEdgeIndex =
+ }
+ }
+}
diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h
new file mode 100644
index 00000000..71b549d0
--- /dev/null
+++ b/src/collision/HalfEdgeStructure.h
@@ -0,0 +1,142 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_HALF_EDGE_STRUCTURE_MESH_H
+#define REACTPHYSICS3D_HALF_EDGE_STRUCTURE_MESH_H
+
+// Libraries
+#include "mathematics/mathematics.h"
+#include <vector>
+
+namespace reactphysics3d {
+
+// Class HalfEdgeStructure
+/**
+ * This class describes a polyhedron mesh made of faces and vertices.
+ * The faces do not have to be triangle. Note that the half-edge structure
+ * is only valid if the mesh is closed (each edge has two adjacent faces).
+ */
+class HalfEdgeStructure {
+
+ public:
+
+ struct Edge {
+ uint vertexIndex; // Index of the vertex at the end of the edge
+ uint twinEdgeIndex; // Index of the twin edge
+ uint faceIndex; // Adjacent face index of the edge
+ uint nextEdgeIndex; // Index of the next edge
+ };
+
+ struct Face {
+ uint edgeIndex; // Index of an half-edge of the face
+ };
+
+ struct Vertex {
+ const Vector3 point; // Coordinates of the vertex
+ uint edgeIndex; // Index of one edge emanting from this vertex
+
+ /// Constructor
+ Vertex(const Vector3& p) { point = p;}
+ };
+
+ private:
+
+ /// All the faces
+ std::vector<Face> mFaces;
+
+ /// All the vertices
+ std::vector<Vertex> mVertices;
+
+ /// All the half-edges
+ std::vector<Edge> mEdges;
+
+ public:
+
+ /// Constructor
+ HalfEdgeStructure() = default;
+
+ /// Destructor
+ ~HalfEdgeStructure() = default;
+
+ /// Initialize the structure
+ void init(std::vector<const Vector3> vertices, std::vector<std::vector<uint>> faces);
+
+ /// Return the number of faces
+ uint getNbFaces() const;
+
+ /// Return the number of edges
+ uint getNbEdges() const;
+
+ /// Return the number of vertices
+ uint getNbVertices() const;
+
+ /// Return a given face
+ Face getFace(uint index) const;
+
+ /// Return a given edge
+ Edge getHalfEdge(uint index) const;
+
+ /// Retunr a given vertex
+ Vertex getVertex(uint index) const;
+
+};
+
+// Return the number of faces
+inline uint HalfEdgeStructure::getNbFaces() const {
+ return mFaces.size();
+}
+
+// Return the number of edges
+inline uint HalfEdgeStructure::getNbEdges() const {
+ return mEdges.size();
+}
+
+// Return the number of vertices
+inline uint HalfEdgeStructure::getNbVertices() const {
+ return mVertices.size();
+}
+
+// Return a given face
+inline HalfEdgeStructure::Face HalfEdgeStructure::getFace(uint index) const {
+ assert(index < mFaces.size());
+ return mFaces[index];
+}
+
+// Return a given edge
+inline HalfEdgeStructure::Edge HalfEdgeStructure::getHalfEdge(uint index) const {
+ assert(index < mEdges.size());
+ return mEdges[index];
+}
+
+// Retunr a given vertex
+inline HalfEdgeStructure::Vertex HalfEdgeStructure::getVertex(uint index) const {
+ assert(index < mVertices.size());
+ return mVertices[index];
+}
+
+}
+
+#endif
+
diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp
new file mode 100644
index 00000000..891a2f47
--- /dev/null
+++ b/src/collision/PolyhedronMesh.cpp
@@ -0,0 +1,62 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "PolyhedronMesh.h"
+
+using namespace reactphysics3d;
+
+
+// Constructor
+PolyhedronMesh::PolyhedronMesh() : mIsFinalized(false) {
+
+}
+
+// Add a vertex into the polyhedron.
+// This method returns the index of the vertex that you need to use
+// to add faces.
+uint PolyhedronMesh::addVertex(const Vector3& vertex) {
+ mVertices.push_back(vertex);
+ return mVertices.size() - 1;
+}
+
+// Add a face into the polyhedron.
+// A face is a list of vertices indices (returned by addVertex() method).
+// The order of the indices are important. You need to specify the vertices of
+// of the face sorted counter-clockwise as seen from the outside of the polyhedron.
+void PolyhedronMesh::addFace(std::vector<uint> faceVertices) {
+ mFaces.push_back(faceVertices);
+}
+
+// Call this method when you are done adding vertices and faces
+void PolyhedronMesh::finalize() {
+
+ if (mIsFinalized) return;
+
+ // Initialize the half-edge structure
+ mHalfEdgeStructure.init(mVertices, mFaces);
+
+ mIsFinalized = true;
+}
diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h
new file mode 100644
index 00000000..8f0e8abf
--- /dev/null
+++ b/src/collision/PolyhedronMesh.h
@@ -0,0 +1,86 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_POLYHEDRON_MESH_H
+#define REACTPHYSICS3D_POLYHEDRON_MESH_H
+
+// Libraries
+#include "mathematics/mathematics.h"
+#include "HalfEdgeStructure.h"
+#include <vector>
+
+namespace reactphysics3d {
+
+// Class PolyhedronMesh
+/**
+ * This class describes a polyhedron mesh made of faces and vertices.
+ * The faces do not have to be triangle
+ */
+class PolyhedronMesh {
+
+ private:
+
+ /// Half-edge structure of the mesh
+ HalfEdgeStructure mHalfEdgeStructure;
+
+ /// True if the half-edge structure has been generated
+ bool mIsFinalized;
+
+ /// All the vertices
+ std::vector<const Vector3> mVertices;
+
+ /// All the indexes of the face vertices
+ std::vector<std::vector<uint>> mFaces;
+
+ public:
+
+ /// Constructor
+ PolyhedronMesh();
+
+ /// Destructor
+ ~PolyhedronMesh() = default;
+
+ /// Add a vertex into the polyhedron
+ uint addVertex(const Vector3& vertex);
+
+ /// Add a face into the polyhedron
+ void addFace(std::vector<uint> faceVertices);
+
+ /// Call this method when you are done adding vertices and faces
+ void finalize();
+
+ /// Return the half-edge structure of the mesh
+ const HalfEdgeStructure& getHalfEdgeStructure() const;
+};
+
+// Return the half-edge structure of the mesh
+inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const {
+ return mHalfEdgeStructure;
+}
+
+}
+
+#endif
+
diff --git a/src/collision/TriangleMesh.h b/src/collision/TriangleMesh.h
index 936824e9..e33fad67 100644
--- a/src/collision/TriangleMesh.h
+++ b/src/collision/TriangleMesh.h
@@ -38,7 +38,7 @@ namespace reactphysics3d {
* This class represents a mesh made of triangles. A TriangleMesh contains
* one or several parts. Each part is a set of triangles represented in a
* TriangleVertexArray object describing all the triangles vertices of the part.
- * A TriangleMesh object is used to create a ConcaveMeshShape from a triangle
+ * A TriangleMesh object can be used to create a ConcaveMeshShape from a triangle
* mesh for instance.
*/
class TriangleMesh {
diff --git a/src/collision/narrowphase/EPA/TrianglesStore.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
similarity index 87%
rename from src/collision/narrowphase/EPA/TrianglesStore.cpp
rename to src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
index 93fb95a6..44badf67 100644
--- a/src/collision/narrowphase/EPA/TrianglesStore.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
@@ -24,12 +24,14 @@
********************************************************************************/
// Libraries
-#include "TrianglesStore.h"
+#include "CapsuleVsCapsuleAlgorithm.h"
+#include "collision/shapes/CapsuleShape.h"
-// We use the ReactPhysics3D namespace
+// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-// Constructor
-TrianglesStore::TrianglesStore() : mNbTriangles(0) {
-
+bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) {
+
+
}
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
new file mode 100644
index 00000000..6a48495e
--- /dev/null
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
@@ -0,0 +1,71 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H
+#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H
+
+// Libraries
+#include "body/Body.h"
+#include "constraint/ContactPoint.h"
+#include "NarrowPhaseAlgorithm.h"
+
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class CapsuleVsCapsuleAlgorithm
+/**
+ * This class is used to compute the narrow-phase collision detection
+ * between two capsule collision shapes.
+ */
+class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
+
+ protected :
+
+ public :
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ CapsuleVsCapsuleAlgorithm() = default;
+
+ /// Destructor
+ virtual ~CapsuleVsCapsuleAlgorithm() override = default;
+
+ /// Deleted copy-constructor
+ CapsuleVsCapsuleAlgorithm(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
+
+ /// Deleted assignment operator
+ CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
+
+ /// Compute a contact info if the two bounding volume collide
+ virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) override;
+};
+
+}
+
+#endif
+
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
index 35b1e50b..4864dbbe 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
@@ -33,18 +33,22 @@ using namespace reactphysics3d;
// use between two types of collision shapes.
NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) {
+
CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
// Convex vs Convex algorithm (GJK algorithm)
- if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
- return &mGJKAlgorithm;
- }
- // Sphere vs Sphere algorithm
- else if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) {
- return &mSphereVsSphereAlgorithm;
- }
- else {
+ if (type1 > type2) {
return nullptr;
}
+ // Sphere vs Sphere algorithm
+ if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) {
+ return &mSphereVsSphereAlgorithm;
+ }
+ // Sphere vs Convex shapes (convex Mesh, BoxShape) algorithm
+ if (shape1Type == CollisionShapeType::SPHERE && CollisionShape::isConvex(shape2Type)) {
+ return &mSphereVsConvexMeshAlgorithm;
+ }
+
+ return nullptr;
}
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h
index f62a6564..07a819c0 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.h
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.h
@@ -30,6 +30,7 @@
#include "CollisionDispatch.h"
#include "ConcaveVsConvexAlgorithm.h"
#include "SphereVsSphereAlgorithm.h"
+#include "SphereVsConvexMeshAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
namespace reactphysics3d {
@@ -47,6 +48,9 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Sphere vs Sphere collision algorithm
SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
+ /// Sphere vs Convex Mesh collision algorithm
+ SphereVsConvexMeshAlgorithm mSphereVsConvexMeshAlgorithm;
+
/// GJK Algorithm
GJKAlgorithm mGJKAlgorithm;
diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
deleted file mode 100644
index 6afa7a10..00000000
--- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp
+++ /dev/null
@@ -1,436 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-// Libraries
-#include "EPAAlgorithm.h"
-#include "engine/Profiler.h"
-#include "collision/narrowphase//GJK/GJKAlgorithm.h"
-#include "TrianglesStore.h"
-
-// We want to use the ReactPhysics3D namespace
-using namespace reactphysics3d;
-
-// Decide if the origin is in the tetrahedron.
-/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
-/// the vertex that is wrong if the origin is not in the tetrahedron
-int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
- const Vector3& p3, const Vector3& p4) const {
-
- // Check vertex 1
- Vector3 normal1 = (p2-p1).cross(p3-p1);
- if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
- return 4;
- }
-
- // Check vertex 2
- Vector3 normal2 = (p4-p2).cross(p3-p2);
- if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
- return 1;
- }
-
- // Check vertex 3
- Vector3 normal3 = (p4-p3).cross(p1-p3);
- if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
- return 2;
- }
-
- // Check vertex 4
- Vector3 normal4 = (p2-p4).cross(p1-p4);
- if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) {
- return 3;
- }
-
- // The origin is in the tetrahedron, we return 0
- return 0;
-}
-
-// Compute the penetration depth with the EPA algorithm.
-/// This method computes the penetration depth and contact points between two
-/// enlarged objects (with margin) where the original objects (without margin)
-/// intersect. An initial simplex that contains origin has been computed with
-/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
-/// the correct penetration depth. This method returns true if the EPA penetration
-/// depth computation has succeeded and false it has failed.
-bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
- const NarrowPhaseInfo* narrowPhaseInfo,
- Vector3& v,
- ContactPointInfo& contactPointInfo) {
-
- PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
-
- decimal gjkPenDepthSquare = v.lengthSquare();
-
- assert(narrowPhaseInfo->collisionShape1->isConvex());
- assert(narrowPhaseInfo->collisionShape2->isConvex());
-
- const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
- const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
-
- void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1;
- void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2;
-
- Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
- Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
- Vector3 points[MAX_SUPPORT_POINTS]; // Current points
- TrianglesStore triangleStore; // Store the triangles
- TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face
- // candidate of the EPA algorithm
-
- // Transform a point from local space of body 2 to local
- // space of body 1 (the GJK algorithm is done in local space of body 1)
- Transform body2Tobody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform;
-
- // Matrix that transform a direction from local
- // space of body 1 into local space of body 2
- Quaternion rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getInverse() *
- narrowPhaseInfo->shape1ToWorldTransform.getOrientation();
-
- // Get the simplex computed previously by the GJK algorithm
- int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
-
- // Compute the tolerance
- decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
-
- // Number of triangles in the polytope
- unsigned int nbTriangles = 0;
-
- // Clear the storing of triangles
- triangleStore.clear();
-
- // Select an action according to the number of points in the simplex
- // computed with GJK algorithm in order to obtain an initial polytope for
- // The EPA algorithm.
- switch(nbVertices) {
- case 1:
- // Only one point in the simplex (which should be the origin).
- // We have a touching contact with zero penetration depth.
- // We drop that kind of contact. Therefore, we return false
- return false;
-
- case 2: {
- // The simplex returned by GJK is a line segment d containing the origin.
- // We add two additional support points to construct a hexahedron (two tetrahedron
- // glued together with triangle faces. The idea is to compute three different vectors
- // v1, v2 and v3 that are orthogonal to the segment d. The three vectors are relatively
- // rotated of 120 degree around the d segment. The the three new points to
- // construct the polytope are the three support points in those three directions
- // v1, v2 and v3.
-
- // Direction of the segment
- Vector3 d = (points[1] - points[0]).getUnit();
-
- // Choose the coordinate axis from the minimal absolute component of the vector d
- int minAxis = d.getAbsoluteVector().getMinAxis();
-
- // Compute sin(60)
- const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5);
-
- // Create a rotation quaternion to rotate the vector v1 to get the vectors
- // v2 and v3
- Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5);
-
- // Compute the vector v1, v2, v3
- Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2));
- Vector3 v2 = rotationQuat * v1;
- Vector3 v3 = rotationQuat * v2;
-
- // Compute the support point in the direction of v1
- suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
- suppPointsB[2] = body2Tobody1 *
- shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData);
- points[2] = suppPointsA[2] - suppPointsB[2];
-
- // Compute the support point in the direction of v2
- suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
- suppPointsB[3] = body2Tobody1 *
- shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData);
- points[3] = suppPointsA[3] - suppPointsB[3];
-
- // Compute the support point in the direction of v3
- suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
- suppPointsB[4] = body2Tobody1 *
- shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData);
- points[4] = suppPointsA[4] - suppPointsB[4];
-
- // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
- // tetrahedron that contains the origin in order that the initial polytope of the
- // EPA algorithm is a tetrahedron, which is simpler to deal with.
-
- // If the origin is in the tetrahedron of points 0, 2, 3, 4
- if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) {
- // We use the point 4 instead of point 1 for the initial tetrahedron
- suppPointsA[1] = suppPointsA[4];
- suppPointsB[1] = suppPointsB[4];
- points[1] = points[4];
- }
- // If the origin is in the tetrahedron of points 1, 2, 3, 4
- else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) {
- // We use the point 4 instead of point 0 for the initial tetrahedron
- suppPointsA[0] = suppPointsA[4];
- suppPointsB[0] = suppPointsB[4];
- points[0] = points[4];
- }
- else {
- // The origin is not in the initial polytope
- return false;
- }
-
- // The polytope contains now 4 vertices
- nbVertices = 4;
- }
- case 4: {
- // The simplex computed by the GJK algorithm is a tetrahedron. Here we check
- // if this tetrahedron contains the origin. If it is the case, we keep it and
- // otherwise we remove the wrong vertex of the tetrahedron and go in the case
- // where the GJK algorithm compute a simplex of three vertices.
-
- // Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise)
- int badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]);
-
- // If the origin is in the tetrahedron
- if (badVertex == 0) {
- // The tetrahedron is a correct initial polytope for the EPA algorithm.
- // Therefore, we construct the tetrahedron.
-
- // Comstruct the 4 triangle faces of the tetrahedron
- TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2);
- TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1);
- TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3);
- TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2);
-
- // If the constructed tetrahedron is not correct
- if (!((face0 != nullptr) && (face1 != nullptr) && (face2 != nullptr) && (face3 != nullptr)
- && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
- && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
- return false;
- }
-
- // Associate the edges of neighbouring triangle faces
- link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
- link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
- link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
- link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
- link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
- link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
-
- // Add the triangle faces in the candidate heap
- addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
- addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
- addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
- addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
-
- break;
- }
-
- // The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron)
- // Remove the wrong vertex and continue to the next case with the
- // three remaining vertices
- if (badVertex < 4) {
-
- suppPointsA[badVertex-1] = suppPointsA[3];
- suppPointsB[badVertex-1] = suppPointsB[3];
- points[badVertex-1] = points[3];
- }
-
- // We have removed the wrong vertex
- nbVertices = 3;
- }
- case 3: {
- // The GJK algorithm returned a triangle that contains the origin.
- // We need two new vertices to create two tetrahedron. The two new
- // vertices are the support points in the "n" and "-n" direction
- // where "n" is the normal of the triangle. Then, we use only the
- // tetrahedron that contains the origin.
-
- // Compute the normal of the triangle
- Vector3 v1 = points[1] - points[0];
- Vector3 v2 = points[2] - points[0];
- Vector3 n = v1.cross(v2);
-
- // Compute the two new vertices to obtain a hexahedron
- suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
- suppPointsB[3] = body2Tobody1 *
- shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData);
- points[3] = suppPointsA[3] - suppPointsB[3];
- suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData);
- suppPointsB[4] = body2Tobody1 *
- shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
- points[4] = suppPointsA[4] - suppPointsB[4];
-
- TriangleEPA* face0 = nullptr;
- TriangleEPA* face1 = nullptr;
- TriangleEPA* face2 = nullptr;
- TriangleEPA* face3 = nullptr;
-
- // If the origin is in the first tetrahedron
- if (isOriginInTetrahedron(points[0], points[1],
- points[2], points[3]) == 0) {
- // The tetrahedron is a correct initial polytope for the EPA algorithm.
- // Therefore, we construct the tetrahedron.
-
- // Comstruct the 4 triangle faces of the tetrahedron
- face0 = triangleStore.newTriangle(points, 0, 1, 2);
- face1 = triangleStore.newTriangle(points, 0, 3, 1);
- face2 = triangleStore.newTriangle(points, 0, 2, 3);
- face3 = triangleStore.newTriangle(points, 1, 3, 2);
- }
- else if (isOriginInTetrahedron(points[0], points[1],
- points[2], points[4]) == 0) {
-
- // The tetrahedron is a correct initial polytope for the EPA algorithm.
- // Therefore, we construct the tetrahedron.
-
- // Comstruct the 4 triangle faces of the tetrahedron
- face0 = triangleStore.newTriangle(points, 0, 1, 2);
- face1 = triangleStore.newTriangle(points, 0, 4, 1);
- face2 = triangleStore.newTriangle(points, 0, 2, 4);
- face3 = triangleStore.newTriangle(points, 1, 4, 2);
- }
- else {
- return false;
- }
-
- // If the constructed tetrahedron is not correct
- if (!((face0 != nullptr) && (face1 != nullptr) && (face2 != nullptr) && (face3 != nullptr)
- && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
- && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
- return false;
- }
-
- // Associate the edges of neighbouring triangle faces
- link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
- link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
- link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
- link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
- link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
- link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
-
- // Add the triangle faces in the candidate heap
- addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
- addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
- addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
- addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
-
- nbVertices = 4;
-
- }
- break;
- }
-
- // At this point, we have a polytope that contains the origin. Therefore, we
- // can run the EPA algorithm.
-
- if (nbTriangles == 0) {
- return false;
- }
-
- TriangleEPA* triangle = 0;
- decimal upperBoundSquarePenDepth = DECIMAL_LARGEST;
-
- do {
- triangle = triangleHeap[0];
-
- // Get the next candidate face (the face closest to the origin)
- std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison);
- nbTriangles--;
-
- // If the candidate face in the heap is not obsolete
- if (!triangle->getIsObsolete()) {
-
- // If we have reached the maximum number of support points
- if (nbVertices == MAX_SUPPORT_POINTS) {
- assert(false);
- break;
- }
-
- // Compute the support point of the Minkowski
- // difference (A-B) in the closest point direction
- suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(
- triangle->getClosestPoint(), shape1CachedCollisionData);
- suppPointsB[nbVertices] = body2Tobody1 *
- shape2->getLocalSupportPointWithMargin(rotateToBody2 *
- (-triangle->getClosestPoint()), shape2CachedCollisionData);
- points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
-
- int indexNewVertex = nbVertices;
- nbVertices++;
-
- // Update the upper bound of the penetration depth
- decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
-
- decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
- if (wDotVSquare < upperBoundSquarePenDepth) {
- upperBoundSquarePenDepth = wDotVSquare;
- }
-
- // Compute the error
- decimal error = wDotv - triangle->getDistSquare();
- if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
- points[indexNewVertex] == points[(*triangle)[0]] ||
- points[indexNewVertex] == points[(*triangle)[1]] ||
- points[indexNewVertex] == points[(*triangle)[2]]) {
- break;
- }
-
- // Now, we compute the silhouette cast by the new vertex. The current triangle
- // face will not be in the convex hull. We start the local recursive silhouette
- // algorithm from the current triangle face.
- int i = triangleStore.getNbTriangles();
- if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
- break;
- }
-
- // Add all the new triangle faces computed with the silhouette algorithm
- // to the candidates list of faces of the current polytope
- while(i != triangleStore.getNbTriangles()) {
- TriangleEPA* newTriangle = &triangleStore[i];
- addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth);
- i++;
- }
- }
- } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
-
- // Compute the contact info
- v = narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * triangle->getClosestPoint();
- Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
- Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
- Vector3 normal = v.getUnit();
- decimal penetrationDepth = v.length();
-
- // If the length of the normal vector is too small, skip this contact point
- if (normal.lengthSquare() < MACHINE_EPSILON) {
- return false;
- }
-
- if (penetrationDepth * penetrationDepth > gjkPenDepthSquare && penetrationDepth > 0) {
-
- // Create the contact info object
- contactPointInfo.init(normal, penetrationDepth, pALocal, pBLocal);
-
- return true;
- }
-
- return false;
-}
diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h
deleted file mode 100644
index 1d241540..00000000
--- a/src/collision/narrowphase/EPA/EPAAlgorithm.h
+++ /dev/null
@@ -1,147 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_EPA_ALGORITHM_H
-#define REACTPHYSICS3D_EPA_ALGORITHM_H
-
-// Libraries
-#include "collision/narrowphase/GJK/VoronoiSimplex.h"
-#include "collision/shapes/CollisionShape.h"
-#include "collision/NarrowPhaseInfo.h"
-#include "constraint/ContactPoint.h"
-#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
-#include "mathematics/mathematics.h"
-#include "TriangleEPA.h"
-#include "memory/PoolAllocator.h"
-#include <algorithm>
-
-/// ReactPhysics3D namespace
-namespace reactphysics3d {
-
-// ---------- Constants ---------- //
-
-/// Maximum number of support points of the polytope
-constexpr unsigned int MAX_SUPPORT_POINTS = 100;
-
-/// Maximum number of facets of the polytope
-constexpr unsigned int MAX_FACETS = 200;
-
-
-// Class TriangleComparison
-/**
- * This class allows the comparison of two triangles in the heap
- * The comparison between two triangles is made using their square distance to the closest
- * point to the origin. The goal is that in the heap, the first triangle is the one with the
- * smallest square distance.
- */
-class TriangleComparison {
-
- public:
-
- /// Comparison operator
- bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) {
- return (face1->getDistSquare() > face2->getDistSquare());
- }
-};
-
-
-// Class EPAAlgorithm
-/**
- * This class is the implementation of the Expanding Polytope Algorithm (EPA).
- * The EPA algorithm computes the penetration depth and contact points between
- * two enlarged objects (with margin) where the original objects (without margin)
- * intersect. The penetration depth of a pair of intersecting objects A and B is
- * the length of a point on the boundary of the Minkowski sum (A-B) closest to the
- * origin. The goal of the EPA algorithm is to start with an initial simplex polytope
- * that contains the origin and expend it in order to find the point on the boundary
- * of (A-B) that is closest to the origin. An initial simplex that contains origin
- * has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex
- * polytope to find the correct penetration depth. The implementation of the EPA
- * algorithm is based on the book "Collision Detection in 3D Environments".
- */
-class EPAAlgorithm {
-
- private:
-
- // -------------------- Attributes -------------------- //
-
- /// Triangle comparison operator
- TriangleComparison mTriangleComparison;
-
- // -------------------- Methods -------------------- //
-
- /// Add a triangle face in the candidate triangle heap
- void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint& nbTriangles,
- decimal upperBoundSquarePenDepth);
-
- /// Decide if the origin is in the tetrahedron.
- int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
- const Vector3& p3, const Vector3& p4) const;
-
- public:
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- EPAAlgorithm() = default;
-
- /// Destructor
- ~EPAAlgorithm() = default;
-
- /// Deleted copy-constructor
- EPAAlgorithm(const EPAAlgorithm& algorithm) = delete;
-
- /// Deleted assignment operator
- EPAAlgorithm& operator=(const EPAAlgorithm& algorithm) = delete;
-
- /// Compute the penetration depth with EPA algorithm.
- bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
- const NarrowPhaseInfo* narrowPhaseInfo,
- Vector3& v,
- ContactPointInfo &contactPointInfo);
-};
-
-// Add a triangle face in the candidate triangle heap in the EPA algorithm
-inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
- uint& nbTriangles, decimal upperBoundSquarePenDepth) {
-
- // If the closest point of the affine hull of triangle
- // points is internal to the triangle and if the distance
- // of the closest point from the origin is at most the
- // penetration depth upper bound
- if (triangle->isClosestPointInternalToTriangle() &&
- triangle->getDistSquare() <= upperBoundSquarePenDepth) {
-
- // Add the triangle face to the list of candidates
- heap[nbTriangles] = triangle;
- nbTriangles++;
- std::push_heap(&heap[0], &heap[nbTriangles], mTriangleComparison);
- }
-}
-
-}
-
-#endif
-
diff --git a/src/collision/narrowphase/EPA/EdgeEPA.cpp b/src/collision/narrowphase/EPA/EdgeEPA.cpp
deleted file mode 100644
index dcbf61fd..00000000
--- a/src/collision/narrowphase/EPA/EdgeEPA.cpp
+++ /dev/null
@@ -1,125 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-// Libraries
-#include "EdgeEPA.h"
-#include "TriangleEPA.h"
-#include "TrianglesStore.h"
-#include <cassert>
-
-// We want to use the ReactPhysics3D namespace
-using namespace reactphysics3d;
-
-// Constructor
-EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index)
- : mOwnerTriangle(ownerTriangle), mIndex(index) {
- assert(index >= 0 && index < 3);
-}
-
-// Copy-constructor
-EdgeEPA::EdgeEPA(const EdgeEPA& edge) {
- mOwnerTriangle = edge.mOwnerTriangle;
- mIndex = edge.mIndex;
-}
-
-// Return the index of the source vertex of the edge (vertex starting the edge)
-uint EdgeEPA::getSourceVertexIndex() const {
- return (*mOwnerTriangle)[mIndex];
-}
-
-// Return the index of the target vertex of the edge (vertex ending the edge)
-uint EdgeEPA::getTargetVertexIndex() const {
- return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)];
-}
-
-// Execute the recursive silhouette algorithm from this edge
-bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex,
- TrianglesStore& triangleStore) {
- // If the edge has not already been visited
- if (!mOwnerTriangle->getIsObsolete()) {
-
- // If the triangle of this edge is not visible from the given point
- if (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) {
- TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
- getTargetVertexIndex(),
- getSourceVertexIndex());
-
- // If the triangle has been created
- if (triangle != nullptr) {
- halfLink(EdgeEPA(triangle, 1), *this);
- return true;
- }
-
- return false;
- }
- else {
-
- // The current triangle is visible and therefore obsolete
- mOwnerTriangle->setIsObsolete(true);
-
- int backup = triangleStore.getNbTriangles();
-
- if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(
- this->mIndex)).computeSilhouette(vertices,
- indexNewVertex,
- triangleStore)) {
- mOwnerTriangle->setIsObsolete(false);
-
- TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
- getTargetVertexIndex(),
- getSourceVertexIndex());
-
- // If the triangle has been created
- if (triangle != nullptr) {
- halfLink(EdgeEPA(triangle, 1), *this);
- return true;
- }
-
- return false;
- }
- else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(
- this->mIndex)).computeSilhouette(vertices,
- indexNewVertex,
- triangleStore)) {
- mOwnerTriangle->setIsObsolete(false);
-
- triangleStore.setNbTriangles(backup);
-
- TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
- getTargetVertexIndex(),
- getSourceVertexIndex());
-
- if (triangle != nullptr) {
- halfLink(EdgeEPA(triangle, 1), *this);
- return true;
- }
-
- return false;
- }
- }
- }
-
- return true;
-}
diff --git a/src/collision/narrowphase/EPA/TriangleEPA.cpp b/src/collision/narrowphase/EPA/TriangleEPA.cpp
deleted file mode 100644
index c322059f..00000000
--- a/src/collision/narrowphase/EPA/TriangleEPA.cpp
+++ /dev/null
@@ -1,145 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-
-// Libraries
-#include "TriangleEPA.h"
-#include "EdgeEPA.h"
-#include "TrianglesStore.h"
-
-// We use the ReactPhysics3D namespace
-using namespace reactphysics3d;
-
-// Constructor
-TriangleEPA::TriangleEPA(uint indexVertex1, uint indexVertex2, uint indexVertex3)
- : mIsObsolete(false) {
- mIndicesVertices[0] = indexVertex1;
- mIndicesVertices[1] = indexVertex2;
- mIndicesVertices[2] = indexVertex3;
-}
-
-// Compute the point v closest to the origin of this triangle
-bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
- const Vector3& p0 = vertices[mIndicesVertices[0]];
-
- Vector3 v1 = vertices[mIndicesVertices[1]] - p0;
- Vector3 v2 = vertices[mIndicesVertices[2]] - p0;
- decimal v1Dotv1 = v1.dot(v1);
- decimal v1Dotv2 = v1.dot(v2);
- decimal v2Dotv2 = v2.dot(v2);
- decimal p0Dotv1 = p0.dot(v1);
- decimal p0Dotv2 = p0.dot(v2);
-
- // Compute determinant
- mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
-
- // Compute lambda values
- mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2;
- mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1;
-
- // If the determinant is positive
- if (mDet > 0.0) {
- // Compute the closest point v
- mClosestPoint = p0 + decimal(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2);
-
- // Compute the square distance of closest point to the origin
- mDistSquare = mClosestPoint.dot(mClosestPoint);
-
- return true;
- }
-
- return false;
-}
-
-/// Link an edge with another one. It means that the current edge of a triangle will
-/// be associated with the edge of another triangle in order that both triangles
-/// are neighbour along both edges).
-bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) {
- bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
- edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
-
- if (isPossible) {
- edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
- edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0;
- }
-
- return isPossible;
-}
-
-/// Make an half link of an edge with another one from another triangle. An half-link
-/// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an
-/// adjacent edge of "edge0" but not the opposite. The opposite edge connection will
-/// be made later.
-void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
- assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
- edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
-
- // Link
- edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
-}
-
-// Execute the recursive silhouette algorithm from this triangle face.
-/// The parameter "vertices" is an array that contains the vertices of the current polytope and the
-/// parameter "indexNewVertex" is the index of the new vertex in this array. The goal of the
-/// silhouette algorithm is to add the new vertex in the polytope by keeping it convex. Therefore,
-/// the triangle faces that are visible from the new vertex must be removed from the polytope and we
-/// need to add triangle faces where each face contains the new vertex and an edge of the silhouette.
-/// The silhouette is the connected set of edges that are part of the border between faces that
-/// are seen and faces that are not seen from the new vertex. This method starts from the nearest
-/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in
-/// order that we always have a convex polytope. The faces visible from the new vertex are set
-/// obselete and will not be considered as being a candidate face in the future.
-bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex,
- TrianglesStore& triangleStore) {
-
- uint first = triangleStore.getNbTriangles();
-
- // Mark the current triangle as obsolete because it
- setIsObsolete(true);
-
- // Execute recursively the silhouette algorithm for the adjacent edges of neighboring
- // triangles of the current triangle
- bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
- mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
- mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
-
- if (result) {
- int i,j;
-
- // For each triangle face that contains the new vertex and an edge of the silhouette
- for (i=first, j=triangleStore.getNbTriangles()-1;
- i != triangleStore.getNbTriangles(); j = i++) {
- TriangleEPA* triangle = &triangleStore[i];
- halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1));
-
- if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) {
- return false;
- }
- }
-
- }
-
- return result;
-}
diff --git a/src/collision/narrowphase/EPA/TriangleEPA.h b/src/collision/narrowphase/EPA/TriangleEPA.h
deleted file mode 100644
index 865c69d4..00000000
--- a/src/collision/narrowphase/EPA/TriangleEPA.h
+++ /dev/null
@@ -1,197 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_TRIANGLE_EPA_H
-#define REACTPHYSICS3D_TRIANGLE_EPA_H
-
-// Libraries
-#include "mathematics/mathematics.h"
-#include "configuration.h"
-#include "EdgeEPA.h"
-#include <cassert>
-
-/// ReactPhysics3D namespace
-namespace reactphysics3d {
-
-// Prototypes
-bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
-void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
-
-
-// Class TriangleEPA
-/**
- * This class represents a triangle face of the current polytope in the EPA algorithm.
- */
-class TriangleEPA {
-
- private:
-
- // -------------------- Attributes -------------------- //
-
- /// Indices of the vertices y_i of the triangle
- uint mIndicesVertices[3];
-
- /// Three adjacent edges of the triangle (edges of other triangles)
- EdgeEPA mAdjacentEdges[3];
-
- /// True if the triangle face is visible from the new support point
- bool mIsObsolete;
-
- /// Determinant
- decimal mDet;
-
- /// Point v closest to the origin on the affine hull of the triangle
- Vector3 mClosestPoint;
-
- /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
- decimal mLambda1;
-
- /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
- decimal mLambda2;
-
- /// Square distance of the point closest point v to the origin
- decimal mDistSquare;
-
- public:
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- TriangleEPA() = default;
-
- /// Constructor
- TriangleEPA(uint v1, uint v2, uint v3);
-
- /// Destructor
- ~TriangleEPA() = default;
-
- /// Deleted copy-constructor
- TriangleEPA(const TriangleEPA& triangle) = delete;
-
- /// Deleted assignment operator
- TriangleEPA& operator=(const TriangleEPA& triangle) = delete;
-
- /// Return an adjacent edge of the triangle
- EdgeEPA& getAdjacentEdge(int index);
-
- /// Set an adjacent edge of the triangle
- void setAdjacentEdge(int index, EdgeEPA& edge);
-
- /// Return the square distance of the closest point to origin
- decimal getDistSquare() const;
-
- /// Set the isObsolete value
- void setIsObsolete(bool isObsolete);
-
- /// Return true if the triangle face is obsolete
- bool getIsObsolete() const;
-
- /// Return the point closest to the origin
- const Vector3& getClosestPoint() const;
-
- // Return true if the closest point on affine hull is inside the triangle
- bool isClosestPointInternalToTriangle() const;
-
- /// Return true if the triangle is visible from a given vertex
- bool isVisibleFromVertex(const Vector3* vertices, uint index) const;
-
- /// Compute the point v closest to the origin of this triangle
- bool computeClosestPoint(const Vector3* vertices);
-
- /// Compute the point of an object closest to the origin
- Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const;
-
- /// Execute the recursive silhouette algorithm from this triangle face.
- bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore);
-
- /// Access operator
- uint operator[](int i) const;
-
- /// Associate two edges
- friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
-
- /// Make a half-link between two edges
- friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
-};
-
-// Return an edge of the triangle
-inline EdgeEPA& TriangleEPA::getAdjacentEdge(int index) {
- assert(index >= 0 && index < 3);
- return mAdjacentEdges[index];
-}
-
-// Set an adjacent edge of the triangle
-inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) {
- assert(index >=0 && index < 3);
- mAdjacentEdges[index] = edge;
-}
-
-// Return the square distance of the closest point to origin
-inline decimal TriangleEPA::getDistSquare() const {
- return mDistSquare;
-}
-
-// Set the isObsolete value
-inline void TriangleEPA::setIsObsolete(bool isObsolete) {
- mIsObsolete = isObsolete;
-}
-
-// Return true if the triangle face is obsolete
-inline bool TriangleEPA::getIsObsolete() const {
- return mIsObsolete;
-}
-
-// Return the point closest to the origin
-inline const Vector3& TriangleEPA::getClosestPoint() const {
- return mClosestPoint;
-}
-
-// Return true if the closest point on affine hull is inside the triangle
-inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
- return (mLambda1 >= 0.0 && mLambda2 >= 0.0 && (mLambda1 + mLambda2) <= mDet);
-}
-
-// Return true if the triangle is visible from a given vertex
-inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const {
- Vector3 closestToVert = vertices[index] - mClosestPoint;
- return (mClosestPoint.dot(closestToVert) > 0.0);
-}
-
-// Compute the point of an object closest to the origin
-inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{
- const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]];
- return p0 + decimal(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) +
- mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0));
-}
-
-// Access operator
-inline uint TriangleEPA::operator[](int i) const {
- assert(i >= 0 && i <3);
- return mIndicesVertices[i];
-}
-
-}
-
-#endif
diff --git a/src/collision/narrowphase/EPA/TrianglesStore.h b/src/collision/narrowphase/EPA/TrianglesStore.h
deleted file mode 100644
index d21ccd35..00000000
--- a/src/collision/narrowphase/EPA/TrianglesStore.h
+++ /dev/null
@@ -1,139 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_TRIANGLES_STORE_H
-#define REACTPHYSICS3D_TRIANGLES_STORE_H
-
-#include "TriangleEPA.h"
-
-
-// Libraries
-#include <cassert>
-
-/// ReactPhysics3D namespace
-namespace reactphysics3d {
-
-// Constants
-constexpr unsigned int MAX_TRIANGLES = 200; // Maximum number of triangles
-
-// Class TriangleStore
-/**
- * This class stores several triangles of the polytope in the EPA algorithm.
- */
-class TrianglesStore {
-
- private:
-
- // -------------------- Attributes -------------------- //
-
- /// Triangles
- TriangleEPA mTriangles[MAX_TRIANGLES];
-
- /// Number of triangles
- int mNbTriangles;
-
- public:
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- TrianglesStore();
-
- /// Destructor
- ~TrianglesStore() = default;
-
- /// Deleted copy-constructor
- TrianglesStore(const TrianglesStore& triangleStore) = delete;
-
- /// Deleted assignment operator
- TrianglesStore& operator=(const TrianglesStore& triangleStore) = delete;
-
- /// Clear all the storage
- void clear();
-
- /// Return the number of triangles
- int getNbTriangles() const;
-
- /// Set the number of triangles
- void setNbTriangles(int backup);
-
- /// Return the last triangle
- TriangleEPA& last();
-
- /// Create a new triangle
- TriangleEPA* newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2);
-
- /// Access operator
- TriangleEPA& operator[](int i);
-};
-
-// Clear all the storage
-inline void TrianglesStore::clear() {
- mNbTriangles = 0;
-}
-
-// Return the number of triangles
-inline int TrianglesStore::getNbTriangles() const {
- return mNbTriangles;
-}
-
-
-inline void TrianglesStore::setNbTriangles(int backup) {
- mNbTriangles = backup;
-}
-
-// Return the last triangle
-inline TriangleEPA& TrianglesStore::last() {
- assert(mNbTriangles > 0);
- return mTriangles[mNbTriangles - 1];
-}
-
-// Create a new triangle
-inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices,
- uint v0,uint v1, uint v2) {
- TriangleEPA* newTriangle = nullptr;
-
- // If we have not reached the maximum number of triangles
- if (mNbTriangles != MAX_TRIANGLES) {
- newTriangle = &mTriangles[mNbTriangles++];
- new (newTriangle) TriangleEPA(v0, v1, v2);
- if (!newTriangle->computeClosestPoint(vertices)) {
- mNbTriangles--;
- newTriangle = nullptr;
- }
- }
-
- // Return the new triangle
- return newTriangle;
-}
-
-// Access operator
-inline TriangleEPA& TrianglesStore::operator[](int i) {
- return mTriangles[i];
-}
-
-}
-
-#endif
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index e1649af0..cd84f5a0 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -45,7 +45,7 @@ using namespace reactphysics3d;
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects.
-bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactPointInfo& contactPointInfo) {
PROFILE("GJKAlgorithm::testCollision()");
@@ -101,8 +101,7 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
// Compute the support points for original objects (without margins) A and B
suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData);
- suppB = body2Tobody1 *
- shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
+ suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@@ -116,7 +115,7 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
narrowPhaseInfo->overlappingPair->setCachedSeparatingAxis(v);
// No intersection, we return
- return false;
+ return GJKResult::SEPARATED;
}
// If the objects intersect only in the margins
@@ -135,7 +134,8 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
// Contact point has been found
contactFound = true;
- break;
+
+ return GJKResult::INTERPENETRATE;
}
// Compute the point of the simplex closest to the origin
@@ -144,13 +144,14 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
// Contact point has been found
contactFound = true;
- break;
+
+ return GJKResult::INTERPENETRATE;
}
// Closest point is almost the origin, go to EPA algorithm
// Vector v to small to continue computing support points
if (v.lengthSquare() < MACHINE_EPSILON) {
- break;
+ return GJKResult::INTERPENETRATE;
}
// Store and update the squared distance of the closest point
@@ -173,20 +174,6 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
} while((isPolytopeShape && !simplex.isFull()) || (!isPolytopeShape && !simplex.isFull() &&
distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()));
- // If no contact has been found (penetration case)
- if (!contactFound) {
-
- // 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.
- if(computePenetrationDepthForEnlargedObjects(narrowPhaseInfo, contactPointInfo, v)) {
-
- // A contact has been found with EPA algorithm, we return true
- return true;
- }
- }
-
if (contactFound && distSquare > MACHINE_EPSILON) {
// Compute the closet points of both objects (without the margins)
@@ -205,103 +192,21 @@ bool GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
// If the penetration depth is negative (due too numerical errors), there is no contact
if (penetrationDepth <= decimal(0.0)) {
- return false;
+ return GJKResult::INTERPENETRATE;
}
// Do not generate a contact point with zero normal length
if (normal.lengthSquare() < MACHINE_EPSILON) {
- return false;
+ return GJKResult::INTERPENETRATE;
}
// Create the contact info object
contactPointInfo.init(normal, penetrationDepth, pA, pB);
- return true;
+ return GJKResult::COLLIDE_IN_MARGIN;
}
- return false;
-}
-
-/// This method runs the GJK algorithm on the two enlarged objects (with margin)
-/// to compute a simplex polytope that contains the origin. The two objects are
-/// assumed to intersect in the original objects (without margin). Therefore such
-/// a polytope must exist. Then, we give that polytope to the EPA algorithm to
-/// compute the correct penetration depth and contact points of the enlarged objects.
-bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo,
- Vector3& v) {
- PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
-
- VoronoiSimplex simplex;
- Vector3 suppA;
- Vector3 suppB;
- Vector3 w;
- decimal vDotw;
- decimal distSquare = DECIMAL_LARGEST;
- decimal prevDistSquare;
-
- assert(narrowPhaseInfo->collisionShape1->isConvex());
- assert(narrowPhaseInfo->collisionShape2->isConvex());
-
- const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
- const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
-
- bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
-
- void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1;
- void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2;
-
- // Transform a point from local space of body 2 to local space
- // of body 1 (the GJK algorithm is done in local space of body 1)
- Transform body2ToBody1 = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * narrowPhaseInfo->shape2ToWorldTransform;
-
- // Matrix that transform a direction from local space of body 1 into local space of body 2
- Matrix3x3 rotateToBody2 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation().getMatrix().getTranspose() *
- narrowPhaseInfo->shape1ToWorldTransform.getOrientation().getMatrix();
-
- do {
- // Compute the support points for the enlarged object A and B
- suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData);
- suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData);
-
- // Compute the support point for the Minkowski difference A-B
- w = suppA - suppB;
-
- vDotw = v.dot(w);
-
- // If the enlarge objects do not intersect
- if (vDotw > decimal(0.0)) {
-
- // No intersection, we return
- return false;
- }
-
- // Add the new support point to the simplex
- simplex.addPoint(w, suppA, suppB);
-
- if (simplex.isAffinelyDependent()) {
- return false;
- }
-
- if (!simplex.computeClosestPoint(v)) {
- return false;
- }
-
- // Store and update the square distance
- prevDistSquare = distSquare;
- distSquare = v.lengthSquare();
-
- if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
- return false;
- }
-
- } while((!simplex.isFull() && isPolytopeShape) || (!isPolytopeShape && !simplex.isFull() &&
- distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()));
-
- // Give the simplex computed with GJK algorithm to the EPA algorithm
- // which will compute the correct penetration depth and contact points
- // between the two enlarged objects
- return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, narrowPhaseInfo, v, contactPointInfo);
+ return GJKResult::SEPARATED;
}
// Use the GJK Algorithm to find if a point is inside a convex collision shape
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h
index e0dad891..8ca25e33 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.h
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h
@@ -27,11 +27,9 @@
#define REACTPHYSICS3D_GJK_ALGORITHM_H
// Libraries
-#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
#include "constraint/ContactPoint.h"
#include "collision/shapes/ConvexShape.h"
-#include "collision/narrowphase/EPA/EPAAlgorithm.h"
-
+#include "VoronoiSimplex.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -57,24 +55,22 @@ constexpr int MAX_ITERATIONS_GJK_RAYCAST = 32;
* Polytope Algorithm) to compute the correct penetration depth between the
* enlarged objects.
*/
-class GJKAlgorithm : public NarrowPhaseAlgorithm {
+class GJKAlgorithm {
private :
// -------------------- Attributes -------------------- //
- /// EPA Algorithm
- EPAAlgorithm mAlgoEPA;
-
// -------------------- Methods -------------------- //
- /// Compute the penetration depth for enlarged objects.
- bool computePenetrationDepthForEnlargedObjects(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo,
- Vector3& v);
-
public :
+ enum class GJKResult {
+ SEPARATED, // The two shapes are separated outside the margin
+ COLLIDE_IN_MARGIN, // The two shapes overlap only in the margin (shallow penetration)
+ INTERPENETRATE // The two shapes overlap event without the margin (deep penetration)
+ };
+
// -------------------- Methods -------------------- //
/// Constructor
@@ -90,14 +86,16 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volumes collide.
- virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) override;
+ GJKResult testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
+
+
};
}
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
new file mode 100644
index 00000000..d74faf09
--- /dev/null
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -0,0 +1,43 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "SATAlgorithm.h"
+#include "constraint/ContactPoint.h"
+#include "configuration.h"
+#include "engine/Profiler.h"
+#include <algorithm>
+#include <cmath>
+#include <cfloat>
+#include <cassert>
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) {
+
+
+}
diff --git a/src/collision/narrowphase/EPA/EdgeEPA.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
similarity index 51%
rename from src/collision/narrowphase/EPA/EdgeEPA.h
rename to src/collision/narrowphase/SAT/SATAlgorithm.h
index ba3dcfee..30156cc2 100644
--- a/src/collision/narrowphase/EPA/EdgeEPA.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -23,100 +23,45 @@
* *
********************************************************************************/
-#ifndef REACTPHYSICS3D_EDGE_EPA_H
-#define REACTPHYSICS3D_EDGE_EPA_H
-
+#ifndef REACTPHYSICS3D_SAT_ALGORITHM_H
+#define REACTPHYSICS3D_SAT_ALGORITHM_H
// Libraries
-#include "mathematics/mathematics.h"
+#include "constraint/ContactPoint.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
-// Class declarations
-class TriangleEPA;
-class TrianglesStore;
+// Class SATAlgorithm
+class SATAlgorithm {
-// Class EdgeEPA
-/**
- * This class represents an edge of the current polytope in the EPA algorithm.
- */
-class EdgeEPA {
-
- private:
+ private :
// -------------------- Attributes -------------------- //
- /// Pointer to the triangle that contains this edge
- TriangleEPA* mOwnerTriangle;
+ // -------------------- Methods -------------------- //
- /// Index of the edge in the triangle (between 0 and 2).
- /// The edge with index i connect triangle vertices i and (i+1 % 3)
- int mIndex;
-
- public:
+ public :
// -------------------- Methods -------------------- //
/// Constructor
- EdgeEPA() = default;
-
- /// Constructor
- EdgeEPA(TriangleEPA* ownerTriangle, int index);
-
- /// Copy-constructor
- EdgeEPA(const EdgeEPA& edge);
+ SATAlgorithm() = default;
/// Destructor
- ~EdgeEPA() = default;
+ ~SATAlgorithm() = default;
- /// Return the pointer to the owner triangle
- TriangleEPA* getOwnerTriangle() const;
+ /// Deleted copy-constructor
+ SATAlgorithm(const SATAlgorithm& algorithm) = delete;
- /// Return the index of the edge in the triangle
- int getIndex() const;
+ /// Deleted assignment operator
+ SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete;
- /// Return index of the source vertex of the edge
- uint getSourceVertexIndex() const;
-
- /// Return the index of the target vertex of the edge
- uint getTargetVertexIndex() const;
-
- /// Execute the recursive silhouette algorithm from this edge
- bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore);
-
- /// Assignment operator
- EdgeEPA& operator=(const EdgeEPA& edge);
+ /// Compute a contact info if the two bounding volumes collide.
+ bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo);
};
-// Return the pointer to the owner triangle
-inline TriangleEPA* EdgeEPA::getOwnerTriangle() const {
- return mOwnerTriangle;
-}
-
-// Return the edge index
-inline int EdgeEPA::getIndex() const {
- return mIndex;
-}
-
-// Assignment operator
-inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) {
- mOwnerTriangle = edge.mOwnerTriangle;
- mIndex = edge.mIndex;
- return *this;
-}
-
-// Return the index of the next counter-clockwise edge of the ownver triangle
-inline int indexOfNextCounterClockwiseEdge(int i) {
- return (i + 1) % 3;
-}
-
-// Return the index of the previous counter-clockwise edge of the ownver triangle
-inline int indexOfPreviousCounterClockwiseEdge(int i) {
- return (i + 2) % 3;
-}
-
}
#endif
-
diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp
new file mode 100644
index 00000000..65371e4f
--- /dev/null
+++ b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp
@@ -0,0 +1,62 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "SphereVsConvexMeshAlgorithm.h"
+#include "SAT/SATAlgorithm.h"
+#include "collision/shapes/SphereShape.h"
+#include "collision/shapes/ConvexMeshShape.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) {
+
+ // Get the local-space to world-space transforms
+ const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
+ const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
+
+ // First, we run the GJK algorithm
+ GJKAlgorithm gjkAlgorithm;
+ GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactPointInfo);
+
+ // If we have found a contact point inside the margins (shallow penetration)
+ if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
+
+ // Return true
+ return true;
+ }
+
+ // If we have overlap even without the margins (deep penetration)
+ if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
+
+ // Run the SAT algorithm to find the separating axis and compute contact point
+ SATAlgorithm satAlgorithm;
+ return satAlgorithm.testCollision(narrowPhaseInfo, contactPointInfo);
+ }
+
+ return false;
+}
diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h
new file mode 100644
index 00000000..448b8a5d
--- /dev/null
+++ b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h
@@ -0,0 +1,71 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_SPHERE_VS_CONVEX_MESH_ALGORITHM_H
+#define REACTPHYSICS3D_SPHERE_VS_CONVEX_MESH_ALGORITHM_H
+
+// Libraries
+#include "body/Body.h"
+#include "constraint/ContactPoint.h"
+#include "NarrowPhaseAlgorithm.h"
+
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class SphereVsConvexMeshAlgorithm
+/**
+ * This class is used to compute the narrow-phase collision detection
+ * between a sphere and a convex mesh.
+ */
+class SphereVsConvexMeshAlgorithm : public NarrowPhaseAlgorithm {
+
+ protected :
+
+ public :
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ SphereVsConvexMeshAlgorithm() = default;
+
+ /// Destructor
+ virtual ~SphereVsConvexMeshAlgorithm() override = default;
+
+ /// Deleted copy-constructor
+ SphereVsConvexMeshAlgorithm(const SphereVsConvexMeshAlgorithm& algorithm) = delete;
+
+ /// Deleted assignment operator
+ SphereVsConvexMeshAlgorithm& operator=(const SphereVsConvexMeshAlgorithm& algorithm) = delete;
+
+ /// Compute a contact info if the two bounding volume collide
+ virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) override;
+};
+
+}
+
+#endif
+
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index b33ed4f1..8a591614 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -40,8 +40,7 @@
namespace reactphysics3d {
/// Type of the collision shape
-enum class CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
- CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
+enum class CollisionShapeType {TRIANGLE, BOX, SPHERE, CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
const int NB_COLLISION_SHAPE_TYPES = 9;
// Declarations
diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp
deleted file mode 100644
index 4a1ece33..00000000
--- a/src/collision/shapes/ConeShape.cpp
+++ /dev/null
@@ -1,209 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-// Libraries
-#include <complex>
-#include "configuration.h"
-#include "ConeShape.h"
-#include "collision/ProxyShape.h"
-
-using namespace reactphysics3d;
-
-// Constructor
-/**
- * @param radius Radius of the cone (in meters)
- * @param height Height of the cone (in meters)
- * @param margin Collision margin (in meters) around the collision shape
- */
-ConeShape::ConeShape(decimal radius, decimal height, decimal margin)
- : ConvexShape(CollisionShapeType::CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) {
- assert(mRadius > decimal(0.0));
- assert(mHalfHeight > decimal(0.0));
-
- // Compute the sine of the semi-angle at the apex point
- mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
-}
-
-// Return a local support point in a given direction without the object margin
-Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const {
-
- const Vector3& v = direction;
- decimal sinThetaTimesLengthV = mSinTheta * v.length();
- Vector3 supportPoint;
-
- if (v.y > sinThetaTimesLengthV) {
- supportPoint = Vector3(0.0, mHalfHeight, 0.0);
- }
- else {
- decimal projectedLength = sqrt(v.x * v.x + v.z * v.z);
- if (projectedLength > MACHINE_EPSILON) {
- decimal d = mRadius / projectedLength;
- supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d);
- }
- else {
- supportPoint = Vector3(0.0, -mHalfHeight, 0.0);
- }
- }
-
- return supportPoint;
-}
-
-// Raycast method with feedback information
-// This implementation is based on the technique described by David Eberly in the article
-// "Intersection of a Line and a Cone" that can be found at
-// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
-bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
-
- const Vector3 r = ray.point2 - ray.point1;
-
- const decimal epsilon = decimal(0.00001);
- Vector3 V(0, mHalfHeight, 0);
- Vector3 centerBase(0, -mHalfHeight, 0);
- Vector3 axis(0, decimal(-1.0), 0);
- decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight;
- decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
- decimal factor = decimal(1.0) - cosThetaSquare;
- Vector3 delta = ray.point1 - V;
- decimal c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y -
- cosThetaSquare * delta.z * delta.z;
- decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
- decimal c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z;
- decimal tHit[] = {decimal(-1.0), decimal(-1.0), decimal(-1.0)};
- Vector3 localHitPoint[3];
- Vector3 localNormal[3];
-
- // If c2 is different from zero
- if (std::abs(c2) > MACHINE_EPSILON) {
- decimal gamma = c1 * c1 - c0 * c2;
-
- // If there is no real roots in the quadratic equation
- if (gamma < decimal(0.0)) {
- return false;
- }
- else if (gamma > decimal(0.0)) { // The equation has two real roots
-
- // Compute two intersections
- decimal sqrRoot = std::sqrt(gamma);
- tHit[0] = (-c1 - sqrRoot) / c2;
- tHit[1] = (-c1 + sqrRoot) / c2;
- }
- else { // If the equation has a single real root
-
- // Compute the intersection
- tHit[0] = -c1 / c2;
- }
- }
- else { // If c2 == 0
-
- // If c2 = 0 and c1 != 0
- if (std::abs(c1) > MACHINE_EPSILON) {
- tHit[0] = -c0 / (decimal(2.0) * c1);
- }
- else { // If c2 = c1 = 0
-
- // If c0 is different from zero, no solution and if c0 = 0, we have a
- // degenerate case, the whole ray is contained in the cone side
- // but we return no hit in this case
- return false;
- }
- }
-
- // If the origin of the ray is inside the cone, we return no hit
- if (testPointInside(ray.point1, nullptr)) return false;
-
- localHitPoint[0] = ray.point1 + tHit[0] * r;
- localHitPoint[1] = ray.point1 + tHit[1] * r;
-
- // Only keep hit points in one side of the double cone (the cone we are interested in)
- if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) {
- tHit[0] = decimal(-1.0);
- }
- if (axis.dot(localHitPoint[1] - V) < decimal(0.0)) {
- tHit[1] = decimal(-1.0);
- }
-
- // Only keep hit points that are within the correct height of the cone
- if (localHitPoint[0].y < decimal(-mHalfHeight)) {
- tHit[0] = decimal(-1.0);
- }
- if (localHitPoint[1].y < decimal(-mHalfHeight)) {
- tHit[1] = decimal(-1.0);
- }
-
- // If the ray is in direction of the base plane of the cone
- if (r.y > epsilon) {
-
- // Compute the intersection with the base plane of the cone
- tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y);
-
- // Only keep this intersection if it is inside the cone radius
- localHitPoint[2] = ray.point1 + tHit[2] * r;
-
- if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) {
- tHit[2] = decimal(-1.0);
- }
-
- // Compute the normal direction
- localNormal[2] = axis;
- }
-
- // Find the smallest positive t value
- int hitIndex = -1;
- decimal t = DECIMAL_LARGEST;
- for (int i=0; i<3; i++) {
- if (tHit[i] < decimal(0.0)) continue;
- if (tHit[i] < t) {
- hitIndex = i;
- t = tHit[hitIndex];
- }
- }
-
- if (hitIndex < 0) return false;
- if (t > ray.maxFraction) return false;
-
- // Compute the normal direction for hit against side of the cone
- if (hitIndex != 2) {
- decimal h = decimal(2.0) * mHalfHeight;
- decimal value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x +
- localHitPoint[hitIndex].z * localHitPoint[hitIndex].z);
- decimal rOverH = mRadius / h;
- decimal value2 = decimal(1.0) + rOverH * rOverH;
- decimal factor = decimal(1.0) / std::sqrt(value1 * value2);
- decimal x = localHitPoint[hitIndex].x * factor;
- decimal z = localHitPoint[hitIndex].z * factor;
- localNormal[hitIndex].x = x;
- localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH;
- localNormal[hitIndex].z = z;
- }
-
- raycastInfo.body = proxyShape->getBody();
- raycastInfo.proxyShape = proxyShape;
- raycastInfo.hitFraction = t;
- raycastInfo.worldPoint = localHitPoint[hitIndex];
- raycastInfo.worldNormal = localNormal[hitIndex];
-
- return true;
-}
diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h
deleted file mode 100644
index 16102163..00000000
--- a/src/collision/shapes/ConeShape.h
+++ /dev/null
@@ -1,194 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_CONE_SHAPE_H
-#define REACTPHYSICS3D_CONE_SHAPE_H
-
-// Libraries
-#include "ConvexShape.h"
-#include "body/CollisionBody.h"
-#include "mathematics/mathematics.h"
-
-/// ReactPhysics3D namespace
-namespace reactphysics3d {
-
-// Class ConeShape
-/**
- * This class represents a cone collision shape centered at the
- * origin and alligned with the Y axis. The cone is defined
- * by its height and by the radius of its base. The center of the
- * cone is at the half of the height. The "transform" of the
- * corresponding rigid body gives an orientation and a position
- * to the cone. This collision shape uses an extra margin distance around
- * it for collision detection purpose. The default margin is 4cm (if your
- * units are meters, which is recommended). In case, you want to simulate small
- * objects (smaller than the margin distance), you might want to reduce the margin
- * by specifying your own margin distance using the "margin" parameter in the
- * constructor of the cone shape. Otherwise, it is recommended to use the
- * default margin distance by not using the "margin" parameter in the constructor.
- */
-class ConeShape : public ConvexShape {
-
- protected :
-
- // -------------------- Attributes -------------------- //
-
- /// Radius of the base
- decimal mRadius;
-
- /// Half height of the cone
- decimal mHalfHeight;
-
- /// sine of the semi angle at the apex point
- decimal mSinTheta;
-
- // -------------------- Methods -------------------- //
-
- /// Return a local support point in a given direction without the object margin
- virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const override;
-
- /// Return true if a point is inside the collision shape
- virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
-
- /// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
-
- /// Return the number of bytes used by the collision shape
- virtual size_t getSizeInBytes() const override;
-
- public :
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- ConeShape(decimal mRadius, decimal height, decimal margin = OBJECT_MARGIN);
-
- /// Destructor
- virtual ~ConeShape() override = default;
-
- /// Deleted copy-constructor
- ConeShape(const ConeShape& shape) = delete;
-
- /// Deleted assignment operator
- ConeShape& operator=(const ConeShape& shape) = delete;
-
- /// Return the radius
- decimal getRadius() const;
-
- /// Return the height
- decimal getHeight() const;
-
- /// Set the scaling vector of the collision shape
- virtual void setLocalScaling(const Vector3& scaling) override;
-
- /// Return the local bounds of the shape in x, y and z directions
- virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
-
- /// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const override;
-
- /// Return the local inertia tensor of the collision shape
- virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
-};
-
-// Return the radius
-/**
- * @return Radius of the cone (in meters)
- */
-inline decimal ConeShape::getRadius() const {
- return mRadius;
-}
-
-// Return the height
-/**
- * @return Height of the cone (in meters)
- */
-inline decimal ConeShape::getHeight() const {
- return decimal(2.0) * mHalfHeight;
-}
-
-// Set the scaling vector of the collision shape
-inline void ConeShape::setLocalScaling(const Vector3& scaling) {
-
- mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
- mRadius = (mRadius / mScaling.x) * scaling.x;
-
- CollisionShape::setLocalScaling(scaling);
-}
-
-// Return the number of bytes used by the collision shape
-inline size_t ConeShape::getSizeInBytes() const {
- return sizeof(ConeShape);
-}
-
-// Return the local bounds of the shape in x, y and z directions
-/**
- * @param min The minimum bounds of the shape in local-space coordinates
- * @param max The maximum bounds of the shape in local-space coordinates
- */
-inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const {
-
- // Maximum bounds
- max.x = mRadius + mMargin;
- max.y = mHalfHeight + mMargin;
- max.z = max.x;
-
- // Minimum bounds
- min.x = -max.x;
- min.y = -max.y;
- min.z = min.x;
-}
-
-// Return true if the collision shape is a polyhedron
-inline bool ConeShape::isPolyhedron() const {
- return false;
-}
-
-// Return the local inertia tensor of the collision shape
-/**
- * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
- * coordinates
- * @param mass Mass to use to compute the inertia tensor of the collision shape
- */
-inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
- decimal rSquare = mRadius * mRadius;
- decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight);
- tensor.setAllValues(diagXZ, 0.0, 0.0,
- 0.0, decimal(0.3) * mass * rSquare,
- 0.0, 0.0, 0.0, diagXZ);
-}
-
-// 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/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp
deleted file mode 100644
index 8cb405f1..00000000
--- a/src/collision/shapes/CylinderShape.cpp
+++ /dev/null
@@ -1,231 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-// Libraries
-#include "CylinderShape.h"
-#include "collision/ProxyShape.h"
-#include "configuration.h"
-
-using namespace reactphysics3d;
-
-// Constructor
-/**
- * @param radius Radius of the cylinder (in meters)
- * @param height Height of the cylinder (in meters)
- * @param margin Collision margin (in meters) around the collision shape
- */
-CylinderShape::CylinderShape(decimal radius, decimal height, decimal margin)
- : ConvexShape(CollisionShapeType::CYLINDER, margin), mRadius(radius),
- mHalfHeight(height/decimal(2.0)) {
- assert(radius > decimal(0.0));
- assert(height > decimal(0.0));
-}
-
-// Return a local support point in a given direction without the object margin
-Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const {
-
- Vector3 supportPoint(0.0, 0.0, 0.0);
- decimal uDotv = direction.y;
- Vector3 w(direction.x, 0.0, direction.z);
- decimal lengthW = std::sqrt(direction.x * direction.x + direction.z * direction.z);
-
- if (lengthW > MACHINE_EPSILON) {
- if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
- else supportPoint.y = mHalfHeight;
- supportPoint += (mRadius / lengthW) * w;
- }
- else {
- if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
- else supportPoint.y = mHalfHeight;
- }
-
- return supportPoint;
-}
-
-// Raycast method with feedback information
-/// Algorithm based on the one described at page 194 in Real-ime Collision Detection by
-/// Morgan Kaufmann.
-bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
-
- const Vector3 n = ray.point2 - ray.point1;
-
- const decimal epsilon = decimal(0.01);
- Vector3 p(decimal(0), -mHalfHeight, decimal(0));
- Vector3 q(decimal(0), mHalfHeight, decimal(0));
- Vector3 d = q - p;
- Vector3 m = ray.point1 - p;
- decimal t;
-
- decimal mDotD = m.dot(d);
- decimal nDotD = n.dot(d);
- decimal dDotD = d.dot(d);
-
- // Test if the segment is outside the cylinder
- if (mDotD < decimal(0.0) && mDotD + nDotD < decimal(0.0)) return false;
- if (mDotD > dDotD && mDotD + nDotD > dDotD) return false;
-
- decimal nDotN = n.dot(n);
- decimal mDotN = m.dot(n);
-
- decimal a = dDotD * nDotN - nDotD * nDotD;
- decimal k = m.dot(m) - mRadius * mRadius;
- decimal c = dDotD * k - mDotD * mDotD;
-
- // If the ray is parallel to the cylinder axis
- if (std::abs(a) < epsilon) {
-
- // If the origin is outside the surface of the cylinder, we return no hit
- if (c > decimal(0.0)) return false;
-
- // Here we know that the segment intersect an endcap of the cylinder
-
- // If the ray intersects with the "p" endcap of the cylinder
- if (mDotD < decimal(0.0)) {
-
- t = -mDotN / nDotN;
-
- // If the intersection is behind the origin of the ray or beyond the maximum
- // raycasting distance, we return no hit
- if (t < decimal(0.0) || t > ray.maxFraction) return false;
-
- // Compute the hit information
- Vector3 localHitPoint = ray.point1 + t * n;
- raycastInfo.body = proxyShape->getBody();
- raycastInfo.proxyShape = proxyShape;
- raycastInfo.hitFraction = t;
- raycastInfo.worldPoint = localHitPoint;
- Vector3 normalDirection(0, decimal(-1), 0);
- raycastInfo.worldNormal = normalDirection;
-
- return true;
- }
- else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder
-
- t = (nDotD - mDotN) / nDotN;
-
- // If the intersection is behind the origin of the ray or beyond the maximum
- // raycasting distance, we return no hit
- if (t < decimal(0.0) || t > ray.maxFraction) return false;
-
- // Compute the hit information
- Vector3 localHitPoint = ray.point1 + t * n;
- raycastInfo.body = proxyShape->getBody();
- raycastInfo.proxyShape = proxyShape;
- raycastInfo.hitFraction = t;
- raycastInfo.worldPoint = localHitPoint;
- Vector3 normalDirection(0, decimal(1.0), 0);
- raycastInfo.worldNormal = normalDirection;
-
- return true;
- }
- else { // If the origin is inside the cylinder, we return no hit
- return false;
- }
- }
- decimal b = dDotD * mDotN - nDotD * mDotD;
- decimal discriminant = b * b - a * c;
-
- // If the discriminant is negative, no real roots and therfore, no hit
- if (discriminant < decimal(0.0)) return false;
-
- // Compute the smallest root (first intersection along the ray)
- decimal t0 = t = (-b - std::sqrt(discriminant)) / a;
-
- // If the intersection is outside the cylinder on "p" endcap side
- decimal value = mDotD + t * nDotD;
- if (value < decimal(0.0)) {
-
- // If the ray is pointing away from the "p" endcap, we return no hit
- if (nDotD <= decimal(0.0)) return false;
-
- // Compute the intersection against the "p" endcap (intersection agains whole plane)
- t = -mDotD / nDotD;
-
- // Keep the intersection if the it is inside the cylinder radius
- if (k + t * (decimal(2.0) * mDotN + t) > decimal(0.0)) return false;
-
- // If the intersection is behind the origin of the ray or beyond the maximum
- // raycasting distance, we return no hit
- if (t < decimal(0.0) || t > ray.maxFraction) return false;
-
- // Compute the hit information
- Vector3 localHitPoint = ray.point1 + t * n;
- raycastInfo.body = proxyShape->getBody();
- raycastInfo.proxyShape = proxyShape;
- raycastInfo.hitFraction = t;
- raycastInfo.worldPoint = localHitPoint;
- Vector3 normalDirection(0, decimal(-1.0), 0);
- raycastInfo.worldNormal = normalDirection;
-
- return true;
- }
- else if (value > dDotD) { // If the intersection is outside the cylinder on the "q" side
-
- // If the ray is pointing away from the "q" endcap, we return no hit
- if (nDotD >= decimal(0.0)) return false;
-
- // Compute the intersection against the "q" endcap (intersection against whole plane)
- t = (dDotD - mDotD) / nDotD;
-
- // Keep the intersection if it is inside the cylinder radius
- if (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) >
- decimal(0.0)) return false;
-
- // If the intersection is behind the origin of the ray or beyond the maximum
- // raycasting distance, we return no hit
- if (t < decimal(0.0) || t > ray.maxFraction) return false;
-
- // Compute the hit information
- Vector3 localHitPoint = ray.point1 + t * n;
- raycastInfo.body = proxyShape->getBody();
- raycastInfo.proxyShape = proxyShape;
- raycastInfo.hitFraction = t;
- raycastInfo.worldPoint = localHitPoint;
- Vector3 normalDirection(0, decimal(1.0), 0);
- raycastInfo.worldNormal = normalDirection;
-
- return true;
- }
-
- t = t0;
-
- // If the intersection is behind the origin of the ray or beyond the maximum
- // raycasting distance, we return no hit
- if (t < decimal(0.0) || t > ray.maxFraction) return false;
-
- // Compute the hit information
- Vector3 localHitPoint = ray.point1 + t * n;
- raycastInfo.body = proxyShape->getBody();
- raycastInfo.proxyShape = proxyShape;
- raycastInfo.hitFraction = t;
- raycastInfo.worldPoint = localHitPoint;
- Vector3 v = localHitPoint - p;
- Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
- Vector3 normalDirection = (localHitPoint - (p + w));
- raycastInfo.worldNormal = normalDirection;
-
- return true;
-}
diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h
deleted file mode 100644
index e0744a80..00000000
--- a/src/collision/shapes/CylinderShape.h
+++ /dev/null
@@ -1,190 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_CYLINDER_SHAPE_H
-#define REACTPHYSICS3D_CYLINDER_SHAPE_H
-
-// Libraries
-#include "ConvexShape.h"
-#include "body/CollisionBody.h"
-#include "mathematics/mathematics.h"
-
-
-/// ReactPhysics3D namespace
-namespace reactphysics3d {
-
-// Class CylinderShape
-/**
- * This class represents a cylinder collision shape around the Y axis
- * and centered at the origin. The cylinder is defined by its height
- * and the radius of its base. The "transform" of the corresponding
- * rigid body gives an orientation and a position to the cylinder.
- * This collision shape uses an extra margin distance around it for collision
- * detection purpose. The default margin is 4cm (if your units are meters,
- * which is recommended). In case, you want to simulate small objects
- * (smaller than the margin distance), you might want to reduce the margin by
- * specifying your own margin distance using the "margin" parameter in the
- * constructor of the cylinder shape. Otherwise, it is recommended to use the
- * default margin distance by not using the "margin" parameter in the constructor.
- */
-class CylinderShape : public ConvexShape {
-
- protected :
-
- // -------------------- Attributes -------------------- //
-
- /// Radius of the base
- decimal mRadius;
-
- /// Half height of the cylinder
- decimal mHalfHeight;
-
- // -------------------- Methods -------------------- //
-
- /// Return a local support point in a given direction without the object margin
- virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const override;
-
- /// Return true if a point is inside the collision shape
- virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
-
- /// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override ;
-
- /// Return the number of bytes used by the collision shape
- virtual size_t getSizeInBytes() const override;
-
- public :
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- CylinderShape(decimal radius, decimal height, decimal margin = OBJECT_MARGIN);
-
- /// Destructor
- virtual ~CylinderShape() override = default;
-
- /// Deleted copy-constructor
- CylinderShape(const CylinderShape& shape) = delete;
-
- /// Deleted assignment operator
- CylinderShape& operator=(const CylinderShape& shape) = delete;
-
- /// Return the radius
- decimal getRadius() const;
-
- /// Return the height
- decimal getHeight() const;
-
- /// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const override;
-
- /// Set the scaling vector of the collision shape
- virtual void setLocalScaling(const Vector3& scaling) override;
-
- /// Return the local bounds of the shape in x, y and z directions
- virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
-
- /// Return the local inertia tensor of the collision shape
- virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
-};
-
-// Return the radius
-/**
- * @return Radius of the cylinder (in meters)
- */
-inline decimal CylinderShape::getRadius() const {
- return mRadius;
-}
-
-// Return the height
-/**
- * @return Height of the cylinder (in meters)
- */
-inline decimal CylinderShape::getHeight() const {
- return mHalfHeight + mHalfHeight;
-}
-
-// Return true if the collision shape is a polyhedron
-inline bool CylinderShape::isPolyhedron() const {
- return false;
-}
-
-// Set the scaling vector of the collision shape
-inline void CylinderShape::setLocalScaling(const Vector3& scaling) {
-
- mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
- mRadius = (mRadius / mScaling.x) * scaling.x;
-
- CollisionShape::setLocalScaling(scaling);
-}
-
-// Return the number of bytes used by the collision shape
-inline size_t CylinderShape::getSizeInBytes() const {
- return sizeof(CylinderShape);
-}
-
-// Return the local bounds of the shape in x, y and z directions
-/**
- * @param min The minimum bounds of the shape in local-space coordinates
- * @param max The maximum bounds of the shape in local-space coordinates
- */
-inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const {
-
- // Maximum bounds
- max.x = mRadius + mMargin;
- max.y = mHalfHeight + mMargin;
- max.z = max.x;
-
- // Minimum bounds
- min.x = -max.x;
- min.y = -max.y;
- min.z = min.x;
-}
-
-// Return the local inertia tensor of the cylinder
-/**
- * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
- * coordinates
- * @param mass Mass to use to compute the inertia tensor of the collision shape
- */
-inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
- decimal height = decimal(2.0) * mHalfHeight;
- decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height);
- tensor.setAllValues(diag, 0.0, 0.0, 0.0,
- decimal(0.5) * mass * mRadius * mRadius, 0.0,
- 0.0, 0.0, diag);
-}
-
-// 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);
-}
-
-}
-
-#endif
-
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index 48658f5c..6df4bf4e 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -24,15 +24,17 @@
********************************************************************************/
// Libraries
+#include <cassert>
#include "OverlappingPair.h"
using namespace reactphysics3d;
-
// Constructor
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
int nbMaxContactManifolds, PoolAllocator& memoryAllocator)
: mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds),
mCachedSeparatingAxis(0.0, 1.0, 0.0) {
+ assert(static_cast<uint>(shape1->getCollisionShape()->getType()) <=
+ static_cast<uint>(shape2->getCollisionShape()->getType()));
}
diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h
index 360cf229..28f7b636 100644
--- a/src/mathematics/Quaternion.h
+++ b/src/mathematics/Quaternion.h
@@ -199,7 +199,7 @@ inline Vector3 Quaternion::getVectorV() const {
// Return the length of the quaternion (inline)
inline decimal Quaternion::length() const {
- return sqrt(x*x + y*y + z*z + w*w);
+ return std::sqrt(x*x + y*y + z*z + w*w);
}
// Return the square of the length of the quaternion
diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h
index 6f1e42f4..a1bef6dc 100644
--- a/src/reactphysics3d.h
+++ b/src/reactphysics3d.h
@@ -46,8 +46,6 @@
#include "collision/shapes/CollisionShape.h"
#include "collision/shapes/BoxShape.h"
#include "collision/shapes/SphereShape.h"
-#include "collision/shapes/ConeShape.h"
-#include "collision/shapes/CylinderShape.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/ConvexMeshShape.h"
#include "collision/shapes/ConcaveMeshShape.h"
diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h
index 94d44a98..242ebcf9 100644
--- a/test/tests/collision/TestCollisionWorld.h
+++ b/test/tests/collision/TestCollisionWorld.h
@@ -48,8 +48,6 @@ class WorldCollisionCallback : public CollisionCallback
public:
bool boxCollideWithSphere1;
- bool boxCollideWithCylinder;
- bool sphere1CollideWithCylinder;
bool sphere1CollideWithSphere2;
CollisionBody* boxBody;
@@ -65,8 +63,6 @@ class WorldCollisionCallback : public CollisionCallback
void reset()
{
boxCollideWithSphere1 = false;
- boxCollideWithCylinder = false;
- sphere1CollideWithCylinder = false;
sphere1CollideWithSphere2 = false;
}
@@ -76,12 +72,6 @@ class WorldCollisionCallback : public CollisionCallback
if (isContactBetweenBodies(boxBody, sphere1Body, collisionCallbackInfo)) {
boxCollideWithSphere1 = true;
}
- else if (isContactBetweenBodies(boxBody, cylinderBody, collisionCallbackInfo)) {
- boxCollideWithCylinder = true;
- }
- else if (isContactBetweenBodies(sphere1Body, cylinderBody, collisionCallbackInfo)) {
- sphere1CollideWithCylinder = true;
- }
else if (isContactBetweenBodies(sphere1Body, sphere2Body, collisionCallbackInfo)) {
sphere1CollideWithSphere2 = true;
}
@@ -118,7 +108,6 @@ class TestCollisionWorld : public Test {
// Collision shapes
BoxShape* mBoxShape;
SphereShape* mSphereShape;
- CylinderShape* mCylinderShape;
// Proxy shapes
ProxyShape* mBoxProxyShape;
@@ -154,11 +143,6 @@ class TestCollisionWorld : public Test {
mSphere2Body = mWorld->createCollisionBody(sphere2Transform);
mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity());
- Transform cylinderTransform(Vector3(10, -5, 0), Quaternion::identity());
- mCylinderBody = mWorld->createCollisionBody(cylinderTransform);
- mCylinderShape = new CylinderShape(2, 5);
- mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, Transform::identity());
-
// Assign collision categories to proxy shapes
mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1);
mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1);
@@ -175,7 +159,6 @@ class TestCollisionWorld : public Test {
~TestCollisionWorld() {
delete mBoxShape;
delete mSphereShape;
- delete mCylinderShape;
}
/// Run the tests
@@ -189,8 +172,6 @@ class TestCollisionWorld : public Test {
mCollisionCallback.reset();
mWorld->testCollision(&mCollisionCallback);
test(mCollisionCallback.boxCollideWithSphere1);
- test(mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithSphere2);
test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body));
@@ -206,22 +187,16 @@ class TestCollisionWorld : public Test {
mCollisionCallback.reset();
mWorld->testCollision(mCylinderBody, &mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
- test(mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithSphere2);
mCollisionCallback.reset();
mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback);
test(mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithSphere2);
mCollisionCallback.reset();
mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
- test(mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithSphere2);
// Move sphere 1 to collide with sphere 2
@@ -230,22 +205,16 @@ class TestCollisionWorld : public Test {
mCollisionCallback.reset();
mWorld->testCollision(&mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
- test(mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
test(mCollisionCallback.sphere1CollideWithSphere2);
mCollisionCallback.reset();
mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithSphere2);
mCollisionCallback.reset();
mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
- test(mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithSphere2);
// Move sphere 1 to collide with box
@@ -260,8 +229,6 @@ class TestCollisionWorld : public Test {
mSphere2Body->setIsActive(false);
mWorld->testCollision(&mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithSphere2);
test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body));
@@ -289,8 +256,6 @@ class TestCollisionWorld : public Test {
mCollisionCallback.reset();
mWorld->testCollision(&mCollisionCallback);
test(mCollisionCallback.boxCollideWithSphere1);
- test(mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithSphere2);
// Move sphere 1 to collide with sphere 2
@@ -299,8 +264,6 @@ class TestCollisionWorld : public Test {
mCollisionCallback.reset();
mWorld->testCollision(&mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
- test(mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
test(mCollisionCallback.sphere1CollideWithSphere2);
mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2);
@@ -311,8 +274,6 @@ class TestCollisionWorld : public Test {
mCollisionCallback.reset();
mWorld->testCollision(&mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.boxCollideWithCylinder);
- test(!mCollisionCallback.sphere1CollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithSphere2);
// Move sphere 1 to collide with box
diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h
index 68345739..dd2a793b 100644
--- a/test/tests/collision/TestPointInside.h
+++ b/test/tests/collision/TestPointInside.h
@@ -31,9 +31,7 @@
#include "collision/shapes/BoxShape.h"
#include "collision/shapes/SphereShape.h"
#include "collision/shapes/CapsuleShape.h"
-#include "collision/shapes/ConeShape.h"
#include "collision/shapes/ConvexMeshShape.h"
-#include "collision/shapes/CylinderShape.h"
/// Reactphysics3D namespace
namespace reactphysics3d {
@@ -65,10 +63,8 @@ class TestPointInside : public Test {
BoxShape* mBoxShape;
SphereShape* mSphereShape;
CapsuleShape* mCapsuleShape;
- ConeShape* mConeShape;
ConvexMeshShape* mConvexMeshShape;
ConvexMeshShape* mConvexMeshShapeBodyEdgesInfo;
- CylinderShape* mCylinderShape;
// Transform
Transform mBodyTransform;
@@ -128,9 +124,6 @@ class TestPointInside : public Test {
mCapsuleShape = new CapsuleShape(2, 10);
mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform);
- mConeShape = new ConeShape(2, 6, 0);
- mConeProxyShape = mConeBody->addCollisionShape(mConeShape, mShapeTransform);
-
mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4)
mConvexMeshShape->addVertex(Vector3(-2, -3, -4));
mConvexMeshShape->addVertex(Vector3(2, -3, -4));
@@ -168,15 +161,12 @@ class TestPointInside : public Test {
mConvexMeshShapeBodyEdgesInfo,
mShapeTransform);
- mCylinderShape = new CylinderShape(3, 8, 0);
- mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, mShapeTransform);
-
- // Compound shape is a cylinder and a sphere
+ // Compound shape is a capsule and a sphere
Vector3 positionShape2(Vector3(4, 2, -3));
Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13);
Transform shapeTransform2(positionShape2, orientationShape2);
mLocalShape2ToWorld = mBodyTransform * shapeTransform2;
- mCompoundBody->addCollisionShape(mCylinderShape, mShapeTransform);
+ mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform);
mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2);
}
@@ -185,10 +175,8 @@ class TestPointInside : public Test {
delete mBoxShape;
delete mSphereShape;
delete mCapsuleShape;
- delete mConeShape;
delete mConvexMeshShape;
delete mConvexMeshShapeBodyEdgesInfo;
- delete mCylinderShape;
}
/// Run the tests
@@ -196,9 +184,7 @@ class TestPointInside : public Test {
testBox();
testSphere();
testCapsule();
- testCone();
testConvexMesh();
- testCylinder();
testCompound();
}
@@ -407,83 +393,6 @@ class TestPointInside : public Test {
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7)));
}
- /// Test the ProxyConeShape::testPointInside() and
- /// CollisionBody::testPointInside() methods
- void testCone() {
-
- // Tests with CollisionBody
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4)));
- test(mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4)));
-
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5)));
- test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5)));
-
- // Tests with ProxyConeShape
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4)));
- test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4)));
-
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5)));
- test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5)));
- }
-
/// Test the ProxyConvexMeshShape::testPointInside() and
/// CollisionBody::testPointInside() methods
void testConvexMesh() {
@@ -597,127 +506,11 @@ class TestPointInside : public Test {
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
}
- /// Test the ProxyCylinderShape::testPointInside() and
- /// CollisionBody::testPointInside() methods
- void testCylinder() {
-
- // Tests with CollisionBody
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7)));
- test(mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7)));
-
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2)));
- test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2)));
-
- // Tests with ProxyCylinderShape
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7)));
- test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7)));
-
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2)));
- test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2)));
- }
-
/// Test the CollisionBody::testPointInside() method
void testCompound() {
- // Points on the cylinder
+ // Points on the capsule
+ // TODO : Previous it was a cylinder (not a capsule). Maybe those tests are wrong now
test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0)));
test(mCompoundBody->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0)));
diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h
index 7295a8d9..b39b751b 100644
--- a/test/tests/collision/TestRaycast.h
+++ b/test/tests/collision/TestRaycast.h
@@ -33,9 +33,7 @@
#include "collision/shapes/BoxShape.h"
#include "collision/shapes/SphereShape.h"
#include "collision/shapes/CapsuleShape.h"
-#include "collision/shapes/ConeShape.h"
#include "collision/shapes/ConvexMeshShape.h"
-#include "collision/shapes/CylinderShape.h"
#include "collision/shapes/TriangleShape.h"
#include "collision/shapes/ConcaveMeshShape.h"
#include "collision/shapes/HeightFieldShape.h"
@@ -130,10 +128,8 @@ class TestRaycast : public Test {
BoxShape* mBoxShape;
SphereShape* mSphereShape;
CapsuleShape* mCapsuleShape;
- ConeShape* mConeShape;
ConvexMeshShape* mConvexMeshShape;
ConvexMeshShape* mConvexMeshShapeEdgesInfo;
- CylinderShape* mCylinderShape;
TriangleShape* mTriangleShape;
ConcaveShape* mConcaveMeshShape;
HeightFieldShape* mHeightFieldShape;
@@ -214,9 +210,6 @@ class TestRaycast : public Test {
mCapsuleShape = new CapsuleShape(2, 5);
mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform);
- mConeShape = new ConeShape(2, 6, 0);
- mConeProxyShape = mConeBody->addCollisionShape(mConeShape, mShapeTransform);
-
// Box of dimension (2, 3, 4)
mConvexMeshShape = new ConvexMeshShape(0.0);
mConvexMeshShape->addVertex(Vector3(-2, -3, -4));
@@ -255,15 +248,12 @@ class TestRaycast : public Test {
mConvexMeshShapeEdgesInfo,
mShapeTransform);
- mCylinderShape = new CylinderShape(2, 5, 0);
- mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, mShapeTransform);
-
// Compound shape is a cylinder and a sphere
Vector3 positionShape2(Vector3(4, 2, -3));
Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13);
Transform shapeTransform2(positionShape2, orientationShape2);
mLocalShape2ToWorld = mBodyTransform * shapeTransform2;
- mCompoundCylinderProxyShape = mCompoundBody->addCollisionShape(mCylinderShape, mShapeTransform);
+ mCompoundCylinderProxyShape = mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform);
mCompoundSphereProxyShape = mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2);
// Concave Mesh shape
@@ -328,10 +318,8 @@ class TestRaycast : public Test {
delete mBoxShape;
delete mSphereShape;
delete mCapsuleShape;
- delete mConeShape;
delete mConvexMeshShape;
delete mConvexMeshShapeEdgesInfo;
- delete mCylinderShape;
delete mTriangleShape;
delete mConcaveMeshShape;
delete mHeightFieldShape;
@@ -344,9 +332,7 @@ class TestRaycast : public Test {
testBox();
testSphere();
testCapsule();
- testCone();
testConvexMesh();
- testCylinder();
testCompound();
testTriangle();
testConcaveMesh();
@@ -1313,253 +1299,6 @@ class TestRaycast : public Test {
mWorld->raycast(Ray(ray6Back.point1, ray6Back.point2, decimal(0.8)), &mCallback);
}
- /// Test the ProxyConeShape::raycast(), CollisionBody::raycast() and
- /// CollisionWorld::raycast() methods.
- void testCone() {
-
- // ----- Test feedback data ----- //
- Vector3 point1A = mLocalShapeToWorld * Vector3(0 , 0, 3);
- Vector3 point1B = mLocalShapeToWorld * Vector3(0, 0, -7);
- Ray ray(point1A, point1B);
- Vector3 hitPoint = mLocalShapeToWorld * Vector3(0, 0, 1);
-
- Vector3 point2A = mLocalShapeToWorld * Vector3(1 , -5, 0);
- Vector3 point2B = mLocalShapeToWorld * Vector3(1, 5, 0);
- Ray rayBottom(point2A, point2B);
- Vector3 hitPoint2 = mLocalShapeToWorld * Vector3(1, -3, 0);
-
- mCallback.shapeToTest = mConeProxyShape;
-
- // CollisionWorld::raycast()
- mCallback.reset();
- mWorld->raycast(ray, &mCallback);
- test(mCallback.isHit);
- test(mCallback.raycastInfo.body == mConeBody);
- test(mCallback.raycastInfo.proxyShape == mConeProxyShape);
- test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(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, CATEGORY2);
- test(mCallback.isHit);
-
- // Wrong category filter mask
- mCallback.reset();
- mWorld->raycast(ray, &mCallback, CATEGORY1);
- test(!mCallback.isHit);
-
- // CollisionBody::raycast()
- RaycastInfo raycastInfo2;
- test(mConeBody->raycast(ray, raycastInfo2));
- test(raycastInfo2.body == mConeBody);
- test(raycastInfo2.proxyShape == mConeProxyShape);
- test(approxEqual(raycastInfo2.hitFraction, decimal(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(mConeProxyShape->raycast(ray, raycastInfo3));
- test(raycastInfo3.body == mConeBody);
- test(raycastInfo3.proxyShape == mConeProxyShape);
- test(approxEqual(raycastInfo3.hitFraction, decimal(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));
-
- mCallback.reset();
- mWorld->raycast(rayBottom, &mCallback);
- test(mCallback.isHit);
- test(mCallback.raycastInfo.body == mConeBody);
- test(mCallback.raycastInfo.proxyShape == mConeProxyShape);
- test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(0.2), epsilon));
- test(approxEqual(mCallback.raycastInfo.worldPoint.x, hitPoint2.x, epsilon));
- test(approxEqual(mCallback.raycastInfo.worldPoint.y, hitPoint2.y, epsilon));
- test(approxEqual(mCallback.raycastInfo.worldPoint.z, hitPoint2.z, epsilon));
-
- // CollisionBody::raycast()
- RaycastInfo raycastInfo5;
- test(mConeBody->raycast(rayBottom, raycastInfo5));
- test(raycastInfo5.body == mConeBody);
- test(raycastInfo5.proxyShape == mConeProxyShape);
- test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon));
- test(approxEqual(raycastInfo5.worldPoint.x, hitPoint2.x, epsilon));
- test(approxEqual(raycastInfo5.worldPoint.y, hitPoint2.y, epsilon));
- test(approxEqual(raycastInfo5.worldPoint.z, hitPoint2.z, epsilon));
-
- // ProxyCollisionShape::raycast()
- RaycastInfo raycastInfo6;
- test(mConeProxyShape->raycast(rayBottom, raycastInfo6));
- test(raycastInfo6.body == mConeBody);
- test(raycastInfo6.proxyShape == mConeProxyShape);
- test(approxEqual(raycastInfo6.hitFraction, decimal(0.2), epsilon));
- test(approxEqual(raycastInfo6.worldPoint.x, hitPoint2.x, epsilon));
- test(approxEqual(raycastInfo6.worldPoint.y, hitPoint2.y, epsilon));
- test(approxEqual(raycastInfo6.worldPoint.z, hitPoint2.z, epsilon));
-
- Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1));
- Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28));
- Ray ray3(mLocalShapeToWorld * Vector3(-1, -2, 1), mLocalShapeToWorld * Vector3(-13, -2, 22));
- Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31));
- Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -1), mLocalShapeToWorld * Vector3(-26, 1, -1));
- Ray ray6(mLocalShapeToWorld * Vector3(3, 4, 1), mLocalShapeToWorld * Vector3(3, -16, 1));
- Ray ray7(mLocalShapeToWorld * Vector3(1, -4, 3), mLocalShapeToWorld * Vector3(1, -4, -17));
- Ray ray8(mLocalShapeToWorld * Vector3(-4, 4, 0), mLocalShapeToWorld * Vector3(26, 4, 0));
- Ray ray9(mLocalShapeToWorld * Vector3(0, -4, -7), mLocalShapeToWorld * Vector3(0, 46, -7));
- Ray ray10(mLocalShapeToWorld * Vector3(-3, -2, -6), mLocalShapeToWorld * Vector3(-3, -2, 74));
- Ray ray11(mLocalShapeToWorld * Vector3(3, -1, 0.5), mLocalShapeToWorld * Vector3(-27, -1, 0.5));
- Ray ray12(mLocalShapeToWorld * Vector3(1, 4, -1), mLocalShapeToWorld * Vector3(1, -26, -1));
- Ray ray13(mLocalShapeToWorld * Vector3(-1, -2, 3), mLocalShapeToWorld * Vector3(-1, -2, -27));
- Ray ray14(mLocalShapeToWorld * Vector3(-2, 0, 0.8), mLocalShapeToWorld * Vector3(30, 0, 0.8));
- Ray ray15(mLocalShapeToWorld * Vector3(0, -4, 1), mLocalShapeToWorld * Vector3(0, 30, 1));
- Ray ray16(mLocalShapeToWorld * Vector3(-0.9, 0, -4), mLocalShapeToWorld * Vector3(-0.9, 0, 30));
-
- // ----- Test raycast miss ----- //
- test(!mConeBody->raycast(ray1, raycastInfo3));
- test(!mConeProxyShape->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(!mConeBody->raycast(ray2, raycastInfo3));
- test(!mConeProxyShape->raycast(ray2, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray2, &mCallback);
- test(!mCallback.isHit);
-
- test(!mConeBody->raycast(ray3, raycastInfo3));
- test(!mConeProxyShape->raycast(ray3, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray3, &mCallback);
- test(!mCallback.isHit);
-
- test(!mConeBody->raycast(ray4, raycastInfo3));
- test(!mConeProxyShape->raycast(ray4, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray4, &mCallback);
- test(!mCallback.isHit);
-
- test(!mConeBody->raycast(ray5, raycastInfo3));
- test(!mConeProxyShape->raycast(ray5, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray5, &mCallback);
- test(!mCallback.isHit);
-
- test(!mConeBody->raycast(ray6, raycastInfo3));
- test(!mConeProxyShape->raycast(ray6, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray6, &mCallback);
- test(!mCallback.isHit);
-
- test(!mConeBody->raycast(ray7, raycastInfo3));
- test(!mConeProxyShape->raycast(ray7, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray7, &mCallback);
- test(!mCallback.isHit);
-
- test(!mConeBody->raycast(ray8, raycastInfo3));
- test(!mConeProxyShape->raycast(ray8, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray8, &mCallback);
- test(!mCallback.isHit);
-
- test(!mConeBody->raycast(ray9, raycastInfo3));
- test(!mConeProxyShape->raycast(ray9, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray9, &mCallback);
- test(!mCallback.isHit);
-
- test(!mConeBody->raycast(ray10, raycastInfo3));
- test(!mConeProxyShape->raycast(ray10, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray10, &mCallback);
- test(!mCallback.isHit);
-
- mCallback.reset();
- mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
-
- // ----- Test raycast hits ----- //
- test(mConeBody->raycast(ray11, raycastInfo3));
- test(mConeProxyShape->raycast(ray11, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray11, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
-
- test(mConeBody->raycast(ray12, raycastInfo3));
- test(mConeProxyShape->raycast(ray12, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray12, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
-
- test(mConeBody->raycast(ray13, raycastInfo3));
- test(mConeProxyShape->raycast(ray13, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray13, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
-
- test(mConeBody->raycast(ray14, raycastInfo3));
- test(mConeProxyShape->raycast(ray14, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray14, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
-
- test(mConeBody->raycast(ray15, raycastInfo3));
- test(mConeProxyShape->raycast(ray15, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray15, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
-
- test(mConeBody->raycast(ray16, raycastInfo3));
- test(mConeProxyShape->raycast(ray16, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray16, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
- }
-
/// Test the ProxyConvexMeshShape::raycast(), CollisionBody::raycast() and
/// CollisionWorld::raycast() methods.
void testConvexMesh() {
@@ -1824,248 +1563,6 @@ class TestRaycast : public Test {
test(mCallback.isHit);
}
- /// Test the ProxyCylinderShape::raycast(), CollisionBody::raycast() and
- /// CollisionWorld::raycast() methods.
- void testCylinder() {
-
- // ----- Test feedback data ----- //
- Vector3 point1A = mLocalShapeToWorld * Vector3(4 , 1, 0);
- Vector3 point1B = mLocalShapeToWorld * Vector3(-6, 1, 0);
- Ray ray(point1A, point1B);
- Vector3 hitPoint = mLocalShapeToWorld * Vector3(2, 1, 0);
-
- Vector3 point2A = mLocalShapeToWorld * Vector3(0 , 4.5, 0);
- Vector3 point2B = mLocalShapeToWorld * Vector3(0, -5.5, 0);
- Ray rayTop(point2A, point2B);
- Vector3 hitPointTop = mLocalShapeToWorld * Vector3(0, decimal(2.5), 0);
-
- Vector3 point3A = mLocalShapeToWorld * Vector3(0 , -4.5, 0);
- Vector3 point3B = mLocalShapeToWorld * Vector3(0, 5.5, 0);
- Ray rayBottom(point3A, point3B);
- Vector3 hitPointBottom = mLocalShapeToWorld * Vector3(0, decimal(-2.5), 0);
-
- mCallback.shapeToTest = mCylinderProxyShape;
-
- // CollisionWorld::raycast()
- mCallback.reset();
- mWorld->raycast(ray, &mCallback);
- test(mCallback.isHit);
- test(mCallback.raycastInfo.body == mCylinderBody);
- test(mCallback.raycastInfo.proxyShape == mCylinderProxyShape);
- test(approxEqual(mCallback.raycastInfo.hitFraction, decimal(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, CATEGORY2);
- test(mCallback.isHit);
-
- // Wrong category filter mask
- mCallback.reset();
- mWorld->raycast(ray, &mCallback, CATEGORY1);
- test(!mCallback.isHit);
-
- // CollisionBody::raycast()
- RaycastInfo raycastInfo2;
- test(mCylinderBody->raycast(ray, raycastInfo2));
- test(raycastInfo2.body == mCylinderBody);
- test(raycastInfo2.proxyShape == mCylinderProxyShape);
- test(approxEqual(raycastInfo2.hitFraction, decimal(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(mCylinderProxyShape->raycast(ray, raycastInfo3));
- test(raycastInfo3.body == mCylinderBody);
- test(raycastInfo3.proxyShape == mCylinderProxyShape);
- test(approxEqual(raycastInfo3.hitFraction, decimal(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));
-
- // ProxyCollisionShape::raycast()
- RaycastInfo raycastInfo5;
- test(mCylinderProxyShape->raycast(rayTop, raycastInfo5));
- test(raycastInfo5.body == mCylinderBody);
- test(raycastInfo5.proxyShape == mCylinderProxyShape);
- test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon));
- test(approxEqual(raycastInfo5.worldPoint.x, hitPointTop.x, epsilon));
- test(approxEqual(raycastInfo5.worldPoint.y, hitPointTop.y, epsilon));
- test(approxEqual(raycastInfo5.worldPoint.z, hitPointTop.z, epsilon));
-
- // ProxyCollisionShape::raycast()
- RaycastInfo raycastInfo6;
- test(mCylinderProxyShape->raycast(rayBottom, raycastInfo6));
- test(raycastInfo6.body == mCylinderBody);
- test(raycastInfo6.proxyShape == mCylinderProxyShape);
- test(approxEqual(raycastInfo6.hitFraction, decimal(0.2), epsilon));
- test(approxEqual(raycastInfo6.worldPoint.x, hitPointBottom.x, epsilon));
- test(approxEqual(raycastInfo6.worldPoint.y, hitPointBottom.y, epsilon));
- test(approxEqual(raycastInfo6.worldPoint.z, hitPointBottom.z, epsilon));
-
- Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1));
- Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 20, 28));
- Ray ray3(mLocalShapeToWorld * Vector3(1, 3, -1), mLocalShapeToWorld * Vector3(-11,3, 20));
- Ray ray4(mLocalShapeToWorld * Vector3(10, 10, 10), mLocalShapeToWorld * Vector3(22, 28, 31));
- Ray ray5(mLocalShapeToWorld * Vector3(4, 1, -5), mLocalShapeToWorld * Vector3(-30, 1, -5));
- Ray ray6(mLocalShapeToWorld * Vector3(4, 9, 1), mLocalShapeToWorld * Vector3(4, -30, 1));
- Ray ray7(mLocalShapeToWorld * Vector3(1, -9, 5), mLocalShapeToWorld * Vector3(1, -9, -30));
- Ray ray8(mLocalShapeToWorld * Vector3(-4, 9, 0), mLocalShapeToWorld * Vector3(30, 9, 0));
- Ray ray9(mLocalShapeToWorld * Vector3(0, -9, -4), mLocalShapeToWorld * Vector3(0, 30, -4));
- Ray ray10(mLocalShapeToWorld * Vector3(-4, 0, -6), mLocalShapeToWorld * Vector3(-4, 0, 30));
- Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1, 1.5));
- Ray ray12(mLocalShapeToWorld * Vector3(1, 9, -1), mLocalShapeToWorld * Vector3(1, -30, -1));
- Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30));
- Ray ray14(mLocalShapeToWorld * Vector3(-3, 2, -1.7), mLocalShapeToWorld * Vector3(30, 2, -1.7));
- Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1));
- Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30));
-
- // ----- Test raycast miss ----- //
- test(!mCylinderBody->raycast(ray1, raycastInfo3));
- test(!mCylinderProxyShape->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(!mCylinderBody->raycast(ray2, raycastInfo3));
- test(!mCylinderProxyShape->raycast(ray2, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray2, &mCallback);
- test(!mCallback.isHit);
-
- test(!mCylinderBody->raycast(ray3, raycastInfo3));
- test(!mCylinderProxyShape->raycast(ray3, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray3, &mCallback);
- test(!mCallback.isHit);
-
- test(!mCylinderBody->raycast(ray4, raycastInfo3));
- test(!mCylinderProxyShape->raycast(ray4, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray4, &mCallback);
- test(!mCallback.isHit);
-
- test(!mCylinderBody->raycast(ray5, raycastInfo3));
- test(!mCylinderProxyShape->raycast(ray5, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray5, &mCallback);
- test(!mCallback.isHit);
-
- test(!mCylinderBody->raycast(ray6, raycastInfo3));
- test(!mCylinderProxyShape->raycast(ray6, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray6, &mCallback);
- test(!mCallback.isHit);
-
- test(!mCylinderBody->raycast(ray7, raycastInfo3));
- test(!mCylinderProxyShape->raycast(ray7, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray7, &mCallback);
- test(!mCallback.isHit);
-
- test(!mCylinderBody->raycast(ray8, raycastInfo3));
- test(!mCylinderProxyShape->raycast(ray8, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray8, &mCallback);
- test(!mCallback.isHit);
-
- test(!mCylinderBody->raycast(ray9, raycastInfo3));
- test(!mCylinderProxyShape->raycast(ray9, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray9, &mCallback);
- test(!mCallback.isHit);
-
- test(!mCylinderBody->raycast(ray10, raycastInfo3));
- test(!mCylinderProxyShape->raycast(ray10, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray10, &mCallback);
- test(!mCallback.isHit);
-
- mCallback.reset();
- mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.01)), &mCallback);
- test(!mCallback.isHit);
-
- // ----- Test raycast hits ----- //
- test(mCylinderBody->raycast(ray11, raycastInfo3));
- test(mCylinderProxyShape->raycast(ray11, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray11, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray11.point1, ray11.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
-
- test(mCylinderBody->raycast(ray12, raycastInfo3));
- test(mCylinderProxyShape->raycast(ray12, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray12, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray12.point1, ray12.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
-
- test(mCylinderBody->raycast(ray13, raycastInfo3));
- test(mCylinderProxyShape->raycast(ray13, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray13, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray13.point1, ray13.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
-
- test(mCylinderBody->raycast(ray14, raycastInfo3));
- test(mCylinderProxyShape->raycast(ray14, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray14, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray14.point1, ray14.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
-
- test(mCylinderBody->raycast(ray15, raycastInfo3));
- test(mCylinderProxyShape->raycast(ray15, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray15, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray15.point1, ray15.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
-
- test(mCylinderBody->raycast(ray16, raycastInfo3));
- test(mCylinderProxyShape->raycast(ray16, raycastInfo3));
- mCallback.reset();
- mWorld->raycast(ray16, &mCallback);
- test(mCallback.isHit);
- mCallback.reset();
- mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback);
- test(mCallback.isHit);
- }
-
/// Test the CollisionBody::raycast() and
/// CollisionWorld::raycast() methods.
void testCompound() {
@@ -2141,7 +1638,8 @@ class TestRaycast : public Test {
mWorld->raycast(Ray(ray6.point1, ray6.point2, decimal(0.8)), &mCallback);
test(mCallback.isHit);
- // Raycast hit agains the cylinder shape
+ // Raycast hit agains the capsule shape
+ // TODO : Previous it was a cylinder, now it is a capsule shape, maybe those tests are wrong now
Ray ray11(mLocalShapeToWorld * Vector3(4, 1, 1.5), mLocalShapeToWorld * Vector3(-30, 1.5, 2));
Ray ray12(mLocalShapeToWorld * Vector3(1.5, 9, -1), mLocalShapeToWorld * Vector3(1.5, -30, -1));
Ray ray13(mLocalShapeToWorld * Vector3(-1, 2, 3), mLocalShapeToWorld * Vector3(-1, 2, -30));
diff --git a/testbed/CMakeLists.txt b/testbed/CMakeLists.txt
index 8d402ad3..a5620028 100644
--- a/testbed/CMakeLists.txt
+++ b/testbed/CMakeLists.txt
@@ -74,8 +74,6 @@ SET(TESTBED_SOURCES
SET(COMMON_SOURCES
common/Box.h
common/Box.cpp
- common/Cone.h
- common/Cone.cpp
common/Sphere.h
common/Sphere.cpp
common/Line.h
@@ -86,8 +84,6 @@ SET(COMMON_SOURCES
common/ConvexMesh.cpp
common/ConcaveMesh.h
common/ConcaveMesh.cpp
- common/Cylinder.h
- common/Cylinder.cpp
common/Dumbbell.h
common/Dumbbell.cpp
common/HeightField.h
diff --git a/testbed/common/Cone.cpp b/testbed/common/Cone.cpp
deleted file mode 100644
index bbd32ed9..00000000
--- a/testbed/common/Cone.cpp
+++ /dev/null
@@ -1,272 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-// Libraries
-#include "Cone.h"
-
-openglframework::VertexBufferObject Cone::mVBOVertices(GL_ARRAY_BUFFER);
-openglframework::VertexBufferObject Cone::mVBONormals(GL_ARRAY_BUFFER);
-openglframework::VertexBufferObject Cone::mVBOTextureCoords(GL_ARRAY_BUFFER);
-openglframework::VertexBufferObject Cone::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER);
-openglframework::VertexArrayObject Cone::mVAO;
-int Cone::totalNbCones = 0;
-
-// Constructor
-Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world,
- const std::string& meshFolderPath)
- : PhysicsObject(meshFolderPath + "cone.obj"), mRadius(radius), mHeight(height) {
-
- // Compute the scaling matrix
- mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
- 0, mHeight, 0, 0,
- 0, 0, mRadius, 0,
- 0, 0, 0, 1);
-
- // Initialize the position where the cone will be rendered
- translateWorld(position);
-
- // Create the collision shape for the rigid body (cone shape) and do
- // not forget to delete it at the end
- mConeShape = new rp3d::ConeShape(mRadius, mHeight);
-
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- mPreviousTransform = transform;
-
- // Create a rigid body corresponding to the cone in the dynamics world
- mBody = world->createCollisionBody(transform);
-
- // Add a collision shape to the body and specify the mass of the shape
- mProxyShape = mBody->addCollisionShape(mConeShape, rp3d::Transform::identity());
-
- mTransformMatrix = mTransformMatrix * mScalingMatrix;
-
- // Create the VBOs and VAO
- if (totalNbCones == 0) {
- createVBOAndVAO();
- }
-
- totalNbCones++;
-}
-
-// Constructor
-Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
- float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
- const std::string& meshFolderPath)
- : PhysicsObject(meshFolderPath + "cone.obj"), mRadius(radius), mHeight(height) {
-
- // Compute the scaling matrix
- mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
- 0, mHeight, 0, 0,
- 0, 0, mRadius, 0,
- 0, 0, 0, 1);
-
- // Initialize the position where the cone will be rendered
- translateWorld(position);
-
- // Create the collision shape for the rigid body (cone shape) and do not
- // forget to delete it at the end
- mConeShape = new rp3d::ConeShape(mRadius, mHeight);
-
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- // Create a rigid body corresponding to the cone in the dynamics world
- rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
-
- // Add a collision shape to the body and specify the mass of the shape
- mProxyShape = body->addCollisionShape(mConeShape, rp3d::Transform::identity(), mass);
-
- mBody = body;
-
- mTransformMatrix = mTransformMatrix * mScalingMatrix;
-
- // Create the VBOs and VAO
- if (totalNbCones == 0) {
- createVBOAndVAO();
- }
-
- totalNbCones++;
-}
-
-// Destructor
-Cone::~Cone() {
-
- if (totalNbCones == 1) {
- // Destroy the mesh
- destroy();
-
- // Destroy the VBOs and VAO
- mVBOIndices.destroy();
- mVBOVertices.destroy();
- mVBONormals.destroy();
- mVBOTextureCoords.destroy();
- mVAO.destroy();
- }
- delete mConeShape;
- totalNbCones--;
-}
-
-// Render the cone at the correct position and with the correct orientation
-void Cone::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
- }
-
- // Bind the shader
- shader.bind();
-
- // Set the model to camera matrix
- shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix);
- shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
-
- // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
- // model-view matrix)
- const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix;
- const openglframework::Matrix3 normalMatrix =
- localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
- shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
-
- // Set the vertex color
- openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor;
- openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a);
- shader.setVector4Uniform("vertexColor", color, false);
-
- // Bind the VAO
- mVAO.bind();
-
- mVBOVertices.bind();
-
- // Get the location of shader attribute variables
- GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
- GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false);
-
- glEnableVertexAttribArray(vertexPositionLoc);
- glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr);
-
- mVBONormals.bind();
-
- if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr);
- if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc);
-
- // For each part of the mesh
- for (unsigned int i=0; i<getNbParts(); i++) {
- glDrawElements(GL_TRIANGLES, getNbFaces(i) * 3, GL_UNSIGNED_INT, (char*)nullptr);
- }
-
- glDisableVertexAttribArray(vertexPositionLoc);
- if (vertexNormalLoc != -1) glDisableVertexAttribArray(vertexNormalLoc);
-
- mVBONormals.unbind();
- mVBOVertices.unbind();
-
- // Unbind the VAO
- mVAO.unbind();
-
- // Unbind the shader
- shader.unbind();
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
- }
-}
-
-// Create the Vertex Buffer Objects used to render with OpenGL.
-/// We create two VBOs (one for vertices and one for indices)
-void Cone::createVBOAndVAO() {
-
- // Create the VBO for the vertices data
- mVBOVertices.create();
- mVBOVertices.bind();
- size_t sizeVertices = mVertices.size() * sizeof(openglframework::Vector3);
- mVBOVertices.copyDataIntoVBO(sizeVertices, getVerticesPointer(), GL_STATIC_DRAW);
- mVBOVertices.unbind();
-
- // Create the VBO for the normals data
- mVBONormals.create();
- mVBONormals.bind();
- size_t sizeNormals = mNormals.size() * sizeof(openglframework::Vector3);
- mVBONormals.copyDataIntoVBO(sizeNormals, getNormalsPointer(), GL_STATIC_DRAW);
- mVBONormals.unbind();
-
- if (hasTexture()) {
- // Create the VBO for the texture co data
- mVBOTextureCoords.create();
- mVBOTextureCoords.bind();
- size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::Vector2);
- mVBOTextureCoords.copyDataIntoVBO(sizeTextureCoords, getUVTextureCoordinatesPointer(), GL_STATIC_DRAW);
- mVBOTextureCoords.unbind();
- }
-
- // Create th VBO for the indices data
- mVBOIndices.create();
- mVBOIndices.bind();
- size_t sizeIndices = mIndices[0].size() * sizeof(unsigned int);
- mVBOIndices.copyDataIntoVBO(sizeIndices, getIndicesPointer(), GL_STATIC_DRAW);
- mVBOIndices.unbind();
-
- // Create the VAO for both VBOs
- mVAO.create();
- mVAO.bind();
-
- // Bind the VBO of vertices
- mVBOVertices.bind();
-
- // Bind the VBO of normals
- mVBONormals.bind();
-
- if (hasTexture()) {
- // Bind the VBO of texture coords
- mVBOTextureCoords.bind();
- }
-
- // Bind the VBO of indices
- mVBOIndices.bind();
-
- // Unbind the VAO
- mVAO.unbind();
-}
-
-// Set the scaling of the object
-void Cone::setScaling(const openglframework::Vector3& scaling) {
-
- // Scale the collision shape
- mProxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z));
-
- // Scale the graphics object
- mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0,
- 0, mHeight * scaling.y, 0, 0,
- 0, 0, mRadius * scaling.z, 0,
- 0, 0, 0, 1);
-}
-
diff --git a/testbed/common/Cone.h b/testbed/common/Cone.h
deleted file mode 100644
index 8f71493c..00000000
--- a/testbed/common/Cone.h
+++ /dev/null
@@ -1,110 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef CONE_H
-#define CONE_H
-
-// Libraries
-#include "openglframework.h"
-#include "reactphysics3d.h"
-#include "PhysicsObject.h"
-
-// Class Cone
-class Cone : public PhysicsObject {
-
- private :
-
- // -------------------- Attributes -------------------- //
-
- /// Radius of the cone
- float mRadius;
-
- /// Height of the cone
- float mHeight;
-
- /// Collision shape
- rp3d::ConeShape* mConeShape;
- rp3d::ProxyShape* mProxyShape;
-
- /// Scaling matrix (applied to a sphere to obtain the correct cone dimensions)
- openglframework::Matrix4 mScalingMatrix;
-
- /// Previous transform (for interpolation)
- rp3d::Transform mPreviousTransform;
-
- /// Vertex Buffer Object for the vertices data
- static openglframework::VertexBufferObject mVBOVertices;
-
- /// Vertex Buffer Object for the normals data
- static openglframework::VertexBufferObject mVBONormals;
-
- /// Vertex Buffer Object for the texture coords
- static openglframework::VertexBufferObject mVBOTextureCoords;
-
- /// Vertex Buffer Object for the indices
- static openglframework::VertexBufferObject mVBOIndices;
-
- /// Vertex Array Object for the vertex data
- static openglframework::VertexArrayObject mVAO;
-
- // Total number of cones created
- static int totalNbCones;
-
- // -------------------- Methods -------------------- //
-
- // Create the Vertex Buffer Objects used to render with OpenGL.
- void createVBOAndVAO();
-
- public :
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- Cone(float radius, float height, const openglframework::Vector3& position,
- rp3d::CollisionWorld* world, const std::string& meshFolderPath);
-
- /// Constructor
- Cone(float radius, float height, const openglframework::Vector3& position,
- float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath);
-
- /// Destructor
- ~Cone();
-
- /// Render the cone at the correct position and with the correct orientation
- void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
-
- /// Update the transform matrix of the object
- virtual void updateTransform(float interpolationFactor) override;
-
- /// Set the scaling of the object
- void setScaling(const openglframework::Vector3& scaling);
-};
-
-inline void Cone::updateTransform(float interpolationFactor) {
- mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix);
-}
-
-#endif
diff --git a/testbed/common/Cylinder.cpp b/testbed/common/Cylinder.cpp
deleted file mode 100644
index 2943c3e8..00000000
--- a/testbed/common/Cylinder.cpp
+++ /dev/null
@@ -1,272 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-// Libraries
-#include "Cylinder.h"
-
-openglframework::VertexBufferObject Cylinder::mVBOVertices(GL_ARRAY_BUFFER);
-openglframework::VertexBufferObject Cylinder::mVBONormals(GL_ARRAY_BUFFER);
-openglframework::VertexBufferObject Cylinder::mVBOTextureCoords(GL_ARRAY_BUFFER);
-openglframework::VertexBufferObject Cylinder::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER);
-openglframework::VertexArrayObject Cylinder::mVAO;
-int Cylinder::totalNbCylinders = 0;
-
-// Constructor
-Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position,
- reactphysics3d::CollisionWorld* world,
- const std::string& meshFolderPath)
- : PhysicsObject(meshFolderPath + "cylinder.obj"), mRadius(radius), mHeight(height) {
-
- // Compute the scaling matrix
- mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
- 0, mHeight, 0, 0,
- 0, 0, mRadius, 0,
- 0, 0, 0, 1);
-
- // Initialize the position where the cylinder will be rendered
- translateWorld(position);
-
- // Create the collision shape for the rigid body (cylinder shape) and do not
- // forget to delete it at the end
- mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight);
-
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- mPreviousTransform = transform;
-
- // Create a rigid body corresponding to the cylinder in the dynamics world
- mBody = world->createCollisionBody(transform);
-
- // Add a collision shape to the body and specify the mass of the shape
- mProxyShape = mBody->addCollisionShape(mCylinderShape, rp3d::Transform::identity());
-
- mTransformMatrix = mTransformMatrix * mScalingMatrix;
-
- // Create the VBOs and VAO
- if (totalNbCylinders == 0) {
- createVBOAndVAO();
- }
-
- totalNbCylinders++;
-}
-
-// Constructor
-Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position,
- float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
- const std::string& meshFolderPath)
- : PhysicsObject(meshFolderPath + "cylinder.obj"), mRadius(radius), mHeight(height) {
-
- // Compute the scaling matrix
- mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
- 0, mHeight, 0, 0,
- 0, 0, mRadius, 0,
- 0, 0, 0, 1);
-
- // Initialize the position where the cylinder will be rendered
- translateWorld(position);
-
- // Create the collision shape for the rigid body (cylinder shape) and do
- // not forget to delete it at the end
- mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight);
-
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- // Create a rigid body corresponding to the cylinder in the dynamics world
- rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
-
- // Add a collision shape to the body and specify the mass of the shape
- mProxyShape = body->addCollisionShape(mCylinderShape, rp3d::Transform::identity(), mass);
-
- mTransformMatrix = mTransformMatrix * mScalingMatrix;
-
- mBody = body;
-
- // Create the VBOs and VAO
- if (totalNbCylinders == 0) {
- createVBOAndVAO();
- }
-
- totalNbCylinders++;
-}
-
-// Destructor
-Cylinder::~Cylinder() {
-
- if (totalNbCylinders == 1) {
-
- // Destroy the mesh
- destroy();
-
- // Destroy the VBOs and VAO
- mVBOIndices.destroy();
- mVBOVertices.destroy();
- mVBONormals.destroy();
- mVBOTextureCoords.destroy();
- mVAO.destroy();
- }
- delete mCylinderShape;
- totalNbCylinders--;
-}
-
-// Render the cylinder at the correct position and with the correct orientation
-void Cylinder::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
- }
-
- // Bind the shader
- shader.bind();
-
- // Set the model to camera matrix
- shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix);
- shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
-
- // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
- // model-view matrix)
- const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix;
- const openglframework::Matrix3 normalMatrix =
- localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
- shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
-
- // Set the vertex color
- openglframework::Color currentColor = mBody->isSleeping() ? mSleepingColor : mColor;
- openglframework::Vector4 color(currentColor.r, currentColor.g, currentColor.b, currentColor.a);
- shader.setVector4Uniform("vertexColor", color, false);
-
- // Bind the VAO
- mVAO.bind();
-
- mVBOVertices.bind();
-
- // Get the location of shader attribute variables
- GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
- GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false);
-
- glEnableVertexAttribArray(vertexPositionLoc);
- glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr);
-
- mVBONormals.bind();
-
- if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr);
- if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc);
-
- // For each part of the mesh
- for (unsigned int i=0; i<getNbParts(); i++) {
- glDrawElements(GL_TRIANGLES, getNbFaces(i) * 3, GL_UNSIGNED_INT, (char*)nullptr);
- }
-
- glDisableVertexAttribArray(vertexPositionLoc);
- if (vertexNormalLoc != -1) glDisableVertexAttribArray(vertexNormalLoc);
-
- mVBONormals.unbind();
- mVBOVertices.unbind();
-
- // Unbind the VAO
- mVAO.unbind();
-
- // Unbind the shader
- shader.unbind();
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
- }
-}
-
-// Create the Vertex Buffer Objects used to render with OpenGL.
-/// We create two VBOs (one for vertices and one for indices)
-void Cylinder::createVBOAndVAO() {
-
- // Create the VBO for the vertices data
- mVBOVertices.create();
- mVBOVertices.bind();
- size_t sizeVertices = mVertices.size() * sizeof(openglframework::Vector3);
- mVBOVertices.copyDataIntoVBO(sizeVertices, getVerticesPointer(), GL_STATIC_DRAW);
- mVBOVertices.unbind();
-
- // Create the VBO for the normals data
- mVBONormals.create();
- mVBONormals.bind();
- size_t sizeNormals = mNormals.size() * sizeof(openglframework::Vector3);
- mVBONormals.copyDataIntoVBO(sizeNormals, getNormalsPointer(), GL_STATIC_DRAW);
- mVBONormals.unbind();
-
- if (hasTexture()) {
- // Create the VBO for the texture co data
- mVBOTextureCoords.create();
- mVBOTextureCoords.bind();
- size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::Vector2);
- mVBOTextureCoords.copyDataIntoVBO(sizeTextureCoords, getUVTextureCoordinatesPointer(), GL_STATIC_DRAW);
- mVBOTextureCoords.unbind();
- }
-
- // Create th VBO for the indices data
- mVBOIndices.create();
- mVBOIndices.bind();
- size_t sizeIndices = mIndices[0].size() * sizeof(unsigned int);
- mVBOIndices.copyDataIntoVBO(sizeIndices, getIndicesPointer(), GL_STATIC_DRAW);
- mVBOIndices.unbind();
-
- // Create the VAO for both VBOs
- mVAO.create();
- mVAO.bind();
-
- // Bind the VBO of vertices
- mVBOVertices.bind();
-
- // Bind the VBO of normals
- mVBONormals.bind();
-
- if (hasTexture()) {
- // Bind the VBO of texture coords
- mVBOTextureCoords.bind();
- }
-
- // Bind the VBO of indices
- mVBOIndices.bind();
-
- // Unbind the VAO
- mVAO.unbind();
-}
-
-// Set the scaling of the object
-void Cylinder::setScaling(const openglframework::Vector3& scaling) {
-
- // Scale the collision shape
- mProxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z));
-
- // Scale the graphics object
- mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0,
- 0, mHeight * scaling.y, 0, 0,
- 0, 0, mRadius * scaling.z, 0,
- 0, 0, 0, 1);
-}
diff --git a/testbed/common/Cylinder.h b/testbed/common/Cylinder.h
deleted file mode 100644
index 3a94b443..00000000
--- a/testbed/common/Cylinder.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef CYLINDER_H
-#define CYLINDER_H
-
-// Libraries
-#include "openglframework.h"
-#include "reactphysics3d.h"
-#include "PhysicsObject.h"
-
-// Class Cylinder
-class Cylinder : public PhysicsObject {
-
- private :
-
- // -------------------- Attributes -------------------- //
-
- /// Radius of the cylinder
- float mRadius;
-
- /// Height of the cylinder
- float mHeight;
-
- /// Scaling matrix (applied to a sphere to obtain the correct cylinder dimensions)
- openglframework::Matrix4 mScalingMatrix;
-
- /// Previous transform (for interpolation)
- rp3d::Transform mPreviousTransform;
-
- /// Collision shape
- rp3d::CylinderShape* mCylinderShape;
- rp3d::ProxyShape* mProxyShape;
-
- /// Vertex Buffer Object for the vertices data
- static openglframework::VertexBufferObject mVBOVertices;
-
- /// Vertex Buffer Object for the normals data
- static openglframework::VertexBufferObject mVBONormals;
-
- /// Vertex Buffer Object for the texture coords
- static openglframework::VertexBufferObject mVBOTextureCoords;
-
- /// Vertex Buffer Object for the indices
- static openglframework::VertexBufferObject mVBOIndices;
-
- /// Vertex Array Object for the vertex data
- static openglframework::VertexArrayObject mVAO;
-
- // Total number of capsules created
- static int totalNbCylinders;
-
- // -------------------- Methods -------------------- //
-
- // Create the Vertex Buffer Objects used to render with OpenGL.
- void createVBOAndVAO();
-
- public :
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- Cylinder(float radius, float height, const openglframework::Vector3& position,
- rp3d::CollisionWorld* world, const std::string &meshFolderPath);
-
- /// Constructor
- Cylinder(float radius, float height, const openglframework::Vector3& position,
- float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string &meshFolderPath);
-
- /// Destructor
- ~Cylinder();
-
- /// Render the cylinder at the correct position and with the correct orientation
- void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
-
- /// Update the transform matrix of the object
- virtual void updateTransform(float interpolationFactor) override;
-
- /// Set the scaling of the object
- void setScaling(const openglframework::Vector3& scaling);
-};
-
-// Update the transform matrix of the object
-inline void Cylinder::updateTransform(float interpolationFactor) {
- mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix);
-}
-
-#endif
diff --git a/testbed/common/Dumbbell.cpp b/testbed/common/Dumbbell.cpp
index 03afd59b..02afe091 100644
--- a/testbed/common/Dumbbell.cpp
+++ b/testbed/common/Dumbbell.cpp
@@ -53,13 +53,13 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
const rp3d::decimal massSphere = rp3d::decimal(2.0);
mSphereShape = new rp3d::SphereShape(radiusSphere);
- // Create a cylinder collision shape for the middle of the dumbbell
+ // Create a capsule collision shape for the middle of the dumbbell
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
- const rp3d::decimal radiusCylinder = rp3d::decimal(0.5);
- const rp3d::decimal heightCylinder = rp3d::decimal(8.0);
+ const rp3d::decimal radiusCapsule = rp3d::decimal(0.5);
+ const rp3d::decimal heightCapsule = rp3d::decimal(7.0);
const rp3d::decimal massCylinder = rp3d::decimal(1.0);
- mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder);
+ mCapsuleShape = new rp3d::CapsuleShape(radiusCapsule, heightCapsule);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@@ -84,7 +84,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
// Add the three collision shapes to the body and specify the mass and transform of the shapes
mProxyShapeSphere1 = body->addCollisionShape(mSphereShape, transformSphereShape1, massSphere);
mProxyShapeSphere2 = body->addCollisionShape(mSphereShape, transformSphereShape2, massSphere);
- mProxyShapeCylinder = body->addCollisionShape(mCylinderShape, transformCylinderShape, massCylinder);
+ mProxyShapeCapsule = body->addCollisionShape(mCapsuleShape, transformCylinderShape, massCylinder);
mBody = body;
@@ -120,9 +120,9 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
// Create a cylinder collision shape for the middle of the dumbbell
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
- const rp3d::decimal radiusCylinder = rp3d::decimal(0.5);
- const rp3d::decimal heightCylinder = rp3d::decimal(8.0);
- mCylinderShape = new rp3d::CylinderShape(radiusCylinder, heightCylinder);
+ const rp3d::decimal radiusCapsule = rp3d::decimal(0.5);
+ const rp3d::decimal heightCapsule = rp3d::decimal(7.0);
+ mCapsuleShape = new rp3d::CapsuleShape(radiusCapsule, heightCapsule);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@@ -145,7 +145,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
// Add the three collision shapes to the body and specify the mass and transform of the shapes
mProxyShapeSphere1 = mBody->addCollisionShape(mSphereShape, transformSphereShape1);
mProxyShapeSphere2 = mBody->addCollisionShape(mSphereShape, transformSphereShape2);
- mProxyShapeCylinder = mBody->addCollisionShape(mCylinderShape, transformCylinderShape);
+ mProxyShapeCapsule = mBody->addCollisionShape(mCapsuleShape, transformCylinderShape);
mTransformMatrix = mTransformMatrix * mScalingMatrix;
@@ -173,7 +173,7 @@ Dumbbell::~Dumbbell() {
mVAO.destroy();
}
delete mSphereShape;
- delete mCylinderShape;
+ delete mCapsuleShape;
totalNbDumbbells--;
}
@@ -304,7 +304,7 @@ void Dumbbell::setScaling(const openglframework::Vector3& scaling) {
// Scale the collision shape
rp3d::Vector3 newScaling(scaling.x, scaling.y, scaling.z);
- mProxyShapeCylinder->setLocalScaling(newScaling);
+ mProxyShapeCapsule->setLocalScaling(newScaling);
mProxyShapeSphere1->setLocalScaling(newScaling);
mProxyShapeSphere2->setLocalScaling(newScaling);
diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h
index 81020935..ea2d3d88 100644
--- a/testbed/common/Dumbbell.h
+++ b/testbed/common/Dumbbell.h
@@ -31,7 +31,7 @@
#include "reactphysics3d.h"
#include "PhysicsObject.h"
-// Class Sphere
+// Class Dumbbell
class Dumbbell : public PhysicsObject {
private :
@@ -42,9 +42,9 @@ class Dumbbell : public PhysicsObject {
float mDistanceBetweenSphere;
/// Collision shapes
- rp3d::CylinderShape* mCylinderShape;
+ rp3d::CapsuleShape* mCapsuleShape;
rp3d::SphereShape* mSphereShape;
- rp3d::ProxyShape* mProxyShapeCylinder;
+ rp3d::ProxyShape* mProxyShapeCapsule;
rp3d::ProxyShape* mProxyShapeSphere1;
rp3d::ProxyShape* mProxyShapeSphere2;
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index b7f57c1d..d3a7de00 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -33,8 +33,6 @@
#include "SceneDemo.h"
#include "Sphere.h"
#include "Box.h"
-#include "Cone.h"
-#include "Cylinder.h"
#include "Capsule.h"
#include "Line.h"
#include "ConvexMesh.h"
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
index 8db4908a..746942ff 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
@@ -125,62 +125,6 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
mSpheres.push_back(sphere);
}
- // Create all the cones of the scene
- for (int i=0; i<NB_CONES; i++) {
-
- // Position
- float angle = i * 50.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 35 + i * (CONE_HEIGHT + 0.3f),
- radius * sin(angle));
-
- // Create a cone and a corresponding rigid in the dynamics world
- Cone* cone = new Cone(CONE_RADIUS, CONE_HEIGHT, position, CONE_MASS, mDynamicsWorld,
- meshFolderPath);
-
- // Add some rolling resistance
- cone->getRigidBody()->getMaterial().setRollingResistance(0.08);
-
- // Set the box color
- cone->setColor(mDemoColors[i % mNbDemoColors]);
- cone->setSleepingColor(mRedColorDemo);
-
- // Change the material properties of the rigid body
- rp3d::Material& material = cone->getRigidBody()->getMaterial();
- material.setBounciness(rp3d::decimal(0.2));
-
- // Add the cone the list of sphere in the scene
- mCones.push_back(cone);
- }
-
- // Create all the cylinders of the scene
- for (int i=0; i<NB_CYLINDERS; i++) {
-
- // Position
- float angle = i * 35.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 25 + i * (CYLINDER_HEIGHT + 0.3f),
- radius * sin(angle));
-
- // Create a cylinder and a corresponding rigid in the dynamics world
- Cylinder* cylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position ,
- CYLINDER_MASS, mDynamicsWorld, meshFolderPath);
-
- // Add some rolling resistance
- cylinder->getRigidBody()->getMaterial().setRollingResistance(0.08);
-
- // Set the box color
- cylinder->setColor(mDemoColors[i % mNbDemoColors]);
- cylinder->setSleepingColor(mRedColorDemo);
-
- // Change the material properties of the rigid body
- rp3d::Material& material = cylinder->getRigidBody()->getMaterial();
- material.setBounciness(rp3d::decimal(0.2));
-
- // Add the cylinder the list of sphere in the scene
- mCylinders.push_back(cylinder);
- }
-
// Create all the capsules of the scene
for (int i=0; i<NB_CAPSULES; i++) {
@@ -306,26 +250,6 @@ CollisionShapesScene::~CollisionShapesScene() {
delete (*it);
}
- // Destroy all the cones of the scene
- for (std::vector<Cone*>::iterator it = mCones.begin(); it != mCones.end(); ++it) {
-
- // Destroy the corresponding rigid body from the dynamics world
- mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
-
- // Destroy the sphere
- delete (*it);
- }
-
- // Destroy all the cylinders of the scene
- for (std::vector<Cylinder*>::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) {
-
- // Destroy the corresponding rigid body from the dynamics world
- mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
-
- // Destroy the sphere
- delete (*it);
- }
-
// Destroy all the capsules of the scene
for (std::vector<Capsule*>::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) {
@@ -410,20 +334,6 @@ void CollisionShapesScene::update() {
(*it)->updateTransform(mInterpolationFactor);
}
- // Update the position and orientation of the cones
- for (std::vector<Cone*>::iterator it = mCones.begin(); it != mCones.end(); ++it) {
-
- // Update the transform used for the rendering
- (*it)->updateTransform(mInterpolationFactor);
- }
-
- // Update the position and orientation of the cylinders
- for (std::vector<Cylinder*>::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) {
-
- // Update the transform used for the rendering
- (*it)->updateTransform(mInterpolationFactor);
- }
-
// Update the position and orientation of the capsules
for (std::vector<Capsule*>::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) {
@@ -469,16 +379,6 @@ void CollisionShapesScene::renderSinglePass(openglframework::Shader& shader,
(*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
}
- // Render all the cones of the scene
- for (std::vector<Cone*>::iterator it = mCones.begin(); it != mCones.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- }
-
- // Render all the cylinders of the scene
- for (std::vector<Cylinder*>::iterator it = mCylinders.begin(); it != mCylinders.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- }
-
// Render all the capsules of the scene
for (std::vector<Capsule*>::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) {
(*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
@@ -563,42 +463,6 @@ void CollisionShapesScene::reset() {
mSpheres[i]->setTransform(transform);
}
- // Create all the cones of the scene
- for (int i=0; i<NB_CONES; i++) {
-
- // Position
- float angle = i * 50.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 35 + i * (CONE_HEIGHT + 0.3f),
- radius * sin(angle));
-
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- // Reset the transform
- mCones[i]->setTransform(transform);
- }
-
- // Create all the cylinders of the scene
- for (int i=0; i<NB_CYLINDERS; i++) {
-
- // Position
- float angle = i * 35.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 25 + i * (CYLINDER_HEIGHT + 0.3f),
- radius * sin(angle));
-
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- // Reset the transform
- mCylinders[i]->setTransform(transform);
- }
-
// Create all the capsules of the scene
for (int i=0; i<NB_CAPSULES; i++) {
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.h b/testbed/scenes/collisionshapes/CollisionShapesScene.h
index 93d14051..b253acef 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.h
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.h
@@ -32,8 +32,6 @@
#include "SceneDemo.h"
#include "Sphere.h"
#include "Box.h"
-#include "Cone.h"
-#include "Cylinder.h"
#include "Capsule.h"
#include "ConvexMesh.h"
#include "ConcaveMesh.h"
@@ -46,8 +44,6 @@ namespace collisionshapesscene {
const float SCENE_RADIUS = 30.0f;
const int NB_BOXES = 5;
const int NB_SPHERES = 5;
-const int NB_CONES = 5;
-const int NB_CYLINDERS = 5;
const int NB_CAPSULES = 5;
const int NB_MESHES = 3;
const int NB_COMPOUND_SHAPES = 3;
@@ -80,10 +76,6 @@ class CollisionShapesScene : public SceneDemo {
std::vector<Sphere*> mSpheres;
- std::vector<Cone*> mCones;
-
- std::vector<Cylinder*> mCylinders;
-
std::vector<Capsule*> mCapsules;
/// All the convex meshes of the scene
diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp
index 58b19055..c403ebb2 100644
--- a/testbed/scenes/raycast/RaycastScene.cpp
+++ b/testbed/scenes/raycast/RaycastScene.cpp
@@ -79,28 +79,6 @@ RaycastScene::RaycastScene(const std::string& name)
mSphere->setColor(mGreyColorDemo);
mSphere->setSleepingColor(mRedColorDemo);
- // ---------- Cone ---------- //
- openglframework::Vector3 position4(0, 0, 0);
-
- // Create a cone and a corresponding collision body in the dynamics world
- mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld,
- mMeshFolderPath);
-
- // Set the color
- mCone->setColor(mGreyColorDemo);
- mCone->setSleepingColor(mRedColorDemo);
-
- // ---------- Cylinder ---------- //
- openglframework::Vector3 position5(0, 0, 0);
-
- // Create a cylinder and a corresponding collision body in the dynamics world
- mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5,
- mCollisionWorld, mMeshFolderPath);
-
- // Set the color
- mCylinder->setColor(mGreyColorDemo);
- mCylinder->setSleepingColor(mRedColorDemo);
-
// ---------- Capsule ---------- //
openglframework::Vector3 position6(0, 0, 0);
@@ -188,8 +166,6 @@ void RaycastScene::changeBody() {
mSphere->getCollisionBody()->setIsActive(false);
mBox->getCollisionBody()->setIsActive(false);
- mCone->getCollisionBody()->setIsActive(false);
- mCylinder->getCollisionBody()->setIsActive(false);
mCapsule->getCollisionBody()->setIsActive(false);
mConvexMesh->getCollisionBody()->setIsActive(false);
mDumbbell->getCollisionBody()->setIsActive(false);
@@ -201,19 +177,15 @@ void RaycastScene::changeBody() {
break;
case 1: mBox->getCollisionBody()->setIsActive(true);
break;
- case 2: mCone->getCollisionBody()->setIsActive(true);
+ case 2: mCapsule->getCollisionBody()->setIsActive(true);
break;
- case 3: mCylinder->getCollisionBody()->setIsActive(true);
+ case 3: mConvexMesh->getCollisionBody()->setIsActive(true);
break;
- case 4: mCapsule->getCollisionBody()->setIsActive(true);
+ case 4: mDumbbell->getCollisionBody()->setIsActive(true);
break;
- case 5: mConvexMesh->getCollisionBody()->setIsActive(true);
+ case 5: mConcaveMesh->getCollisionBody()->setIsActive(true);
break;
- case 6: mDumbbell->getCollisionBody()->setIsActive(true);
- break;
- case 7: mConcaveMesh->getCollisionBody()->setIsActive(true);
- break;
- case 8: mHeightField->getCollisionBody()->setIsActive(true);
+ case 6: mHeightField->getCollisionBody()->setIsActive(true);
break;
}
@@ -238,16 +210,6 @@ RaycastScene::~RaycastScene() {
mCollisionWorld->destroyCollisionBody(mSphere->getCollisionBody());
delete mSphere;
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody());
- delete mCone;
-
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody());
-
- // Destroy the sphere
- delete mCylinder;
-
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody());
@@ -378,8 +340,6 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader,
// Render the shapes
if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mSphere->getCollisionBody()->isActive()) mSphere->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h
index 56ebb565..be698e53 100644
--- a/testbed/scenes/raycast/RaycastScene.h
+++ b/testbed/scenes/raycast/RaycastScene.h
@@ -34,8 +34,6 @@
#include "SceneDemo.h"
#include "Sphere.h"
#include "Box.h"
-#include "Cone.h"
-#include "Cylinder.h"
#include "Capsule.h"
#include "Line.h"
#include "ConvexMesh.h"
@@ -141,8 +139,6 @@ class RaycastScene : public SceneDemo {
/// All objects on the scene
Box* mBox;
Sphere* mSphere;
- Cone* mCone;
- Cylinder* mCylinder;
Capsule* mCapsule;
ConvexMesh* mConvexMesh;
Dumbbell* mDumbbell;
From 30e0132e158accaa87a5eb21eeab6ab8b79fd476 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 2 Feb 2017 23:10:01 +0100
Subject: [PATCH 035/133] Add capsule/capsule and capsule/sphere collision
algorithm
---
CMakeLists.txt | 2 +
.../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 188 +++-
.../narrowphase/CapsuleVsCapsuleAlgorithm.h | 142 +--
.../narrowphase/SphereVsSphereAlgorithm.cpp | 138 +--
.../narrowphase/SphereVsSphereAlgorithm.h | 142 +--
src/mathematics/mathematics_functions.cpp | 261 ++++--
src/mathematics/mathematics_functions.h | 190 ++--
.../mathematics/TestMathematicsFunctions.h | 312 ++++---
.../CollisionDetectionScene.cpp | 832 +++++++++---------
.../CollisionDetectionScene.h | 466 +++++-----
10 files changed, 1500 insertions(+), 1173 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9b6af8c9..37b8febe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -78,6 +78,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/narrowphase/SphereVsSphereAlgorithm.cpp"
"src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h"
"src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp"
+ "src/collision/narrowphase/SphereVsCapsuleAlgorithm.h"
+ "src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp"
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
"src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h"
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
index 44badf67..2f9f8703 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
@@ -1,37 +1,151 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-// Libraries
-#include "CapsuleVsCapsuleAlgorithm.h"
-#include "collision/shapes/CapsuleShape.h"
-
-// We want to use the ReactPhysics3D namespace
-using namespace reactphysics3d;
-
-bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) {
-
-
-}
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "CapsuleVsCapsuleAlgorithm.h"
+#include "collision/shapes/CapsuleShape.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) {
+
+ const decimal parallelEpsilon = decimal(0.001);
+
+ // Get the capsule collision shapes
+ const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape1);
+ const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
+
+ // Get the transform from capsule 1 local-space to capsule 2 local-space
+ const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse();
+
+ // Compute the end-points of the inner segment of the first capsule
+ Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0);
+ Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0);
+ capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA;
+ capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB;
+
+ // Compute the end-points of the inner segment of the second capsule
+ const Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0);
+ const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0);
+
+ // The two inner capsule segments
+ const Vector3 seg1 = capsule1SegB - capsule1SegA;
+ const Vector3 seg2 = capsule2SegB - capsule2SegA;
+
+ // Compute the sum of the radius of the two capsules (virtual spheres)
+ decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
+
+ // If the two capsules are parallel (we create two contact points)
+ if (seg1.cross(seg2).lengthSquare() < parallelEpsilon * parallelEpsilon) {
+
+ // If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
+ const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
+ if (segmentsDistance > sumRadius || segmentsDistance < MACHINE_EPSILON) {
+
+ // The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius.
+ // Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision.
+ return false;
+ }
+
+ // Compute the planes that goes through the extrem points of the inner segment of capsule 1
+ decimal d1 = seg1.dot(capsule1SegA);
+ decimal d2 = -seg1.dot(capsule1SegB);
+
+ // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
+ // segment of capsule 1
+ decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
+ decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
+
+ bool isClipValid = false; // True if the segments were overlapping (the clip segment is valid)
+
+ // Clip the inner segment of capsule 2
+ Vector3 clipPointA, clipPointB;
+ if (t1 >= decimal(0.0)) {
+
+ if (t1 > decimal(1.0)) t1 = decimal(1.0);
+ clipPointA = capsule2SegB - t1 * seg2;
+ isClipValid = true;
+ }
+ if (t2 >= decimal(0.0) && t2 <= decimal(1.0)) {
+
+ if (t2 > decimal(1.0)) t2 = decimal(1.0);
+ clipPointB = capsule2SegA + t2 * seg2;
+ isClipValid = true;
+ }
+
+ // If we have a valid clip segment
+ if (isClipValid) {
+
+ Vector3 segment1ToSegment2 = (capsule2SegA - capsule1SegA);
+ Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
+
+ const Vector3 contactPointACapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
+ const Vector3 contactPointBCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
+ const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius();
+ const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius();
+
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized;
+
+ decimal penetrationDepth = sumRadius - segmentsDistance;
+
+ // Create the contact info object
+ // TODO : Make sure we create two contact points at the same time (same method here)
+ contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointBCapsule1Local);
+ contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule2Local, contactPointBCapsule2Local);
+ }
+ }
+
+ // Compute the closest points between the two inner capsule segments
+ Vector3 closestPointCapsule1Seg;
+ Vector3 closestPointCapsule2Seg;
+ computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB,
+ closestPointCapsule1Seg, closestPointCapsule2Seg);
+
+ // Compute the distance between the sphere center and the closest point on the segment
+ Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg);
+ const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
+
+ // If the collision shapes overlap
+ if (closestPointsDistanceSquare <= sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) {
+
+ decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
+ closestPointsSeg1ToSeg2 /= closestPointsDistance;
+
+ const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
+ const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
+
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
+
+ decimal penetrationDepth = sumRadius - closestPointsDistance;
+
+ // Create the contact info object
+ contactPointInfo.init(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
+
+ return true;
+ }
+
+ return false;
+}
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
index 6a48495e..7a46f73a 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
@@ -1,71 +1,71 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H
-#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H
-
-// Libraries
-#include "body/Body.h"
-#include "constraint/ContactPoint.h"
-#include "NarrowPhaseAlgorithm.h"
-
-
-/// Namespace ReactPhysics3D
-namespace reactphysics3d {
-
-// Class CapsuleVsCapsuleAlgorithm
-/**
- * This class is used to compute the narrow-phase collision detection
- * between two capsule collision shapes.
- */
-class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
-
- protected :
-
- public :
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- CapsuleVsCapsuleAlgorithm() = default;
-
- /// Destructor
- virtual ~CapsuleVsCapsuleAlgorithm() override = default;
-
- /// Deleted copy-constructor
- CapsuleVsCapsuleAlgorithm(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
-
- /// Deleted assignment operator
- CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
-
- /// Compute a contact info if the two bounding volume collide
- virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) override;
-};
-
-}
-
-#endif
-
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H
+#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H
+
+// Libraries
+#include "body/Body.h"
+#include "constraint/ContactPoint.h"
+#include "NarrowPhaseAlgorithm.h"
+
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class CapsuleVsCapsuleAlgorithm
+/**
+ * This class is used to compute the narrow-phase collision detection
+ * between two capsules collision shapes.
+ */
+class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
+
+ protected :
+
+ public :
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ CapsuleVsCapsuleAlgorithm() = default;
+
+ /// Destructor
+ virtual ~CapsuleVsCapsuleAlgorithm() override = default;
+
+ /// Deleted copy-constructor
+ CapsuleVsCapsuleAlgorithm(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
+
+ /// Deleted assignment operator
+ CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
+
+ /// Compute a contact info if the two bounding volume collide
+ virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) override;
+};
+
+}
+
+#endif
+
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
index 6b28372d..e80688d5 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
@@ -1,69 +1,69 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-// Libraries
-#include "SphereVsSphereAlgorithm.h"
-#include "collision/shapes/SphereShape.h"
-
-// We want to use the ReactPhysics3D namespace
-using namespace reactphysics3d;
-
-bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) {
-
- // Get the sphere collision shapes
- const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
- const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape2);
-
- // Get the local-space to world-space transforms
- const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
- const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
-
- // Compute the distance between the centers
- Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
- decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
-
- // Compute the sum of the radius
- decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
-
- // If the sphere collision shapes intersect
- if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
- Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
- Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
- Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
- centerSphere2InBody1LocalSpace.getUnit();
- Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
- centerSphere1InBody2LocalSpace.getUnit();
- decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
-
- // Create the contact info object
- contactPointInfo.init(vectorBetweenCenters.getUnit(), penetrationDepth,
- intersectionOnBody1, intersectionOnBody2);
-
- return true;
- }
-
- return false;
-}
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "SphereVsSphereAlgorithm.h"
+#include "collision/shapes/SphereShape.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) {
+
+ // Get the sphere collision shapes
+ const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
+ const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape2);
+
+ // Get the local-space to world-space transforms
+ const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
+ const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
+
+ // Compute the distance between the centers
+ Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
+ decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
+
+ // Compute the sum of the radius
+ decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
+
+ // If the sphere collision shapes intersect
+ if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
+ Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
+ Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
+ Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
+ centerSphere2InBody1LocalSpace.getUnit();
+ Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
+ centerSphere1InBody2LocalSpace.getUnit();
+ decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
+
+ // Create the contact info object
+ contactPointInfo.init(vectorBetweenCenters.getUnit(), penetrationDepth,
+ intersectionOnBody1, intersectionOnBody2);
+
+ return true;
+ }
+
+ return false;
+}
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
index 24886f7f..be10d5a1 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
@@ -1,71 +1,71 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
-#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
-
-// Libraries
-#include "body/Body.h"
-#include "constraint/ContactPoint.h"
-#include "NarrowPhaseAlgorithm.h"
-
-
-/// Namespace ReactPhysics3D
-namespace reactphysics3d {
-
-// Class SphereVsSphereAlgorithm
-/**
- * This class is used to compute the narrow-phase collision detection
- * between two sphere collision shapes.
- */
-class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
-
- protected :
-
- public :
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- SphereVsSphereAlgorithm() = default;
-
- /// Destructor
- virtual ~SphereVsSphereAlgorithm() override = default;
-
- /// Deleted copy-constructor
- SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete;
-
- /// Deleted assignment operator
- SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
-
- /// Compute a contact info if the two bounding volume collide
- virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) override;
-};
-
-}
-
-#endif
-
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
+#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
+
+// Libraries
+#include "body/Body.h"
+#include "constraint/ContactPoint.h"
+#include "NarrowPhaseAlgorithm.h"
+
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class SphereVsSphereAlgorithm
+/**
+ * This class is used to compute the narrow-phase collision detection
+ * between two sphere collision shapes.
+ */
+class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
+
+ protected :
+
+ public :
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ SphereVsSphereAlgorithm() = default;
+
+ /// Destructor
+ virtual ~SphereVsSphereAlgorithm() override = default;
+
+ /// Deleted copy-constructor
+ SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete;
+
+ /// Deleted assignment operator
+ SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
+
+ /// Compute a contact info if the two bounding volume collide
+ virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) override;
+};
+
+}
+
+#endif
+
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 97bf6dcf..578cecdd 100644
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -1,59 +1,202 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-// Libraries
-#include "mathematics_functions.h"
-#include "Vector3.h"
-
-using namespace reactphysics3d;
-
-/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
-/// This method uses the technique described in the book Real-Time collision detection by
-/// Christer Ericson.
-void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
- const Vector3& p, decimal& u, decimal& v, decimal& w) {
- const Vector3 v0 = b - a;
- const Vector3 v1 = c - a;
- const Vector3 v2 = p - a;
-
- decimal d00 = v0.dot(v0);
- decimal d01 = v0.dot(v1);
- decimal d11 = v1.dot(v1);
- decimal d20 = v2.dot(v0);
- decimal d21 = v2.dot(v1);
-
- decimal denom = d00 * d11 - d01 * d01;
- v = (d11 * d20 - d01 * d21) / denom;
- w = (d00 * d21 - d01 * d20) / denom;
- u = decimal(1.0) - v - w;
-}
-
-// Clamp a vector such that it is no longer than a given maximum length
-Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) {
- if (vector.lengthSquare() > maxLength * maxLength) {
- return vector.getUnit() * maxLength;
- }
- return vector;
-}
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "mathematics_functions.h"
+#include "Vector3.h"
+#include <cassert>
+
+using namespace reactphysics3d;
+
+/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
+/// This method uses the technique described in the book Real-Time collision detection by
+/// Christer Ericson.
+void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
+ const Vector3& p, decimal& u, decimal& v, decimal& w) {
+ const Vector3 v0 = b - a;
+ const Vector3 v1 = c - a;
+ const Vector3 v2 = p - a;
+
+ decimal d00 = v0.dot(v0);
+ decimal d01 = v0.dot(v1);
+ decimal d11 = v1.dot(v1);
+ decimal d20 = v2.dot(v0);
+ decimal d21 = v2.dot(v1);
+
+ decimal denom = d00 * d11 - d01 * d01;
+ v = (d11 * d20 - d01 * d21) / denom;
+ w = (d00 * d21 - d01 * d20) / denom;
+ u = decimal(1.0) - v - w;
+}
+
+/// Clamp a vector such that it is no longer than a given maximum length
+Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) {
+ if (vector.lengthSquare() > maxLength * maxLength) {
+ return vector.getUnit() * maxLength;
+ }
+ return vector;
+}
+
+/// 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) {
+
+ const Vector3 ab = segPointB - segPointA;
+
+ decimal abLengthSquare = ab.lengthSquare();
+
+ // If the segment has almost zero length
+ if (abLengthSquare < MACHINE_EPSILON) {
+
+ // Return one end-point of the segment as the closest point
+ return segPointA;
+ }
+
+ // Project point C onto "AB" line
+ decimal t = (pointC - segPointA).dot(ab) / abLengthSquare;
+
+ // If projected point onto the line is outside the segment, clamp it to the segment
+ if (t < decimal(0.0)) t = decimal(0.0);
+ if (t > decimal(1.0)) t = decimal(1.0);
+
+ // Return the closest point on the segment
+ return segPointA + t * ab;
+}
+
+/// Compute the closest points between two segments
+/// This method uses the technique described in the book Real-Time
+/// collision detection by Christer Ericson.
+void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB,
+ const Vector3& seg2PointA, const Vector3& seg2PointB,
+ Vector3& closestPointSeg1, Vector3& closestPointSeg2) {
+
+ const Vector3 d1 = seg1PointB - seg1PointA;
+ const Vector3 d2 = seg2PointB - seg2PointA;
+ const Vector3 r = seg1PointA - seg2PointA;
+ decimal a = d1.lengthSquare();
+ decimal e = d2.lengthSquare();
+ decimal f = d2.dot(r);
+ decimal s, t;
+
+ // If both segments degenerate into points
+ if (a <= MACHINE_EPSILON && e <= MACHINE_EPSILON) {
+
+ closestPointSeg1 = seg1PointA;
+ closestPointSeg2 = seg2PointA;
+ return;
+ }
+ if (a <= MACHINE_EPSILON) { // If first segment degenerates into a point
+
+ s = decimal(0.0);
+
+ // Compute the closest point on second segment
+ t = clamp(f / e, decimal(0.0), decimal(1.0));
+ }
+ else {
+
+ decimal c = d1.dot(r);
+
+ // If the second segment degenerates into a point
+ if (e <= MACHINE_EPSILON) {
+
+ t = decimal(0.0);
+ s = clamp(-c / a, decimal(0.0), decimal(1.0));
+ }
+ else {
+
+ decimal b = d1.dot(d2);
+ decimal denom = a * e - b * b;
+
+ // If the segments are not parallel
+ if (denom != decimal(0.0)) {
+
+ // Compute the closest point on line 1 to line 2 and
+ // clamp to first segment.
+ s = clamp((b * f - c * e) / denom, decimal(0.0), decimal(1.0));
+ }
+ else {
+
+ // Pick an arbitrary point on first segment
+ s = decimal(0.0);
+ }
+
+ // Compute the point on line 2 closest to the closest point
+ // we have just found
+ t = (b * s + f) / e;
+
+ // If this closest point is inside second segment (t in [0, 1]), we are done.
+ // Otherwise, we clamp the point to the second segment and compute again the
+ // closest point on segment 1
+ if (t < decimal(0.0)) {
+ t = decimal(0.0);
+ s = clamp(-c / a, decimal(0.0), decimal(1.0));
+ }
+ else if (t > decimal(1.0)) {
+ t = decimal(1.0);
+ s = clamp((b - c) / a, decimal(0.0), decimal(1.0));
+ }
+ }
+ }
+
+ // Compute the closest points on both segments
+ closestPointSeg1 = seg1PointA + d1 * s;
+ closestPointSeg2 = seg2PointA + d2 * t;
+}
+
+/// Compute the intersection between a plane and a segment
+// Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method
+// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such
+// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise,
+// there is no intersection between the plane and the segment.
+decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) {
+
+ const decimal parallelEpsilon = decimal(0.0001);
+ decimal t = decimal(-1);
+
+ // Segment AB
+ const Vector3 ab = segB - segA;
+
+ decimal nDotAB = planeNormal.dot(ab);
+
+ // If the segment is not parallel to the plane
+ if (nDotAB > parallelEpsilon) {
+ t = (planeD - planeNormal.dot(segA)) / nDotAB;
+ }
+
+ return t;
+}
+
+/// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB"
+decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) {
+
+ decimal distAB = (linePointB - linePointA).length();
+
+ if (distAB < MACHINE_EPSILON) {
+ return (point - linePointA).length();
+ }
+
+ return ((point - linePointA).cross(point - linePointB)).length() / distAB;
+}
+
+
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index 7efe6d44..315de65c 100644
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -1,87 +1,103 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H
-#define REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H
-
-// Libraries
-#include "configuration.h"
-#include "decimal.h"
-#include <algorithm>
-#include <cassert>
-#include <cmath>
-
-/// ReactPhysics3D namespace
-namespace reactphysics3d {
-
-struct Vector3;
-
-// ---------- Mathematics functions ---------- //
-
-/// Function to test if two real numbers are (almost) equal
-/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
-inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) {
- return (std::fabs(a - b) < epsilon);
-}
-
-/// Function that returns the result of the "value" clamped by
-/// two others values "lowerLimit" and "upperLimit"
-inline int clamp(int value, int lowerLimit, int upperLimit) {
- assert(lowerLimit <= upperLimit);
- return std::min(std::max(value, lowerLimit), upperLimit);
-}
-
-/// Function that returns the result of the "value" clamped by
-/// two others values "lowerLimit" and "upperLimit"
-inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) {
- assert(lowerLimit <= upperLimit);
- return std::min(std::max(value, lowerLimit), upperLimit);
-}
-
-/// Return the minimum value among three values
-inline decimal min3(decimal a, decimal b, decimal c) {
- return std::min(std::min(a, b), c);
-}
-
-/// Return the maximum value among three values
-inline decimal max3(decimal a, decimal b, decimal c) {
- return std::max(std::max(a, b), c);
-}
-
-/// Return true if two values have the same sign
-inline bool sameSign(decimal a, decimal b) {
- return a * b >= decimal(0.0);
-}
-
-/// Clamp a vector such that it is no longer than a given maximum length
-Vector3 clamp(const Vector3& vector, decimal maxLength);
-
-/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
-void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
- const Vector3& p, decimal& u, decimal& v, decimal& w);
-
-}
-
-#endif
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H
+#define REACTPHYSICS3D_MATHEMATICS_FUNCTIONS_H
+
+// Libraries
+#include "configuration.h"
+#include "decimal.h"
+#include <algorithm>
+#include <cassert>
+#include <cmath>
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+struct Vector3;
+
+// ---------- Mathematics functions ---------- //
+
+/// Function to test if two real numbers are (almost) equal
+/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
+inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) {
+ return (std::fabs(a - b) < epsilon);
+}
+
+/// Function that returns the result of the "value" clamped by
+/// two others values "lowerLimit" and "upperLimit"
+inline int clamp(int value, int lowerLimit, int upperLimit) {
+ assert(lowerLimit <= upperLimit);
+ return std::min(std::max(value, lowerLimit), upperLimit);
+}
+
+/// Function that returns the result of the "value" clamped by
+/// two others values "lowerLimit" and "upperLimit"
+inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) {
+ assert(lowerLimit <= upperLimit);
+ return std::min(std::max(value, lowerLimit), upperLimit);
+}
+
+/// Return the minimum value among three values
+inline decimal min3(decimal a, decimal b, decimal c) {
+ return std::min(std::min(a, b), c);
+}
+
+/// Return the maximum value among three values
+inline decimal max3(decimal a, decimal b, decimal c) {
+ return std::max(std::max(a, b), c);
+}
+
+/// Return true if two values have the same sign
+inline bool sameSign(decimal a, decimal b) {
+ return a * b >= decimal(0.0);
+}
+
+/// Clamp a vector such that it is no longer than a given maximum length
+Vector3 clamp(const Vector3& vector, decimal maxLength);
+
+// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC"
+Vector3 computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC);
+
+// Compute the closest points between two segments
+void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB,
+ const Vector3& seg2PointA, const Vector3& seg2PointB,
+ Vector3& closestPointSeg1, Vector3& closestPointSeg2);
+
+/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
+void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
+ const Vector3& p, decimal& u, decimal& v, decimal& w);
+
+/// Compute the intersection between a plane and a segment
+decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal);
+
+/// Compute the distance between a point and a line
+decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point);
+
+}
+
+
+
+#endif
diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h
index 5041fd48..32e19aad 100644
--- a/test/tests/mathematics/TestMathematicsFunctions.h
+++ b/test/tests/mathematics/TestMathematicsFunctions.h
@@ -1,133 +1,179 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef TEST_MATHEMATICS_FUNCTIONS_H
-#define TEST_MATHEMATICS_FUNCTIONS_H
-
-// Libraries
-#include "Test.h"
-#include "mathematics/mathematics_functions.h"
-
-/// Reactphysics3D namespace
-namespace reactphysics3d {
-
-// Class TestMathematicsFunctions
-/**
- * Unit test for mathematics functions
- */
-class TestMathematicsFunctions : public Test {
-
- private :
-
- // ---------- Atributes ---------- //
-
-
-
- public :
-
- // ---------- Methods ---------- //
-
- /// Constructor
- TestMathematicsFunctions(const std::string& name): Test(name) {}
-
- /// Run the tests
- void run() {
-
- // Test approxEqual()
- test(approxEqual(2, 7, 5.2));
- test(approxEqual(7, 2, 5.2));
- test(approxEqual(6, 6));
- test(!approxEqual(1, 5));
- test(!approxEqual(1, 5, 3));
- test(approxEqual(-2, -2));
- test(approxEqual(-2, -7, 6));
- test(!approxEqual(-2, 7, 2));
- test(approxEqual(-3, 8, 12));
- test(!approxEqual(-3, 8, 6));
-
- // Test clamp()
- test(clamp(4, -3, 5) == 4);
- test(clamp(-3, 1, 8) == 1);
- test(clamp(45, -6, 7) == 7);
- test(clamp(-5, -2, -1) == -2);
- test(clamp(-5, -9, -1) == -5);
- test(clamp(6, 6, 9) == 6);
- test(clamp(9, 6, 9) == 9);
- test(clamp(decimal(4), decimal(-3), decimal(5)) == decimal(4));
- test(clamp(decimal(-3), decimal(1), decimal(8)) == decimal(1));
- test(clamp(decimal(45), decimal(-6), decimal(7)) == decimal(7));
- test(clamp(decimal(-5), decimal(-2), decimal(-1)) == decimal(-2));
- test(clamp(decimal(-5), decimal(-9), decimal(-1)) == decimal(-5));
- test(clamp(decimal(6), decimal(6), decimal(9)) == decimal(6));
- test(clamp(decimal(9), decimal(6), decimal(9)) == decimal(9));
-
- // Test min3()
- test(min3(1, 5, 7) == 1);
- test(min3(-4, 2, 4) == -4);
- test(min3(-1, -5, -7) == -7);
- test(min3(13, 5, 47) == 5);
- test(min3(4, 4, 4) == 4);
-
- // Test max3()
- test(max3(1, 5, 7) == 7);
- test(max3(-4, 2, 4) == 4);
- test(max3(-1, -5, -7) == -1);
- test(max3(13, 5, 47) == 47);
- test(max3(4, 4, 4) == 4);
-
- // Test sameSign()
- test(sameSign(4, 53));
- test(sameSign(-4, -8));
- test(!sameSign(4, -7));
- test(!sameSign(-4, 53));
-
- // Test computeBarycentricCoordinatesInTriangle()
- Vector3 a(0, 0, 0);
- Vector3 b(5, 0, 0);
- Vector3 c(0, 0, 5);
- Vector3 testPoint(4, 0, 1);
- decimal u,v,w;
- computeBarycentricCoordinatesInTriangle(a, b, c, a, u, v, w);
- test(approxEqual(u, 1.0, 0.000001));
- test(approxEqual(v, 0.0, 0.000001));
- test(approxEqual(w, 0.0, 0.000001));
- computeBarycentricCoordinatesInTriangle(a, b, c, b, u, v, w);
- test(approxEqual(u, 0.0, 0.000001));
- test(approxEqual(v, 1.0, 0.000001));
- test(approxEqual(w, 0.0, 0.000001));
- computeBarycentricCoordinatesInTriangle(a, b, c, c, u, v, w);
- test(approxEqual(u, 0.0, 0.000001));
- test(approxEqual(v, 0.0, 0.000001));
- test(approxEqual(w, 1.0, 0.000001));
-
- computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w);
- test(approxEqual(u + v + w, 1.0, 0.000001));
- }
-
- };
-
-}
-
-#endif
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef TEST_MATHEMATICS_FUNCTIONS_H
+#define TEST_MATHEMATICS_FUNCTIONS_H
+
+// Libraries
+#include "Test.h"
+#include "mathematics/mathematics_functions.h"
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Class TestMathematicsFunctions
+/**
+ * Unit test for mathematics functions
+ */
+class TestMathematicsFunctions : public Test {
+
+ private :
+
+ // ---------- Atributes ---------- //
+
+
+
+ public :
+
+ // ---------- Methods ---------- //
+
+ /// Constructor
+ TestMathematicsFunctions(const std::string& name): Test(name) {}
+
+ /// Run the tests
+ void run() {
+
+ // Test approxEqual()
+ test(approxEqual(2, 7, 5.2));
+ test(approxEqual(7, 2, 5.2));
+ test(approxEqual(6, 6));
+ test(!approxEqual(1, 5));
+ test(!approxEqual(1, 5, 3));
+ test(approxEqual(-2, -2));
+ test(approxEqual(-2, -7, 6));
+ test(!approxEqual(-2, 7, 2));
+ test(approxEqual(-3, 8, 12));
+ test(!approxEqual(-3, 8, 6));
+
+ // Test clamp()
+ test(clamp(4, -3, 5) == 4);
+ test(clamp(-3, 1, 8) == 1);
+ test(clamp(45, -6, 7) == 7);
+ test(clamp(-5, -2, -1) == -2);
+ test(clamp(-5, -9, -1) == -5);
+ test(clamp(6, 6, 9) == 6);
+ test(clamp(9, 6, 9) == 9);
+ test(clamp(decimal(4), decimal(-3), decimal(5)) == decimal(4));
+ test(clamp(decimal(-3), decimal(1), decimal(8)) == decimal(1));
+ test(clamp(decimal(45), decimal(-6), decimal(7)) == decimal(7));
+ test(clamp(decimal(-5), decimal(-2), decimal(-1)) == decimal(-2));
+ test(clamp(decimal(-5), decimal(-9), decimal(-1)) == decimal(-5));
+ test(clamp(decimal(6), decimal(6), decimal(9)) == decimal(6));
+ test(clamp(decimal(9), decimal(6), decimal(9)) == decimal(9));
+
+ // Test min3()
+ test(min3(1, 5, 7) == 1);
+ test(min3(-4, 2, 4) == -4);
+ test(min3(-1, -5, -7) == -7);
+ test(min3(13, 5, 47) == 5);
+ test(min3(4, 4, 4) == 4);
+
+ // Test max3()
+ test(max3(1, 5, 7) == 7);
+ test(max3(-4, 2, 4) == 4);
+ test(max3(-1, -5, -7) == -1);
+ test(max3(13, 5, 47) == 47);
+ test(max3(4, 4, 4) == 4);
+
+ // Test sameSign()
+ test(sameSign(4, 53));
+ test(sameSign(-4, -8));
+ test(!sameSign(4, -7));
+ test(!sameSign(-4, 53));
+
+ // Test computeBarycentricCoordinatesInTriangle()
+ Vector3 a(0, 0, 0);
+ Vector3 b(5, 0, 0);
+ Vector3 c(0, 0, 5);
+ Vector3 testPoint(4, 0, 1);
+ decimal u,v,w;
+ computeBarycentricCoordinatesInTriangle(a, b, c, a, u, v, w);
+ test(approxEqual(u, 1.0, 0.000001));
+ test(approxEqual(v, 0.0, 0.000001));
+ test(approxEqual(w, 0.0, 0.000001));
+ computeBarycentricCoordinatesInTriangle(a, b, c, b, u, v, w);
+ test(approxEqual(u, 0.0, 0.000001));
+ test(approxEqual(v, 1.0, 0.000001));
+ test(approxEqual(w, 0.0, 0.000001));
+ computeBarycentricCoordinatesInTriangle(a, b, c, c, u, v, w);
+ test(approxEqual(u, 0.0, 0.000001));
+ test(approxEqual(v, 0.0, 0.000001));
+ test(approxEqual(w, 1.0, 0.000001));
+
+ computeBarycentricCoordinatesInTriangle(a, b, c, testPoint, u, v, w);
+ test(approxEqual(u + v + w, 1.0, 0.000001));
+
+ // Test computeClosestPointBetweenTwoSegments()
+ Vector3 closestSeg1, closestSeg2;
+ computeClosestPointBetweenTwoSegments(Vector3(4, 0, 0), Vector3(6, 0, 0), Vector3(8, 0, 0), Vector3(8, 6, 0), closestSeg1, closestSeg2);
+ test(approxEqual(closestSeg1.x, 6.0, 0.000001));
+ test(approxEqual(closestSeg1.y, 0.0, 0.000001));
+ test(approxEqual(closestSeg1.z, 0.0, 0.000001));
+ test(approxEqual(closestSeg2.x, 8.0, 0.000001));
+ test(approxEqual(closestSeg2.y, 0.0, 0.000001));
+ test(approxEqual(closestSeg2.z, 0.0, 0.000001));
+ computeClosestPointBetweenTwoSegments(Vector3(4, 6, 5), Vector3(4, 6, 5), Vector3(8, 3, -9), Vector3(8, 3, -9), closestSeg1, closestSeg2);
+ test(approxEqual(closestSeg1.x, 4.0, 0.000001));
+ test(approxEqual(closestSeg1.y, 6.0, 0.000001));
+ test(approxEqual(closestSeg1.z, 5.0, 0.000001));
+ test(approxEqual(closestSeg2.x, 8.0, 0.000001));
+ test(approxEqual(closestSeg2.y, 3.0, 0.000001));
+ test(approxEqual(closestSeg2.z, -9.0, 0.000001));
+ computeClosestPointBetweenTwoSegments(Vector3(0, -5, 0), Vector3(0, 8, 0), Vector3(6, 3, 0), Vector3(10, -3, 0), closestSeg1, closestSeg2);
+ test(approxEqual(closestSeg1.x, 0.0, 0.000001));
+ test(approxEqual(closestSeg1.y, 3.0, 0.000001));
+ test(approxEqual(closestSeg1.z, 0.0, 0.000001));
+ test(approxEqual(closestSeg2.x, 6.0, 0.000001));
+ test(approxEqual(closestSeg2.y, 3.0, 0.000001));
+ test(approxEqual(closestSeg2.z, 0.0, 0.000001));
+ computeClosestPointBetweenTwoSegments(Vector3(1, -4, -5), Vector3(1, 4, -5), Vector3(-6, 5, -5), Vector3(6, 5, -5), closestSeg1, closestSeg2);
+ test(approxEqual(closestSeg1.x, 1.0, 0.000001));
+ test(approxEqual(closestSeg1.y, 5.0, 0.000001));
+ test(approxEqual(closestSeg1.z, -5.0, 0.000001));
+ test(approxEqual(closestSeg2.x, 1.0, 0.000001));
+ test(approxEqual(closestSeg2.y, 5.0, 0.000001));
+ test(approxEqual(closestSeg2.z, -5.0, 0.000001));
+
+ // Test computePlaneSegmentIntersection();
+ test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(-1, 0, 0)), 0.5, 0.000001));
+ test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(1, 0, 0)), 0.5, 0.000001));
+ test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 12, 0), Vector3(5, 4, 0), 6, Vector3(0, 1, 0)), 0.75, 0.000001));
+ test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 4, 8), Vector3(9, 14, 8), 4, Vector3(0, 1, 0)), 0.0, 0.000001));
+ decimal tIntersect = computePlaneSegmentIntersection(Vector3(5, 4, 0), Vector3(9, 4, 0), 4, Vector3(0, 1, 0));
+ test(tIntersect < 0.0 && tIntersect > 1.0);
+
+ // Test computeDistancePointToLineDistance()
+ test(approxEqual(computeDistancePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001));
+ test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(4, 3, 0)), 8.0, 0.000001));
+ test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001));
+ test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001));
+ test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001));
+ }
+
+ };
+
+}
+
+#endif
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index 8aae0145..3d692501 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -1,415 +1,417 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-// Libraries
-#include "CollisionDetectionScene.h"
-
-// Namespaces
-using namespace openglframework;
-using namespace collisiondetectionscene;
-
-// Constructor
-CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
- : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
- mContactManager(mPhongShader, mMeshFolderPath),
- mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) {
-
- mSelectedShapeIndex = 0;
- mIsContactPointsDisplayed = true;
- mIsWireframeEnabled = true;
-
- // Compute the radius and the center of the scene
- openglframework::Vector3 center(0, 0, 0);
-
- // Set the center of the scene
- setScenePosition(center, SCENE_RADIUS);
-
- // Create the dynamics world for the physics simulation
- mCollisionWorld = new rp3d::CollisionWorld();
-
- // ---------- Sphere 1 ---------- //
- openglframework::Vector3 position1(0, 0, 0);
-
- // Create a sphere and a corresponding collision body in the dynamics world
- mSphere1 = new Sphere(6, position1, mCollisionWorld, mMeshFolderPath);
- mAllShapes.push_back(mSphere1);
-
- // Set the color
- mSphere1->setColor(mGreyColorDemo);
- mSphere1->setSleepingColor(mRedColorDemo);
-
- // ---------- Sphere 2 ---------- //
- openglframework::Vector3 position2(4, 0, 0);
-
- // Create a sphere and a corresponding collision body in the dynamics world
- mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath);
- mAllShapes.push_back(mSphere2);
-
- // Set the color
- mSphere2->setColor(mGreyColorDemo);
- mSphere2->setSleepingColor(mRedColorDemo);
-
- // ---------- Cone ---------- //
- //openglframework::Vector3 position4(0, 0, 0);
-
- // Create a cone and a corresponding collision body in the dynamics world
- //mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld,
- // mMeshFolderPath);
-
- // Set the color
- //mCone->setColor(mGreyColorDemo);
- //mCone->setSleepingColor(mRedColorDemo);
-
- // ---------- Cylinder ---------- //
- //openglframework::Vector3 position5(0, 0, 0);
-
- // Create a cylinder and a corresponding collision body in the dynamics world
- //mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5,
- // mCollisionWorld, mMeshFolderPath);
-
- // Set the color
- //mCylinder->setColor(mGreyColorDemo);
- //mCylinder->setSleepingColor(mRedColorDemo);
-
- // ---------- Capsule ---------- //
- //openglframework::Vector3 position6(0, 0, 0);
-
- // Create a cylinder and a corresponding collision body in the dynamics world
- //mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 ,
- // mCollisionWorld, mMeshFolderPath);
-
- // Set the color
- //mCapsule->setColor(mGreyColorDemo);
- //mCapsule->setSleepingColor(mRedColorDemo);
-
- // ---------- Convex Mesh ---------- //
- //openglframework::Vector3 position7(0, 0, 0);
-
- // Create a convex mesh and a corresponding collision body in the dynamics world
- //mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj");
-
- // Set the color
- //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 + "city.obj");
-
- // Set the color
- //mConcaveMesh->setColor(mGreyColorDemo);
- //mConcaveMesh->setSleepingColor(mRedColorDemo);
-
- // ---------- Heightfield ---------- //
- //openglframework::Vector3 position9(0, 0, 0);
-
- // Create a convex mesh and a corresponding collision body in the dynamics world
- //mHeightField = new HeightField(position9, mCollisionWorld);
-
- // Set the color
- //mHeightField->setColor(mGreyColorDemo);
- //mHeightField->setSleepingColor(mRedColorDemo);
-
- // Create the VBO and VAO to render the lines
- createVBOAndVAO(mPhongShader);
-
- mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo);
-}
-
-// Reset the scene
-void CollisionDetectionScene::reset() {
-
-}
-
-// Destructor
-CollisionDetectionScene::~CollisionDetectionScene() {
-
- // Destroy the shader
- mPhongShader.destroy();
-
- // Destroy the box rigid body from the dynamics world
- //mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody());
- //delete mBox;
-
- // Destroy the spheres
- mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody());
- delete mSphere1;
-
- mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody());
- delete mSphere2;
-
- /*
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody());
- delete mCone;
-
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody());
-
- // Destroy the sphere
- delete mCylinder;
-
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody());
-
- // Destroy the sphere
- delete mCapsule;
-
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody());
-
- // Destroy the convex mesh
- delete mConvexMesh;
-
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody());
-
- // Destroy the dumbbell
- delete mDumbbell;
-
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody());
-
- // Destroy the convex mesh
- delete mConcaveMesh;
-
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody());
-
- // Destroy the convex mesh
- delete mHeightField;
- */
-
- mContactManager.resetPoints();
-
- // Destroy the static data for the visual contact points
- VisualContactPoint::destroyStaticData();
-
- // Destroy the collision world
- delete mCollisionWorld;
-
- // Destroy the VBOs and VAO
- mVBOVertices.destroy();
- mVAO.destroy();
-}
-
-// Update the physics world (take a simulation step)
-void CollisionDetectionScene::updatePhysics() {
-
-
-}
-
-// Take a step for the simulation
-void CollisionDetectionScene::update() {
-
- mContactManager.resetPoints();
-
- mCollisionWorld->testCollision(&mContactManager);
-
- SceneDemo::update();
-}
-
-// Render the scene
-void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
-
- /*
- // Bind the VAO
- mVAO.bind();
-
- // Bind the shader
- shader.bind();
-
- mVBOVertices.bind();
-
- // Set the model to camera matrix
- const Matrix4 localToCameraMatrix = Matrix4::identity();
- shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix);
- shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
-
- // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
- // model-view matrix)
- const openglframework::Matrix3 normalMatrix =
- localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
- shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
-
- // Set the vertex color
- openglframework::Vector4 color(1, 0, 0, 1);
- shader.setVector4Uniform("vertexColor", color, false);
-
- // Get the location of shader attribute variables
- GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
-
- glEnableVertexAttribArray(vertexPositionLoc);
- glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL);
-
- // Draw the lines
- glDrawArrays(GL_LINES, 0, NB_RAYS);
-
- glDisableVertexAttribArray(vertexPositionLoc);
-
- mVBOVertices.unbind();
-
- // Unbind the VAO
- mVAO.unbind();
-
- shader.unbind();
- */
-
- // Render the shapes
- if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
-
- /*
- if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
- if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix);
- if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix);
- 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);
- if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix);
- */
-
- shader.unbind();
-}
-
-// Create the Vertex Buffer Objects used to render with OpenGL.
-/// We create two VBOs (one for vertices and one for indices)
-void CollisionDetectionScene::createVBOAndVAO(openglframework::Shader& shader) {
-
- // Bind the shader
- shader.bind();
-
- // Create the VBO for the vertices data
- mVBOVertices.create();
- mVBOVertices.bind();
- size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::Vector3);
- mVBOVertices.copyDataIntoVBO(sizeVertices, &mLinePoints[0], GL_STATIC_DRAW);
- mVBOVertices.unbind();
-
- // Create the VAO for both VBOs
- mVAO.create();
- mVAO.bind();
-
- // Bind the VBO of vertices
- mVBOVertices.bind();
-
- // Unbind the VAO
- mVAO.unbind();
-
- // Unbind the shader
- shader.unbind();
-}
-
-void CollisionDetectionScene::selectNextShape() {
-
- int previousIndex = mSelectedShapeIndex;
- mSelectedShapeIndex++;
- if (mSelectedShapeIndex >= mAllShapes.size()) {
- mSelectedShapeIndex = 0;
- }
-
- mAllShapes[previousIndex]->setColor(mGreyColorDemo);
- mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo);
-}
-
-// Called when a keyboard event occurs
-bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, int mods) {
-
- // If the space key has been pressed
- if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) {
- selectNextShape();
- return true;
- }
-
- float stepDist = 0.5f;
- float stepAngle = 20 * (3.14f / 180.0f);
-
- if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setPosition(transform.getPosition() + rp3d::Vector3(stepDist, 0, 0));
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
- else if (key == GLFW_KEY_LEFT && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setPosition(transform.getPosition() + rp3d::Vector3(-stepDist, 0, 0));
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
- else if (key == GLFW_KEY_UP && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setPosition(transform.getPosition() + rp3d::Vector3(0, stepDist, 0));
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
- else if (key == GLFW_KEY_DOWN && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setPosition(transform.getPosition() + rp3d::Vector3(0, -stepDist, 0));
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
- else if (key == GLFW_KEY_Z && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, stepDist));
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
- else if (key == GLFW_KEY_H && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, -stepDist));
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
- else if (key == GLFW_KEY_A && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(0, stepAngle, 0) * transform.getOrientation());
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
- else if (key == GLFW_KEY_D && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(0, -stepAngle, 0) * transform.getOrientation());
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
- else if (key == GLFW_KEY_W && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(stepAngle, 0, 0) * transform.getOrientation());
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
- else if (key == GLFW_KEY_S && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(-stepAngle, 0, 0) * transform.getOrientation());
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
- else if (key == GLFW_KEY_F && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(0, 0, stepAngle) * transform.getOrientation());
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
- else if (key == GLFW_KEY_G && action == GLFW_PRESS) {
- rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(0, 0, -stepAngle) * transform.getOrientation());
- mAllShapes[mSelectedShapeIndex]->setTransform(transform);
- }
-
- return false;
-}
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "CollisionDetectionScene.h"
+
+// Namespaces
+using namespace openglframework;
+using namespace collisiondetectionscene;
+
+// Constructor
+CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
+ : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
+ mContactManager(mPhongShader, mMeshFolderPath),
+ mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) {
+
+ mSelectedShapeIndex = 0;
+ mIsContactPointsDisplayed = true;
+ mIsWireframeEnabled = true;
+
+ // Compute the radius and the center of the scene
+ openglframework::Vector3 center(0, 0, 0);
+
+ // Set the center of the scene
+ setScenePosition(center, SCENE_RADIUS);
+
+ // Create the dynamics world for the physics simulation
+ mCollisionWorld = new rp3d::CollisionWorld();
+
+ // ---------- Sphere 1 ---------- //
+ openglframework::Vector3 position1(0, 0, 0);
+
+ // Create a sphere and a corresponding collision body in the dynamics world
+ mSphere1 = new Sphere(6, position1, mCollisionWorld, mMeshFolderPath);
+ mAllShapes.push_back(mSphere1);
+
+ // Set the color
+ mSphere1->setColor(mGreyColorDemo);
+ mSphere1->setSleepingColor(mRedColorDemo);
+
+ // ---------- Sphere 2 ---------- //
+ openglframework::Vector3 position2(4, 0, 0);
+
+ // Create a sphere and a corresponding collision body in the dynamics world
+ mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath);
+ mAllShapes.push_back(mSphere2);
+
+ // Set the color
+ mSphere2->setColor(mGreyColorDemo);
+ mSphere2->setSleepingColor(mRedColorDemo);
+
+ // ---------- Capsule 1 ---------- //
+ openglframework::Vector3 position3(4, 0, 0);
+
+ // Create a cylinder and a corresponding collision body in the dynamics world
+ mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath);
+ mAllShapes.push_back(mCapsule1);
+
+ // Set the color
+ mCapsule1->setColor(mGreyColorDemo);
+ mCapsule1->setSleepingColor(mRedColorDemo);
+
+ // ---------- Cone ---------- //
+ //openglframework::Vector3 position4(0, 0, 0);
+
+ // Create a cone and a corresponding collision body in the dynamics world
+ //mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld,
+ // mMeshFolderPath);
+
+ // Set the color
+ //mCone->setColor(mGreyColorDemo);
+ //mCone->setSleepingColor(mRedColorDemo);
+
+ // ---------- Cylinder ---------- //
+ //openglframework::Vector3 position5(0, 0, 0);
+
+ // Create a cylinder and a corresponding collision body in the dynamics world
+ //mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5,
+ // mCollisionWorld, mMeshFolderPath);
+
+ // Set the color
+ //mCylinder->setColor(mGreyColorDemo);
+ //mCylinder->setSleepingColor(mRedColorDemo);
+
+
+ // ---------- Convex Mesh ---------- //
+ //openglframework::Vector3 position7(0, 0, 0);
+
+ // Create a convex mesh and a corresponding collision body in the dynamics world
+ //mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj");
+
+ // Set the color
+ //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 + "city.obj");
+
+ // Set the color
+ //mConcaveMesh->setColor(mGreyColorDemo);
+ //mConcaveMesh->setSleepingColor(mRedColorDemo);
+
+ // ---------- Heightfield ---------- //
+ //openglframework::Vector3 position9(0, 0, 0);
+
+ // Create a convex mesh and a corresponding collision body in the dynamics world
+ //mHeightField = new HeightField(position9, mCollisionWorld);
+
+ // Set the color
+ //mHeightField->setColor(mGreyColorDemo);
+ //mHeightField->setSleepingColor(mRedColorDemo);
+
+ // Create the VBO and VAO to render the lines
+ //createVBOAndVAO(mPhongShader);
+
+ mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo);
+}
+
+// Reset the scene
+void CollisionDetectionScene::reset() {
+
+}
+
+// Destructor
+CollisionDetectionScene::~CollisionDetectionScene() {
+
+ // Destroy the shader
+ mPhongShader.destroy();
+
+ // Destroy the box rigid body from the dynamics world
+ //mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody());
+ //delete mBox;
+
+ // Destroy the spheres
+ mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody());
+ delete mSphere1;
+
+ mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody());
+ delete mSphere2;
+
+ /*
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody());
+ delete mCone;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody());
+
+ // Destroy the sphere
+ delete mCylinder;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody());
+
+ // Destroy the sphere
+ delete mCapsule;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody());
+
+ // Destroy the convex mesh
+ delete mConvexMesh;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody());
+
+ // Destroy the dumbbell
+ delete mDumbbell;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody());
+
+ // Destroy the convex mesh
+ delete mConcaveMesh;
+
+ // Destroy the corresponding rigid body from the dynamics world
+ mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody());
+
+ // Destroy the convex mesh
+ delete mHeightField;
+ */
+
+ mContactManager.resetPoints();
+
+ // Destroy the static data for the visual contact points
+ VisualContactPoint::destroyStaticData();
+
+ // Destroy the collision world
+ delete mCollisionWorld;
+
+ // Destroy the VBOs and VAO
+ mVBOVertices.destroy();
+ mVAO.destroy();
+}
+
+// Update the physics world (take a simulation step)
+void CollisionDetectionScene::updatePhysics() {
+
+
+}
+
+// Take a step for the simulation
+void CollisionDetectionScene::update() {
+
+ mContactManager.resetPoints();
+
+ mCollisionWorld->testCollision(&mContactManager);
+
+ SceneDemo::update();
+}
+
+// Render the scene
+void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
+ const openglframework::Matrix4& worldToCameraMatrix) {
+
+ /*
+ // Bind the VAO
+ mVAO.bind();
+
+ // Bind the shader
+ shader.bind();
+
+ mVBOVertices.bind();
+
+ // Set the model to camera matrix
+ const Matrix4 localToCameraMatrix = Matrix4::identity();
+ shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix);
+ shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
+
+ // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
+ // model-view matrix)
+ const openglframework::Matrix3 normalMatrix =
+ localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
+ shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
+
+ // Set the vertex color
+ openglframework::Vector4 color(1, 0, 0, 1);
+ shader.setVector4Uniform("vertexColor", color, false);
+
+ // Get the location of shader attribute variables
+ GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
+
+ glEnableVertexAttribArray(vertexPositionLoc);
+ glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL);
+
+ // Draw the lines
+ glDrawArrays(GL_LINES, 0, NB_RAYS);
+
+ glDisableVertexAttribArray(vertexPositionLoc);
+
+ mVBOVertices.unbind();
+
+ // Unbind the VAO
+ mVAO.unbind();
+
+ shader.unbind();
+ */
+
+ // Render the shapes
+ if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+
+ /*
+ if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
+ if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix);
+ if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix);
+ 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);
+ if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix);
+ */
+
+ shader.unbind();
+}
+
+// Create the Vertex Buffer Objects used to render with OpenGL.
+/// We create two VBOs (one for vertices and one for indices)
+void CollisionDetectionScene::createVBOAndVAO(openglframework::Shader& shader) {
+
+ // Bind the shader
+ shader.bind();
+
+ // Create the VBO for the vertices data
+ mVBOVertices.create();
+ mVBOVertices.bind();
+ size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::Vector3);
+ mVBOVertices.copyDataIntoVBO(sizeVertices, &mLinePoints[0], GL_STATIC_DRAW);
+ mVBOVertices.unbind();
+
+ // Create the VAO for both VBOs
+ mVAO.create();
+ mVAO.bind();
+
+ // Bind the VBO of vertices
+ mVBOVertices.bind();
+
+ // Unbind the VAO
+ mVAO.unbind();
+
+ // Unbind the shader
+ shader.unbind();
+}
+
+void CollisionDetectionScene::selectNextShape() {
+
+ int previousIndex = mSelectedShapeIndex;
+ mSelectedShapeIndex++;
+ if (mSelectedShapeIndex >= mAllShapes.size()) {
+ mSelectedShapeIndex = 0;
+ }
+
+ mAllShapes[previousIndex]->setColor(mGreyColorDemo);
+ mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo);
+}
+
+// Called when a keyboard event occurs
+bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, int mods) {
+
+ // If the space key has been pressed
+ if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) {
+ selectNextShape();
+ return true;
+ }
+
+ float stepDist = 0.5f;
+ float stepAngle = 20 * (3.14f / 180.0f);
+
+ if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(stepDist, 0, 0));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_LEFT && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(-stepDist, 0, 0));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_UP && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(0, stepDist, 0));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_DOWN && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(0, -stepDist, 0));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_Z && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, stepDist));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_H && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setPosition(transform.getPosition() + rp3d::Vector3(0, 0, -stepDist));
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_A && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(0, stepAngle, 0) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_D && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(0, -stepAngle, 0) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_W && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(stepAngle, 0, 0) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_S && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(-stepAngle, 0, 0) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_F && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(0, 0, stepAngle) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+ else if (key == GLFW_KEY_G && action == GLFW_PRESS) {
+ rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
+ transform.setOrientation(rp3d::Quaternion(0, 0, -stepAngle) * transform.getOrientation());
+ mAllShapes[mSelectedShapeIndex]->setTransform(transform);
+ }
+
+ return false;
+}
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index d3a7de00..323b9c34 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -1,231 +1,235 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-
-#ifndef COLLISION_DETECTION_SCENE_H
-#define COLLISION_DETECTION_SCENE_H
-
-// Libraries
-#include <cmath>
-#include "openglframework.h"
-#include "reactphysics3d.h"
-#include "SceneDemo.h"
-#include "Sphere.h"
-#include "Box.h"
-#include "Capsule.h"
-#include "Line.h"
-#include "ConvexMesh.h"
-#include "ConcaveMesh.h"
-#include "HeightField.h"
-#include "Dumbbell.h"
-#include "VisualContactPoint.h"
-
-namespace collisiondetectionscene {
-
-// Constants
-const float SCENE_RADIUS = 30.0f;
-const openglframework::Vector3 BOX_SIZE(4, 2, 1);
-const float SPHERE_RADIUS = 3.0f;
-const float CONE_RADIUS = 3.0f;
-const float CONE_HEIGHT = 5.0f;
-const float CYLINDER_RADIUS = 3.0f;
-const float CYLINDER_HEIGHT = 5.0f;
-const float CAPSULE_RADIUS = 3.0f;
-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 = 9;
-
-// Raycast manager
-class ContactManager : public rp3d::CollisionCallback {
-
- private:
-
- /// All the visual contact points
- std::vector<ContactPoint> mContactPoints;
-
- /// All the normals at contact points
- std::vector<Line*> mNormals;
-
- /// Contact point mesh folder path
- std::string mMeshFolderPath;
-
- public:
-
- ContactManager(openglframework::Shader& shader,
- const std::string& meshFolderPath)
- : mMeshFolderPath(meshFolderPath) {
-
- }
-
- /// This method will be called for each reported contact point
- virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
-
- rp3d::Vector3 point1 = collisionCallbackInfo.contactPoint.localPoint1;
- point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
- openglframework::Vector3 position1(point1.x, point1.y, point1.z);
- mContactPoints.push_back(ContactPoint(position1));
-
- rp3d::Vector3 point2 = collisionCallbackInfo.contactPoint.localPoint2;
- point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
- openglframework::Vector3 position2(point2.x, point2.y, point2.z);
- mContactPoints.push_back(ContactPoint(position2));
-
- // Create a line to display the normal at hit point
- rp3d::Vector3 n = collisionCallbackInfo.contactPoint.normal;
- openglframework::Vector3 normal(n.x, n.y, n.z);
- Line* normalLine = new Line(position1, position1 + normal);
- mNormals.push_back(normalLine);
- }
-
- void resetPoints() {
-
- mContactPoints.clear();
-
- // Destroy all the normals
- for (std::vector<Line*>::iterator it = mNormals.begin();
- it != mNormals.end(); ++it) {
- delete (*it);
- }
- mNormals.clear();
- }
-
- std::vector<ContactPoint> getContactPoints() const {
- return mContactPoints;
- }
-};
-
-// Class CollisionDetectionScene
-class CollisionDetectionScene : public SceneDemo {
-
- private :
-
- // -------------------- Attributes -------------------- //
-
- /// Contact point mesh folder path
- std::string mMeshFolderPath;
-
- /// Contact manager
- ContactManager mContactManager;
-
- bool mAreNormalsDisplayed;
-
- /// All objects on the scene
- //Box* mBox;
- Sphere* mSphere1;
- Sphere* mSphere2;
- //Cone* mCone;
- //Cylinder* mCylinder;
- //Capsule* mCapsule;
- //ConvexMesh* mConvexMesh;
- //Dumbbell* mDumbbell;
- //ConcaveMesh* mConcaveMesh;
- //HeightField* mHeightField;
-
- std::vector<PhysicsObject*> mAllShapes;
-
- int mSelectedShapeIndex;
-
- /// Collision world used for the physics simulation
- rp3d::CollisionWorld* mCollisionWorld;
-
- /// All the points to render the lines
- std::vector<openglframework::Vector3> mLinePoints;
-
- /// Vertex Buffer Object for the vertices data
- openglframework::VertexBufferObject mVBOVertices;
-
- /// Vertex Array Object for the vertex data
- openglframework::VertexArrayObject mVAO;
-
- /// Create the Vertex Buffer Objects used to render with OpenGL.
- void createVBOAndVAO(openglframework::Shader& shader);
-
- /// Select the next shape
- void selectNextShape();
-
- public:
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- CollisionDetectionScene(const std::string& name);
-
- /// Destructor
- virtual ~CollisionDetectionScene() override;
-
- /// Update the physics world (take a simulation step)
- /// Can be called several times per frame
- virtual void updatePhysics() override;
-
- /// Take a step for the simulation
- virtual void update() override;
-
- /// Render the scene in a single pass
- virtual void renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) override;
-
- /// Reset the scene
- virtual void reset() override;
-
- /// Display or not the surface normals at hit points
- void showHideNormals();
-
- /// Called when a keyboard event occurs
- virtual bool keyboardEvent(int key, int scancode, int action, int mods) override;
-
- /// Enabled/Disable the shadow mapping
- virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override;
-
- /// Display/Hide the contact points
- virtual void setIsContactPointsDisplayed(bool display) override;
-
- /// Return all the contact points of the scene
- virtual std::vector<ContactPoint> getContactPoints() const override;
-};
-
-// Display or not the surface normals at hit points
-inline void CollisionDetectionScene::showHideNormals() {
- mAreNormalsDisplayed = !mAreNormalsDisplayed;
-}
-
-// Enabled/Disable the shadow mapping
-inline void CollisionDetectionScene::setIsShadowMappingEnabled(bool isShadowMappingEnabled) {
- SceneDemo::setIsShadowMappingEnabled(false);
-}
-
-// Display/Hide the contact points
-inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) {
- SceneDemo::setIsContactPointsDisplayed(true);
-}
-
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> CollisionDetectionScene::getContactPoints() const {
- return mContactManager.getContactPoints();
-}
-
-}
-
-#endif
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef COLLISION_DETECTION_SCENE_H
+#define COLLISION_DETECTION_SCENE_H
+
+// Libraries
+#include <cmath>
+#include "openglframework.h"
+#include "reactphysics3d.h"
+#include "SceneDemo.h"
+#include "Sphere.h"
+#include "Box.h"
+#include "Cone.h"
+#include "Cylinder.h"
+#include "Capsule.h"
+#include "Line.h"
+#include "ConvexMesh.h"
+#include "ConcaveMesh.h"
+#include "HeightField.h"
+#include "Dumbbell.h"
+#include "VisualContactPoint.h"
+
+namespace collisiondetectionscene {
+
+// Constants
+const float SCENE_RADIUS = 30.0f;
+const openglframework::Vector3 BOX_SIZE(4, 2, 1);
+const float SPHERE_RADIUS = 3.0f;
+const float CONE_RADIUS = 3.0f;
+const float CONE_HEIGHT = 5.0f;
+const float CYLINDER_RADIUS = 3.0f;
+const float CYLINDER_HEIGHT = 5.0f;
+const float CAPSULE_RADIUS = 3.0f;
+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 = 9;
+
+// Raycast manager
+class ContactManager : public rp3d::CollisionCallback {
+
+ private:
+
+ /// All the visual contact points
+ std::vector<ContactPoint> mContactPoints;
+
+ /// All the normals at contact points
+ std::vector<Line*> mNormals;
+
+ /// Contact point mesh folder path
+ std::string mMeshFolderPath;
+
+ public:
+
+ ContactManager(openglframework::Shader& shader,
+ const std::string& meshFolderPath)
+ : mMeshFolderPath(meshFolderPath) {
+
+ }
+
+ /// This method will be called for each reported contact point
+ virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
+
+ rp3d::Vector3 point1 = collisionCallbackInfo.contactPoint.localPoint1;
+ point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
+ openglframework::Vector3 position1(point1.x, point1.y, point1.z);
+ mContactPoints.push_back(ContactPoint(position1));
+
+ rp3d::Vector3 point2 = collisionCallbackInfo.contactPoint.localPoint2;
+ point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
+ openglframework::Vector3 position2(point2.x, point2.y, point2.z);
+ mContactPoints.push_back(ContactPoint(position2));
+
+ // Create a line to display the normal at hit point
+ rp3d::Vector3 n = collisionCallbackInfo.contactPoint.normal;
+ openglframework::Vector3 normal(n.x, n.y, n.z);
+ Line* normalLine = new Line(position1, position1 + normal);
+ mNormals.push_back(normalLine);
+ }
+
+ void resetPoints() {
+
+ mContactPoints.clear();
+
+ // Destroy all the normals
+ for (std::vector<Line*>::iterator it = mNormals.begin();
+ it != mNormals.end(); ++it) {
+ delete (*it);
+ }
+ mNormals.clear();
+ }
+
+ std::vector<ContactPoint> getContactPoints() const {
+ return mContactPoints;
+ }
+};
+
+// Class CollisionDetectionScene
+class CollisionDetectionScene : public SceneDemo {
+
+ private :
+
+ // -------------------- Attributes -------------------- //
+
+ /// Contact point mesh folder path
+ std::string mMeshFolderPath;
+
+ /// Contact manager
+ ContactManager mContactManager;
+
+ bool mAreNormalsDisplayed;
+
+ /// All objects on the scene
+ //Box* mBox;
+ Sphere* mSphere1;
+ Sphere* mSphere2;
+ Capsule* mCapsule1;
+ Capsule* mCapsule2;
+ //Cone* mCone;
+ //Cylinder* mCylinder;
+ //Capsule* mCapsule;
+ //ConvexMesh* mConvexMesh;
+ //Dumbbell* mDumbbell;
+ //ConcaveMesh* mConcaveMesh;
+ //HeightField* mHeightField;
+
+ std::vector<PhysicsObject*> mAllShapes;
+
+ int mSelectedShapeIndex;
+
+ /// Collision world used for the physics simulation
+ rp3d::CollisionWorld* mCollisionWorld;
+
+ /// All the points to render the lines
+ std::vector<openglframework::Vector3> mLinePoints;
+
+ /// Vertex Buffer Object for the vertices data
+ openglframework::VertexBufferObject mVBOVertices;
+
+ /// Vertex Array Object for the vertex data
+ openglframework::VertexArrayObject mVAO;
+
+ /// Create the Vertex Buffer Objects used to render with OpenGL.
+ void createVBOAndVAO(openglframework::Shader& shader);
+
+ /// Select the next shape
+ void selectNextShape();
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ CollisionDetectionScene(const std::string& name);
+
+ /// Destructor
+ virtual ~CollisionDetectionScene() override;
+
+ /// Update the physics world (take a simulation step)
+ /// Can be called several times per frame
+ virtual void updatePhysics() override;
+
+ /// Take a step for the simulation
+ virtual void update() override;
+
+ /// Render the scene in a single pass
+ virtual void renderSinglePass(openglframework::Shader& shader,
+ const openglframework::Matrix4& worldToCameraMatrix) override;
+
+ /// Reset the scene
+ virtual void reset() override;
+
+ /// Display or not the surface normals at hit points
+ void showHideNormals();
+
+ /// Called when a keyboard event occurs
+ virtual bool keyboardEvent(int key, int scancode, int action, int mods) override;
+
+ /// Enabled/Disable the shadow mapping
+ virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override;
+
+ /// Display/Hide the contact points
+ virtual void setIsContactPointsDisplayed(bool display) override;
+
+ /// Return all the contact points of the scene
+ virtual std::vector<ContactPoint> getContactPoints() const override;
+};
+
+// Display or not the surface normals at hit points
+inline void CollisionDetectionScene::showHideNormals() {
+ mAreNormalsDisplayed = !mAreNormalsDisplayed;
+}
+
+// Enabled/Disable the shadow mapping
+inline void CollisionDetectionScene::setIsShadowMappingEnabled(bool isShadowMappingEnabled) {
+ SceneDemo::setIsShadowMappingEnabled(false);
+}
+
+// Display/Hide the contact points
+inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) {
+ SceneDemo::setIsContactPointsDisplayed(true);
+}
+
+// Return all the contact points of the scene
+inline std::vector<ContactPoint> CollisionDetectionScene::getContactPoints() const {
+ return mContactManager.getContactPoints();
+}
+
+}
+
+#endif
From 7a656aedc93e7931b0afca9983696e8f754f7a71 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 13 Feb 2017 22:38:47 +0100
Subject: [PATCH 036/133] Working on HalfEdgeStructure
---
src/collision/HalfEdgeStructure.cpp | 42 +++++-----
src/collision/HalfEdgeStructure.h | 6 +-
src/collision/PolyhedronMesh.h | 2 +-
.../narrowphase/DefaultCollisionDispatch.cpp | 11 ++-
.../narrowphase/DefaultCollisionDispatch.h | 9 ++-
.../narrowphase/GJK/GJKAlgorithm.cpp | 1 +
.../narrowphase/SphereVsCapsuleAlgorithm.cpp | 80 +++++++++++++++++++
.../narrowphase/SphereVsCapsuleAlgorithm.h | 71 ++++++++++++++++
src/collision/shapes/CollisionShape.h | 2 +-
src/mathematics/mathematics_functions.cpp | 6 +-
src/reactphysics3d.h | 1 +
.../CollisionDetectionScene.h | 2 -
12 files changed, 197 insertions(+), 36 deletions(-)
create mode 100644 src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
create mode 100644 src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp
index 16a79704..dba49c15 100644
--- a/src/collision/HalfEdgeStructure.cpp
+++ b/src/collision/HalfEdgeStructure.cpp
@@ -30,13 +30,14 @@
using namespace reactphysics3d;
// Initialize the structure
-void HalfEdgeStructure::init(std::vector<const Vector3> vertices, std::vector<std::vector<uint>> faces) {
+void HalfEdgeStructure::init(std::vector<Vector3> vertices, std::vector<std::vector<uint>> faces) {
using edgeKey = std::pair<uint, uint>;
std::map<edgeKey, Edge> edges;
std::map<edgeKey, edgeKey> nextEdges;
std::map<edgeKey, uint> mapEdgeToStartVertex;
+ std::map<edgeKey, uint> mapEdgeToIndex;
// For each vertices
for (uint v=0; v<vertices.size(); v++) {
@@ -58,10 +59,10 @@ void HalfEdgeStructure::init(std::vector<const Vector3> vertices, std::vector<st
edgeKey firstEdgeKey;
- // For each edge of the face
- for (uint e=0; e < faceVertices.size(); e++) {
- uint v1Index = faceVertices[e];
- uint v2Index = faceVertices[e == (faceVertices.size() - 1) ? 0 : e + 1];
+ // For each vertex of the face
+ for (uint v=0; v < faceVertices.size(); v++) {
+ uint v1Index = faceVertices[v];
+ uint v2Index = faceVertices[v == (faceVertices.size() - 1) ? 0 : v + 1];
const edgeKey pairV1V2 = std::make_pair(v1Index, v2Index);
@@ -69,21 +70,21 @@ void HalfEdgeStructure::init(std::vector<const Vector3> vertices, std::vector<st
Edge edge;
edge.faceIndex = f;
edge.vertexIndex = v1Index;
- if (e == 0) {
+ if (v == 0) {
firstEdgeKey = pairV1V2;
}
- else if (e >= 1) {
- nextEdges.insert(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2);
+ else if (v >= 1) {
+ nextEdges.insert(std::make_pair(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2));
}
- if (e == (faceVertices.size() - 1)) {
- nextEdges.insert(pairV1V2, firstEdgeKey);
+ if (v == (faceVertices.size() - 1)) {
+ nextEdges.insert(std::make_pair(pairV1V2, firstEdgeKey));
}
- edges.insert(pairV1V2, edge);
+ edges.insert(std::make_pair(pairV1V2, edge));
const edgeKey pairV2V1 = std::make_pair(v2Index, v1Index);
- mapEdgeToStartVertex.insert(pairV1V2, v1Index);
- mapEdgeToStartVertex.insert(pairV2V1, v2Index);
+ mapEdgeToStartVertex.insert(std::make_pair(pairV1V2, v1Index));
+ mapEdgeToStartVertex.insert(std::make_pair(pairV2V1, v2Index));
auto itEdge = edges.find(pairV2V1);
if (itEdge != edges.end()) {
@@ -93,24 +94,25 @@ void HalfEdgeStructure::init(std::vector<const Vector3> vertices, std::vector<st
mEdges.push_back(edge);
itEdge->second.twinEdgeIndex = edgeIndex + 1;
- itEdge->second.nextEdgeIndex = nextEdges[pairV2V1];
edge.twinEdgeIndex = edgeIndex;
- edge.nextEdgeIndex = edges[nextEdges[pairV1V2]].;
mVertices[v1Index].edgeIndex = edgeIndex;
mVertices[v2Index].edgeIndex = edgeIndex + 1;
+ mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1));
+ mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex));
+
face.edgeIndex = edgeIndex + 1;
}
currentFaceEdges.push_back(pairV1V2);
}
+ }
- // For each edge of the face
- for (uint e=0; e < currentFaceEdges.size(); e++) {
- Edge& edge = currentFaceEdges[e];
- edge.nextEdgeIndex =
- }
+ // Set next edges
+ std::map<edgeKey, Edge>::iterator it;
+ for (it = edges.begin(); it != edges.end(); ++it) {
+ it->second.nextEdgeIndex = mapEdgeToIndex[nextEdges[it->first]];
}
}
diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h
index 71b549d0..4a3f67ed 100644
--- a/src/collision/HalfEdgeStructure.h
+++ b/src/collision/HalfEdgeStructure.h
@@ -54,11 +54,11 @@ class HalfEdgeStructure {
};
struct Vertex {
- const Vector3 point; // Coordinates of the vertex
+ Vector3 point; // Coordinates of the vertex
uint edgeIndex; // Index of one edge emanting from this vertex
/// Constructor
- Vertex(const Vector3& p) { point = p;}
+ Vertex(Vector3& p) { point = p;}
};
private:
@@ -81,7 +81,7 @@ class HalfEdgeStructure {
~HalfEdgeStructure() = default;
/// Initialize the structure
- void init(std::vector<const Vector3> vertices, std::vector<std::vector<uint>> faces);
+ void init(std::vector<Vector3> vertices, std::vector<std::vector<uint>> faces);
/// Return the number of faces
uint getNbFaces() const;
diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h
index 8f0e8abf..c7182bd8 100644
--- a/src/collision/PolyhedronMesh.h
+++ b/src/collision/PolyhedronMesh.h
@@ -49,7 +49,7 @@ class PolyhedronMesh {
bool mIsFinalized;
/// All the vertices
- std::vector<const Vector3> mVertices;
+ std::vector<Vector3> mVertices;
/// All the indexes of the face vertices
std::vector<std::vector<uint>> mFaces;
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
index 4864dbbe..88d4b9b2 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
@@ -33,7 +33,6 @@ using namespace reactphysics3d;
// use between two types of collision shapes.
NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) {
-
CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
@@ -45,9 +44,13 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) {
return &mSphereVsSphereAlgorithm;
}
- // Sphere vs Convex shapes (convex Mesh, BoxShape) algorithm
- if (shape1Type == CollisionShapeType::SPHERE && CollisionShape::isConvex(shape2Type)) {
- return &mSphereVsConvexMeshAlgorithm;
+ // Sphere vs Capsule algorithm
+ if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::SPHERE) {
+ return &mSphereVsCapsuleAlgorithm;
+ }
+ // Capsule vs Capsule algorithm
+ if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CAPSULE) {
+ return &mCapsuleVsCapsuleAlgorithm;
}
return nullptr;
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h
index 07a819c0..65e05497 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.h
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.h
@@ -31,6 +31,8 @@
#include "ConcaveVsConvexAlgorithm.h"
#include "SphereVsSphereAlgorithm.h"
#include "SphereVsConvexMeshAlgorithm.h"
+#include "SphereVsCapsuleAlgorithm.h"
+#include "CapsuleVsCapsuleAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
namespace reactphysics3d {
@@ -51,8 +53,11 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Sphere vs Convex Mesh collision algorithm
SphereVsConvexMeshAlgorithm mSphereVsConvexMeshAlgorithm;
- /// GJK Algorithm
- GJKAlgorithm mGJKAlgorithm;
+ /// Sphere vs Capsule collision algorithm
+ SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm;
+
+ /// Capsule vs Capsule collision algorithm
+ CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm;
public:
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index cd84f5a0..4ea55ba4 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -26,6 +26,7 @@
// Libraries
#include "GJKAlgorithm.h"
#include "constraint/ContactPoint.h"
+#include "engine/OverlappingPair.h"
#include "configuration.h"
#include "engine/Profiler.h"
#include <algorithm>
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
new file mode 100644
index 00000000..24cb4e96
--- /dev/null
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
@@ -0,0 +1,80 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "SphereVsCapsuleAlgorithm.h"
+#include "collision/shapes/SphereShape.h"
+#include "collision/shapes/CapsuleShape.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) {
+
+ // Get the collision shapes
+ const SphereShape* sphereShape = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
+ const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
+
+ // Get the transform from sphere local-space to capsule local-space
+ const Transform sphereToCapsuleSpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse();
+
+ // Transform the center of the sphere into the local-space of the capsule shape
+ const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition();
+
+ // Compute the end-points of the inner segment of the capsule
+ const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
+ const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
+
+ // Compute the point on the inner capsule segment that is the closes to center of sphere
+ const Vector3 closestPointOnSegment = computeClosestPointOnSegment(capsuleSegA, capsuleSegB, sphereCenter);
+
+ // Compute the distance between the sphere center and the closest point on the segment
+ Vector3 sphereCenterToSegment = (closestPointOnSegment - sphereCenter);
+ const decimal sphereSegmentDistanceSquare = sphereCenterToSegment.lengthSquare();
+
+ // Compute the sum of the radius of the sphere and the capsule (virtual sphere)
+ decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius();
+
+ // If the collision shapes overlap
+ if (sphereSegmentDistanceSquare <= sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) {
+
+ decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
+ sphereCenterToSegment /= sphereSegmentDistance;
+
+ const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
+ const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
+
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * sphereCenterToSegment;
+
+ decimal penetrationDepth = sumRadius - sphereSegmentDistance;
+
+ // Create the contact info object
+ contactPointInfo.init(normalWorld, penetrationDepth, contactPointSphereLocal, contactPointCapsuleLocal);
+
+ return true;
+ }
+
+ return false;
+}
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
new file mode 100644
index 00000000..98eaf894
--- /dev/null
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
@@ -0,0 +1,71 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_SPHERE_VS_CAPSULE_ALGORITHM_H
+#define REACTPHYSICS3D_SPHERE_VS_CAPSULE_ALGORITHM_H
+
+// Libraries
+#include "body/Body.h"
+#include "constraint/ContactPoint.h"
+#include "NarrowPhaseAlgorithm.h"
+
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class SphereVsCapsuleAlgorithm
+/**
+ * This class is used to compute the narrow-phase collision detection
+ * between a sphere collision shape and a capsule collision shape.
+ */
+class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
+
+ protected :
+
+ public :
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ SphereVsCapsuleAlgorithm() = default;
+
+ /// Destructor
+ virtual ~SphereVsCapsuleAlgorithm() override = default;
+
+ /// Deleted copy-constructor
+ SphereVsCapsuleAlgorithm(const SphereVsCapsuleAlgorithm& algorithm) = delete;
+
+ /// Deleted assignment operator
+ SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
+
+ /// Compute a contact info if the two bounding volume collide
+ virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactPointInfo& contactPointInfo) override;
+};
+
+}
+
+#endif
+
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index 8a591614..464c1bd5 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -40,7 +40,7 @@
namespace reactphysics3d {
/// Type of the collision shape
-enum class CollisionShapeType {TRIANGLE, BOX, SPHERE, CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
+enum class CollisionShapeType {TRIANGLE, BOX, CAPSULE, SPHERE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
const int NB_COLLISION_SHAPE_TYPES = 9;
// Declarations
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 578cecdd..1cd750ed 100644
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -87,7 +87,7 @@ Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, c
/// Compute the closest points between two segments
/// This method uses the technique described in the book Real-Time
/// collision detection by Christer Ericson.
-void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB,
+void reactphysics3d::computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB,
const Vector3& seg2PointA, const Vector3& seg2PointB,
Vector3& closestPointSeg1, Vector3& closestPointSeg2) {
@@ -169,7 +169,7 @@ void computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vect
// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such
// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise,
// there is no intersection between the plane and the segment.
-decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) {
+decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) {
const decimal parallelEpsilon = decimal(0.0001);
decimal t = decimal(-1);
@@ -188,7 +188,7 @@ decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB
}
/// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB"
-decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) {
+decimal reactphysics3d::computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) {
decimal distAB = (linePointB - linePointA).length();
diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h
index a1bef6dc..3dbbd302 100644
--- a/src/reactphysics3d.h
+++ b/src/reactphysics3d.h
@@ -50,6 +50,7 @@
#include "collision/shapes/ConvexMeshShape.h"
#include "collision/shapes/ConcaveMeshShape.h"
#include "collision/shapes/HeightFieldShape.h"
+#include "collision/PolyhedronMesh.h"
#include "collision/shapes/AABB.h"
#include "collision/ProxyShape.h"
#include "collision/RaycastInfo.h"
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index 323b9c34..c2aec0cd 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -33,8 +33,6 @@
#include "SceneDemo.h"
#include "Sphere.h"
#include "Box.h"
-#include "Cone.h"
-#include "Cylinder.h"
#include "Capsule.h"
#include "Line.h"
#include "ConvexMesh.h"
From 6a01abfae8c70896096420e338779ce88c2de5e3 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 20 Feb 2017 17:11:13 +0200
Subject: [PATCH 037/133] Fix issues, work on HalfEdgeStructure and add unit
tests
---
src/collision/CollisionDetection.cpp | 2 +-
src/collision/CollisionDetection.h | 12 -
src/collision/HalfEdgeStructure.cpp | 27 +-
src/collision/HalfEdgeStructure.h | 10 +-
src/mathematics/mathematics_functions.cpp | 6 +-
test/main.cpp | 2 +
test/tests/collision/TestAABB.h | 2 +-
test/tests/collision/TestCollisionWorld.h | 22 +-
test/tests/collision/TestHalfEdgeStructure.h | 245 ++++++++++++++++++
test/tests/collision/TestPointInside.h | 2 +-
test/tests/collision/TestRaycast.h | 18 +-
.../mathematics/TestMathematicsFunctions.h | 6 +-
12 files changed, 286 insertions(+), 68 deletions(-)
create mode 100644 test/tests/collision/TestHalfEdgeStructure.h
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 161f6328..96f683d7 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -463,7 +463,7 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
// Make sure the shape with the smallest collision shape type comes first
const uint shape1TypeIndex = static_cast<const uint>(shape1->getCollisionShape()->getType());
const uint shape2TypeIndex = static_cast<const uint>(shape2->getCollisionShape()->getType());
- if (shape2TypeIndex > shape1TypeIndex) {
+ if (shape1TypeIndex > shape2TypeIndex) {
// Swap the two shapes
ProxyShape* temp = shape1;
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index c06a6357..9444b319 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -179,12 +179,6 @@ class CollisionDetection {
/// Compute the collision detection
void computeCollisionDetection();
- // TODO : Remove this method
- /// Report collision between two sets of shapes
- //void reportCollisionBetweenShapes(CollisionCallback* callback,
- // const std::set<uint>& shapes1,
- // const std::set<uint>& shapes2) ;
-
/// Ray casting method
void raycast(RaycastCallback* raycastCallback, const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const;
@@ -210,12 +204,6 @@ class CollisionDetection {
/// Allow the broadphase to notify the collision detection about an overlapping pair.
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
- // TODO : Remove this method
- /// Compute the narrow-phase collision detection
- //void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
- // const std::set<uint>& shapes1,
- // const std::set<uint>& shapes2);
-
/// Return a pointer to the world
CollisionWorld* getWorld();
diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp
index dba49c15..6393acb0 100644
--- a/src/collision/HalfEdgeStructure.cpp
+++ b/src/collision/HalfEdgeStructure.cpp
@@ -38,6 +38,8 @@ void HalfEdgeStructure::init(std::vector<Vector3> vertices, std::vector<std::vec
std::map<edgeKey, edgeKey> nextEdges;
std::map<edgeKey, uint> mapEdgeToStartVertex;
std::map<edgeKey, uint> mapEdgeToIndex;
+ std::map<uint, edgeKey> mapEdgeIndexToKey;
+ std::map<uint, edgeKey> mapFaceIndexToEdgeKey;
// For each vertices
for (uint v=0; v<vertices.size(); v++) {
@@ -86,24 +88,27 @@ void HalfEdgeStructure::init(std::vector<Vector3> vertices, std::vector<std::vec
mapEdgeToStartVertex.insert(std::make_pair(pairV1V2, v1Index));
mapEdgeToStartVertex.insert(std::make_pair(pairV2V1, v2Index));
+ mapFaceIndexToEdgeKey.insert(std::make_pair(f, pairV1V2));
+
auto itEdge = edges.find(pairV2V1);
if (itEdge != edges.end()) {
const uint edgeIndex = mEdges.size();
- mEdges.push_back(itEdge->second);
- mEdges.push_back(edge);
itEdge->second.twinEdgeIndex = edgeIndex + 1;
-
edge.twinEdgeIndex = edgeIndex;
- mVertices[v1Index].edgeIndex = edgeIndex;
- mVertices[v2Index].edgeIndex = edgeIndex + 1;
+ mapEdgeIndexToKey[edgeIndex] = pairV2V1;
+ mapEdgeIndexToKey[edgeIndex + 1] = pairV1V2;
+
+ mVertices[v1Index].edgeIndex = edgeIndex + 1;
+ mVertices[v2Index].edgeIndex = edgeIndex;
mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1));
mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex));
- face.edgeIndex = edgeIndex + 1;
+ mEdges.push_back(itEdge->second);
+ mEdges.push_back(edge);
}
currentFaceEdges.push_back(pairV1V2);
@@ -111,8 +116,12 @@ void HalfEdgeStructure::init(std::vector<Vector3> vertices, std::vector<std::vec
}
// Set next edges
- std::map<edgeKey, Edge>::iterator it;
- for (it = edges.begin(); it != edges.end(); ++it) {
- it->second.nextEdgeIndex = mapEdgeToIndex[nextEdges[it->first]];
+ for (uint i=0; i < mEdges.size(); i++) {
+ mEdges[i].nextEdgeIndex = mapEdgeToIndex[nextEdges[mapEdgeIndexToKey[i]]];
+ }
+
+ // Set face edge
+ for (uint f=0; f < faces.size(); f++) {
+ mFaces[f].edgeIndex = mapEdgeToIndex[mapFaceIndexToEdgeKey[f]];
}
}
diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h
index 4a3f67ed..707c7d83 100644
--- a/src/collision/HalfEdgeStructure.h
+++ b/src/collision/HalfEdgeStructure.h
@@ -43,7 +43,7 @@ class HalfEdgeStructure {
public:
struct Edge {
- uint vertexIndex; // Index of the vertex at the end of the edge
+ uint vertexIndex; // Index of the vertex at the beginning of the edge
uint twinEdgeIndex; // Index of the twin edge
uint faceIndex; // Adjacent face index of the edge
uint nextEdgeIndex; // Index of the next edge
@@ -54,7 +54,7 @@ class HalfEdgeStructure {
};
struct Vertex {
- Vector3 point; // Coordinates of the vertex
+ Vector3 point; // Coordinates of the vertex
uint edgeIndex; // Index of one edge emanting from this vertex
/// Constructor
@@ -86,8 +86,8 @@ class HalfEdgeStructure {
/// Return the number of faces
uint getNbFaces() const;
- /// Return the number of edges
- uint getNbEdges() const;
+ /// Return the number of half-edges
+ uint getNbHalfEdges() const;
/// Return the number of vertices
uint getNbVertices() const;
@@ -109,7 +109,7 @@ inline uint HalfEdgeStructure::getNbFaces() const {
}
// Return the number of edges
-inline uint HalfEdgeStructure::getNbEdges() const {
+inline uint HalfEdgeStructure::getNbHalfEdges() const {
return mEdges.size();
}
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 1cd750ed..7de898ae 100644
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -171,16 +171,16 @@ void reactphysics3d::computeClosestPointBetweenTwoSegments(const Vector3& seg1Po
// there is no intersection between the plane and the segment.
decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal) {
- const decimal parallelEpsilon = decimal(0.0001);
+ const decimal parallelEpsilon = decimal(0.0001);
decimal t = decimal(-1);
// Segment AB
const Vector3 ab = segB - segA;
- decimal nDotAB = planeNormal.dot(ab);
+ decimal nDotAB = planeNormal.dot(ab);
// If the segment is not parallel to the plane
- if (nDotAB > parallelEpsilon) {
+ if (std::abs(nDotAB) > parallelEpsilon) {
t = (planeD - planeNormal.dot(segA)) / nDotAB;
}
diff --git a/test/main.cpp b/test/main.cpp
index bf949030..54bfbb8f 100644
--- a/test/main.cpp
+++ b/test/main.cpp
@@ -37,6 +37,7 @@
#include "tests/collision/TestCollisionWorld.h"
#include "tests/collision/TestAABB.h"
#include "tests/collision/TestDynamicAABBTree.h"
+#include "tests/collision/TestHalfEdgeStructure.h"
using namespace reactphysics3d;
@@ -61,6 +62,7 @@ int main() {
testSuite.addTest(new TestRaycast("Raycasting"));
testSuite.addTest(new TestCollisionWorld("CollisionWorld"));
testSuite.addTest(new TestDynamicAABBTree("DynamicAABBTree"));
+ testSuite.addTest(new TestHalfEdgeStructure("HalfEdgeStructure"));
// Run the tests
testSuite.run();
diff --git a/test/tests/collision/TestAABB.h b/test/tests/collision/TestAABB.h
index 32d03f45..4328625c 100644
--- a/test/tests/collision/TestAABB.h
+++ b/test/tests/collision/TestAABB.h
@@ -72,7 +72,7 @@ class TestAABB : public Test {
}
/// Destructor
- ~TestAABB() {
+ virtual ~TestAABB() {
}
diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h
index 242ebcf9..678b5f21 100644
--- a/test/tests/collision/TestCollisionWorld.h
+++ b/test/tests/collision/TestCollisionWorld.h
@@ -103,7 +103,6 @@ class TestCollisionWorld : public Test {
CollisionBody* mBoxBody;
CollisionBody* mSphere1Body;
CollisionBody* mSphere2Body;
- CollisionBody* mCylinderBody;
// Collision shapes
BoxShape* mBoxShape;
@@ -113,7 +112,6 @@ class TestCollisionWorld : public Test {
ProxyShape* mBoxProxyShape;
ProxyShape* mSphere1ProxyShape;
ProxyShape* mSphere2ProxyShape;
- ProxyShape* mCylinderProxyShape;
// Collision callback class
WorldCollisionCallback mCollisionCallback;
@@ -147,16 +145,14 @@ class TestCollisionWorld : public Test {
mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1);
mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1);
mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2);
- mCylinderProxyShape->setCollisionCategoryBits(CATEGORY_3);
mCollisionCallback.boxBody = mBoxBody;
mCollisionCallback.sphere1Body = mSphere1Body;
mCollisionCallback.sphere2Body = mSphere2Body;
- mCollisionCallback.cylinderBody = mCylinderBody;
}
/// Destructor
- ~TestCollisionWorld() {
+ virtual ~TestCollisionWorld() {
delete mBoxShape;
delete mSphereShape;
}
@@ -175,17 +171,12 @@ class TestCollisionWorld : public Test {
test(!mCollisionCallback.sphere1CollideWithSphere2);
test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body));
- test(mWorld->testAABBOverlap(mBoxBody, mCylinderBody));
- test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody));
test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
test(mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB()));
- test(mBoxProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
- test(!mSphere1ProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB()));
mCollisionCallback.reset();
- mWorld->testCollision(mCylinderBody, &mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
test(!mCollisionCallback.sphere1CollideWithSphere2);
@@ -195,7 +186,6 @@ class TestCollisionWorld : public Test {
test(!mCollisionCallback.sphere1CollideWithSphere2);
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
test(!mCollisionCallback.sphere1CollideWithSphere2);
@@ -213,7 +203,6 @@ class TestCollisionWorld : public Test {
test(!mCollisionCallback.sphere1CollideWithSphere2);
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody, mCylinderBody, &mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
test(!mCollisionCallback.sphere1CollideWithSphere2);
@@ -224,7 +213,6 @@ class TestCollisionWorld : public Test {
mCollisionCallback.reset();
mBoxBody->setIsActive(false);
- mCylinderBody->setIsActive(false);
mSphere1Body->setIsActive(false);
mSphere2Body->setIsActive(false);
mWorld->testCollision(&mCollisionCallback);
@@ -232,17 +220,12 @@ class TestCollisionWorld : public Test {
test(!mCollisionCallback.sphere1CollideWithSphere2);
test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body));
- test(!mWorld->testAABBOverlap(mBoxBody, mCylinderBody));
- test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody));
test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
test(!mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB()));
- test(!mBoxProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
- test(!mSphere1ProxyShape->testAABBOverlap(mCylinderProxyShape->getWorldAABB()));
test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB()));
mBoxBody->setIsActive(true);
- mCylinderBody->setIsActive(true);
mSphere1Body->setIsActive(true);
mSphere2Body->setIsActive(true);
@@ -251,7 +234,6 @@ class TestCollisionWorld : public Test {
mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3);
mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2);
mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1);
- mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1);
mCollisionCallback.reset();
mWorld->testCollision(&mCollisionCallback);
@@ -269,7 +251,6 @@ class TestCollisionWorld : public Test {
mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2);
mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2);
mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3);
- mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1);
mCollisionCallback.reset();
mWorld->testCollision(&mCollisionCallback);
@@ -282,7 +263,6 @@ class TestCollisionWorld : public Test {
mBoxProxyShape->setCollideWithMaskBits(0xFFFF);
mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF);
mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF);
- mCylinderProxyShape->setCollideWithMaskBits(0xFFFF);
}
};
diff --git a/test/tests/collision/TestHalfEdgeStructure.h b/test/tests/collision/TestHalfEdgeStructure.h
new file mode 100644
index 00000000..871a8ed1
--- /dev/null
+++ b/test/tests/collision/TestHalfEdgeStructure.h
@@ -0,0 +1,245 @@
+#ifndef TEST_HALF_EDGE_STRUCTURE_H
+#define TEST_HALF_EDGE_STRUCTURE_H
+
+// Libraries
+#include "reactphysics3d.h"
+#include "Test.h"
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+
+// Class TestHalfEdgeStructure
+/**
+ * Unit test for the HalfEdgeStructure class.
+ */
+class TestHalfEdgeStructure : public Test {
+
+ private :
+
+ // ---------- Atributes ---------- //
+
+
+ public :
+
+ // ---------- Methods ---------- //
+
+ /// Constructor
+ TestHalfEdgeStructure(const std::string& name) : Test(name) {
+
+ }
+
+ /// Destructor
+ virtual ~TestHalfEdgeStructure() {
+
+ }
+
+ /// Run the tests
+ void run() {
+ testCube();
+ testTetrahedron();
+ }
+
+ void testCube() {
+
+ // Create the half-edge structure for a cube
+ std::vector<rp3d::Vector3> vertices;
+ std::vector<std::vector<uint>> faces;
+ rp3d::HalfEdgeStructure cubeStructure;
+
+ // Vertices
+ vertices.push_back(rp3d::Vector3(-0.5, -0.5, 0.5));
+ vertices.push_back(rp3d::Vector3(0.5, -0.5, 0.5));
+ vertices.push_back(rp3d::Vector3(0.5, 0.5, 0.5));
+ vertices.push_back(rp3d::Vector3(-0.5, 0.5, 0.5));
+ vertices.push_back(rp3d::Vector3(-0.5, -0.5, -0.5));
+ vertices.push_back(rp3d::Vector3(0.5, -0.5, -0.5));
+ vertices.push_back(rp3d::Vector3(0.5, 0.5, -0.5));
+ vertices.push_back(rp3d::Vector3(-0.5, 0.5, -0.5));
+
+ // Faces
+ std::vector<uint> face0;
+ face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3);
+ std::vector<uint> face1;
+ face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2);
+ std::vector<uint> face2;
+ face2.push_back(5); face2.push_back(4); face2.push_back(7); face2.push_back(6);
+ std::vector<uint> face3;
+ face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7);
+ std::vector<uint> face4;
+ face4.push_back(0); face4.push_back(4); face4.push_back(5); face4.push_back(1);
+ std::vector<uint> face5;
+ face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3);
+
+ faces.push_back(face0);
+ faces.push_back(face1);
+ faces.push_back(face2);
+ faces.push_back(face3);
+ faces.push_back(face4);
+ faces.push_back(face5);
+
+ cubeStructure.init(vertices, faces);
+
+ // --- Test that the half-edge structure of the cube is valid --- //
+
+ test(cubeStructure.getNbFaces() == 6);
+ test(cubeStructure.getNbVertices() == 8);
+ test(cubeStructure.getNbHalfEdges() == 24);
+
+ // Test vertices
+ test(cubeStructure.getVertex(0).point.x == -0.5);
+ test(cubeStructure.getVertex(0).point.y == -0.5);
+ test(cubeStructure.getVertex(0).point.z == 0.5);
+ test(cubeStructure.getHalfEdge(cubeStructure.getVertex(0).edgeIndex).vertexIndex == 0);
+
+ test(cubeStructure.getVertex(1).point.x == 0.5);
+ test(cubeStructure.getVertex(1).point.y == -0.5);
+ test(cubeStructure.getVertex(1).point.z == 0.5);
+ test(cubeStructure.getHalfEdge(cubeStructure.getVertex(1).edgeIndex).vertexIndex == 1);
+
+ test(cubeStructure.getVertex(2).point.x == 0.5);
+ test(cubeStructure.getVertex(2).point.y == 0.5);
+ test(cubeStructure.getVertex(2).point.z == 0.5);
+ test(cubeStructure.getHalfEdge(cubeStructure.getVertex(2).edgeIndex).vertexIndex == 2);
+
+ test(cubeStructure.getVertex(3).point.x == -0.5);
+ test(cubeStructure.getVertex(3).point.y == 0.5);
+ test(cubeStructure.getVertex(3).point.z == 0.5);
+ test(cubeStructure.getHalfEdge(cubeStructure.getVertex(3).edgeIndex).vertexIndex == 3);
+
+ test(cubeStructure.getVertex(4).point.x == -0.5);
+ test(cubeStructure.getVertex(4).point.y == -0.5);
+ test(cubeStructure.getVertex(4).point.z == -0.5);
+ test(cubeStructure.getHalfEdge(cubeStructure.getVertex(4).edgeIndex).vertexIndex == 4);
+
+ test(cubeStructure.getVertex(5).point.x == 0.5);
+ test(cubeStructure.getVertex(5).point.y == -0.5);
+ test(cubeStructure.getVertex(5).point.z == -0.5);
+ test(cubeStructure.getHalfEdge(cubeStructure.getVertex(5).edgeIndex).vertexIndex == 5);
+
+ test(cubeStructure.getVertex(6).point.x == 0.5);
+ test(cubeStructure.getVertex(6).point.y == 0.5);
+ test(cubeStructure.getVertex(6).point.z == -0.5);
+ test(cubeStructure.getHalfEdge(cubeStructure.getVertex(6).edgeIndex).vertexIndex == 6);
+
+ test(cubeStructure.getVertex(7).point.x == -0.5);
+ test(cubeStructure.getVertex(7).point.y == 0.5);
+ test(cubeStructure.getVertex(7).point.z == -0.5);
+ test(cubeStructure.getHalfEdge(cubeStructure.getVertex(7).edgeIndex).vertexIndex == 7);
+
+ // Test faces
+ for (uint f=0; f<6; f++) {
+ test(cubeStructure.getHalfEdge(cubeStructure.getFace(f).edgeIndex).faceIndex == f);
+ }
+
+ // Test edges
+ for (uint f=0; f<6; f++) {
+
+
+ uint edgeIndex = cubeStructure.getFace(f).edgeIndex;
+ const uint firstEdgeIndex = edgeIndex;
+
+ // For each half-edge of the face
+ for (uint e=0; e<4; e++) {
+
+ rp3d::HalfEdgeStructure::Edge edge = cubeStructure.getHalfEdge(edgeIndex);
+
+ test(cubeStructure.getHalfEdge(edge.twinEdgeIndex).twinEdgeIndex == edgeIndex);
+ test(edge.faceIndex == f);
+
+ // Go to the next edge
+ edgeIndex = edge.nextEdgeIndex;
+ }
+
+ test(firstEdgeIndex == edgeIndex);
+ }
+ }
+
+ void testTetrahedron() {
+
+ // Create the half-edge structure for a tetrahedron
+ std::vector<rp3d::Vector3> vertices;
+ std::vector<std::vector<uint>> faces;
+ rp3d::HalfEdgeStructure tetrahedron;
+
+ // Vertices
+ vertices.push_back(rp3d::Vector3(1, -1, -1));
+ vertices.push_back(rp3d::Vector3(-1, -1, -1));
+ vertices.push_back(rp3d::Vector3(0, -1, 1));
+ vertices.push_back(rp3d::Vector3(0, 1, 0));
+
+ // Faces
+ std::vector<uint> face0;
+ face0.push_back(0); face0.push_back(1); face0.push_back(2);
+ std::vector<uint> face1;
+ face1.push_back(0); face1.push_back(3); face1.push_back(1);
+ std::vector<uint> face2;
+ face2.push_back(1); face2.push_back(3); face2.push_back(2);
+ std::vector<uint> face3;
+ face3.push_back(0); face3.push_back(2); face3.push_back(3);
+
+ faces.push_back(face0);
+ faces.push_back(face1);
+ faces.push_back(face2);
+ faces.push_back(face3);
+
+ tetrahedron.init(vertices, faces);
+
+ // --- Test that the half-edge structure of the tetrahedron is valid --- //
+
+ test(tetrahedron.getNbFaces() == 4);
+ test(tetrahedron.getNbVertices() == 4);
+ test(tetrahedron.getNbHalfEdges() == 12);
+
+ // Test vertices
+ test(tetrahedron.getVertex(0).point.x == 1);
+ test(tetrahedron.getVertex(0).point.y == -1);
+ test(tetrahedron.getVertex(0).point.z == -1);
+ test(tetrahedron.getHalfEdge(tetrahedron.getVertex(0).edgeIndex).vertexIndex == 0);
+
+ test(tetrahedron.getVertex(1).point.x == -1);
+ test(tetrahedron.getVertex(1).point.y == -1);
+ test(tetrahedron.getVertex(1).point.z == -1);
+ test(tetrahedron.getHalfEdge(tetrahedron.getVertex(1).edgeIndex).vertexIndex == 1);
+
+ test(tetrahedron.getVertex(2).point.x == 0);
+ test(tetrahedron.getVertex(2).point.y == -1);
+ test(tetrahedron.getVertex(2).point.z == 1);
+ test(tetrahedron.getHalfEdge(tetrahedron.getVertex(2).edgeIndex).vertexIndex == 2);
+
+ test(tetrahedron.getVertex(3).point.x == 0);
+ test(tetrahedron.getVertex(3).point.y == 1);
+ test(tetrahedron.getVertex(3).point.z == 0);
+ test(tetrahedron.getHalfEdge(tetrahedron.getVertex(3).edgeIndex).vertexIndex == 3);
+
+ // Test faces
+ for (uint f=0; f<4; f++) {
+ test(tetrahedron.getHalfEdge(tetrahedron.getFace(f).edgeIndex).faceIndex == f);
+ }
+
+ // Test edges
+ for (uint f=0; f<4; f++) {
+
+ uint edgeIndex = tetrahedron.getFace(f).edgeIndex;
+ const uint firstEdgeIndex = edgeIndex;
+
+ // For each half-edge of the face
+ for (uint e=0; e<3; e++) {
+
+ rp3d::HalfEdgeStructure::Edge edge = tetrahedron.getHalfEdge(edgeIndex);
+
+ test(tetrahedron.getHalfEdge(edge.twinEdgeIndex).twinEdgeIndex == edgeIndex);
+ test(edge.faceIndex == f);
+
+ // Go to the next edge
+ edgeIndex = edge.nextEdgeIndex;
+ }
+
+ test(firstEdgeIndex == edgeIndex);
+ }
+ }
+ };
+
+}
+
+#endif
diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h
index dd2a793b..9fd20110 100644
--- a/test/tests/collision/TestPointInside.h
+++ b/test/tests/collision/TestPointInside.h
@@ -171,7 +171,7 @@ class TestPointInside : public Test {
}
/// Destructor
- ~TestPointInside() {
+ virtual ~TestPointInside() {
delete mBoxShape;
delete mSphereShape;
delete mCapsuleShape;
diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h
index b39b751b..852455c2 100644
--- a/test/tests/collision/TestRaycast.h
+++ b/test/tests/collision/TestRaycast.h
@@ -109,7 +109,6 @@ class TestRaycast : public Test {
CollisionBody* mBoxBody;
CollisionBody* mSphereBody;
CollisionBody* mCapsuleBody;
- CollisionBody* mConeBody;
CollisionBody* mConvexMeshBody;
CollisionBody* mConvexMeshBodyEdgesInfo;
CollisionBody* mCylinderBody;
@@ -138,12 +137,10 @@ class TestRaycast : public Test {
ProxyShape* mBoxProxyShape;
ProxyShape* mSphereProxyShape;
ProxyShape* mCapsuleProxyShape;
- ProxyShape* mConeProxyShape;
ProxyShape* mConvexMeshProxyShape;
ProxyShape* mConvexMeshProxyShapeEdgesInfo;
- ProxyShape* mCylinderProxyShape;
ProxyShape* mCompoundSphereProxyShape;
- ProxyShape* mCompoundCylinderProxyShape;
+ ProxyShape* mCompoundCapsuleProxyShape;
ProxyShape* mTriangleProxyShape;
ProxyShape* mConcaveMeshProxyShape;
ProxyShape* mHeightFieldProxyShape;
@@ -177,7 +174,6 @@ class TestRaycast : public Test {
mBoxBody = mWorld->createCollisionBody(mBodyTransform);
mSphereBody = mWorld->createCollisionBody(mBodyTransform);
mCapsuleBody = mWorld->createCollisionBody(mBodyTransform);
- mConeBody = mWorld->createCollisionBody(mBodyTransform);
mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform);
mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform);
mCylinderBody = mWorld->createCollisionBody(mBodyTransform);
@@ -253,7 +249,7 @@ class TestRaycast : public Test {
Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13);
Transform shapeTransform2(positionShape2, orientationShape2);
mLocalShape2ToWorld = mBodyTransform * shapeTransform2;
- mCompoundCylinderProxyShape = mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform);
+ mCompoundCapsuleProxyShape = mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform);
mCompoundSphereProxyShape = mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2);
// Concave Mesh shape
@@ -284,7 +280,7 @@ class TestRaycast : public Test {
new TriangleVertexArray(8, &(mConcaveMeshVertices[0]), sizeof(Vector3),
12, &(mConcaveMeshIndices[0]), sizeof(uint),
vertexType,
- TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
+ TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
// Add the triangle vertex array of the subpart to the triangle mesh
@@ -302,19 +298,17 @@ class TestRaycast : public Test {
mBoxProxyShape->setCollisionCategoryBits(CATEGORY1);
mSphereProxyShape->setCollisionCategoryBits(CATEGORY1);
mCapsuleProxyShape->setCollisionCategoryBits(CATEGORY1);
- mConeProxyShape->setCollisionCategoryBits(CATEGORY2);
mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2);
mConvexMeshProxyShapeEdgesInfo->setCollisionCategoryBits(CATEGORY2);
- mCylinderProxyShape->setCollisionCategoryBits(CATEGORY2);
mCompoundSphereProxyShape->setCollisionCategoryBits(CATEGORY2);
- mCompoundCylinderProxyShape->setCollisionCategoryBits(CATEGORY2);
+ mCompoundCapsuleProxyShape->setCollisionCategoryBits(CATEGORY2);
mTriangleProxyShape->setCollisionCategoryBits(CATEGORY1);
mConcaveMeshProxyShape->setCollisionCategoryBits(CATEGORY2);
mHeightFieldProxyShape->setCollisionCategoryBits(CATEGORY2);
}
/// Destructor
- ~TestRaycast() {
+ virtual ~TestRaycast() {
delete mBoxShape;
delete mSphereShape;
delete mCapsuleShape;
@@ -1647,7 +1641,7 @@ class TestRaycast : public Test {
Ray ray15(mLocalShapeToWorld * Vector3(0, -9, 1), mLocalShapeToWorld * Vector3(0, 30, 1));
Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30));
- mCallback.shapeToTest = mCompoundCylinderProxyShape;
+ mCallback.shapeToTest = mCompoundCapsuleProxyShape;
test(mCompoundBody->raycast(ray11, raycastInfo));
mCallback.reset();
diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h
index 32e19aad..42e1065c 100644
--- a/test/tests/mathematics/TestMathematicsFunctions.h
+++ b/test/tests/mathematics/TestMathematicsFunctions.h
@@ -150,19 +150,19 @@ class TestMathematicsFunctions : public Test {
test(approxEqual(closestSeg2.z, 0.0, 0.000001));
computeClosestPointBetweenTwoSegments(Vector3(1, -4, -5), Vector3(1, 4, -5), Vector3(-6, 5, -5), Vector3(6, 5, -5), closestSeg1, closestSeg2);
test(approxEqual(closestSeg1.x, 1.0, 0.000001));
- test(approxEqual(closestSeg1.y, 5.0, 0.000001));
+ test(approxEqual(closestSeg1.y, 4.0, 0.000001));
test(approxEqual(closestSeg1.z, -5.0, 0.000001));
test(approxEqual(closestSeg2.x, 1.0, 0.000001));
test(approxEqual(closestSeg2.y, 5.0, 0.000001));
test(approxEqual(closestSeg2.z, -5.0, 0.000001));
// Test computePlaneSegmentIntersection();
- test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(-1, 0, 0)), 0.5, 0.000001));
+ test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(-1, 0, 0)), 0.5, 0.000001));
test(approxEqual(computePlaneSegmentIntersection(Vector3(-6, 3, 0), Vector3(6, 3, 0), 0.0, Vector3(1, 0, 0)), 0.5, 0.000001));
test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 12, 0), Vector3(5, 4, 0), 6, Vector3(0, 1, 0)), 0.75, 0.000001));
test(approxEqual(computePlaneSegmentIntersection(Vector3(5, 4, 8), Vector3(9, 14, 8), 4, Vector3(0, 1, 0)), 0.0, 0.000001));
decimal tIntersect = computePlaneSegmentIntersection(Vector3(5, 4, 0), Vector3(9, 4, 0), 4, Vector3(0, 1, 0));
- test(tIntersect < 0.0 && tIntersect > 1.0);
+ test(tIntersect < 0.0 || tIntersect > 1.0);
// Test computeDistancePointToLineDistance()
test(approxEqual(computeDistancePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001));
From b21a6bb59b104e54364ddf6a004484a1c5824f25 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 26 Feb 2017 13:48:50 +0200
Subject: [PATCH 038/133] Refactor contact manifold and contact point creation
---
CMakeLists.txt | 2 +
src/collision/CollisionDetection.cpp | 216 ++-------------
src/collision/ContactManifold.cpp | 254 ++++--------------
src/collision/ContactManifold.h | 27 +-
src/collision/ContactManifoldInfo.h | 130 +++++++++
src/collision/ContactManifoldSet.cpp | 217 +++++++++------
src/collision/ContactManifoldSet.h | 17 +-
src/collision/ContactPointInfo.h | 88 ++++++
.../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 25 +-
.../narrowphase/CapsuleVsCapsuleAlgorithm.h | 2 +-
.../narrowphase/DefaultCollisionDispatch.cpp | 2 +-
.../narrowphase/GJK/GJKAlgorithm.cpp | 6 +-
src/collision/narrowphase/GJK/GJKAlgorithm.h | 5 +-
.../narrowphase/NarrowPhaseAlgorithm.h | 4 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 2 +-
src/collision/narrowphase/SAT/SATAlgorithm.h | 5 +-
.../narrowphase/SphereVsCapsuleAlgorithm.cpp | 11 +-
.../narrowphase/SphereVsCapsuleAlgorithm.h | 2 +-
.../SphereVsConvexMeshAlgorithm.cpp | 6 +-
.../narrowphase/SphereVsConvexMeshAlgorithm.h | 2 +-
.../narrowphase/SphereVsSphereAlgorithm.cpp | 11 +-
.../narrowphase/SphereVsSphereAlgorithm.h | 2 +-
src/collision/shapes/CollisionShape.h | 2 +-
src/constraint/ContactPoint.cpp | 32 ++-
src/constraint/ContactPoint.h | 86 ++----
src/engine/CollisionWorld.h | 6 +-
src/engine/EventListener.h | 6 +-
src/engine/OverlappingPair.h | 16 +-
28 files changed, 543 insertions(+), 641 deletions(-)
create mode 100644 src/collision/ContactManifoldInfo.h
create mode 100644 src/collision/ContactPointInfo.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 37b8febe..58ea37d7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -60,6 +60,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/body/CollisionBody.cpp"
"src/body/RigidBody.h"
"src/body/RigidBody.cpp"
+ "src/collision/ContactPointInfo.h"
+ "src/collision/ContactManifoldInfo.h"
"src/collision/broadphase/BroadPhaseAlgorithm.h"
"src/collision/broadphase/BroadPhaseAlgorithm.cpp"
"src/collision/broadphase/DynamicAABBTree.h"
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 96f683d7..4a0ef718 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -72,66 +72,6 @@ void CollisionDetection::computeCollisionDetection() {
mNarrowPhaseInfoList = nullptr;
}
-// TODO : Remove this method
-// Report collision between two sets of shapes
-/*
-void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback,
- const std::set<uint>& shapes1,
- const std::set<uint>& shapes2) {
-
- // For each possible collision pair of bodies
- map<overlappingpairid, OverlappingPair*>::iterator it;
- for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
-
- OverlappingPair* pair = it->second;
-
- const ProxyShape* shape1 = pair->getShape1();
- const ProxyShape* shape2 = pair->getShape2();
-
- assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
-
- // If both shapes1 and shapes2 sets are non-empty, we check that
- // shape1 is among on set and shape2 is among the other one
- if (!shapes1.empty() && !shapes2.empty() &&
- (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) &&
- (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) {
- continue;
- }
- if (!shapes1.empty() && shapes2.empty() &&
- shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0)
- {
- continue;
- }
- if (!shapes2.empty() && shapes1.empty() &&
- shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0)
- {
- continue;
- }
-
- // For each contact manifold set of the overlapping pair
- const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
- for (int j=0; j<manifoldSet.getNbContactManifolds(); j++) {
-
- const ContactManifold* manifold = manifoldSet.getContactManifold(j);
-
- // For each contact manifold of the manifold set
- for (uint i=0; i<manifold->getNbContactPoints(); i++) {
-
- ContactPoint* contactPoint = manifold->getContactPoint(i);
-
- // Create the contact info object for the contact
- ContactPointInfo contactInfo;
- contactInfo.init(contactPoint->getNormal(), contactPoint->getPenetrationDepth(),
- contactPoint->getLocalPointOnBody1(), contactPoint->getLocalPointOnBody2());
-
- // Notify the collision callback about this new contact
- if (callback != nullptr) callback->notifyContact(contactInfo);
- }
- }
- }
-}
-*/
-
// Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() {
@@ -191,9 +131,6 @@ void CollisionDetection::computeMiddlePhase() {
CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody();
- // Update the contact cache of the overlapping pair
- pair->update();
-
// Check that at least one body is awake and not static
bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC;
bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC;
@@ -308,25 +245,21 @@ void CollisionDetection::computeNarrowPhase() {
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactPointInfo contactPointInfo;
- if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactPointInfo)) {
+ ContactManifoldInfo contactManifoldInfo(mSingleFrameAllocator);
+ if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactManifoldInfo)) {
+
+ // Reduce the number of points in the contact manifold
+ contactManifoldInfo.reduce();
// If it is the first contact since the pairs are overlapping
if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) {
// Trigger a callback event
- if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactPointInfo);
+ if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactManifoldInfo);
}
- // Create a new contact
- ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint)))
- ContactPoint(contactPointInfo);
-
- contact->updateWorldContactPoints(currentNarrowPhaseInfo->shape1ToWorldTransform,
- currentNarrowPhaseInfo->shape2ToWorldTransform);
-
- // Add the contact to the contact manifold set of the corresponding overlapping pair
- currentNarrowPhaseInfo->overlappingPair->addContact(contact);
+ // Add the contact manifold to the corresponding overlapping pair
+ currentNarrowPhaseInfo->overlappingPair->addContactManifold(contactManifoldInfo);
// Add the overlapping pair into the set of pairs in contact during narrow-phase
overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(),
@@ -334,7 +267,7 @@ void CollisionDetection::computeNarrowPhase() {
mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair;
// Trigger a callback event for the new contact
- if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactPointInfo);
+ if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo);
}
}
@@ -345,111 +278,6 @@ void CollisionDetection::computeNarrowPhase() {
addAllContactManifoldsToBodies();
}
-// TODO : Remove this method
-// Compute the narrow-phase collision detection
-/*
-void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
- const std::set<uint>& shapes1,
- const std::set<uint>& shapes2) {
-
- mContactOverlappingPairs.clear();
-
- // For each possible collision pair of bodies
- map<overlappingpairid, OverlappingPair*>::iterator it;
- for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
-
- OverlappingPair* pair = it->second;
-
- ProxyShape* shape1 = pair->getShape1();
- ProxyShape* shape2 = pair->getShape2();
-
- assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
-
- // If both shapes1 and shapes2 sets are non-empty, we check that
- // shape1 is among on set and shape2 is among the other one
- if (!shapes1.empty() && !shapes2.empty() &&
- (shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) &&
- (shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) {
- ++it;
- continue;
- }
- if (!shapes1.empty() && shapes2.empty() &&
- shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0)
- {
- ++it;
- continue;
- }
- if (!shapes2.empty() && shapes1.empty() &&
- shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0)
- {
- ++it;
- continue;
- }
-
- // Check if the collision filtering allows collision between the two shapes and
- // that the two shapes are still overlapping. Otherwise, we destroy the
- // overlapping pair
- if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
- (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) ||
- !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
-
- std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
- ++it;
-
- // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
-
- // Destroy the overlapping pair
- itToRemove->second->~OverlappingPair();
- mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair));
- mOverlappingPairs.erase(itToRemove);
- continue;
- }
- else {
- ++it;
- }
-
- CollisionBody* const body1 = shape1->getBody();
- CollisionBody* const body2 = shape2->getBody();
-
- // Update the contact cache of the overlapping pair
- pair->update();
-
- // Check if the two bodies are allowed to collide, otherwise, we do not test for collision
- if (body1->getType() != BodyType::DYNAMIC && body2->getType() != BodyType::DYNAMIC) continue;
- bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
- if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
-
- // Check if the two bodies are sleeping, if so, we do no test collision between them
- if (body1->isSleeping() && body2->isSleeping()) continue;
-
- // Select the narrow phase algorithm to use according to the two collision shapes
- const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
- const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
- const int shape1Index = static_cast<int>(shape1Type);
- const int shape2Index = static_cast<int>(shape2Type);
- NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
-
- // If there is no collision algorithm between those two kinds of shapes
- if (narrowPhaseAlgorithm == nullptr) continue;
-
- // Create the CollisionShapeInfo objects
- CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
- pair, shape1->getCachedCollisionData());
- CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
- pair, shape2->getCachedCollisionData());
-
- TestCollisionBetweenShapesCallback narrowPhaseCallback(callback);
-
- // Use the narrow-phase collision detection algorithm to check
- // if there really is a collision
- narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback);
- }
-
- // Add all the contact manifolds (between colliding bodies) to the bodies
- addAllContactManifoldsToBodies();
-}
-*/
-
// Allow the broadphase to notify the collision detection about an overlapping pair.
/// This method is called by the broad-phase collision detection algorithm
void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
@@ -698,8 +526,8 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactPointInfo contactPointInfo;
- isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo);
+ ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
+ isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo);
}
}
@@ -786,8 +614,8 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactPointInfo contactPointInfo;
- isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo);
+ ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
+ isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo);
}
}
@@ -859,10 +687,10 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactPointInfo contactPointInfo;
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
+ ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
- CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body1, body2,
+ CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body1, body2,
body1ProxyShape, body2ProxyShape);
// Report the contact to the user
@@ -937,10 +765,10 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactPointInfo contactPointInfo;
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
+ ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
- CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body,
+ CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body,
proxyShape->getBody(), bodyProxyShape,
proxyShape);
@@ -1008,10 +836,10 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactPointInfo contactPointInfo;
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
+ ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
- CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, shape1->getBody(),
+ CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, shape1->getBody(),
shape2->getBody(), shape1, shape2);
// Report the contact to the user
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index dc40774b..36103642 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -30,13 +30,30 @@
using namespace reactphysics3d;
// Constructor
-ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
+ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
PoolAllocator& memoryAllocator, short normalDirectionId)
: mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
mMemoryAllocator(memoryAllocator) {
+ // For each contact point info in the manifold
+ const ContactPointInfo* pointInfo = manifoldInfo.getFirstContactPointInfo();
+ while(pointInfo != nullptr) {
+
+ // Create the new contact point
+ ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
+ ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
+
+ // Add the new contact point into the manifold
+ mContactPoints[mNbContactPoints] = contact;
+ mNbContactPoints++;
+
+ pointInfo = pointInfo->next;
+ }
+
+ assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
+ assert(mNbContactPoints > 0);
}
// Destructor
@@ -44,209 +61,6 @@ ContactManifold::~ContactManifold() {
clear();
}
-// Add a contact point in the manifold
-void ContactManifold::addContactPoint(ContactPoint* contact) {
-
- // For contact already in the manifold
- for (uint i=0; i<mNbContactPoints; i++) {
-
- // Check if the new point point does not correspond to a same contact point
- // already in the manifold.
- decimal distance = (mContactPoints[i]->getWorldPointOnBody1() -
- contact->getWorldPointOnBody1()).lengthSquare();
- if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
-
- // Delete the new contact
- contact->~ContactPoint();
- mMemoryAllocator.release(contact, sizeof(ContactPoint));
-
- assert(mNbContactPoints > 0);
-
- return;
- }
- }
-
- // If the contact manifold is full
- if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
- int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
- int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
- removeContactPoint(indexToRemove);
- }
-
- // Add the new contact point in the manifold
- mContactPoints[mNbContactPoints] = contact;
- mNbContactPoints++;
-
- assert(mNbContactPoints > 0);
-}
-
-// Remove a contact point from the manifold
-void ContactManifold::removeContactPoint(uint index) {
- assert(index < mNbContactPoints);
- assert(mNbContactPoints > 0);
-
- // Call the destructor explicitly and tell the memory allocator that
- // the corresponding memory block is now free
- mContactPoints[index]->~ContactPoint();
- mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint));
-
- // If we don't remove the last index
- if (index < mNbContactPoints - 1) {
- mContactPoints[index] = mContactPoints[mNbContactPoints - 1];
- }
-
- mNbContactPoints--;
-}
-
-// Update the contact manifold
-/// First the world space coordinates of the current contacts in the manifold are recomputed from
-/// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
-/// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
-/// the contacts with a too large distance between the contact points in the plane orthogonal to the
-/// contact normal.
-void ContactManifold::update(const Transform& transform1, const Transform& transform2) {
-
- if (mNbContactPoints == 0) return;
-
- // Update the world coordinates and penetration depth of the contact points in the manifold
- for (uint i=0; i<mNbContactPoints; i++) {
-
- mContactPoints[i]->updateWorldContactPoints(transform1, transform2);
- mContactPoints[i]->updatePenetrationDepth();
- }
-
- const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
- PERSISTENT_CONTACT_DIST_THRESHOLD;
-
- // Remove the contact points that don't represent very well the contact manifold
- for (int i=static_cast<int>(mNbContactPoints)-1; i>=0; i--) {
- assert(i < static_cast<int>(mNbContactPoints));
-
- // Compute the distance between contact points in the normal direction
- decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth();
-
- // If the contacts points are too far from each other in the normal direction
- if (distanceNormal > squarePersistentContactThreshold) {
- removeContactPoint(i);
- }
- else {
- // Compute the distance of the two contact points in the plane
- // orthogonal to the contact normal
- Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() +
- mContactPoints[i]->getNormal() * distanceNormal;
- Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
-
- // If the orthogonal distance is larger than the valid distance
- // threshold, we remove the contact
- if (projDifference.lengthSquare() > squarePersistentContactThreshold) {
- removeContactPoint(i);
- }
- }
- }
-}
-
-// Return the index of the contact point with the larger penetration depth.
-/// This corresponding contact will be kept in the cache. The method returns -1 is
-/// the new contact is the deepest.
-int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
- assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
- int indexMaxPenetrationDepth = -1;
- decimal maxPenetrationDepth = newContact->getPenetrationDepth();
-
- // For each contact in the cache
- for (uint i=0; i<mNbContactPoints; i++) {
-
- // If the current contact has a larger penetration depth
- if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
- maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth();
- indexMaxPenetrationDepth = i;
- }
- }
-
- // Return the index of largest penetration depth
- return indexMaxPenetrationDepth;
-}
-
-// Return the index that will be removed.
-/// The index of the contact point with the larger penetration
-/// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
-/// the different area and we want to keep the contacts with the largest area. The new point is also
-/// kept. In order to compute the area of a quadrilateral, we use the formula :
-/// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
-/// when we compute this area, we do not calculate it exactly but we
-/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
-/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
-/// by Erwin Coumans (http://wwww.bulletphysics.org).
-int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
-
- assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
-
- decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint
- decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint
- decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint
- decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint
-
- if (indexMaxPenetration != 0) {
- // Compute the area
- Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1();
- Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
- mContactPoints[2]->getLocalPointOnBody1();
- Vector3 crossProduct = vector1.cross(vector2);
- area0 = crossProduct.lengthSquare();
- }
- if (indexMaxPenetration != 1) {
- // Compute the area
- Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
- Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
- mContactPoints[2]->getLocalPointOnBody1();
- Vector3 crossProduct = vector1.cross(vector2);
- area1 = crossProduct.lengthSquare();
- }
- if (indexMaxPenetration != 2) {
- // Compute the area
- Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
- Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
- mContactPoints[1]->getLocalPointOnBody1();
- Vector3 crossProduct = vector1.cross(vector2);
- area2 = crossProduct.lengthSquare();
- }
- if (indexMaxPenetration != 3) {
- // Compute the area
- Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
- Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() -
- mContactPoints[1]->getLocalPointOnBody1();
- Vector3 crossProduct = vector1.cross(vector2);
- area3 = crossProduct.lengthSquare();
- }
-
- // Return the index of the contact to remove
- return getMaxArea(area0, area1, area2, area3);
-}
-
-// Return the index of maximum area
-int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
- if (area0 < area1) {
- if (area1 < area2) {
- if (area2 < area3) return 3;
- else return 2;
- }
- else {
- if (area1 < area3) return 3;
- else return 1;
- }
- }
- else {
- if (area0 < area2) {
- if (area2 < area3) return 3;
- else return 2;
- }
- else {
- if (area0 < area3) return 3;
- else return 0;
- }
- }
-}
-
// Clear the contact manifold
void ContactManifold::clear() {
for (uint i=0; i<mNbContactPoints; i++) {
@@ -258,3 +72,35 @@ void ContactManifold::clear() {
}
mNbContactPoints = 0;
}
+
+// Add a contact point
+void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
+
+ assert(mNbContactPoints < MAX_CONTACT_POINTS_IN_MANIFOLD);
+
+ // Create the new contact point
+ ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
+ ContactPoint(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
+
+ // Add the new contact point into the manifold
+ mContactPoints[mNbContactPoints] = contactPoint;
+ mNbContactPoints++;
+
+}
+
+// Remove a contact point
+void ContactManifold::removeContactPoint(int index) {
+
+ assert(mNbContactPoints > 0);
+ assert(index >= 0 && index < mNbContactPoints);
+
+ // Delete the contact
+ mContactPoints[index]->~ContactPoint();
+ mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint));
+
+ for (int i=index; (i+1) < mNbContactPoints; i++) {
+ mContactPoints[i] = mContactPoints[i+1];
+ }
+
+ mNbContactPoints--;
+}
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index c36c1476..da323c0f 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -31,6 +31,7 @@
#include "body/CollisionBody.h"
#include "collision/ProxyShape.h"
#include "constraint/ContactPoint.h"
+#include "collision/ContactManifoldInfo.h"
#include "memory/PoolAllocator.h"
/// ReactPhysics3D namespace
@@ -130,18 +131,6 @@ class ContactManifold {
// -------------------- Methods -------------------- //
- /// Return the index of maximum area
- int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
-
- /// Return the index of the contact with the larger penetration depth.
- int getIndexOfDeepestPenetration(ContactPoint* newContact) const;
-
- /// Return the index that will be removed.
- int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;
-
- /// Remove a contact point from the manifold
- void removeContactPoint(uint index);
-
/// Return true if the contact manifold has already been added into an island
bool isAlreadyInIsland() const;
@@ -150,7 +139,7 @@ class ContactManifold {
// -------------------- Methods -------------------- //
/// Constructor
- ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
+ ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
PoolAllocator& memoryAllocator, short int normalDirectionId);
/// Destructor
@@ -177,12 +166,6 @@ class ContactManifold {
/// Return the normal direction Id
short int getNormalDirectionId() const;
- /// Add a contact point to the manifold
- void addContactPoint(ContactPoint* contact);
-
- /// Update the contact manifold.
- void update(const Transform& transform1, const Transform& transform2);
-
/// Clear the contact manifold
void clear();
@@ -231,6 +214,12 @@ class ContactManifold {
/// Return the largest depth of all the contact points
decimal getLargestContactDepth() const;
+ /// Add a contact point
+ void addContactPoint(const ContactPointInfo* contactPointInfo);
+
+ /// Remove a contact point
+ void removeContactPoint(int index);
+
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h
new file mode 100644
index 00000000..d4afc590
--- /dev/null
+++ b/src/collision/ContactManifoldInfo.h
@@ -0,0 +1,130 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
+#define REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
+
+// Libraries
+#include "collision/ContactPointInfo.h"
+#include "memory/Allocator.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+
+// Class ContactManifoldInfo
+/**
+ * This class is used to collect the list of ContactPointInfo that come
+ * from a collision test between two shapes.
+ */
+class ContactManifoldInfo {
+
+ private:
+
+ // -------------------- Attributes -------------------- //
+
+ /// Linked list with all the contact points
+ ContactPointInfo* mContactPointsList;
+
+ /// Memory allocator used to allocate contact points
+ Allocator& mAllocator;
+
+ // -------------------- Methods -------------------- //
+
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ ContactManifoldInfo(Allocator& allocator) : mContactPointsList(nullptr), mAllocator(allocator) {}
+
+ /// Destructor
+ ~ContactManifoldInfo() {
+
+ // Delete all the contact points in the linked list
+ ContactPointInfo* element = mContactPointsList;
+ while(element != nullptr) {
+ ContactPointInfo* elementToDelete = element;
+ element = element->next;
+
+ // Delete the current element
+ mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
+ }
+ }
+
+ /// Deleted copy-constructor
+ ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete;
+
+ /// Deleted assignment operator
+ ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete;
+
+ /// Add a new contact point into the manifold
+ void addContactPoint(const Vector3& contactNormal, decimal penDepth,
+ const Vector3& localPt1, const Vector3& localPt2) {
+
+ assert(penDepth > decimal(0.0));
+
+ // Create the contact point info
+ ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo)))
+ ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
+
+ // Add it into the linked list of contact points
+ contactPointInfo->next = mContactPointsList;
+ mContactPointsList = contactPointInfo;
+ }
+
+ /// Get the first contact point info of the linked list of contact points
+ ContactPointInfo* getFirstContactPointInfo() const {
+ return mContactPointsList;
+ }
+
+ /// Reduce the number of points in the contact manifold
+ void reduce() {
+
+ // TODO : Implement this (do not forget to deallocate removed points)
+ }
+
+ /// Return the largest penetration depth among the contact points
+ decimal getLargestPenetrationDepth() const {
+
+ decimal maxDepth = decimal(0.0);
+ ContactPointInfo* element = mContactPointsList;
+ while(element != nullptr) {
+
+ if (element->penetrationDepth > maxDepth) {
+ maxDepth = element->penetrationDepth;
+ }
+
+ element = element->next;
+ }
+
+ return maxDepth;
+ }
+};
+
+}
+#endif
+
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index a7dc9d2e..c9b0d5bb 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -43,92 +43,147 @@ ContactManifoldSet::~ContactManifoldSet() {
clear();
}
-// Add a contact point to the manifold set
-void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
+void ContactManifoldSet::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) {
- // Compute an Id corresponding to the normal direction (using a cubemap)
- short int normalDirectionId = computeCubemapNormalId(contact->getNormal());
+ assert(contactManifoldInfo.getFirstContactPointInfo() != nullptr);
// If there is no contact manifold yet
if (mNbManifolds == 0) {
- createManifold(normalDirectionId);
- mManifolds[0]->addContactPoint(contact);
- assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
- for (int i=0; i<mNbManifolds; i++) {
- assert(mManifolds[i]->getNbContactPoints() > 0);
+ // If the maximum number of manifold is 1
+ if (mNbMaxManifolds == 1) {
+ createManifold(contactManifoldInfo, 0);
+ }
+ else {
+
+ // Compute an Id corresponding to the normal direction (using a cubemap)
+ short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal);
+
+ createManifold(contactManifoldInfo, normalDirectionId);
+ }
+ }
+ else { // If there is already at least one contact manifold in the set
+
+ // If the maximum number of manifold is 1
+ if (mNbMaxManifolds == 1) {
+
+ // Replace the old manifold with the new one
+ updateManifoldWithNewOne(0, contactManifoldInfo);
+ }
+ else {
+
+ // Compute an Id corresponding to the normal direction (using a cubemap)
+ short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal);
+
+ // Select the manifold with the most similar normal (if exists)
+ int similarManifoldIndex = 0;
+ if (mNbMaxManifolds > 1) {
+ similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
+ }
+
+ // If a similar manifold has been found
+ if (similarManifoldIndex != -1) {
+
+ // Replace the old manifold with the new one
+ updateManifoldWithNewOne(similarManifoldIndex, contactManifoldInfo);
+ }
+ else {
+
+ // If we have not reach the maximum number of manifolds
+ if (mNbManifolds < mNbMaxManifolds) {
+
+ // Create the new contact manifold
+ createManifold(contactManifoldInfo, normalDirectionId);
+ }
+ else {
+
+ decimal newManifoldPenDepth = contactManifoldInfo.getLargestPenetrationDepth();
+
+ // We have reached the maximum number of manifold, we do not
+ // want to keep the manifold with the smallest penetration detph
+ int smallestPenDepthManifoldIndex = getManifoldWithSmallestContactPenetrationDepth(newManifoldPenDepth);
+
+ // If the manifold with the smallest penetration depth is not the new one,
+ // we have to keep the new manifold and remove the one with the smallest depth
+ if (smallestPenDepthManifoldIndex >= 0) {
+ removeManifold(smallestPenDepthManifoldIndex);
+ createManifold(contactManifoldInfo, normalDirectionId);
+ }
+ }
+ }
+ }
+ }
+}
+
+// Update a previous similar manifold with a new one
+void ContactManifoldSet::updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold) {
+
+ // For each contact point of the previous manifold
+ for (int i=0; i<mManifolds[oldManifoldIndex]->getNbContactPoints(); ) {
+
+ ContactPoint* contactPoint = mManifolds[oldManifoldIndex]->getContactPoint(i);
+
+ // For each contact point in the new manifold
+ ContactPointInfo* newPoint = newManifold.getFirstContactPointInfo();
+ bool needToRemovePoint = true;
+ while (newPoint != nullptr) {
+
+ // If the new contact point is similar (very close) to the old contact point
+ if (!newPoint->isUsed && contactPoint->isSimilarWithContactPoint(newPoint)) {
+
+ // Replace (update) the old contact point with the new one
+ contactPoint->update(newPoint, mManifolds[oldManifoldIndex]->getShape1()->getLocalToWorldTransform(),
+ mManifolds[oldManifoldIndex]->getShape2()->getLocalToWorldTransform());
+ needToRemovePoint = false;
+ newPoint->isUsed = true;
+ break;
+ }
+
+ newPoint = newPoint->next;
+ }
+
+ // If no new contact point is similar to the old one
+ if (needToRemovePoint) {
+
+ // Remove the old contact point
+ mManifolds[oldManifoldIndex]->removeContactPoint(i);
+ }
+ else {
+ i++;
+ }
+ }
+
+ // For each point of the new manifold that have not been used yet (to update
+ // an existing point in the previous manifold), add it into the previous manifold
+ const ContactPointInfo* newPointInfo = newManifold.getFirstContactPointInfo();
+ while (newPointInfo != nullptr) {
+
+ if (!newPointInfo->isUsed) {
+ mManifolds[oldManifoldIndex]->addContactPoint(newPointInfo);
}
- return;
- }
+ newPointInfo = newPointInfo->next;
+ }
+}
- // Select the manifold with the most similar normal (if exists)
- int similarManifoldIndex = 0;
- if (mNbMaxManifolds > 1) {
- similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
- }
-
- // If a similar manifold has been found
- if (similarManifoldIndex != -1) {
-
- // Add the contact point to that similar manifold
- mManifolds[similarManifoldIndex]->addContactPoint(contact);
- assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0);
-
- return;
- }
-
- // If the maximum number of manifold has not been reached yet
- if (mNbManifolds < mNbMaxManifolds) {
-
- // Create a new manifold for the contact point
- createManifold(normalDirectionId);
- mManifolds[mNbManifolds-1]->addContactPoint(contact);
- for (int i=0; i<mNbManifolds; i++) {
- assert(mManifolds[i]->getNbContactPoints() > 0);
- }
-
- return;
- }
+// Return the manifold with the smallest contact penetration depth
+int ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const {
// The contact point will be in a new contact manifold, we now have too much
- // manifolds condidates. We need to remove one. We choose to keep the manifolds
- // with the largest contact depth among their points
- int smallestDepthIndex = -1;
- decimal minDepth = contact->getPenetrationDepth();
+ // manifolds condidates. We need to remove one. We choose to remove the manifold
+ // with the smallest contact depth among their points
+ int smallestDepthManifoldIndex = -1;
+ decimal minDepth = initDepth;
assert(mNbManifolds == mNbMaxManifolds);
for (int i=0; i<mNbManifolds; i++) {
decimal depth = mManifolds[i]->getLargestContactDepth();
if (depth < minDepth) {
minDepth = depth;
- smallestDepthIndex = i;
+ smallestDepthManifoldIndex = i;
}
}
- // If we do not want to keep to new manifold (not created yet) with the
- // new contact point
- if (smallestDepthIndex == -1) {
-
- // Delete the new contact
- contact->~ContactPoint();
- mMemoryAllocator.release(contact, sizeof(ContactPoint));
-
- return;
- }
-
- assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds);
-
- // Here we need to replace an existing manifold with a new one (that contains
- // the new contact point)
- removeManifold(smallestDepthIndex);
- createManifold(normalDirectionId);
- mManifolds[mNbManifolds-1]->addContactPoint(contact);
- assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
- for (int i=0; i<mNbManifolds; i++) {
- assert(mManifolds[i]->getNbContactPoints() > 0);
- }
-
- return;
+ return smallestDepthManifoldIndex;
}
// Return the index of the contact manifold with a similar average normal.
@@ -154,7 +209,7 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
int faceNo;
decimal u, v;
- decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
+ decimal max = max3(std::fabs(normal.x), std::fabs(normal.y), std::fabs(normal.z));
Vector3 normalScaled = normal / max;
if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
@@ -173,8 +228,8 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
v = normalScaled.y;
}
- int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
- int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
+ int indexU = std::floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
+ int indexV = std::floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--;
if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--;
@@ -182,22 +237,6 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
return faceNo * 200 + indexU * nbSubDivInFace + indexV;
}
-// Update the contact manifolds
-void ContactManifoldSet::update() {
-
- for (int i=mNbManifolds-1; i>=0; i--) {
-
- // Update the contact manifold
- mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
- mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform());
-
- // Remove the contact manifold if has no contact points anymore
- if (mManifolds[i]->getNbContactPoints() == 0) {
- removeManifold(i);
- }
- }
-}
-
// Clear the contact manifold set
void ContactManifoldSet::clear() {
@@ -210,11 +249,11 @@ void ContactManifoldSet::clear() {
}
// Create a new contact manifold and add it to the set
-void ContactManifoldSet::createManifold(short int normalDirectionId) {
+void ContactManifoldSet::createManifold(const ContactManifoldInfo& manifoldInfo, short int normalDirectionId) {
assert(mNbManifolds < mNbMaxManifolds);
mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
- ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId);
+ ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator, normalDirectionId);
mNbManifolds++;
}
diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h
index 21944e36..1a2bac97 100644
--- a/src/collision/ContactManifoldSet.h
+++ b/src/collision/ContactManifoldSet.h
@@ -69,7 +69,7 @@ class ContactManifoldSet {
// -------------------- Methods -------------------- //
/// Create a new contact manifold and add it to the set
- void createManifold(short normalDirectionId);
+ void createManifold(const ContactManifoldInfo& manifoldInfo, short normalDirectionId);
/// Remove a contact manifold from the set
void removeManifold(int index);
@@ -82,6 +82,12 @@ class ContactManifoldSet {
// normal vector into of the of the bucket and returns a unique Id for the bucket
short int computeCubemapNormalId(const Vector3& normal) const;
+ /// Return the manifold with the smallest contact penetration depth
+ int getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const;
+
+ /// Update a previous similar manifold with a new one
+ void updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold);
+
public:
// -------------------- Methods -------------------- //
@@ -93,18 +99,15 @@ class ContactManifoldSet {
/// Destructor
~ContactManifoldSet();
+ /// Add a contact manifold in the set
+ void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
+
/// Return the first proxy shape
ProxyShape* getShape1() const;
/// Return the second proxy shape
ProxyShape* getShape2() const;
- /// Add a contact point to the manifold set
- void addContactPoint(ContactPoint* contact);
-
- /// Update the contact manifolds
- void update();
-
/// Clear the contact manifold set
void clear();
diff --git a/src/collision/ContactPointInfo.h b/src/collision/ContactPointInfo.h
new file mode 100644
index 00000000..1ff8bfdd
--- /dev/null
+++ b/src/collision/ContactPointInfo.h
@@ -0,0 +1,88 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONTACT_POINT_INFO_H
+#define REACTPHYSICS3D_CONTACT_POINT_INFO_H
+
+// Libraries
+#include "body/CollisionBody.h"
+#include "mathematics/mathematics.h"
+#include "configuration.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Structure ContactPointInfo
+/**
+ * This structure contains informations about a collision contact
+ * computed during the narrow-phase collision detection. Those
+ * informations are used to compute the contact set for a contact
+ * between two bodies.
+ */
+struct ContactPointInfo {
+
+ private:
+
+ // -------------------- Methods -------------------- //
+
+ public:
+
+ // -------------------- Attributes -------------------- //
+
+ /// Normalized normal vector of the collision contact in world space
+ Vector3 normal;
+
+ /// Penetration depth of the contact
+ decimal penetrationDepth;
+
+ /// Contact point of body 1 in local space of body 1
+ Vector3 localPoint1;
+
+ /// Contact point of body 2 in local space of body 2
+ Vector3 localPoint2;
+
+ /// Pointer to the next contact point info
+ ContactPointInfo* next;
+
+ /// True if the contact point has already been inserted into a manifold
+ bool isUsed;
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ ContactPointInfo(const Vector3& contactNormal, decimal penDepth,
+ const Vector3& localPt1, const Vector3& localPt2)
+ : normal(contactNormal), penetrationDepth(penDepth),
+ localPoint1(localPt1), localPoint2(localPt2), next(nullptr), isUsed(false) {
+
+ }
+
+ /// Destructor
+ ~ContactPointInfo() = default;
+};
+
+}
+
+#endif
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
index 2f9f8703..e83d694c 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
@@ -30,8 +30,11 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) {
+bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
+ assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
+
const decimal parallelEpsilon = decimal(0.001);
// Get the capsule collision shapes
@@ -39,7 +42,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
// Get the transform from capsule 1 local-space to capsule 2 local-space
- const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse();
+ const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform;
// Compute the end-points of the inner segment of the first capsule
Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0);
@@ -63,7 +66,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
- if (segmentsDistance > sumRadius || segmentsDistance < MACHINE_EPSILON) {
+ if (segmentsDistance >= sumRadius || segmentsDistance < MACHINE_EPSILON) {
// The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius.
// Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision.
@@ -102,8 +105,9 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
Vector3 segment1ToSegment2 = (capsule2SegA - capsule1SegA);
Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
- const Vector3 contactPointACapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
- const Vector3 contactPointBCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
+ Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
+ const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
+ const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius();
const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius();
@@ -112,9 +116,10 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
decimal penetrationDepth = sumRadius - segmentsDistance;
// Create the contact info object
- // TODO : Make sure we create two contact points at the same time (same method here)
- contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointBCapsule1Local);
- contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule2Local, contactPointBCapsule2Local);
+ contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
+ contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
+
+ return true;
}
}
@@ -129,7 +134,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
// If the collision shapes overlap
- if (closestPointsDistanceSquare <= sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) {
+ if (closestPointsDistanceSquare < sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) {
decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
closestPointsSeg1ToSeg2 /= closestPointsDistance;
@@ -142,7 +147,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
decimal penetrationDepth = sumRadius - closestPointsDistance;
// Create the contact info object
- contactPointInfo.init(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
+ contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
return true;
}
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
index 7a46f73a..898d6337 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
@@ -62,7 +62,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) override;
+ ContactManifoldInfo& contactManifoldInfo) override;
};
}
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
index 88d4b9b2..5fdd485b 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
@@ -45,7 +45,7 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
return &mSphereVsSphereAlgorithm;
}
// Sphere vs Capsule algorithm
- if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::SPHERE) {
+ if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CAPSULE) {
return &mSphereVsCapsuleAlgorithm;
}
// Capsule vs Capsule algorithm
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index 4ea55ba4..6be90769 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -47,7 +47,7 @@ using namespace reactphysics3d;
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects.
GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) {
+ ContactManifoldInfo& contactManifoldInfo) {
PROFILE("GJKAlgorithm::testCollision()");
@@ -201,8 +201,8 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro
return GJKResult::INTERPENETRATE;
}
- // Create the contact info object
- contactPointInfo.init(normal, penetrationDepth, pA, pB);
+ // Add a new contact point
+ contactManifoldInfo.addContactPoint(normal, penetrationDepth, pA, pB);
return GJKResult::COLLIDE_IN_MARGIN;
}
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h
index 8ca25e33..f40592c4 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.h
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h
@@ -27,7 +27,8 @@
#define REACTPHYSICS3D_GJK_ALGORITHM_H
// Libraries
-#include "constraint/ContactPoint.h"
+#include "collision/ContactManifoldInfo.h"
+#include "collision/NarrowPhaseInfo.h"
#include "collision/shapes/ConvexShape.h"
#include "VoronoiSimplex.h"
@@ -87,7 +88,7 @@ class GJKAlgorithm {
/// Compute a contact info if the two bounding volumes collide.
GJKResult testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo);
+ ContactManifoldInfo& contactManifoldInfo);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
index f7f68907..f665181b 100644
--- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h
+++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
@@ -28,7 +28,7 @@
// Libraries
#include "body/Body.h"
-#include "constraint/ContactPoint.h"
+#include "collision/ContactManifoldInfo.h"
#include "memory/PoolAllocator.h"
#include "engine/OverlappingPair.h"
#include "collision/NarrowPhaseInfo.h"
@@ -84,7 +84,7 @@ class NarrowPhaseAlgorithm {
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
- virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo)=0;
+ virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo)=0;
};
}
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index d74faf09..b46ca386 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -37,7 +37,7 @@
using namespace reactphysics3d;
bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) {
+ ContactManifoldInfo& contactManifoldInfo) {
}
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 30156cc2..80feece1 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -27,7 +27,8 @@
#define REACTPHYSICS3D_SAT_ALGORITHM_H
// Libraries
-#include "constraint/ContactPoint.h"
+#include "collision/ContactManifoldInfo.h"
+#include "collision/NarrowPhaseInfo.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -59,7 +60,7 @@ class SATAlgorithm {
/// Compute a contact info if the two bounding volumes collide.
bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo);
+ ContactManifoldInfo& contactManifoldInfo);
};
}
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
index 24cb4e96..aace17fd 100644
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
@@ -31,14 +31,17 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) {
+bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
+ assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
+
// Get the collision shapes
const SphereShape* sphereShape = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
// Get the transform from sphere local-space to capsule local-space
- const Transform sphereToCapsuleSpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse();
+ const Transform sphereToCapsuleSpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform;
// Transform the center of the sphere into the local-space of the capsule shape
const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition();
@@ -58,7 +61,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius();
// If the collision shapes overlap
- if (sphereSegmentDistanceSquare <= sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) {
+ if (sphereSegmentDistanceSquare < sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) {
decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
sphereCenterToSegment /= sphereSegmentDistance;
@@ -71,7 +74,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
decimal penetrationDepth = sumRadius - sphereSegmentDistance;
// Create the contact info object
- contactPointInfo.init(normalWorld, penetrationDepth, contactPointSphereLocal, contactPointCapsuleLocal);
+ contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointSphereLocal, contactPointCapsuleLocal);
return true;
}
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
index 98eaf894..784df6f9 100644
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
@@ -62,7 +62,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) override;
+ ContactManifoldInfo& contactManifoldInfo) override;
};
}
diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp
index 65371e4f..06dbc615 100644
--- a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp
@@ -33,7 +33,7 @@
using namespace reactphysics3d;
bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) {
+ ContactManifoldInfo& contactManifoldInfo) {
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
@@ -41,7 +41,7 @@ bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPha
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
- GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactPointInfo);
+ GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
// If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
@@ -55,7 +55,7 @@ bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPha
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
- return satAlgorithm.testCollision(narrowPhaseInfo, contactPointInfo);
+ return satAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
}
return false;
diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h
index 448b8a5d..f0046545 100644
--- a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h
@@ -62,7 +62,7 @@ class SphereVsConvexMeshAlgorithm : public NarrowPhaseAlgorithm {
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) override;
+ ContactManifoldInfo& contactManifoldInfo) override;
};
}
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
index e80688d5..54e51e52 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
@@ -31,8 +31,11 @@
using namespace reactphysics3d;
bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) {
+ ContactManifoldInfo& contactManifoldInfo) {
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
+ assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
+
// Get the sphere collision shapes
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape2);
@@ -49,7 +52,7 @@ bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseIn
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes intersect
- if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
+ if (squaredDistanceBetweenCenters < sumRadius * sumRadius) {
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
@@ -59,8 +62,8 @@ bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseIn
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
- contactPointInfo.init(vectorBetweenCenters.getUnit(), penetrationDepth,
- intersectionOnBody1, intersectionOnBody2);
+ contactManifoldInfo.addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
+ intersectionOnBody1, intersectionOnBody2);
return true;
}
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
index be10d5a1..b70befd2 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
@@ -62,7 +62,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactPointInfo& contactPointInfo) override;
+ ContactManifoldInfo& contactManifoldInfo) override;
};
}
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index 464c1bd5..c2db3466 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -40,7 +40,7 @@
namespace reactphysics3d {
/// Type of the collision shape
-enum class CollisionShapeType {TRIANGLE, BOX, CAPSULE, SPHERE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
+enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, BOX, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
const int NB_COLLISION_SHAPE_TYPES = 9;
// Declarations
diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp
index 00ed60d7..f420bbff 100644
--- a/src/constraint/ContactPoint.cpp
+++ b/src/constraint/ContactPoint.cpp
@@ -31,13 +31,35 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
-ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
- : mNormal(contactInfo.normal),
- mPenetrationDepth(contactInfo.penetrationDepth),
- mLocalPointOnBody1(contactInfo.localPoint1),
- mLocalPointOnBody2(contactInfo.localPoint2),
+ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform,
+ const Transform& body2Transform)
+ : mNormal(contactInfo->normal),
+ mPenetrationDepth(contactInfo->penetrationDepth),
+ mLocalPointOnBody1(contactInfo->localPoint1),
+ mLocalPointOnBody2(contactInfo->localPoint2),
mIsRestingContact(false) {
assert(mPenetrationDepth > decimal(0.0));
+ // Compute the world position of the contact points
+ mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
+ mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
+}
+
+// Update the contact point with a new one that is similar (very close)
+/// The idea is to keep the cache impulse (for warm starting the contact solver)
+void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
+ const Transform& body2Transform) {
+
+ assert(isSimilarWithContactPoint(contactInfo));
+ assert(contactInfo->penetrationDepth > decimal(0.0));
+
+ mNormal = contactInfo->normal;
+ mPenetrationDepth = contactInfo->penetrationDepth;
+ mLocalPointOnBody1 = contactInfo->localPoint1;
+ mLocalPointOnBody2 = contactInfo->localPoint2;
+
+ // Compute the world position of the contact points
+ mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
+ mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
}
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index 4028ed8f..ec4c6556 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -29,60 +29,13 @@
// Libraries
#include "body/CollisionBody.h"
#include "collision/NarrowPhaseInfo.h"
+#include "collision/ContactPointInfo.h"
#include "configuration.h"
#include "mathematics/mathematics.h"
-#include "configuration.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
-// Structure ContactPointInfo
-/**
- * This structure contains informations about a collision contact
- * computed during the narrow-phase collision detection. Those
- * informations are used to compute the contact set for a contact
- * between two bodies.
- */
-struct ContactPointInfo {
-
- private:
-
- // -------------------- Methods -------------------- //
-
- public:
-
- // -------------------- Attributes -------------------- //
-
- /// Normalized normal vector of the collision contact in world space
- Vector3 normal;
-
- /// Penetration depth of the contact
- decimal penetrationDepth;
-
- /// Contact point of body 1 in local space of body 1
- Vector3 localPoint1;
-
- /// Contact point of body 2 in local space of body 2
- Vector3 localPoint2;
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- ContactPointInfo() = default;
-
- /// Destructor
- ~ContactPointInfo() = default;
-
- /// Initialize the contact point info
- void init(const Vector3& contactNormal, decimal penDepth,
- const Vector3& localPt1, const Vector3& localPt2) {
- normal = contactNormal;
- penetrationDepth = penDepth;
- localPoint1 = localPt1;
- localPoint2 = localPt2;
- }
-};
-
// Class ContactPoint
/**
* This class represents a collision contact point between two
@@ -95,16 +48,16 @@ class ContactPoint {
// -------------------- Attributes -------------------- //
/// Normalized normal vector of the contact (from body1 toward body2) in world space
- const Vector3 mNormal;
+ Vector3 mNormal;
/// Penetration depth
decimal mPenetrationDepth;
/// Contact point on body 1 in local space of body 1
- const Vector3 mLocalPointOnBody1;
+ Vector3 mLocalPointOnBody1;
/// Contact point on body 2 in local space of body 2
- const Vector3 mLocalPointOnBody2;
+ Vector3 mLocalPointOnBody2;
/// Contact point on body 1 in world space
Vector3 mWorldPointOnBody1;
@@ -123,7 +76,8 @@ class ContactPoint {
// -------------------- Methods -------------------- //
/// Constructor
- ContactPoint(const ContactPointInfo& contactInfo);
+ ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform,
+ const Transform& body2Transform);
/// Destructor
~ContactPoint() = default;
@@ -134,11 +88,9 @@ class ContactPoint {
/// Deleted assignment operator
ContactPoint& operator=(const ContactPoint& contact) = delete;
- /// Update the world contact points
- void updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform);
-
- /// Update the penetration depth
- void updatePenetrationDepth();
+ /// Update the contact point with a new one that is similar (very close)
+ void update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
+ const Transform& body2Transform);
/// Return the normal vector of the contact
Vector3 getNormal() const;
@@ -158,6 +110,9 @@ class ContactPoint {
/// Return the cached penetration impulse
decimal getPenetrationImpulse() const;
+ /// Return true if the contact point is similar (close enougth) to another given contact point
+ bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
+
/// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse);
@@ -174,17 +129,6 @@ class ContactPoint {
size_t getSizeInBytes() const;
};
-// Update the world contact points
-inline void ContactPoint::updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform) {
- mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
- mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
-}
-
-// Update the penetration depth
-inline void ContactPoint::updatePenetrationDepth() {
- mPenetrationDepth = (mWorldPointOnBody1 - mWorldPointOnBody2).dot(mNormal);
-}
-
// Return the normal vector of the contact
inline Vector3 ContactPoint::getNormal() const {
return mNormal;
@@ -215,6 +159,12 @@ inline decimal ContactPoint::getPenetrationImpulse() const {
return mPenetrationImpulse;
}
+// Return true if the contact point is similar (close enougth) to another given contact point
+inline bool ContactPoint::isSimilarWithContactPoint(const ContactPointInfo* localContactPointBody1) const {
+ return (localContactPointBody1->localPoint1 - mLocalPointOnBody1).lengthSquare() <= (PERSISTENT_CONTACT_DIST_THRESHOLD *
+ PERSISTENT_CONTACT_DIST_THRESHOLD);
+}
+
// Set the cached penetration impulse
inline void ContactPoint::setPenetrationImpulse(decimal impulse) {
mPenetrationImpulse = impulse;
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index dc162716..0e24f0c8 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -247,16 +247,16 @@ class CollisionCallback {
struct CollisionCallbackInfo {
public:
- const ContactPointInfo& contactPoint;
+ const ContactManifoldInfo& contactManifold;
CollisionBody* body1;
CollisionBody* body2;
const ProxyShape* proxyShape1;
const ProxyShape* proxyShape2;
// Constructor
- CollisionCallbackInfo(const ContactPointInfo& point, CollisionBody* b1, CollisionBody* b2,
+ CollisionCallbackInfo(const ContactManifoldInfo& manifold, CollisionBody* b1, CollisionBody* b2,
const ProxyShape* proxShape1, const ProxyShape* proxShape2) :
- contactPoint(point), body1(b1), body2(b2),
+ contactManifold(manifold), body1(b1), body2(b2),
proxyShape1(proxShape1), proxyShape2(proxShape2) {
}
diff --git a/src/engine/EventListener.h b/src/engine/EventListener.h
index 562e8ac7..e8594255 100644
--- a/src/engine/EventListener.h
+++ b/src/engine/EventListener.h
@@ -27,7 +27,7 @@
#define REACTPHYSICS3D_EVENT_LISTENER_H
// Libraries
-#include "constraint/ContactPoint.h"
+#include "collision/ContactManifoldInfo.h"
namespace reactphysics3d {
@@ -53,13 +53,13 @@ class EventListener {
/**
* @param contact Information about the contact
*/
- virtual void beginContact(const ContactPointInfo& contact) {}
+ virtual void beginContact(const ContactManifoldInfo& contactManifold) {}
/// Called when a new contact point is found between two bodies
/**
* @param contact Information about the contact
*/
- virtual void newContact(const ContactPointInfo& contact) {}
+ virtual void newContact(const ContactManifoldInfo& contactManifold) {}
/// Called at the beginning of an internal tick of the simulation step.
/// Each time the DynamicsWorld::update() method is called, the physics
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index 70ee1c3f..c34b91b7 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -80,11 +80,8 @@ class OverlappingPair {
/// Return the pointer to second body
ProxyShape* getShape2() const;
- /// Add a contact to the contact cache
- void addContact(ContactPoint* contact);
-
- /// Update the contact cache
- void update();
+ /// Add a contact manifold
+ void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
/// Return the cached separating axis
Vector3 getCachedSeparatingAxis() const;
@@ -123,13 +120,8 @@ inline ProxyShape* OverlappingPair::getShape2() const {
}
// Add a contact to the contact manifold
-inline void OverlappingPair::addContact(ContactPoint* contact) {
- mContactManifoldSet.addContactPoint(contact);
-}
-
-// Update the contact manifold
-inline void OverlappingPair::update() {
- mContactManifoldSet.update();
+inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) {
+ mContactManifoldSet.addContactManifold(contactManifoldInfo);
}
// Return the cached separating axis
From 0ecd554f500c4e292560091c04d085004a8c8050 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 27 Feb 2017 19:06:28 +0100
Subject: [PATCH 039/133] Update collision detection scene in testbed
application
---
.../CollisionDetectionScene.cpp | 18 +++++++++
.../CollisionDetectionScene.h | 38 +++++++++++--------
2 files changed, 41 insertions(+), 15 deletions(-)
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index 3d692501..8c0196b3 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -82,6 +82,17 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mCapsule1->setColor(mGreyColorDemo);
mCapsule1->setSleepingColor(mRedColorDemo);
+ // ---------- Capsule 2 ---------- //
+ openglframework::Vector3 position4(-4, 0, 0);
+
+ // Create a cylinder and a corresponding collision body in the dynamics world
+ mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position4, mCollisionWorld, mMeshFolderPath);
+ mAllShapes.push_back(mCapsule2);
+
+ // Set the color
+ mCapsule2->setColor(mGreyColorDemo);
+ mCapsule2->setSleepingColor(mRedColorDemo);
+
// ---------- Cone ---------- //
//openglframework::Vector3 position4(0, 0, 0);
@@ -163,6 +174,12 @@ CollisionDetectionScene::~CollisionDetectionScene() {
mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody());
delete mSphere2;
+ mCollisionWorld->destroyCollisionBody(mCapsule1->getCollisionBody());
+ delete mCapsule1;
+
+ mCollisionWorld->destroyCollisionBody(mCapsule2->getCollisionBody());
+ delete mCapsule2;
+
/*
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody());
@@ -285,6 +302,7 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
/*
if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index c2aec0cd..2c33b3ac 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -51,8 +51,8 @@ const float CONE_RADIUS = 3.0f;
const float CONE_HEIGHT = 5.0f;
const float CYLINDER_RADIUS = 3.0f;
const float CYLINDER_HEIGHT = 5.0f;
-const float CAPSULE_RADIUS = 3.0f;
-const float CAPSULE_HEIGHT = 5.0f;
+const float CAPSULE_RADIUS = 1.0f;
+const float CAPSULE_HEIGHT = 1.0f;
const float DUMBBELL_HEIGHT = 5.0f;
const int NB_RAYS = 100;
const float RAY_LENGTH = 30.0f;
@@ -83,21 +83,29 @@ class ContactManager : public rp3d::CollisionCallback {
/// This method will be called for each reported contact point
virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
- rp3d::Vector3 point1 = collisionCallbackInfo.contactPoint.localPoint1;
- point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
- openglframework::Vector3 position1(point1.x, point1.y, point1.z);
- mContactPoints.push_back(ContactPoint(position1));
+ // For each contact point
+ rp3d::ContactPointInfo* contactPointInfo = collisionCallbackInfo.contactManifold.getFirstContactPointInfo();
+ while (contactPointInfo != nullptr) {
- rp3d::Vector3 point2 = collisionCallbackInfo.contactPoint.localPoint2;
- point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
- openglframework::Vector3 position2(point2.x, point2.y, point2.z);
- mContactPoints.push_back(ContactPoint(position2));
+ rp3d::Vector3 point1 = contactPointInfo->localPoint1;
+ point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
+ openglframework::Vector3 position1(point1.x, point1.y, point1.z);
+ mContactPoints.push_back(ContactPoint(position1));
- // Create a line to display the normal at hit point
- rp3d::Vector3 n = collisionCallbackInfo.contactPoint.normal;
- openglframework::Vector3 normal(n.x, n.y, n.z);
- Line* normalLine = new Line(position1, position1 + normal);
- mNormals.push_back(normalLine);
+
+ rp3d::Vector3 point2 = contactPointInfo->localPoint2;
+ point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
+ openglframework::Vector3 position2(point2.x, point2.y, point2.z);
+ mContactPoints.push_back(ContactPoint(position2));
+
+ // Create a line to display the normal at hit point
+ rp3d::Vector3 n = contactPointInfo->normal;
+ openglframework::Vector3 normal(n.x, n.y, n.z);
+ Line* normalLine = new Line(position1, position1 + normal);
+ mNormals.push_back(normalLine);
+
+ contactPointInfo = contactPointInfo->next;
+ }
}
void resetPoints() {
From 050e8b36dcfc921a19007548a932470a29ab59d3 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 22 Mar 2017 19:07:31 +0100
Subject: [PATCH 040/133] Refactor convex mesh shape (create PolyhedronMesh,
ConvexPolyhedron classes)
---
CMakeLists.txt | 4 +
src/collision/HalfEdgeStructure.cpp | 29 +--
src/collision/HalfEdgeStructure.h | 37 +++-
src/collision/PolygonVertexArray.cpp | 75 +++++++
src/collision/PolygonVertexArray.h | 188 +++++++++++++++++
src/collision/PolyhedronMesh.cpp | 79 +++++--
src/collision/PolyhedronMesh.h | 39 ++--
.../narrowphase/SAT/SATAlgorithm.cpp | 57 ++++-
src/collision/narrowphase/SAT/SATAlgorithm.h | 16 ++
src/collision/shapes/BoxShape.cpp | 2 +-
src/collision/shapes/BoxShape.h | 4 +-
src/collision/shapes/CollisionShape.h | 2 +-
src/collision/shapes/ConvexMeshShape.cpp | 198 +++---------------
src/collision/shapes/ConvexMeshShape.h | 110 +---------
src/collision/shapes/ConvexPolyhedron.cpp | 37 ++++
src/collision/shapes/ConvexPolyhedron.h | 68 ++++++
src/reactphysics3d.h | 2 +
test/tests/collision/TestHalfEdgeStructure.h | 141 +++++++------
test/tests/collision/TestPointInside.h | 10 +-
test/tests/collision/TestRaycast.h | 10 +-
testbed/common/ConvexMesh.cpp | 58 +++--
testbed/common/ConvexMesh.h | 6 +-
22 files changed, 751 insertions(+), 421 deletions(-)
create mode 100644 src/collision/PolygonVertexArray.cpp
create mode 100644 src/collision/PolygonVertexArray.h
create mode 100644 src/collision/shapes/ConvexPolyhedron.cpp
create mode 100644 src/collision/shapes/ConvexPolyhedron.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 58ea37d7..4f6ab0bc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -90,6 +90,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/shapes/AABB.cpp"
"src/collision/shapes/ConvexShape.h"
"src/collision/shapes/ConvexShape.cpp"
+ "src/collision/shapes/ConvexPolyhedron.h"
+ "src/collision/shapes/ConvexPolyhedron.cpp"
"src/collision/shapes/ConcaveShape.h"
"src/collision/shapes/ConcaveShape.cpp"
"src/collision/shapes/BoxShape.h"
@@ -114,6 +116,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/ProxyShape.cpp"
"src/collision/TriangleVertexArray.h"
"src/collision/TriangleVertexArray.cpp"
+ "src/collision/PolygonVertexArray.h"
+ "src/collision/PolygonVertexArray.cpp"
"src/collision/TriangleMesh.h"
"src/collision/TriangleMesh.cpp"
"src/collision/PolyhedronMesh.h"
diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp
index 6393acb0..0f7b8f6e 100644
--- a/src/collision/HalfEdgeStructure.cpp
+++ b/src/collision/HalfEdgeStructure.cpp
@@ -29,8 +29,8 @@
using namespace reactphysics3d;
-// Initialize the structure
-void HalfEdgeStructure::init(std::vector<Vector3> vertices, std::vector<std::vector<uint>> faces) {
+// Initialize the structure (when all vertices and faces have been added)
+void HalfEdgeStructure::init() {
using edgeKey = std::pair<uint, uint>;
@@ -41,30 +41,19 @@ void HalfEdgeStructure::init(std::vector<Vector3> vertices, std::vector<std::vec
std::map<uint, edgeKey> mapEdgeIndexToKey;
std::map<uint, edgeKey> mapFaceIndexToEdgeKey;
- // For each vertices
- for (uint v=0; v<vertices.size(); v++) {
- Vertex vertex(vertices[v]);
- mVertices.push_back(vertex);
- }
-
// For each face
- for (uint f=0; f<faces.size(); f++) {
+ for (uint f=0; f<mFaces.size(); f++) {
- // Create a new face
- Face face;
- mFaces.push_back(face);
-
- // Vertices of the current face
- std::vector<uint>& faceVertices = faces[f];
+ Face face = mFaces[f];
std::vector<edgeKey> currentFaceEdges;
edgeKey firstEdgeKey;
// For each vertex of the face
- for (uint v=0; v < faceVertices.size(); v++) {
- uint v1Index = faceVertices[v];
- uint v2Index = faceVertices[v == (faceVertices.size() - 1) ? 0 : v + 1];
+ for (uint v=0; v < face.faceVertices.size(); v++) {
+ uint v1Index = face.faceVertices[v];
+ uint v2Index = face.faceVertices[v == (face.faceVertices.size() - 1) ? 0 : v + 1];
const edgeKey pairV1V2 = std::make_pair(v1Index, v2Index);
@@ -78,7 +67,7 @@ void HalfEdgeStructure::init(std::vector<Vector3> vertices, std::vector<std::vec
else if (v >= 1) {
nextEdges.insert(std::make_pair(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2));
}
- if (v == (faceVertices.size() - 1)) {
+ if (v == (face.faceVertices.size() - 1)) {
nextEdges.insert(std::make_pair(pairV1V2, firstEdgeKey));
}
edges.insert(std::make_pair(pairV1V2, edge));
@@ -121,7 +110,7 @@ void HalfEdgeStructure::init(std::vector<Vector3> vertices, std::vector<std::vec
}
// Set face edge
- for (uint f=0; f < faces.size(); f++) {
+ for (uint f=0; f < mFaces.size(); f++) {
mFaces[f].edgeIndex = mapEdgeToIndex[mapFaceIndexToEdgeKey[f]];
}
}
diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h
index 707c7d83..01e305c5 100644
--- a/src/collision/HalfEdgeStructure.h
+++ b/src/collision/HalfEdgeStructure.h
@@ -50,15 +50,19 @@ class HalfEdgeStructure {
};
struct Face {
- uint edgeIndex; // Index of an half-edge of the face
+ uint edgeIndex; // Index of an half-edge of the face
+ std::vector<uint> faceVertices; // Index of the vertices of the face
+
+ /// Constructor
+ Face(std::vector<uint> vertices) : faceVertices(vertices) {}
};
struct Vertex {
- Vector3 point; // Coordinates of the vertex
+ uint vertexPointIndex; // Index of the vertex point in the origin vertex array
uint edgeIndex; // Index of one edge emanting from this vertex
/// Constructor
- Vertex(Vector3& p) { point = p;}
+ Vertex(uint vertexCoordsIndex) : vertexPointIndex(vertexCoordsIndex) { }
};
private:
@@ -80,8 +84,14 @@ class HalfEdgeStructure {
/// Destructor
~HalfEdgeStructure() = default;
- /// Initialize the structure
- void init(std::vector<Vector3> vertices, std::vector<std::vector<uint>> faces);
+ /// Initialize the structure (when all vertices and faces have been added)
+ void init();
+
+ /// Add a vertex
+ uint addVertex(uint vertexPointIndex);
+
+ /// Add a face
+ void addFace(std::vector<uint> faceVertices);
/// Return the number of faces
uint getNbFaces() const;
@@ -98,11 +108,26 @@ class HalfEdgeStructure {
/// Return a given edge
Edge getHalfEdge(uint index) const;
- /// Retunr a given vertex
+ /// Return a given vertex
Vertex getVertex(uint index) const;
};
+// Add a vertex
+inline uint HalfEdgeStructure::addVertex(uint vertexPointIndex) {
+ Vertex vertex(vertexPointIndex);
+ mVertices.push_back(vertex);
+ return mVertices.size() - 1;
+}
+
+// Add a face
+inline void HalfEdgeStructure::addFace(std::vector<uint> faceVertices) {
+
+ // Create a new face
+ Face face(faceVertices);
+ mFaces.push_back(face);
+}
+
// Return the number of faces
inline uint HalfEdgeStructure::getNbFaces() const {
return mFaces.size();
diff --git a/src/collision/PolygonVertexArray.cpp b/src/collision/PolygonVertexArray.cpp
new file mode 100644
index 00000000..309a0d62
--- /dev/null
+++ b/src/collision/PolygonVertexArray.cpp
@@ -0,0 +1,75 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "PolygonVertexArray.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+/// Note that your data will not be copied into the PolygonVertexArray and
+/// therefore, you need to make sure that those data are always valid during
+/// the lifetime of the PolygonVertexArray.
+/**
+ */
+PolygonVertexArray::PolygonVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
+ void* indexesStart, int indexesStride,
+ uint nbFaces, PolygonFace* facesStart,
+ VertexDataType vertexDataType, IndexDataType indexDataType) {
+ mNbVertices = nbVertices;
+ mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
+ mVerticesStride = verticesStride;
+ mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
+ mIndicesStride = indexesStride;
+ mNbFaces = nbFaces;
+ mPolygonFacesStart = facesStart;
+ mVertexDataType = vertexDataType;
+ mIndexDataType = indexDataType;
+}
+
+// Return the vertex index of a given vertex in a face
+uint PolygonVertexArray::getVertexIndexInFace(uint faceIndex, uint noVertexInFace) const {
+
+ assert(faceIndex < mNbFaces);
+
+ // Get the face
+ PolygonFace* face = getPolygonFace(faceIndex);
+
+ assert(noVertexInFace < face->nbVertices);
+
+ void* vertexIndexPointer = mIndicesStart + (face->indexBase + noVertexInFace) * mIndicesStride;
+
+ if (mIndexDataType == PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
+ return *((uint*)vertexIndexPointer);
+ }
+ else if (mIndexDataType == PolygonVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
+ return *((unsigned short*)vertexIndexPointer);
+ }
+ else {
+ assert(false);
+ }
+
+ return 0;
+}
diff --git a/src/collision/PolygonVertexArray.h b/src/collision/PolygonVertexArray.h
new file mode 100644
index 00000000..5ae02e0e
--- /dev/null
+++ b/src/collision/PolygonVertexArray.h
@@ -0,0 +1,188 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_POLYGON_VERTEX_ARRAY_H
+#define REACTPHYSICS3D_POLYGON_VERTEX_ARRAY_H
+
+// Libraries
+#include "configuration.h"
+#include <cassert>
+
+namespace reactphysics3d {
+
+// Class PolygonVertexArray
+/**
+ * This class is used to describe the vertices and faces of a polyhedron mesh.
+ * A PolygonVertexArray represents an array of vertices and polygon faces
+ * of a polyhedron mesh. When you create a PolygonVertexArray, no data is copied
+ * into the array. It only stores pointer to the data. The purpose is to allow
+ * the user to share vertices data between the physics engine and the rendering
+ * part. Therefore, make sure that the data pointed by a PolygonVertexArray
+ * remains valid during the PolygonVertexArray life.
+ */
+class PolygonVertexArray {
+
+ public:
+
+ /// Data type for the vertices in the array
+ enum class VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
+
+ /// Data type for the indices in the array
+ enum class IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
+
+ /// Represent a polygon face of the polyhedron
+ struct PolygonFace {
+
+ /// Number of vertices in the polygon face
+ uint nbVertices;
+
+ /// Index of the first vertex of the polygon face
+ /// inside the array with all vertex indices
+ uint indexBase;
+ };
+
+ protected:
+
+ /// Number of vertices in the array
+ uint mNbVertices;
+
+ /// Pointer to the first vertex value in the array
+ unsigned char* mVerticesStart;
+
+ /// Stride (number of bytes) between the beginning of two vertices
+ /// values in the array
+ int mVerticesStride;
+
+ /// Pointer to the first vertex index of the array
+ unsigned char* mIndicesStart;
+
+ /// Stride (number of bytes) between the beginning of two indices in
+ /// the array
+ int mIndicesStride;
+
+ /// Number of polygon faces in the array
+ uint mNbFaces;
+
+ /// Pointer to the first polygon face in the polyhedron
+ PolygonFace* mPolygonFacesStart;
+
+ /// Data type of the vertices in the array
+ VertexDataType mVertexDataType;
+
+ /// Data type of the indices in the array
+ IndexDataType mIndexDataType;
+
+ public:
+
+ /// Constructor
+ PolygonVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
+ void* indexesStart, int indexesStride,
+ uint nbFaces, PolygonFace* facesStart,
+ VertexDataType vertexDataType, IndexDataType indexDataType);
+
+ /// Destructor
+ ~PolygonVertexArray() = default;
+
+ /// Return the vertex data type
+ VertexDataType getVertexDataType() const;
+
+ /// Return the index data type
+ IndexDataType getIndexDataType() const;
+
+ /// Return the number of vertices
+ uint getNbVertices() const;
+
+ /// Return the number of faces
+ uint getNbFaces() const;
+
+ /// Return the vertices stride (number of bytes)
+ int getVerticesStride() const;
+
+ /// Return the indices stride (number of bytes)
+ int getIndicesStride() const;
+
+ /// Return the vertex index of a given vertex in a face
+ uint getVertexIndexInFace(uint faceIndex, uint noVertexInFace) const;
+
+ /// Return a polygon face of the polyhedron
+ PolygonFace* getPolygonFace(uint faceIndex) const;
+
+ /// Return the pointer to the start of the vertices array
+ unsigned char* getVerticesStart() const;
+
+ /// Return the pointer to the start of the indices array
+ unsigned char* getIndicesStart() const;
+};
+
+// Return the vertex data type
+inline PolygonVertexArray::VertexDataType PolygonVertexArray::getVertexDataType() const {
+ return mVertexDataType;
+}
+
+// Return the index data type
+inline PolygonVertexArray::IndexDataType PolygonVertexArray::getIndexDataType() const {
+ return mIndexDataType;
+}
+
+// Return the number of vertices
+inline uint PolygonVertexArray::getNbVertices() const {
+ return mNbVertices;
+}
+
+// Return the number of faces
+inline uint PolygonVertexArray::getNbFaces() const {
+ return mNbFaces;
+}
+
+// Return the vertices stride (number of bytes)
+inline int PolygonVertexArray::getVerticesStride() const {
+ return mVerticesStride;
+}
+
+// Return the indices stride (number of bytes)
+inline int PolygonVertexArray::getIndicesStride() const {
+ return mIndicesStride;
+}
+
+// Return a polygon face of the polyhedron
+inline PolygonVertexArray::PolygonFace* PolygonVertexArray::getPolygonFace(uint faceIndex) const {
+ assert(faceIndex < mNbFaces);
+ return &mPolygonFacesStart[faceIndex];
+}
+
+// Return the pointer to the start of the vertices array
+inline unsigned char* PolygonVertexArray::getVerticesStart() const {
+ return mVerticesStart;
+}
+
+// Return the pointer to the start of the indices array
+inline unsigned char* PolygonVertexArray::getIndicesStart() const {
+ return mIndicesStart;
+}
+
+}
+
+#endif
+
diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp
index 891a2f47..df43a47a 100644
--- a/src/collision/PolyhedronMesh.cpp
+++ b/src/collision/PolyhedronMesh.cpp
@@ -30,33 +30,70 @@ using namespace reactphysics3d;
// Constructor
-PolyhedronMesh::PolyhedronMesh() : mIsFinalized(false) {
+PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) {
+ mPolygonVertexArray = polygonVertexArray;
+
+ // Create the half-edge structure of the mesh
+ createHalfEdgeStructure();
}
-// Add a vertex into the polyhedron.
-// This method returns the index of the vertex that you need to use
-// to add faces.
-uint PolyhedronMesh::addVertex(const Vector3& vertex) {
- mVertices.push_back(vertex);
- return mVertices.size() - 1;
-}
+// Create the half-edge structure of the mesh
+void PolyhedronMesh::createHalfEdgeStructure() {
-// Add a face into the polyhedron.
-// A face is a list of vertices indices (returned by addVertex() method).
-// The order of the indices are important. You need to specify the vertices of
-// of the face sorted counter-clockwise as seen from the outside of the polyhedron.
-void PolyhedronMesh::addFace(std::vector<uint> faceVertices) {
- mFaces.push_back(faceVertices);
-}
+ // For each vertex of the mesh
+ for (uint v=0; v < mPolygonVertexArray->getNbVertices(); v++) {
+ mHalfEdgeStructure.addVertex(v);
+ }
-// Call this method when you are done adding vertices and faces
-void PolyhedronMesh::finalize() {
+ // For each polygon face of the mesh
+ for (uint f=0; f < mPolygonVertexArray->getNbFaces(); f++) {
- if (mIsFinalized) return;
+ // Get the polygon face
+ PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f);
+
+ std::vector<uint> faceVertices;
+
+ // For each vertex of the face
+ for (uint v=0; v < face->nbVertices; v++) {
+ faceVertices.push_back(mPolygonVertexArray->getVertexIndexInFace(f, v));
+ }
+
+ // Addd the face into the half-edge structure
+ mHalfEdgeStructure.addFace(faceVertices);
+ }
// Initialize the half-edge structure
- mHalfEdgeStructure.init(mVertices, mFaces);
-
- mIsFinalized = true;
+ mHalfEdgeStructure.init();
+}
+
+/// Return a vertex
+Vector3 PolyhedronMesh::getVertex(uint index) const {
+ assert(index < getNbVertices());
+
+ // Get the vertex index in the array with all vertices
+ uint vertexIndex = mHalfEdgeStructure.getVertex(index).vertexPointIndex;
+
+ PolygonVertexArray::VertexDataType vertexType = mPolygonVertexArray->getVertexDataType();
+ unsigned char* verticesStart = mPolygonVertexArray->getVerticesStart();
+ int vertexStride = mPolygonVertexArray->getVerticesStride();
+
+ Vector3 vertex;
+ if (vertexType == PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
+ const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
+ vertex.x = decimal(vertices[0]);
+ vertex.y = decimal(vertices[1]);
+ vertex.z = decimal(vertices[2]);
+ }
+ else if (vertexType == PolygonVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
+ const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
+ vertex.x = decimal(vertices[0]);
+ vertex.y = decimal(vertices[1]);
+ vertex.z = decimal(vertices[2]);
+ }
+ else {
+ assert(false);
+ }
+
+ return vertex;
}
diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h
index c7182bd8..f842cae1 100644
--- a/src/collision/PolyhedronMesh.h
+++ b/src/collision/PolyhedronMesh.h
@@ -29,6 +29,7 @@
// Libraries
#include "mathematics/mathematics.h"
#include "HalfEdgeStructure.h"
+#include "collision/PolygonVertexArray.h"
#include <vector>
namespace reactphysics3d {
@@ -36,45 +37,51 @@ namespace reactphysics3d {
// Class PolyhedronMesh
/**
* This class describes a polyhedron mesh made of faces and vertices.
- * The faces do not have to be triangle
+ * The faces do not have to be triangles.
*/
class PolyhedronMesh {
private:
+ // -------------------- Attributes -------------------- //
+
+ /// Pointer the the polygon vertex array with vertices and faces
+ /// of the mesh
+ PolygonVertexArray* mPolygonVertexArray;
+
/// Half-edge structure of the mesh
HalfEdgeStructure mHalfEdgeStructure;
- /// True if the half-edge structure has been generated
- bool mIsFinalized;
+ // -------------------- Methods -------------------- //
- /// All the vertices
- std::vector<Vector3> mVertices;
-
- /// All the indexes of the face vertices
- std::vector<std::vector<uint>> mFaces;
+ /// Create the half-edge structure of the mesh
+ void createHalfEdgeStructure();
public:
+ // -------------------- Methods -------------------- //
+
/// Constructor
- PolyhedronMesh();
+ PolyhedronMesh(PolygonVertexArray* polygonVertexArray);
/// Destructor
~PolyhedronMesh() = default;
- /// Add a vertex into the polyhedron
- uint addVertex(const Vector3& vertex);
+ /// Return the number of vertices
+ uint getNbVertices() const;
- /// Add a face into the polyhedron
- void addFace(std::vector<uint> faceVertices);
-
- /// Call this method when you are done adding vertices and faces
- void finalize();
+ /// Return a vertex
+ Vector3 getVertex(uint index) const;
/// Return the half-edge structure of the mesh
const HalfEdgeStructure& getHalfEdgeStructure() const;
};
+// Return the number of vertices
+inline uint PolyhedronMesh::getNbVertices() const {
+ return mHalfEdgeStructure.getNbVertices();
+}
+
// Return the half-edge structure of the mesh
inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const {
return mHalfEdgeStructure;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index b46ca386..61910acc 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -26,6 +26,7 @@
// Libraries
#include "SATAlgorithm.h"
#include "constraint/ContactPoint.h"
+#include "collision/PolyhedronMesh.h"
#include "configuration.h"
#include "engine/Profiler.h"
#include <algorithm>
@@ -36,8 +37,60 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) {
+bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
+ assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+
+ switch (narrowPhaseInfo->collisionShape1->getType()) {
+ case CollisionShapeType::CONVEX_POLYHEDRON:
+ return testCollisionConvexMeshVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
+ case CollisionShapeType::SPHERE:
+ return testCollisionSphereVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
+ case CollisionShapeType::CAPSULE:
+ return testCollisionCapsuleVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
+ case CollisionShapeType::TRIANGLE:
+ return testCollisionTriangleVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
+ default: assert(false);
+ }
+
+ return false;
+}
+
+// Test collision between a sphere and a convex mesh
+bool SATAlgorithm::testCollisionSphereVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
}
+
+// Test collision between a capsule and a convex mesh
+bool SATAlgorithm::testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+
+}
+
+// Test collision between a triangle and a convex mesh
+bool SATAlgorithm::testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+
+}
+
+// Test collision between two convex meshes
+bool SATAlgorithm::testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+
+}
+
+
+// Return true if the arcs AB and CD on the Gauss Map (unit sphere) intersect
+/// This is used to know if the edge between faces with normal A and B on first polyhedron
+/// and edge between faces with normal C and D on second polygon create a face on the Minkowski
+/// sum of both polygons. If this is the case, it means that the cross product of both edges
+/// might be a separating axis.
+bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b,
+ const Vector3& c, const Vector3& d) const {
+ const Vector3 bCrossA = b.cross(a);
+ const Vector3 dCrossC = d.cross(c);
+
+ const decimal cba = c.dot(bCrossA);
+ const decimal dba = d.dot(bCrossA);
+ const decimal adc = a.dot(dCrossC);
+ const decimal bdc = b.dot(dCrossC);
+
+ return cba * dba < decimal(0.0) && adc * bdc < decimal(0.0) && cba * bdc > decimal(0.0);
+}
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 80feece1..dc5bc5f2 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -42,6 +42,22 @@ class SATAlgorithm {
// -------------------- Methods -------------------- //
+ /// Return true if the arcs AB and CD on the Gauss Map intersect
+ bool testGaussMapArcsIntersect(const Vector3& a, const Vector3& b,
+ const Vector3& c, const Vector3& d) const;
+
+ /// Test collision between a sphere and a convex mesh
+ bool testCollisionSphereVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+
+ /// Test collision between a capsule and a convex mesh
+ bool testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+
+ /// Test collision between a triangle and a convex mesh
+ bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+
+ /// Test collision between two convex meshes
+ bool testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+
public :
// -------------------- Methods -------------------- //
diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp
index ce3197e3..ba5dfa1a 100644
--- a/src/collision/shapes/BoxShape.cpp
+++ b/src/collision/shapes/BoxShape.cpp
@@ -38,7 +38,7 @@ using namespace reactphysics3d;
* @param margin The collision margin (in meters) around the collision shape
*/
BoxShape::BoxShape(const Vector3& extent, decimal margin)
- : ConvexShape(CollisionShapeType::BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) {
+ : ConvexPolyhedron(margin), mExtent(extent - Vector3(margin, margin, margin)) {
assert(extent.x > decimal(0.0) && extent.x > margin);
assert(extent.y > decimal(0.0) && extent.y > margin);
assert(extent.z > decimal(0.0) && extent.z > margin);
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index 776356ba..c3d5caf2 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -28,7 +28,7 @@
// Libraries
#include <cfloat>
-#include "ConvexShape.h"
+#include "ConvexPolyhedron.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
@@ -50,7 +50,7 @@ namespace reactphysics3d {
* constructor of the box shape. Otherwise, it is recommended to use the
* default margin distance by not using the "margin" parameter in the constructor.
*/
-class BoxShape : public ConvexShape {
+class BoxShape : public ConvexPolyhedron {
protected :
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index c2db3466..28e724fd 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -40,7 +40,7 @@
namespace reactphysics3d {
/// Type of the collision shape
-enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, BOX, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
+enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_MESH, HEIGHTFIELD};
const int NB_COLLISION_SHAPE_TYPES = 9;
// Declarations
diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp
index cc880a25..40382aeb 100644
--- a/src/collision/shapes/ConvexMeshShape.cpp
+++ b/src/collision/shapes/ConvexMeshShape.cpp
@@ -30,6 +30,9 @@
using namespace reactphysics3d;
+// TODO : Check in every collision shape that localScalling is used correctly and even with SAT
+// algorithm (not only in getLocalSupportPoint***() methods)
+
// Constructor to initialize with an array of 3D vertices.
/// This method creates an internal copy of the input vertices.
/**
@@ -38,108 +41,13 @@ using namespace reactphysics3d;
* @param stride Stride between the beginning of two elements in the vertices array
* @param margin Collision margin (in meters) around the collision shape
*/
-ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, decimal margin)
- : ConvexShape(CollisionShapeType::CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0),
- mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
- assert(nbVertices > 0);
- assert(stride > 0);
-
- const unsigned char* vertexPointer = (const unsigned char*) arrayVertices;
-
- // Copy all the vertices into the internal array
- for (uint i=0; i<mNbVertices; i++) {
- const decimal* newPoint = (const decimal*) vertexPointer;
- mVertices.push_back(Vector3(newPoint[0], newPoint[1], newPoint[2]));
- vertexPointer += stride;
- }
+ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin)
+ : ConvexPolyhedron(margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) {
// Recalculate the bounds of the mesh
recalculateBounds();
}
-// Constructor to initialize with a triangle mesh
-/// This method creates an internal copy of the input vertices.
-/**
- * @param triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh
- * @param isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory)
- * @param margin Collision margin (in meters) around the collision shape
- */
-ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed, decimal margin)
- : ConvexShape(CollisionShapeType::CONVEX_MESH, margin), mMinBounds(0, 0, 0),
- mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(isEdgesInformationUsed) {
-
- TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
- TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
- unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
- unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
- int vertexStride = triangleVertexArray->getVerticesStride();
- int indexStride = triangleVertexArray->getIndicesStride();
-
- // For each vertex of the mesh
- for (uint v = 0; v < triangleVertexArray->getNbVertices(); v++) {
-
- // Get the vertices components of the triangle
- if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
- const float* vertices = (float*)(verticesStart + v * vertexStride);
-
- Vector3 vertex(vertices[0], vertices[1], vertices[2] );
- vertex = vertex * mScaling;
- mVertices.push_back(vertex);
- }
- else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
- const double* vertices = (double*)(verticesStart + v * vertexStride);
-
- Vector3 vertex(vertices[0], vertices[1], vertices[2] );
- vertex = vertex * mScaling;
- mVertices.push_back(vertex);
- }
- }
-
- // If we need to use the edges information of the mesh
- if (mIsEdgesInformationUsed) {
-
- // For each triangle of the mesh
- for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
-
- void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
-
- uint vertexIndex[3] = {0, 0, 0};
-
- // For each vertex of the triangle
- for (int k=0; k < 3; k++) {
-
- // Get the index of the current vertex in the triangle
- if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
- vertexIndex[k] = ((uint*)vertexIndexPointer)[k];
- }
- else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
- vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k];
- }
- else {
- assert(false);
- }
- }
-
- // Add information about the edges
- addEdge(vertexIndex[0], vertexIndex[1]);
- addEdge(vertexIndex[0], vertexIndex[2]);
- addEdge(vertexIndex[1], vertexIndex[2]);
- }
- }
-
- mNbVertices = mVertices.size();
- recalculateBounds();
-}
-
-// Constructor.
-/// If you use this constructor, you will need to set the vertices manually one by one using
-/// the addVertex() method.
-ConvexMeshShape::ConvexMeshShape(decimal margin)
- : ConvexShape(CollisionShapeType::CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0),
- mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
-
-}
-
// Return a local support point in a given direction without the object margin.
/// If the edges information is not used for collision detection, this method will go through
/// the whole vertices list and pick up the vertex with the largest dot product in the support
@@ -151,78 +59,28 @@ ConvexMeshShape::ConvexMeshShape(decimal margin)
Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
- assert(mNbVertices == mVertices.size());
- assert(cachedCollisionData != nullptr);
+ // TODO : Do we still need to have cachedCollisionData or we can remove it from everywhere ?
- // Allocate memory for the cached collision data if not allocated yet
- if ((*cachedCollisionData) == nullptr) {
- *cachedCollisionData = (int*) malloc(sizeof(int));
- *((int*)(*cachedCollisionData)) = 0;
- }
+ double maxDotProduct = DECIMAL_SMALLEST;
+ uint indexMaxDotProduct = 0;
- // If the edges information is used to speed up the collision detection
- if (mIsEdgesInformationUsed) {
+ // For each vertex of the mesh
+ for (uint i=0; i<mPolyhedronMesh->getNbVertices(); i++) {
- assert(mEdgesAdjacencyList.size() == mNbVertices);
+ // Compute the dot product of the current vertex
+ double dotProduct = direction.dot(mPolyhedronMesh->getVertex(i));
- uint maxVertex = *((int*)(*cachedCollisionData));
- decimal maxDotProduct = direction.dot(mVertices[maxVertex]);
- bool isOptimal;
-
- // Perform hill-climbing (local search)
- do {
- isOptimal = true;
-
- assert(mEdgesAdjacencyList.at(maxVertex).size() > 0);
-
- // For all neighbors of the current vertex
- std::set<uint>::const_iterator it;
- std::set<uint>::const_iterator itBegin = mEdgesAdjacencyList.at(maxVertex).begin();
- std::set<uint>::const_iterator itEnd = mEdgesAdjacencyList.at(maxVertex).end();
- for (it = itBegin; it != itEnd; ++it) {
-
- // Compute the dot product
- decimal dotProduct = direction.dot(mVertices[*it]);
-
- // If the current vertex is a better vertex (larger dot product)
- if (dotProduct > maxDotProduct) {
- maxVertex = *it;
- maxDotProduct = dotProduct;
- isOptimal = false;
- }
- }
-
- } while(!isOptimal);
-
- // Cache the support vertex
- *((int*)(*cachedCollisionData)) = maxVertex;
-
- // Return the support vertex
- return mVertices[maxVertex] * mScaling;
- }
- else { // If the edges information is not used
-
- double maxDotProduct = DECIMAL_SMALLEST;
- uint indexMaxDotProduct = 0;
-
- // For each vertex of the mesh
- for (uint i=0; i<mNbVertices; i++) {
-
- // Compute the dot product of the current vertex
- double dotProduct = direction.dot(mVertices[i]);
-
- // If the current dot product is larger than the maximum one
- if (dotProduct > maxDotProduct) {
- indexMaxDotProduct = i;
- maxDotProduct = dotProduct;
- }
+ // If the current dot product is larger than the maximum one
+ if (dotProduct > maxDotProduct) {
+ indexMaxDotProduct = i;
+ maxDotProduct = dotProduct;
}
-
- assert(maxDotProduct >= decimal(0.0));
-
- // Return the vertex with the largest dot product in the support direction
- return mVertices[indexMaxDotProduct] * mScaling;
}
+
+ assert(maxDotProduct >= decimal(0.0));
+
+ // Return the vertex with the largest dot product in the support direction
+ return mPolyhedronMesh->getVertex(indexMaxDotProduct) * mScaling;
}
// Recompute the bounds of the mesh
@@ -235,16 +93,16 @@ void ConvexMeshShape::recalculateBounds() {
mMaxBounds.setToZero();
// For each vertex of the mesh
- for (uint i=0; i<mNbVertices; i++) {
+ for (uint i=0; i<mPolyhedronMesh->getNbVertices(); i++) {
- if (mVertices[i].x > mMaxBounds.x) mMaxBounds.x = mVertices[i].x;
- if (mVertices[i].x < mMinBounds.x) mMinBounds.x = mVertices[i].x;
+ if (mPolyhedronMesh->getVertex(i).x > mMaxBounds.x) mMaxBounds.x = mPolyhedronMesh->getVertex(i).x;
+ if (mPolyhedronMesh->getVertex(i).x < mMinBounds.x) mMinBounds.x = mPolyhedronMesh->getVertex(i).x;
- if (mVertices[i].y > mMaxBounds.y) mMaxBounds.y = mVertices[i].y;
- if (mVertices[i].y < mMinBounds.y) mMinBounds.y = mVertices[i].y;
+ if (mPolyhedronMesh->getVertex(i).y > mMaxBounds.y) mMaxBounds.y = mPolyhedronMesh->getVertex(i).y;
+ if (mPolyhedronMesh->getVertex(i).y < mMinBounds.y) mMinBounds.y = mPolyhedronMesh->getVertex(i).y;
- if (mVertices[i].z > mMaxBounds.z) mMaxBounds.z = mVertices[i].z;
- if (mVertices[i].z < mMinBounds.z) mMinBounds.z = mVertices[i].z;
+ if (mPolyhedronMesh->getVertex(i).z > mMaxBounds.z) mMaxBounds.z = mPolyhedronMesh->getVertex(i).z;
+ if (mPolyhedronMesh->getVertex(i).z < mMinBounds.z) mMinBounds.z = mPolyhedronMesh->getVertex(i).z;
}
// Apply the local scaling factor
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index 66b624aa..c33fd5bd 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -1,4 +1,4 @@
-/********************************************************************************
+ /********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
@@ -27,10 +27,11 @@
#define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H
// Libraries
-#include "ConvexShape.h"
+#include "ConvexPolyhedron.h"
#include "engine/CollisionWorld.h"
#include "mathematics/mathematics.h"
#include "collision/TriangleMesh.h"
+#include "collision/PolyhedronMesh.h"
#include "collision/narrowphase/GJK/GJKAlgorithm.h"
#include <vector>
#include <set>
@@ -58,17 +59,14 @@ class CollisionWorld;
* with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
* in order to use the edges information for collision detection.
*/
-class ConvexMeshShape : public ConvexShape {
+class ConvexMeshShape : public ConvexPolyhedron {
protected :
// -------------------- Attributes -------------------- //
- /// Array with the vertices of the mesh
- std::vector<Vector3> mVertices;
-
- /// Number of vertices in the mesh
- uint mNbVertices;
+ /// Polyhedron structure of the mesh
+ PolyhedronMesh* mPolyhedronMesh;
/// Mesh minimum bounds in the three local x, y and z directions
Vector3 mMinBounds;
@@ -76,13 +74,6 @@ class ConvexMeshShape : public ConvexShape {
/// Mesh maximum bounds in the three local x, y and z directions
Vector3 mMaxBounds;
- /// True if the shape contains the edges of the convex mesh in order to
- /// make the collision detection faster
- bool mIsEdgesInformationUsed;
-
- /// Adjacency list representing the edges of the mesh
- std::map<uint, std::set<uint> > mEdgesAdjacencyList;
-
// -------------------- Methods -------------------- //
/// Recompute the bounds of the mesh
@@ -108,16 +99,10 @@ class ConvexMeshShape : public ConvexShape {
// -------------------- Methods -------------------- //
- /// Constructor to initialize with an array of 3D vertices.
- ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride,
- decimal margin = OBJECT_MARGIN);
-
- /// Constructor to initialize with a triangle vertex array
- ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true,
- decimal margin = OBJECT_MARGIN);
-
- /// Constructor.
- ConvexMeshShape(decimal margin = OBJECT_MARGIN);
+ /// Constructor
+ // TODO : Do we really need to use the margin anymore ? Maybe for raycasting ? If not, remove all the
+ // comments documentation about margin
+ ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin = OBJECT_MARGIN);
/// Destructor
virtual ~ConvexMeshShape() override = default;
@@ -134,21 +119,8 @@ class ConvexMeshShape : public ConvexShape {
/// Return the local inertia tensor of the collision shape.
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
- /// Add a vertex into the convex mesh
- void addVertex(const Vector3& vertex);
-
- /// Add an edge into the convex mesh by specifying the two vertex indices of the edge.
- void addEdge(uint v1, uint v2);
-
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const override;
-
- /// Return true if the edges information is used to speed up the collision detection
- bool isEdgesInformationUsed() const;
-
- /// Set the variable to know if the edges information is used to speed up the
- /// collision detection
- void setIsEdgesInformationUsed(bool isEdgesUsed);
};
/// Set the scaling vector of the collision shape
@@ -197,68 +169,6 @@ inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decima
0.0, 0.0, factor * (xSquare + ySquare));
}
-// Add a vertex into the convex mesh
-/**
- * @param vertex Vertex to be added
- */
-inline void ConvexMeshShape::addVertex(const Vector3& vertex) {
-
- // Add the vertex in to vertices array
- mVertices.push_back(vertex);
- mNbVertices++;
-
- // Update the bounds of the mesh
- if (vertex.x * mScaling.x > mMaxBounds.x) mMaxBounds.x = vertex.x * mScaling.x;
- if (vertex.x * mScaling.x < mMinBounds.x) mMinBounds.x = vertex.x * mScaling.x;
- if (vertex.y * mScaling.y > mMaxBounds.y) mMaxBounds.y = vertex.y * mScaling.y;
- if (vertex.y * mScaling.y < mMinBounds.y) mMinBounds.y = vertex.y * mScaling.y;
- if (vertex.z * mScaling.z > mMaxBounds.z) mMaxBounds.z = vertex.z * mScaling.z;
- if (vertex.z * mScaling.z < mMinBounds.z) mMinBounds.z = vertex.z * mScaling.z;
-}
-
-// Add an edge into the convex mesh by specifying the two vertex indices of the edge.
-/// Note that the vertex indices start at zero and need to correspond to the order of
-/// the vertices in the vertices array in the constructor or the order of the calls
-/// of the addVertex() methods that you use to add vertices into the convex mesh.
-/**
-* @param v1 Index of the first vertex of the edge to add
-* @param v2 Index of the second vertex of the edge to add
-*/
-inline void ConvexMeshShape::addEdge(uint v1, uint v2) {
-
- // If the entry for vertex v1 does not exist in the adjacency list
- if (mEdgesAdjacencyList.count(v1) == 0) {
- mEdgesAdjacencyList.insert(std::make_pair(v1, std::set<uint>()));
- }
-
- // If the entry for vertex v2 does not exist in the adjacency list
- if (mEdgesAdjacencyList.count(v2) == 0) {
- mEdgesAdjacencyList.insert(std::make_pair(v2, std::set<uint>()));
- }
-
- // Add the edge in the adjacency list
- mEdgesAdjacencyList[v1].insert(v2);
- mEdgesAdjacencyList[v2].insert(v1);
-}
-
-// Return true if the edges information is used to speed up the collision detection
-/**
- * @return True if the edges information is used and false otherwise
- */
-inline bool ConvexMeshShape::isEdgesInformationUsed() const {
- return mIsEdgesInformationUsed;
-}
-
-// Set the variable to know if the edges information is used to speed up the
-// collision detection
-/**
- * @param isEdgesUsed True if you want to use the edges information to speed up
- * the collision detection with the convex mesh shape
- */
-inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
- mIsEdgesInformationUsed = isEdgesUsed;
-}
-
// Return true if a point is inside the collision shape
inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
ProxyShape* proxyShape) const {
diff --git a/src/collision/shapes/ConvexPolyhedron.cpp b/src/collision/shapes/ConvexPolyhedron.cpp
new file mode 100644
index 00000000..26964554
--- /dev/null
+++ b/src/collision/shapes/ConvexPolyhedron.cpp
@@ -0,0 +1,37 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "ConvexPolyhedron.h"
+
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+ConvexPolyhedron::ConvexPolyhedron(decimal margin)
+ : ConvexShape(CollisionShapeType::CONVEX_POLYHEDRON, margin) {
+
+}
diff --git a/src/collision/shapes/ConvexPolyhedron.h b/src/collision/shapes/ConvexPolyhedron.h
new file mode 100644
index 00000000..27bbc0b5
--- /dev/null
+++ b/src/collision/shapes/ConvexPolyhedron.h
@@ -0,0 +1,68 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONVEX_POLYHEDRON_H
+#define REACTPHYSICS3D_CONVEX_POLYHEDRON_H
+
+// Libraries
+#include "ConvexShape.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class ConvexPolyhedron
+/**
+ * This abstract class represents a convex polyhedron collision shape associated with a
+ * body that is used during the narrow-phase collision detection.
+ */
+class ConvexPolyhedron : public ConvexShape {
+
+ protected :
+
+ // -------------------- Attributes -------------------- //
+
+ // -------------------- Methods -------------------- //
+
+ public :
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ ConvexPolyhedron(decimal margin);
+
+ /// Destructor
+ virtual ~ConvexPolyhedron() override = default;
+
+ /// Deleted copy-constructor
+ ConvexPolyhedron(const ConvexPolyhedron& shape) = delete;
+
+ /// Deleted assignment operator
+ ConvexPolyhedron& operator=(const ConvexPolyhedron& shape) = delete;
+};
+
+}
+
+#endif
+
diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h
index 3dbbd302..f6b8b8bf 100644
--- a/src/reactphysics3d.h
+++ b/src/reactphysics3d.h
@@ -55,7 +55,9 @@
#include "collision/ProxyShape.h"
#include "collision/RaycastInfo.h"
#include "collision/TriangleMesh.h"
+#include "collision/PolyhedronMesh.h"
#include "collision/TriangleVertexArray.h"
+#include "collision/PolygonVertexArray.h"
#include "constraint/BallAndSocketJoint.h"
#include "constraint/SliderJoint.h"
#include "constraint/HingeJoint.h"
diff --git a/test/tests/collision/TestHalfEdgeStructure.h b/test/tests/collision/TestHalfEdgeStructure.h
index 871a8ed1..4c4a2b39 100644
--- a/test/tests/collision/TestHalfEdgeStructure.h
+++ b/test/tests/collision/TestHalfEdgeStructure.h
@@ -43,19 +43,28 @@ class TestHalfEdgeStructure : public Test {
void testCube() {
// Create the half-edge structure for a cube
- std::vector<rp3d::Vector3> vertices;
- std::vector<std::vector<uint>> faces;
rp3d::HalfEdgeStructure cubeStructure;
+ rp3d::Vector3 vertices[8] = {
+ rp3d::Vector3(-0.5, -0.5, 0.5),
+ rp3d::Vector3(0.5, -0.5, 0.5),
+ rp3d::Vector3(0.5, 0.5, 0.5),
+ rp3d::Vector3(-0.5, 0.5, 0.5),
+ rp3d::Vector3(-0.5, -0.5, -0.5),
+ rp3d::Vector3(0.5, -0.5, -0.5),
+ rp3d::Vector3(0.5, 0.5, -0.5),
+ rp3d::Vector3(-0.5, 0.5, -0.5)
+ };
+
// Vertices
- vertices.push_back(rp3d::Vector3(-0.5, -0.5, 0.5));
- vertices.push_back(rp3d::Vector3(0.5, -0.5, 0.5));
- vertices.push_back(rp3d::Vector3(0.5, 0.5, 0.5));
- vertices.push_back(rp3d::Vector3(-0.5, 0.5, 0.5));
- vertices.push_back(rp3d::Vector3(-0.5, -0.5, -0.5));
- vertices.push_back(rp3d::Vector3(0.5, -0.5, -0.5));
- vertices.push_back(rp3d::Vector3(0.5, 0.5, -0.5));
- vertices.push_back(rp3d::Vector3(-0.5, 0.5, -0.5));
+ cubeStructure.addVertex(0);
+ cubeStructure.addVertex(1);
+ cubeStructure.addVertex(2);
+ cubeStructure.addVertex(3);
+ cubeStructure.addVertex(4);
+ cubeStructure.addVertex(5);
+ cubeStructure.addVertex(6);
+ cubeStructure.addVertex(7);
// Faces
std::vector<uint> face0;
@@ -71,14 +80,14 @@ class TestHalfEdgeStructure : public Test {
std::vector<uint> face5;
face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3);
- faces.push_back(face0);
- faces.push_back(face1);
- faces.push_back(face2);
- faces.push_back(face3);
- faces.push_back(face4);
- faces.push_back(face5);
+ cubeStructure.addFace(face0);
+ cubeStructure.addFace(face1);
+ cubeStructure.addFace(face2);
+ cubeStructure.addFace(face3);
+ cubeStructure.addFace(face4);
+ cubeStructure.addFace(face5);
- cubeStructure.init(vertices, faces);
+ cubeStructure.init();
// --- Test that the half-edge structure of the cube is valid --- //
@@ -87,44 +96,44 @@ class TestHalfEdgeStructure : public Test {
test(cubeStructure.getNbHalfEdges() == 24);
// Test vertices
- test(cubeStructure.getVertex(0).point.x == -0.5);
- test(cubeStructure.getVertex(0).point.y == -0.5);
- test(cubeStructure.getVertex(0).point.z == 0.5);
+ test(vertices[cubeStructure.getVertex(0).vertexPointIndex].x == -0.5);
+ test(vertices[cubeStructure.getVertex(0).vertexPointIndex].y == -0.5);
+ test(vertices[cubeStructure.getVertex(0).vertexPointIndex].z == 0.5);
test(cubeStructure.getHalfEdge(cubeStructure.getVertex(0).edgeIndex).vertexIndex == 0);
- test(cubeStructure.getVertex(1).point.x == 0.5);
- test(cubeStructure.getVertex(1).point.y == -0.5);
- test(cubeStructure.getVertex(1).point.z == 0.5);
+ test(vertices[cubeStructure.getVertex(1).vertexPointIndex].x == 0.5);
+ test(vertices[cubeStructure.getVertex(1).vertexPointIndex].y == -0.5);
+ test(vertices[cubeStructure.getVertex(1).vertexPointIndex].z == 0.5);
test(cubeStructure.getHalfEdge(cubeStructure.getVertex(1).edgeIndex).vertexIndex == 1);
- test(cubeStructure.getVertex(2).point.x == 0.5);
- test(cubeStructure.getVertex(2).point.y == 0.5);
- test(cubeStructure.getVertex(2).point.z == 0.5);
+ test(vertices[cubeStructure.getVertex(2).vertexPointIndex].x == 0.5);
+ test(vertices[cubeStructure.getVertex(2).vertexPointIndex].y == 0.5);
+ test(vertices[cubeStructure.getVertex(2).vertexPointIndex].z == 0.5);
test(cubeStructure.getHalfEdge(cubeStructure.getVertex(2).edgeIndex).vertexIndex == 2);
- test(cubeStructure.getVertex(3).point.x == -0.5);
- test(cubeStructure.getVertex(3).point.y == 0.5);
- test(cubeStructure.getVertex(3).point.z == 0.5);
+ test(vertices[cubeStructure.getVertex(3).vertexPointIndex].x == -0.5);
+ test(vertices[cubeStructure.getVertex(3).vertexPointIndex].y == 0.5);
+ test(vertices[cubeStructure.getVertex(3).vertexPointIndex].z == 0.5);
test(cubeStructure.getHalfEdge(cubeStructure.getVertex(3).edgeIndex).vertexIndex == 3);
- test(cubeStructure.getVertex(4).point.x == -0.5);
- test(cubeStructure.getVertex(4).point.y == -0.5);
- test(cubeStructure.getVertex(4).point.z == -0.5);
+ test(vertices[cubeStructure.getVertex(4).vertexPointIndex].x == -0.5);
+ test(vertices[cubeStructure.getVertex(4).vertexPointIndex].y == -0.5);
+ test(vertices[cubeStructure.getVertex(4).vertexPointIndex].z == -0.5);
test(cubeStructure.getHalfEdge(cubeStructure.getVertex(4).edgeIndex).vertexIndex == 4);
- test(cubeStructure.getVertex(5).point.x == 0.5);
- test(cubeStructure.getVertex(5).point.y == -0.5);
- test(cubeStructure.getVertex(5).point.z == -0.5);
+ test(vertices[cubeStructure.getVertex(5).vertexPointIndex].x == 0.5);
+ test(vertices[cubeStructure.getVertex(5).vertexPointIndex].y == -0.5);
+ test(vertices[cubeStructure.getVertex(5).vertexPointIndex].z == -0.5);
test(cubeStructure.getHalfEdge(cubeStructure.getVertex(5).edgeIndex).vertexIndex == 5);
- test(cubeStructure.getVertex(6).point.x == 0.5);
- test(cubeStructure.getVertex(6).point.y == 0.5);
- test(cubeStructure.getVertex(6).point.z == -0.5);
+ test(vertices[cubeStructure.getVertex(6).vertexPointIndex].x == 0.5);
+ test(vertices[cubeStructure.getVertex(6).vertexPointIndex].y == 0.5);
+ test(vertices[cubeStructure.getVertex(6).vertexPointIndex].z == -0.5);
test(cubeStructure.getHalfEdge(cubeStructure.getVertex(6).edgeIndex).vertexIndex == 6);
- test(cubeStructure.getVertex(7).point.x == -0.5);
- test(cubeStructure.getVertex(7).point.y == 0.5);
- test(cubeStructure.getVertex(7).point.z == -0.5);
+ test(vertices[cubeStructure.getVertex(7).vertexPointIndex].x == -0.5);
+ test(vertices[cubeStructure.getVertex(7).vertexPointIndex].y == 0.5);
+ test(vertices[cubeStructure.getVertex(7).vertexPointIndex].z == -0.5);
test(cubeStructure.getHalfEdge(cubeStructure.getVertex(7).edgeIndex).vertexIndex == 7);
// Test faces
@@ -158,15 +167,21 @@ class TestHalfEdgeStructure : public Test {
void testTetrahedron() {
// Create the half-edge structure for a tetrahedron
- std::vector<rp3d::Vector3> vertices;
std::vector<std::vector<uint>> faces;
rp3d::HalfEdgeStructure tetrahedron;
// Vertices
- vertices.push_back(rp3d::Vector3(1, -1, -1));
- vertices.push_back(rp3d::Vector3(-1, -1, -1));
- vertices.push_back(rp3d::Vector3(0, -1, 1));
- vertices.push_back(rp3d::Vector3(0, 1, 0));
+ rp3d::Vector3 vertices[4] = {
+ rp3d::Vector3(1, -1, -1),
+ rp3d::Vector3(-1, -1, -1),
+ rp3d::Vector3(0, -1, 1),
+ rp3d::Vector3(0, 1, 0)
+ };
+
+ tetrahedron.addVertex(0);
+ tetrahedron.addVertex(1);
+ tetrahedron.addVertex(2);
+ tetrahedron.addVertex(3);
// Faces
std::vector<uint> face0;
@@ -178,12 +193,12 @@ class TestHalfEdgeStructure : public Test {
std::vector<uint> face3;
face3.push_back(0); face3.push_back(2); face3.push_back(3);
- faces.push_back(face0);
- faces.push_back(face1);
- faces.push_back(face2);
- faces.push_back(face3);
+ tetrahedron.addFace(face0);
+ tetrahedron.addFace(face1);
+ tetrahedron.addFace(face2);
+ tetrahedron.addFace(face3);
- tetrahedron.init(vertices, faces);
+ tetrahedron.init();
// --- Test that the half-edge structure of the tetrahedron is valid --- //
@@ -192,24 +207,24 @@ class TestHalfEdgeStructure : public Test {
test(tetrahedron.getNbHalfEdges() == 12);
// Test vertices
- test(tetrahedron.getVertex(0).point.x == 1);
- test(tetrahedron.getVertex(0).point.y == -1);
- test(tetrahedron.getVertex(0).point.z == -1);
+ test(vertices[tetrahedron.getVertex(0).vertexPointIndex].x == 1);
+ test(vertices[tetrahedron.getVertex(0).vertexPointIndex].y == -1);
+ test(vertices[tetrahedron.getVertex(0).vertexPointIndex].z == -1);
test(tetrahedron.getHalfEdge(tetrahedron.getVertex(0).edgeIndex).vertexIndex == 0);
- test(tetrahedron.getVertex(1).point.x == -1);
- test(tetrahedron.getVertex(1).point.y == -1);
- test(tetrahedron.getVertex(1).point.z == -1);
+ test(vertices[tetrahedron.getVertex(1).vertexPointIndex].x == -1);
+ test(vertices[tetrahedron.getVertex(1).vertexPointIndex].y == -1);
+ test(vertices[tetrahedron.getVertex(1).vertexPointIndex].z == -1);
test(tetrahedron.getHalfEdge(tetrahedron.getVertex(1).edgeIndex).vertexIndex == 1);
- test(tetrahedron.getVertex(2).point.x == 0);
- test(tetrahedron.getVertex(2).point.y == -1);
- test(tetrahedron.getVertex(2).point.z == 1);
+ test(vertices[tetrahedron.getVertex(2).vertexPointIndex].x == 0);
+ test(vertices[tetrahedron.getVertex(2).vertexPointIndex].y == -1);
+ test(vertices[tetrahedron.getVertex(2).vertexPointIndex].z == 1);
test(tetrahedron.getHalfEdge(tetrahedron.getVertex(2).edgeIndex).vertexIndex == 2);
- test(tetrahedron.getVertex(3).point.x == 0);
- test(tetrahedron.getVertex(3).point.y == 1);
- test(tetrahedron.getVertex(3).point.z == 0);
+ test(vertices[tetrahedron.getVertex(3).vertexPointIndex].x == 0);
+ test(vertices[tetrahedron.getVertex(3).vertexPointIndex].y == 1);
+ test(vertices[tetrahedron.getVertex(3).vertexPointIndex].z == 0);
test(tetrahedron.getHalfEdge(tetrahedron.getVertex(3).edgeIndex).vertexIndex == 3);
// Test faces
diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h
index 9fd20110..abb54791 100644
--- a/test/tests/collision/TestPointInside.h
+++ b/test/tests/collision/TestPointInside.h
@@ -124,7 +124,8 @@ class TestPointInside : public Test {
mCapsuleShape = new CapsuleShape(2, 10);
mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform);
- mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4)
+ // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again
+ /*mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4)
mConvexMeshShape->addVertex(Vector3(-2, -3, -4));
mConvexMeshShape->addVertex(Vector3(2, -3, -4));
mConvexMeshShape->addVertex(Vector3(2, -3, 4));
@@ -160,6 +161,7 @@ class TestPointInside : public Test {
mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape(
mConvexMeshShapeBodyEdgesInfo,
mShapeTransform);
+ */
// Compound shape is a capsule and a sphere
Vector3 positionShape2(Vector3(4, 2, -3));
@@ -175,8 +177,8 @@ class TestPointInside : public Test {
delete mBoxShape;
delete mSphereShape;
delete mCapsuleShape;
- delete mConvexMeshShape;
- delete mConvexMeshShapeBodyEdgesInfo;
+ //delete mConvexMeshShape;
+ //delete mConvexMeshShapeBodyEdgesInfo;
}
/// Run the tests
@@ -399,6 +401,7 @@ class TestPointInside : public Test {
// ----- Tests without using edges information ----- //
+ /*
// Tests with CollisionBody
test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
@@ -504,6 +507,7 @@ class TestPointInside : public Test {
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
+ */
}
/// Test the CollisionBody::testPointInside() method
diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h
index 852455c2..fe8926d5 100644
--- a/test/tests/collision/TestRaycast.h
+++ b/test/tests/collision/TestRaycast.h
@@ -206,8 +206,9 @@ class TestRaycast : public Test {
mCapsuleShape = new CapsuleShape(2, 5);
mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform);
+ // TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again
// Box of dimension (2, 3, 4)
- mConvexMeshShape = new ConvexMeshShape(0.0);
+ /*mConvexMeshShape = new ConvexMeshShape(0.0);
mConvexMeshShape->addVertex(Vector3(-2, -3, -4));
mConvexMeshShape->addVertex(Vector3(2, -3, -4));
mConvexMeshShape->addVertex(Vector3(2, -3, 4));
@@ -243,6 +244,7 @@ class TestRaycast : public Test {
mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape(
mConvexMeshShapeEdgesInfo,
mShapeTransform);
+ */
// Compound shape is a cylinder and a sphere
Vector3 positionShape2(Vector3(4, 2, -3));
@@ -312,8 +314,8 @@ class TestRaycast : public Test {
delete mBoxShape;
delete mSphereShape;
delete mCapsuleShape;
- delete mConvexMeshShape;
- delete mConvexMeshShapeEdgesInfo;
+ //delete mConvexMeshShape;
+ //delete mConvexMeshShapeEdgesInfo;
delete mTriangleShape;
delete mConcaveMeshShape;
delete mHeightFieldShape;
@@ -1297,6 +1299,7 @@ class TestRaycast : public Test {
/// CollisionWorld::raycast() methods.
void testConvexMesh() {
+ /*
// ----- Test feedback data ----- //
Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 6);
Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4);
@@ -1555,6 +1558,7 @@ class TestRaycast : public Test {
mCallback.reset();
mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback);
test(mCallback.isHit);
+ */
}
/// Test the CollisionBody::raycast() and
diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp
index 7842c529..11be5732 100644
--- a/testbed/common/ConvexMesh.cpp
+++ b/testbed/common/ConvexMesh.cpp
@@ -40,17 +40,36 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
-
// Vertex and Indices array for the triangle mesh (data in shared and not copied)
- mPhysicsTriangleVertexArray =
+ /*mPolygonVertexArray =
new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3),
getNbFaces(0), &(mIndices[0][0]), sizeof(int),
rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
- rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
+ rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);*/
+
+ // Polygon faces descriptions for the polyhedron
+ mPolygonFaces = new rp3d::PolygonVertexArray::PolygonFace[getNbFaces(0)];
+ rp3d::PolygonVertexArray::PolygonFace* face = mPolygonFaces;
+ for (int f=0; f < getNbFaces(0); f++) {
+ face->indexBase = f * 3;
+ face->nbVertices = 3;
+ face++;
+ }
+
+ // Create the polygon vertex array
+ mPolygonVertexArray =
+ new rp3d::PolygonVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3),
+ &(mIndices[0][0]), sizeof(int),
+ getNbFaces(0), mPolygonFaces,
+ rp3d::PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
+ rp3d::PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
+
+ // Create the polyhedron mesh
+ mPolyhedronMesh = new rp3d::PolyhedronMesh(mPolygonVertexArray);
// Create the collision shape for the rigid body (convex mesh shape) and
// do not forget to delete it at the end
- mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray);
+ mConvexShape = new rp3d::ConvexMeshShape(mPolyhedronMesh);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@@ -85,16 +104,29 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
- // Vertex and Indices array for the triangle mesh (data in shared and not copied)
- mPhysicsTriangleVertexArray =
- new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3),
- getNbFaces(0), &(mIndices[0][0]), sizeof(int),
- rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
- rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
+ // Polygon faces descriptions for the polyhedron
+ mPolygonFaces = new rp3d::PolygonVertexArray::PolygonFace[getNbFaces(0)];
+ rp3d::PolygonVertexArray::PolygonFace* face = mPolygonFaces;
+ for (int f=0; f < getNbFaces(0); f++) {
+ face->indexBase = f * 3;
+ face->nbVertices = 3;
+ face++;
+ }
+
+ // Create the polygon vertex array
+ mPolygonVertexArray =
+ new rp3d::PolygonVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3),
+ &(mIndices[0][0]), sizeof(int),
+ getNbFaces(0), mPolygonFaces,
+ rp3d::PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
+ rp3d::PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
+
+ // Create the polyhedron mesh
+ mPolyhedronMesh = new rp3d::PolyhedronMesh(mPolygonVertexArray);
// Create the collision shape for the rigid body (convex mesh shape) and do
// not forget to delete it at the end
- mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray);
+ mConvexShape = new rp3d::ConvexMeshShape(mPolyhedronMesh);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@@ -128,7 +160,9 @@ ConvexMesh::~ConvexMesh() {
mVBOTextureCoords.destroy();
mVAO.destroy();
- delete mPhysicsTriangleVertexArray;
+ delete mPolyhedronMesh;
+ delete mPolygonVertexArray;
+ delete[] mPolygonFaces;
delete mConvexShape;
}
diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h
index b6cb90c1..e2ba5836 100644
--- a/testbed/common/ConvexMesh.h
+++ b/testbed/common/ConvexMesh.h
@@ -41,7 +41,11 @@ class ConvexMesh : public PhysicsObject {
/// Previous transform (for interpolation)
rp3d::Transform mPreviousTransform;
- rp3d::TriangleVertexArray* mPhysicsTriangleVertexArray;
+ rp3d::PolygonVertexArray::PolygonFace* mPolygonFaces;
+
+ rp3d::PolygonVertexArray* mPolygonVertexArray;
+
+ rp3d::PolyhedronMesh* mPolyhedronMesh;
/// Collision shape
rp3d::ConvexMeshShape* mConvexShape;
From a9b3afae59f092a75755e6edb767d079303c31bf Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 28 Mar 2017 23:07:10 +0200
Subject: [PATCH 041/133] Finish implementing capsule vs capsule narrow-phase
algorithm
---
.../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 74 +++++++++----------
1 file changed, 37 insertions(+), 37 deletions(-)
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
index e83d694c..11160b8c 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
@@ -66,60 +66,60 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
- if (segmentsDistance >= sumRadius || segmentsDistance < MACHINE_EPSILON) {
+ if (segmentsDistance >= sumRadius) {
// The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius.
// Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision.
return false;
}
- // Compute the planes that goes through the extrem points of the inner segment of capsule 1
- decimal d1 = seg1.dot(capsule1SegA);
- decimal d2 = -seg1.dot(capsule1SegB);
+ // If the distance between the two segments is larger than zero (inner segments of capsules are not overlapping)
+ // If the inner segments are overlapping, we cannot compute a contact normal (unknown direction). In this case,
+ // we skip the parallel contact points calculation (there might still be contact in the spherical caps of the capsules)
+ if (segmentsDistance > MACHINE_EPSILON) {
- // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
- // segment of capsule 1
- decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
- decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
+ // Compute the planes that goes through the extreme points of the inner segment of capsule 1
+ decimal d1 = seg1.dot(capsule1SegA);
+ decimal d2 = -seg1.dot(capsule1SegB);
- bool isClipValid = false; // True if the segments were overlapping (the clip segment is valid)
+ // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
+ // segment of capsule 1
+ decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
+ decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
- // Clip the inner segment of capsule 2
- Vector3 clipPointA, clipPointB;
- if (t1 >= decimal(0.0)) {
+ // If the segments were overlapping (the clip segment is valid)
+ if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
- if (t1 > decimal(1.0)) t1 = decimal(1.0);
- clipPointA = capsule2SegB - t1 * seg2;
- isClipValid = true;
- }
- if (t2 >= decimal(0.0) && t2 <= decimal(1.0)) {
+ // Clip the inner segment of capsule 2
+ if (t1 > decimal(1.0)) t1 = decimal(1.0);
+ const Vector3 clipPointA = capsule2SegB - t1 * seg2;
+ if (t2 > decimal(1.0)) t2 = decimal(1.0);
+ const Vector3 clipPointB = capsule2SegA + t2 * seg2;
- if (t2 > decimal(1.0)) t2 = decimal(1.0);
- clipPointB = capsule2SegA + t2 * seg2;
- isClipValid = true;
- }
+ // Project point capsule2SegA onto line of innner segment of capsule 1
+ const Vector3 seg1Normalized = seg1.getUnit();
+ Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
- // If we have a valid clip segment
- if (isClipValid) {
+ // Compute a perpendicular vector from segment 1 to segment 2
+ Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
+ Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
- Vector3 segment1ToSegment2 = (capsule2SegA - capsule1SegA);
- Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
+ Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
+ const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
+ const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
+ const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius();
+ const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius();
- Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
- const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
- const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
- const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius();
- const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius();
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized;
- const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized;
+ decimal penetrationDepth = sumRadius - segmentsDistance;
- decimal penetrationDepth = sumRadius - segmentsDistance;
+ // Create the contact info object
+ contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
+ contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
- // Create the contact info object
- contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
- contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
-
- return true;
+ return true;
+ }
}
}
From 951ba3e42c3680f42e8204ca4b3f9a01d1d28c07 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 30 Mar 2017 22:39:06 +0200
Subject: [PATCH 042/133] Working on ConvexPolyhedron class
---
src/collision/PolyhedronMesh.cpp | 27 +++++++
src/collision/PolyhedronMesh.h | 17 +++-
src/collision/shapes/BoxShape.cpp | 31 ++++++++
src/collision/shapes/BoxShape.h | 100 ++++++++++++++++++++++--
src/collision/shapes/ConvexMeshShape.h | 75 ++++++++++++++++--
src/collision/shapes/ConvexPolyhedron.h | 33 ++++++++
src/collision/shapes/ConvexShape.h | 2 +-
7 files changed, 268 insertions(+), 17 deletions(-)
diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp
index df43a47a..50ef5bab 100644
--- a/src/collision/PolyhedronMesh.cpp
+++ b/src/collision/PolyhedronMesh.cpp
@@ -36,6 +36,17 @@ PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) {
// Create the half-edge structure of the mesh
createHalfEdgeStructure();
+
+ // Create the face normals array
+ mFacesNormals = new Vector3[mHalfEdgeStructure.getNbFaces()];
+
+ // Compute the faces normals
+ computeFacesNormals();
+}
+
+// Destructor
+PolyhedronMesh::~PolyhedronMesh() {
+ delete[] mFacesNormals;
}
// Create the half-edge structure of the mesh
@@ -59,6 +70,8 @@ void PolyhedronMesh::createHalfEdgeStructure() {
faceVertices.push_back(mPolygonVertexArray->getVertexIndexInFace(f, v));
}
+ assert(faceVertices.size() >= 3);
+
// Addd the face into the half-edge structure
mHalfEdgeStructure.addFace(faceVertices);
}
@@ -97,3 +110,17 @@ Vector3 PolyhedronMesh::getVertex(uint index) const {
return vertex;
}
+
+void PolyhedronMesh::computeFacesNormals() {
+
+ // For each face
+ for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) {
+ HalfEdgeStructure::Face face = mHalfEdgeStructure.getFace(f);
+
+ assert(face.faceVertices.size() >= 3);
+
+ const Vector3 vec1 = getVertex(face.faceVertices[1]) - getVertex(face.faceVertices[0]);
+ const Vector3 vec2 = getVertex(face.faceVertices[2]) - getVertex(face.faceVertices[0]);
+ mFacesNormals[f] = vec1.cross(vec2);
+ }
+}
diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h
index f842cae1..940b2db0 100644
--- a/src/collision/PolyhedronMesh.h
+++ b/src/collision/PolyhedronMesh.h
@@ -52,11 +52,17 @@ class PolyhedronMesh {
/// Half-edge structure of the mesh
HalfEdgeStructure mHalfEdgeStructure;
+ /// Array with the face normals
+ Vector3* mFacesNormals;
+
// -------------------- Methods -------------------- //
/// Create the half-edge structure of the mesh
void createHalfEdgeStructure();
+ /// Compute the faces normals
+ void computeFacesNormals();
+
public:
// -------------------- Methods -------------------- //
@@ -65,7 +71,7 @@ class PolyhedronMesh {
PolyhedronMesh(PolygonVertexArray* polygonVertexArray);
/// Destructor
- ~PolyhedronMesh() = default;
+ ~PolyhedronMesh();
/// Return the number of vertices
uint getNbVertices() const;
@@ -73,6 +79,9 @@ class PolyhedronMesh {
/// Return a vertex
Vector3 getVertex(uint index) const;
+ /// Return a face normal
+ Vector3 getFaceNormal(uint faceIndex) const;
+
/// Return the half-edge structure of the mesh
const HalfEdgeStructure& getHalfEdgeStructure() const;
};
@@ -82,6 +91,12 @@ inline uint PolyhedronMesh::getNbVertices() const {
return mHalfEdgeStructure.getNbVertices();
}
+// Return a face normal
+inline Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const {
+ assert(faceIndex < mHalfEdgeStructure.getNbFaces());
+ return mFacesNormals[faceIndex];
+}
+
// Return the half-edge structure of the mesh
inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const {
return mHalfEdgeStructure;
diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp
index ba5dfa1a..f00210c2 100644
--- a/src/collision/shapes/BoxShape.cpp
+++ b/src/collision/shapes/BoxShape.cpp
@@ -42,6 +42,37 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin)
assert(extent.x > decimal(0.0) && extent.x > margin);
assert(extent.y > decimal(0.0) && extent.y > margin);
assert(extent.z > decimal(0.0) && extent.z > margin);
+
+ // Vertices
+ mHalfEdgeStructure.addVertex(0);
+ mHalfEdgeStructure.addVertex(1);
+ mHalfEdgeStructure.addVertex(2);
+ mHalfEdgeStructure.addVertex(3);
+ mHalfEdgeStructure.addVertex(4);
+ mHalfEdgeStructure.addVertex(5);
+ mHalfEdgeStructure.addVertex(6);
+ mHalfEdgeStructure.addVertex(7);
+
+ // Faces
+ std::vector<uint> face0;
+ face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3);
+ std::vector<uint> face1;
+ face0.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2);
+ std::vector<uint> face2;
+ face0.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5);
+ std::vector<uint> face3;
+ face0.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7);
+ std::vector<uint> face4;
+ face0.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0);
+ std::vector<uint> face5;
+ face0.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3);
+
+ mHalfEdgeStructure.addFace(face0);
+ mHalfEdgeStructure.addFace(face1);
+ mHalfEdgeStructure.addFace(face2);
+ mHalfEdgeStructure.addFace(face3);
+ mHalfEdgeStructure.addFace(face4);
+ mHalfEdgeStructure.addFace(face5);
}
// Return the local inertia tensor of the collision shape
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index c3d5caf2..d78bda25 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -59,6 +59,9 @@ class BoxShape : public ConvexPolyhedron {
/// Extent sizes of the box in the x, y and z direction
Vector3 mExtent;
+ /// Half-edge structure of the polyhedron
+ HalfEdgeStructure mHalfEdgeStructure;
+
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
@@ -99,11 +102,32 @@ class BoxShape : public ConvexPolyhedron {
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
- /// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const override;
-
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
+
+ /// Return the number of faces of the polyhedron
+ virtual uint getNbFaces() const override;
+
+ /// Return a given face of the polyhedron
+ virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override;
+
+ /// Return the number of vertices of the polyhedron
+ virtual uint getNbVertices() const override;
+
+ /// Return a given vertex of the polyhedron
+ virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override;
+
+ /// Return the number of half-edges of the polyhedron
+ virtual uint getNbHalfEdges() const override;
+
+ /// Return a given half-edge of the polyhedron
+ virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override;
+
+ /// Return the position of a given vertex
+ virtual Vector3 getVertexPosition(uint vertexIndex) const override;
+
+ /// Return the normal vector of a given face of the polyhedron
+ virtual Vector3 getFaceNormal(uint faceIndex) const override;
};
// Return the extents of the box
@@ -137,11 +161,6 @@ inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const {
min = -max;
}
-// Return true if the collision shape is a polyhedron
-inline bool BoxShape::isPolyhedron() const {
- return true;
-}
-
// Return the number of bytes used by the collision shape
inline size_t BoxShape::getSizeInBytes() const {
return sizeof(BoxShape);
@@ -163,6 +182,71 @@ inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* pro
localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]);
}
+// Return the number of faces of the polyhedron
+inline uint BoxShape::getNbFaces() const {
+ return 6;
+}
+
+// Return a given face of the polyhedron
+inline HalfEdgeStructure::Face BoxShape::getFace(uint faceIndex) const {
+ assert(faceIndex < mHalfEdgeStructure.getNbFaces());
+ return mHalfEdgeStructure.getFace(faceIndex);
+}
+
+// Return the number of vertices of the polyhedron
+inline uint BoxShape::getNbVertices() const {
+ return 8;
+}
+
+// Return a given vertex of the polyhedron
+inline HalfEdgeStructure::Vertex BoxShape::getVertex(uint vertexIndex) const {
+ assert(vertexIndex < getNbVertices());
+ return mHalfEdgeStructure.getVertex(vertexIndex);
+}
+
+// Return the position of a given vertex
+inline Vector3 BoxShape::getVertexPosition(uint vertexIndex) const {
+ assert(vertexIndex < getNbVertices());
+
+ Vector3 extent = getExtent();
+
+ switch(vertexIndex) {
+ case 0: return Vector3(-extent.x, -extent.y, extent.z);
+ case 1: return Vector3(extent.x, -extent.y, extent.z);
+ case 2: return Vector3(extent.x, extent.y, extent.z);
+ case 3: return Vector3(-extent.x, extent.y, extent.z);
+ case 4: return Vector3(-extent.x, -extent.y, -extent.z);
+ case 5: return Vector3(extent.x, -extent.y, -extent.z);
+ case 6: return Vector3(extent.x, extent.y, -extent.z);
+ case 7: return Vector3(-extent.x, extent.y, -extent.z);
+ }
+}
+
+// Return the normal vector of a given face of the polyhedron
+inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const {
+ assert(faceIndex < getNbFaces());
+
+ switch(faceIndex) {
+ case 0: return Vector3(0, 0, 1);
+ case 1: return Vector3(1, 0, 0);
+ case 2: return Vector3(0, 0, -1);
+ case 3: return Vector3(-1, 0, 0);
+ case 4: return Vector3(0, -1, 0);
+ case 5: return Vector3(0, 1, 0);
+ }
+}
+
+// Return the number of half-edges of the polyhedron
+inline uint BoxShape::getNbHalfEdges() const {
+ return 24;
+}
+
+// Return a given half-edge of the polyhedron
+inline HalfEdgeStructure::Edge BoxShape::getHalfEdge(uint edgeIndex) const {
+ assert(edgeIndex < getNbHalfEdges());
+ return mHalfEdgeStructure.getHalfEdge(edgeIndex);
+}
+
}
#endif
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index c33fd5bd..cc88d38d 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -119,8 +119,29 @@ class ConvexMeshShape : public ConvexPolyhedron {
/// Return the local inertia tensor of the collision shape.
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
- /// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const override;
+ /// Return the number of faces of the polyhedron
+ virtual uint getNbFaces() const override;
+
+ /// Return a given face of the polyhedron
+ virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override;
+
+ /// Return the number of vertices of the polyhedron
+ virtual uint getNbVertices() const override;
+
+ /// Return a given vertex of the polyhedron
+ virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override;
+
+ /// Return the number of half-edges of the polyhedron
+ virtual uint getNbHalfEdges() const override;
+
+ /// Return a given half-edge of the polyhedron
+ virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override;
+
+ /// Return the position of a given vertex
+ virtual Vector3 getVertexPosition(uint vertexIndex) const override;
+
+ /// Return the normal vector of a given face of the polyhedron
+ virtual Vector3 getFaceNormal(uint faceIndex) const override;
};
/// Set the scaling vector of the collision shape
@@ -134,11 +155,6 @@ inline size_t ConvexMeshShape::getSizeInBytes() const {
return sizeof(ConvexMeshShape);
}
-// Return true if the collision shape is a polyhedron
-inline bool ConvexMeshShape::isPolyhedron() const {
- return true;
-}
-
// Return the local bounds of the shape in x, y and z directions
/**
* @param min The minimum bounds of the shape in local-space coordinates
@@ -178,6 +194,51 @@ inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
}
+// Return the number of faces of the polyhedron
+inline uint ConvexMeshShape::getNbFaces() const {
+ return mPolyhedronMesh->getHalfEdgeStructure().getNbFaces();
+}
+
+// Return a given face of the polyhedron
+inline HalfEdgeStructure::Face ConvexMeshShape::getFace(uint faceIndex) const {
+ assert(faceIndex < getNbFaces());
+ return mPolyhedronMesh->getHalfEdgeStructure().getFace(faceIndex);
+}
+
+// Return the number of vertices of the polyhedron
+inline uint ConvexMeshShape::getNbVertices() const {
+ return mPolyhedronMesh->getHalfEdgeStructure().getNbVertices();
+}
+
+// Return a given vertex of the polyhedron
+inline HalfEdgeStructure::Vertex ConvexMeshShape::getVertex(uint vertexIndex) const {
+ assert(vertexIndex < getNbVertices());
+ return mPolyhedronMesh->getHalfEdgeStructure().getVertex(vertexIndex);
+}
+
+// Return the number of half-edges of the polyhedron
+inline uint ConvexMeshShape::getNbHalfEdges() const {
+ return mPolyhedronMesh->getHalfEdgeStructure().getNbHalfEdges();
+}
+
+// Return a given half-edge of the polyhedron
+inline HalfEdgeStructure::Edge ConvexMeshShape::getHalfEdge(uint edgeIndex) const {
+ assert(edgeIndex < getNbHalfEdges());
+ return mPolyhedronMesh->getHalfEdgeStructure().getHalfEdge(edgeIndex);
+}
+
+// Return the position of a given vertex
+inline Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const {
+ assert(vertexIndex < getNbVertices());
+ return mPolyhedronMesh->getVertex(vertexIndex);
+}
+
+// Return the normal vector of a given face of the polyhedron
+inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const {
+ assert(faceIndex < getNbFaces());
+ return mPolyhedronMesh->getFaceNormal(faceIndex);
+}
+
}
#endif
diff --git a/src/collision/shapes/ConvexPolyhedron.h b/src/collision/shapes/ConvexPolyhedron.h
index 27bbc0b5..2a544077 100644
--- a/src/collision/shapes/ConvexPolyhedron.h
+++ b/src/collision/shapes/ConvexPolyhedron.h
@@ -28,6 +28,7 @@
// Libraries
#include "ConvexShape.h"
+#include "collision/HalfEdgeStructure.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -60,8 +61,40 @@ class ConvexPolyhedron : public ConvexShape {
/// Deleted assignment operator
ConvexPolyhedron& operator=(const ConvexPolyhedron& shape) = delete;
+
+ /// Return the number of faces of the polyhedron
+ virtual uint getNbFaces() const=0;
+
+ /// Return a given face of the polyhedron
+ virtual HalfEdgeStructure::Face getFace(uint faceIndex) const=0;
+
+ /// Return the number of vertices of the polyhedron
+ virtual uint getNbVertices() const=0;
+
+ /// Return a given vertex of the polyhedron
+ virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const=0;
+
+ /// Return the position of a given vertex
+ virtual Vector3 getVertexPosition(uint vertexIndex) const=0;
+
+ /// Return the normal vector of a given face of the polyhedron
+ virtual Vector3 getFaceNormal(uint faceIndex) const=0;
+
+ /// Return the number of half-edges of the polyhedron
+ virtual uint getNbHalfEdges() const=0;
+
+ /// Return a given half-edge of the polyhedron
+ virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const=0;
+
+ /// Return true if the collision shape is a polyhedron
+ virtual bool isPolyhedron() const override;
};
+// Return true if the collision shape is a polyhedron
+inline bool ConvexPolyhedron::isPolyhedron() const {
+ return true;
+}
+
}
#endif
diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h
index 4b12cae4..9180fefe 100644
--- a/src/collision/shapes/ConvexShape.h
+++ b/src/collision/shapes/ConvexShape.h
@@ -84,7 +84,7 @@ class ConvexShape : public CollisionShape {
friend class EPAAlgorithm;
};
-/// Return true if the collision shape is convex, false if it is concave
+// Return true if the collision shape is convex, false if it is concave
inline bool ConvexShape::isConvex() const {
return true;
}
From 57da79492f3cfc99e5758fb650988a4c778dcf4e Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 2 Apr 2017 00:33:29 +0200
Subject: [PATCH 043/133] Add sphere vs convex polyhedron test in SAT algorithm
---
CMakeLists.txt | 8 +--
src/collision/PolyhedronMesh.cpp | 2 +
.../narrowphase/DefaultCollisionDispatch.cpp | 4 ++
.../narrowphase/DefaultCollisionDispatch.h | 4 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 64 ++++++++++++++++++-
src/collision/narrowphase/SAT/SATAlgorithm.h | 2 +-
... => SphereVsConvexPolyhedronAlgorithm.cpp} | 4 +-
....h => SphereVsConvexPolyhedronAlgorithm.h} | 14 ++--
src/collision/shapes/BoxShape.cpp | 12 ++--
src/collision/shapes/BoxShape.h | 4 +-
src/collision/shapes/CollisionShape.h | 2 +-
src/collision/shapes/ConvexMeshShape.cpp | 2 +-
src/collision/shapes/ConvexMeshShape.h | 4 +-
...lyhedron.cpp => ConvexPolyhedronShape.cpp} | 4 +-
...exPolyhedron.h => ConvexPolyhedronShape.h} | 14 ++--
15 files changed, 105 insertions(+), 39 deletions(-)
rename src/collision/narrowphase/{SphereVsConvexMeshAlgorithm.cpp => SphereVsConvexPolyhedronAlgorithm.cpp} (95%)
rename src/collision/narrowphase/{SphereVsConvexMeshAlgorithm.h => SphereVsConvexPolyhedronAlgorithm.h} (84%)
rename src/collision/shapes/{ConvexPolyhedron.cpp => ConvexPolyhedronShape.cpp} (95%)
rename src/collision/shapes/{ConvexPolyhedron.h => ConvexPolyhedronShape.h} (90%)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4f6ab0bc..0a30f8c2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -84,14 +84,14 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp"
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
- "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h"
- "src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp"
+ "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h"
+ "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp"
"src/collision/shapes/AABB.h"
"src/collision/shapes/AABB.cpp"
"src/collision/shapes/ConvexShape.h"
"src/collision/shapes/ConvexShape.cpp"
- "src/collision/shapes/ConvexPolyhedron.h"
- "src/collision/shapes/ConvexPolyhedron.cpp"
+ "src/collision/shapes/ConvexPolyhedronShape.h"
+ "src/collision/shapes/ConvexPolyhedronShape.cpp"
"src/collision/shapes/ConcaveShape.h"
"src/collision/shapes/ConcaveShape.cpp"
"src/collision/shapes/BoxShape.h"
diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp
index 50ef5bab..60a3be9f 100644
--- a/src/collision/PolyhedronMesh.cpp
+++ b/src/collision/PolyhedronMesh.cpp
@@ -111,6 +111,7 @@ Vector3 PolyhedronMesh::getVertex(uint index) const {
return vertex;
}
+// Compute the faces normals
void PolyhedronMesh::computeFacesNormals() {
// For each face
@@ -122,5 +123,6 @@ void PolyhedronMesh::computeFacesNormals() {
const Vector3 vec1 = getVertex(face.faceVertices[1]) - getVertex(face.faceVertices[0]);
const Vector3 vec2 = getVertex(face.faceVertices[2]) - getVertex(face.faceVertices[0]);
mFacesNormals[f] = vec1.cross(vec2);
+ mFacesNormals[f].normalize();
}
}
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
index 5fdd485b..0a1e498c 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
@@ -52,6 +52,10 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CAPSULE) {
return &mCapsuleVsCapsuleAlgorithm;
}
+ // Sphere vs Convex Polyhedron algorithm
+ if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
+ return &mSphereVsConvexPolyhedronAlgorithm;
+ }
return nullptr;
}
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h
index 65e05497..7c5d77ea 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.h
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.h
@@ -30,7 +30,7 @@
#include "CollisionDispatch.h"
#include "ConcaveVsConvexAlgorithm.h"
#include "SphereVsSphereAlgorithm.h"
-#include "SphereVsConvexMeshAlgorithm.h"
+#include "SphereVsConvexPolyhedronAlgorithm.h"
#include "SphereVsCapsuleAlgorithm.h"
#include "CapsuleVsCapsuleAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
@@ -51,7 +51,7 @@ class DefaultCollisionDispatch : public CollisionDispatch {
SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
/// Sphere vs Convex Mesh collision algorithm
- SphereVsConvexMeshAlgorithm mSphereVsConvexMeshAlgorithm;
+ SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm;
/// Sphere vs Capsule collision algorithm
SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 61910acc..ee241c77 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -27,6 +27,8 @@
#include "SATAlgorithm.h"
#include "constraint/ContactPoint.h"
#include "collision/PolyhedronMesh.h"
+#include "collision/shapes/ConvexPolyhedronShape.h"
+#include "collision/shapes/SphereShape.h"
#include "configuration.h"
#include "engine/Profiler.h"
#include <algorithm>
@@ -45,7 +47,7 @@ bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, Contact
case CollisionShapeType::CONVEX_POLYHEDRON:
return testCollisionConvexMeshVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
case CollisionShapeType::SPHERE:
- return testCollisionSphereVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
+ return testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
case CollisionShapeType::CAPSULE:
return testCollisionCapsuleVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
case CollisionShapeType::TRIANGLE:
@@ -57,23 +59,81 @@ bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, Contact
}
// Test collision between a sphere and a convex mesh
-bool SATAlgorithm::testCollisionSphereVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
+ assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+
+ // Get the capsule collision shapes
+ const SphereShape* sphere = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
+ const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfo->collisionShape2);
+
+
+ // Get the transform from sphere local-space to polyhedron local-space
+ const Transform sphereToPolyhedronSpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() *
+ narrowPhaseInfo->shape1ToWorldTransform;
+
+ // Transform the center of the sphere into the local-space of the convex polyhedron
+ const Vector3 sphereCenter = sphereToPolyhedronSpaceTransform.getPosition();
+
+ // Minimum penetration depth
+ decimal minPenetrationDepth = DECIMAL_LARGEST;
+ uint minFaceIndex = 0;
+
+ // For each face of the convex mesh
+ for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
+
+ // Get the face
+ HalfEdgeStructure::Face face = polyhedron->getFace(f);
+
+ // Get the face normal
+ const Vector3 faceNormal = polyhedron->getFaceNormal(f);
+
+ Vector3 sphereCenterToFacePoint = polyhedron->getVertexPosition(face.faceVertices[0]) - sphereCenter;
+ decimal penetrationDepth = sphereCenterToFacePoint.dot(faceNormal) + sphere->getRadius();
+
+ // If the penetration depth is negative, we have found a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+ return false;
+ }
+
+ // Check if we have found a new minimum penetration axis
+ if (penetrationDepth < minPenetrationDepth) {
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = f;
+ }
+ }
+
+ const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
+ const Vector3 normalWorld = -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minFaceNormal);
+ const Vector3 contactPointSphereLocal = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * normalWorld * sphere->getRadius();
+ const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
+
+ // Create the contact info object
+ contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, contactPointSphereLocal, contactPointPolyhedronLocal);
+
+ return true;
}
// Test collision between a capsule and a convex mesh
bool SATAlgorithm::testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
+ assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
}
// Test collision between a triangle and a convex mesh
bool SATAlgorithm::testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::TRIANGLE);
+ assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
}
// Test collision between two convex meshes
bool SATAlgorithm::testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+ assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
}
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index dc5bc5f2..f3ae2ab2 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -47,7 +47,7 @@ class SATAlgorithm {
const Vector3& c, const Vector3& d) const;
/// Test collision between a sphere and a convex mesh
- bool testCollisionSphereVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+ bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
/// Test collision between a capsule and a convex mesh
bool testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
similarity index 95%
rename from src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp
rename to src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index 06dbc615..107348db 100644
--- a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -24,7 +24,7 @@
********************************************************************************/
// Libraries
-#include "SphereVsConvexMeshAlgorithm.h"
+#include "SphereVsConvexPolyhedronAlgorithm.h"
#include "SAT/SATAlgorithm.h"
#include "collision/shapes/SphereShape.h"
#include "collision/shapes/ConvexMeshShape.h"
@@ -32,7 +32,7 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) {
// Get the local-space to world-space transforms
diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
similarity index 84%
rename from src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h
rename to src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
index f0046545..0bdac19a 100644
--- a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
@@ -35,12 +35,12 @@
/// Namespace ReactPhysics3D
namespace reactphysics3d {
-// Class SphereVsConvexMeshAlgorithm
+// Class SphereVsConvexPolyhedronAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
- * between a sphere and a convex mesh.
+ * between a sphere and a convex polyhedron.
*/
-class SphereVsConvexMeshAlgorithm : public NarrowPhaseAlgorithm {
+class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
protected :
@@ -49,16 +49,16 @@ class SphereVsConvexMeshAlgorithm : public NarrowPhaseAlgorithm {
// -------------------- Methods -------------------- //
/// Constructor
- SphereVsConvexMeshAlgorithm() = default;
+ SphereVsConvexPolyhedronAlgorithm() = default;
/// Destructor
- virtual ~SphereVsConvexMeshAlgorithm() override = default;
+ virtual ~SphereVsConvexPolyhedronAlgorithm() override = default;
/// Deleted copy-constructor
- SphereVsConvexMeshAlgorithm(const SphereVsConvexMeshAlgorithm& algorithm) = delete;
+ SphereVsConvexPolyhedronAlgorithm(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Deleted assignment operator
- SphereVsConvexMeshAlgorithm& operator=(const SphereVsConvexMeshAlgorithm& algorithm) = delete;
+ SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp
index f00210c2..e1c97844 100644
--- a/src/collision/shapes/BoxShape.cpp
+++ b/src/collision/shapes/BoxShape.cpp
@@ -38,7 +38,7 @@ using namespace reactphysics3d;
* @param margin The collision margin (in meters) around the collision shape
*/
BoxShape::BoxShape(const Vector3& extent, decimal margin)
- : ConvexPolyhedron(margin), mExtent(extent - Vector3(margin, margin, margin)) {
+ : ConvexPolyhedronShape(margin), mExtent(extent - Vector3(margin, margin, margin)) {
assert(extent.x > decimal(0.0) && extent.x > margin);
assert(extent.y > decimal(0.0) && extent.y > margin);
assert(extent.z > decimal(0.0) && extent.z > margin);
@@ -57,15 +57,15 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin)
std::vector<uint> face0;
face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3);
std::vector<uint> face1;
- face0.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2);
+ face1.push_back(1); face0.push_back(5); face0.push_back(6); face0.push_back(2);
std::vector<uint> face2;
- face0.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5);
+ face2.push_back(4); face0.push_back(7); face0.push_back(6); face0.push_back(5);
std::vector<uint> face3;
- face0.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7);
+ face3.push_back(4); face0.push_back(0); face0.push_back(3); face0.push_back(7);
std::vector<uint> face4;
- face0.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0);
+ face4.push_back(4); face0.push_back(5); face0.push_back(1); face0.push_back(0);
std::vector<uint> face5;
- face0.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3);
+ face5.push_back(2); face0.push_back(6); face0.push_back(7); face0.push_back(3);
mHalfEdgeStructure.addFace(face0);
mHalfEdgeStructure.addFace(face1);
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index d78bda25..be79e3dd 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -28,7 +28,7 @@
// Libraries
#include <cfloat>
-#include "ConvexPolyhedron.h"
+#include "ConvexPolyhedronShape.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
@@ -50,7 +50,7 @@ namespace reactphysics3d {
* constructor of the box shape. Otherwise, it is recommended to use the
* default margin distance by not using the "margin" parameter in the constructor.
*/
-class BoxShape : public ConvexPolyhedron {
+class BoxShape : public ConvexPolyhedronShape {
protected :
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index 28e724fd..2b1fa576 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -41,7 +41,7 @@ namespace reactphysics3d {
/// Type of the collision shape
enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_MESH, HEIGHTFIELD};
-const int NB_COLLISION_SHAPE_TYPES = 9;
+const int NB_COLLISION_SHAPE_TYPES = 6;
// Declarations
class ProxyShape;
diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp
index 40382aeb..263bb736 100644
--- a/src/collision/shapes/ConvexMeshShape.cpp
+++ b/src/collision/shapes/ConvexMeshShape.cpp
@@ -42,7 +42,7 @@ using namespace reactphysics3d;
* @param margin Collision margin (in meters) around the collision shape
*/
ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin)
- : ConvexPolyhedron(margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) {
+ : ConvexPolyhedronShape(margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) {
// Recalculate the bounds of the mesh
recalculateBounds();
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index cc88d38d..ff11c232 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -27,7 +27,7 @@
#define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H
// Libraries
-#include "ConvexPolyhedron.h"
+#include "ConvexPolyhedronShape.h"
#include "engine/CollisionWorld.h"
#include "mathematics/mathematics.h"
#include "collision/TriangleMesh.h"
@@ -59,7 +59,7 @@ class CollisionWorld;
* with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
* in order to use the edges information for collision detection.
*/
-class ConvexMeshShape : public ConvexPolyhedron {
+class ConvexMeshShape : public ConvexPolyhedronShape {
protected :
diff --git a/src/collision/shapes/ConvexPolyhedron.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp
similarity index 95%
rename from src/collision/shapes/ConvexPolyhedron.cpp
rename to src/collision/shapes/ConvexPolyhedronShape.cpp
index 26964554..e73a7cd7 100644
--- a/src/collision/shapes/ConvexPolyhedron.cpp
+++ b/src/collision/shapes/ConvexPolyhedronShape.cpp
@@ -24,14 +24,14 @@
********************************************************************************/
// Libraries
-#include "ConvexPolyhedron.h"
+#include "ConvexPolyhedronShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
-ConvexPolyhedron::ConvexPolyhedron(decimal margin)
+ConvexPolyhedronShape::ConvexPolyhedronShape(decimal margin)
: ConvexShape(CollisionShapeType::CONVEX_POLYHEDRON, margin) {
}
diff --git a/src/collision/shapes/ConvexPolyhedron.h b/src/collision/shapes/ConvexPolyhedronShape.h
similarity index 90%
rename from src/collision/shapes/ConvexPolyhedron.h
rename to src/collision/shapes/ConvexPolyhedronShape.h
index 2a544077..059e5a3b 100644
--- a/src/collision/shapes/ConvexPolyhedron.h
+++ b/src/collision/shapes/ConvexPolyhedronShape.h
@@ -33,12 +33,12 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
-// Class ConvexPolyhedron
+// Class ConvexPolyhedronShape
/**
* This abstract class represents a convex polyhedron collision shape associated with a
* body that is used during the narrow-phase collision detection.
*/
-class ConvexPolyhedron : public ConvexShape {
+class ConvexPolyhedronShape : public ConvexShape {
protected :
@@ -51,16 +51,16 @@ class ConvexPolyhedron : public ConvexShape {
// -------------------- Methods -------------------- //
/// Constructor
- ConvexPolyhedron(decimal margin);
+ ConvexPolyhedronShape(decimal margin);
/// Destructor
- virtual ~ConvexPolyhedron() override = default;
+ virtual ~ConvexPolyhedronShape() override = default;
/// Deleted copy-constructor
- ConvexPolyhedron(const ConvexPolyhedron& shape) = delete;
+ ConvexPolyhedronShape(const ConvexPolyhedronShape& shape) = delete;
/// Deleted assignment operator
- ConvexPolyhedron& operator=(const ConvexPolyhedron& shape) = delete;
+ ConvexPolyhedronShape& operator=(const ConvexPolyhedronShape& shape) = delete;
/// Return the number of faces of the polyhedron
virtual uint getNbFaces() const=0;
@@ -91,7 +91,7 @@ class ConvexPolyhedron : public ConvexShape {
};
// Return true if the collision shape is a polyhedron
-inline bool ConvexPolyhedron::isPolyhedron() const {
+inline bool ConvexPolyhedronShape::isPolyhedron() const {
return true;
}
From f61fea8b8ab3d592c42a3074200307ebd7014d36 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 16 Apr 2017 22:09:59 +0200
Subject: [PATCH 044/133] Add clippling segment/polygons methods, fix issues
and add convex vs capsule algorithm
---
CMakeLists.txt | 2 +
src/collision/CollisionDetection.cpp | 11 --
src/collision/CollisionDetection.h | 11 +-
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 61 ++++++++
.../CapsuleVsConvexPolyhedronAlgorithm.h | 71 +++++++++
.../narrowphase/SAT/SATAlgorithm.cpp | 50 +++---
src/collision/narrowphase/SAT/SATAlgorithm.h | 27 ++--
.../narrowphase/SphereVsCapsuleAlgorithm.cpp | 33 ++--
.../SphereVsConvexPolyhedronAlgorithm.cpp | 7 +-
.../SphereVsConvexPolyhedronAlgorithm.h | 4 +-
src/engine/OverlappingPair.cpp | 2 -
src/mathematics/mathematics_functions.cpp | 147 +++++++++++++++++-
src/mathematics/mathematics_functions.h | 11 +-
test/Test.h | 2 +-
test/tests/collision/TestRaycast.h | 6 +-
.../mathematics/TestMathematicsFunctions.h | 90 ++++++++++-
16 files changed, 452 insertions(+), 83 deletions(-)
create mode 100644 src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
create mode 100644 src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0a30f8c2..5cacac60 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -86,6 +86,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
"src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h"
"src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp"
+ "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h"
+ "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp"
"src/collision/shapes/AABB.h"
"src/collision/shapes/AABB.cpp"
"src/collision/shapes/ConvexShape.h"
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 4a0ef718..908c5bf5 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -288,17 +288,6 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return;
- // Make sure the shape with the smallest collision shape type comes first
- const uint shape1TypeIndex = static_cast<const uint>(shape1->getCollisionShape()->getType());
- const uint shape2TypeIndex = static_cast<const uint>(shape2->getCollisionShape()->getType());
- if (shape1TypeIndex > shape2TypeIndex) {
-
- // Swap the two shapes
- ProxyShape* temp = shape1;
- shape1 = shape2;
- shape2 = temp;
- }
-
// Compute the overlapping pair ID
overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 9444b319..29200ec7 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -266,8 +266,15 @@ inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, con
inline NarrowPhaseAlgorithm* CollisionDetection::selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type,
const CollisionShapeType& shape2Type) const {
- const unsigned int shape1Index = static_cast<unsigned int>(shape1Type);
- const unsigned int shape2Index = static_cast<unsigned int>(shape2Type);
+ uint shape1Index = static_cast<unsigned int>(shape1Type);
+ uint shape2Index = static_cast<unsigned int>(shape2Type);
+
+ // Swap the shape types if necessary
+ if (shape1Index > shape2Index) {
+ const uint tempIndex = shape1Index;
+ shape1Index = shape2Index;
+ shape2Index = tempIndex;
+ }
assert(shape1Index <= shape2Index);
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
new file mode 100644
index 00000000..8deb17e5
--- /dev/null
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -0,0 +1,61 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "CapsuleVsConvexPolyhedronAlgorithm.h"
+#include "SAT/SATAlgorithm.h"
+#include "GJK/GJKAlgorithm.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactManifoldInfo& contactManifoldInfo) {
+
+ // Get the local-space to world-space transforms
+ const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
+ const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
+
+ // First, we run the GJK algorithm
+ GJKAlgorithm gjkAlgorithm;
+ GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
+
+ // If we have found a contact point inside the margins (shallow penetration)
+ if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
+
+ // Return true
+ return true;
+ }
+
+ // If we have overlap even without the margins (deep penetration)
+ if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
+
+ // Run the SAT algorithm to find the separating axis and compute contact point
+ SATAlgorithm satAlgorithm;
+ return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+ }
+
+ return false;
+}
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
new file mode 100644
index 00000000..e9905596
--- /dev/null
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
@@ -0,0 +1,71 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CAPSULE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
+#define REACTPHYSICS3D_CAPSULE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
+
+// Libraries
+#include "body/Body.h"
+#include "constraint/ContactPoint.h"
+#include "NarrowPhaseAlgorithm.h"
+
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class CapsuleVsConvexPolyhedronAlgorithm
+/**
+ * This class is used to compute the narrow-phase collision detection
+ * between a capsule and a convex polyhedron.
+ */
+class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
+
+ protected :
+
+ public :
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ CapsuleVsConvexPolyhedronAlgorithm() = default;
+
+ /// Destructor
+ virtual ~CapsuleVsConvexPolyhedronAlgorithm() override = default;
+
+ /// Deleted copy-constructor
+ CapsuleVsConvexPolyhedronAlgorithm(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
+
+ /// Deleted assignment operator
+ CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
+
+ /// Compute a contact info if the two bounding volume collide
+ virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactManifoldInfo& contactManifoldInfo) override;
+};
+
+}
+
+#endif
+
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index ee241c77..1b57e36f 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -39,39 +39,25 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
-
- assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
-
- switch (narrowPhaseInfo->collisionShape1->getType()) {
- case CollisionShapeType::CONVEX_POLYHEDRON:
- return testCollisionConvexMeshVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
- case CollisionShapeType::SPHERE:
- return testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
- case CollisionShapeType::CAPSULE:
- return testCollisionCapsuleVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
- case CollisionShapeType::TRIANGLE:
- return testCollisionTriangleVsConvexMesh(narrowPhaseInfo, contactManifoldInfo);
- default: assert(false);
- }
-
- return false;
-}
-
// Test collision between a sphere and a convex mesh
bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
- assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
- assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+ bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
+
+ assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+ assert(!isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+ assert(!isSphereShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the capsule collision shapes
- const SphereShape* sphere = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
- const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfo->collisionShape2);
+ const SphereShape* sphere = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
+ const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
+ const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
+ const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
// Get the transform from sphere local-space to polyhedron local-space
- const Transform sphereToPolyhedronSpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() *
- narrowPhaseInfo->shape1ToWorldTransform;
+ const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse();
+ const Transform sphereToPolyhedronSpaceTransform = worldToPolyhedronTransform * sphereToWorldTransform;
// Transform the center of the sphere into the local-space of the convex polyhedron
const Vector3 sphereCenter = sphereToPolyhedronSpaceTransform.getPosition();
@@ -105,18 +91,24 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
}
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
- const Vector3 normalWorld = -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minFaceNormal);
- const Vector3 contactPointSphereLocal = narrowPhaseInfo->shape1ToWorldTransform.getInverse() * normalWorld * sphere->getRadius();
+ Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal);
+ const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse() * normalWorld * sphere->getRadius();
const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
+ if (!isSphereShape1) {
+ normalWorld = -normalWorld;
+ }
+
// Create the contact info object
- contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, contactPointSphereLocal, contactPointPolyhedronLocal);
+ contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+ isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
+ isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
return true;
}
// Test collision between a capsule and a convex mesh
-bool SATAlgorithm::testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index f3ae2ab2..b779d711 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -46,18 +46,6 @@ class SATAlgorithm {
bool testGaussMapArcsIntersect(const Vector3& a, const Vector3& b,
const Vector3& c, const Vector3& d) const;
- /// Test collision between a sphere and a convex mesh
- bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
-
- /// Test collision between a capsule and a convex mesh
- bool testCollisionCapsuleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
-
- /// Test collision between a triangle and a convex mesh
- bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
-
- /// Test collision between two convex meshes
- bool testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
-
public :
// -------------------- Methods -------------------- //
@@ -74,9 +62,18 @@ class SATAlgorithm {
/// Deleted assignment operator
SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete;
- /// Compute a contact info if the two bounding volumes collide.
- bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo);
+ /// Test collision between a sphere and a convex mesh
+ bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+
+ /// Test collision between a capsule and a convex mesh
+ bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+
+ /// Test collision between a triangle and a convex mesh
+ bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+
+ /// Test collision between two convex meshes
+ bool testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+
};
}
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
index aace17fd..a311b2bf 100644
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
@@ -32,23 +32,30 @@
using namespace reactphysics3d;
bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
-
- assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
- assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
+
+ bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
+
+ assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
+ assert(!isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
+ assert(!isSphereShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the collision shapes
- const SphereShape* sphereShape = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
- const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
+ const SphereShape* sphereShape = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
+ const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
// Get the transform from sphere local-space to capsule local-space
- const Transform sphereToCapsuleSpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform;
+ const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
+ const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
+ const Transform worldToCapsuleTransform = capsuleToWorldTransform.getInverse();
+ const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform;
// Transform the center of the sphere into the local-space of the capsule shape
const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition();
// Compute the end-points of the inner segment of the capsule
- const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
- const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
+ const decimal capsuleHalfHeight = capsuleShape->getHeight() * decimal(0.5);
+ const Vector3 capsuleSegA(0, -capsuleHalfHeight, 0);
+ const Vector3 capsuleSegB(0, capsuleHalfHeight, 0);
// Compute the point on the inner capsule segment that is the closes to center of sphere
const Vector3 closestPointOnSegment = computeClosestPointOnSegment(capsuleSegA, capsuleSegB, sphereCenter);
@@ -69,12 +76,18 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
- const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * sphereCenterToSegment;
+ Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
decimal penetrationDepth = sumRadius - sphereSegmentDistance;
+
+ if (!isSphereShape1) {
+ normalWorld = -normalWorld;
+ }
// Create the contact info object
- contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointSphereLocal, contactPointCapsuleLocal);
+ contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
+ isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
+ isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
return true;
}
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index 107348db..44959971 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -25,15 +25,14 @@
// Libraries
#include "SphereVsConvexPolyhedronAlgorithm.h"
+#include "GJK/GJKAlgorithm.h"
#include "SAT/SATAlgorithm.h"
-#include "collision/shapes/SphereShape.h"
-#include "collision/shapes/ConvexMeshShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) {
+ ContactManifoldInfo& contactManifoldInfo) {
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
@@ -55,7 +54,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
- return satAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
+ return satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
}
return false;
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
index 0bdac19a..591ebd44 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
@@ -23,8 +23,8 @@
* *
********************************************************************************/
-#ifndef REACTPHYSICS3D_SPHERE_VS_CONVEX_MESH_ALGORITHM_H
-#define REACTPHYSICS3D_SPHERE_VS_CONVEX_MESH_ALGORITHM_H
+#ifndef REACTPHYSICS3D_SPHERE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
+#define REACTPHYSICS3D_SPHERE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
// Libraries
#include "body/Body.h"
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index 6df4bf4e..1f5d894e 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -35,6 +35,4 @@ OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
: mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds),
mCachedSeparatingAxis(0.0, 1.0, 0.0) {
- assert(static_cast<uint>(shape1->getCollisionShape()->getType()) <=
- static_cast<uint>(shape2->getCollisionShape()->getType()));
}
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 7de898ae..07e3530f 100644
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -27,6 +27,7 @@
#include "mathematics_functions.h"
#include "Vector3.h"
#include <cassert>
+#include <vector>
using namespace reactphysics3d;
@@ -187,7 +188,7 @@ decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, con
return t;
}
-/// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB"
+// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB"
decimal reactphysics3d::computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) {
decimal distAB = (linePointB - linePointA).length();
@@ -199,4 +200,148 @@ decimal reactphysics3d::computeDistancePointToLineDistance(const Vector3& linePo
return ((point - linePointA).cross(point - linePointB)).length() / distAB;
}
+// Clip a segment against multiple planes and return the clipped segment vertices
+// This method implements the Sutherland–Hodgman clipping algorithm
+std::vector<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
+ const std::vector<Vector3>& planesPoints,
+ const std::vector<Vector3>& planesNormals) {
+
+ assert(planesPoints.size() == planesNormals.size());
+
+ std::vector<Vector3> inputVertices = {segA, segB};
+ std::vector<Vector3> outputVertices;
+
+ // For each clipping plane
+ for (uint p=0; p<planesPoints.size(); p++) {
+
+ // If there is no more vertices, stop
+ if (inputVertices.empty()) return inputVertices;
+
+ assert(inputVertices.size() == 2);
+
+ outputVertices.clear();
+
+ Vector3& v1 = inputVertices[0];
+ Vector3& v2 = inputVertices[1];
+
+ decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
+ decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
+
+ // If the second vertex is in front of the clippling plane
+ if (v2DotN >= decimal(0.0)) {
+
+ // If the first vertex is not in front of the clippling plane
+ if (v1DotN < decimal(0.0)) {
+
+ // The second point we keep is the intersection between the segment v1, v2 and the clipping plane
+ decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]);
+
+ if (t >= decimal(0) && t <= decimal(1.0)) {
+ outputVertices.push_back(v1 + t * (v2 - v1));
+ }
+ else {
+ outputVertices.push_back(v2);
+ }
+ }
+ else {
+ outputVertices.push_back(v1);
+ }
+
+ // Add the second vertex
+ outputVertices.push_back(v2);
+ }
+ else { // If the second vertex is behind the clipping plane
+
+ // If the first vertex is in front of the clippling plane
+ if (v1DotN >= decimal(0.0)) {
+
+ outputVertices.push_back(v1);
+
+ // The first point we keep is the intersection between the segment v1, v2 and the clipping plane
+ decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]);
+
+ if (t >= decimal(0.0) && t <= decimal(1.0)) {
+ outputVertices.push_back(v1 + t * (v2 - v1));
+ }
+ }
+ }
+
+ inputVertices = outputVertices;
+ }
+
+ return outputVertices;
+}
+
+/// Clip a polygon against multiple planes and return the clipped polygon vertices
+/// This method implements the Sutherland–Hodgman clipping algorithm
+std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
+ const std::vector<Vector3>& planesNormals) {
+
+ assert(planesPoints.size() == planesNormals.size());
+
+ std::vector<Vector3> inputVertices(polygonVertices);
+ std::vector<Vector3> outputVertices;
+
+ // For each clipping plane
+ for (uint p=0; p<planesPoints.size(); p++) {
+
+ outputVertices.clear();
+
+ uint vStart = inputVertices.size() - 1;
+
+ // For each edge of the polygon
+ for (uint vEnd = 0; vEnd<inputVertices.size(); vEnd++) {
+
+ Vector3& v1 = inputVertices[vStart];
+ Vector3& v2 = inputVertices[vEnd];
+
+ decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
+ decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
+
+ // If the second vertex is in front of the clippling plane
+ if (v2DotN >= decimal(0.0)) {
+
+ // If the first vertex is not in front of the clippling plane
+ if (v1DotN < decimal(0.0)) {
+
+ // The second point we keep is the intersection between the segment v1, v2 and the clipping plane
+ decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]);
+
+ if (t >= decimal(0) && t <= decimal(1.0)) {
+ outputVertices.push_back(v1 + t * (v2 - v1));
+ }
+ else {
+ outputVertices.push_back(v2);
+ }
+ }
+
+ // Add the second vertex
+ outputVertices.push_back(v2);
+ }
+ else { // If the second vertex is behind the clipping plane
+
+ // If the first vertex is in front of the clippling plane
+ if (v1DotN >= decimal(0.0)) {
+
+ // The first point we keep is the intersection between the segment v1, v2 and the clipping plane
+ decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]);
+
+ if (t >= decimal(0.0) && t <= decimal(1.0)) {
+ outputVertices.push_back(v1 + t * (v2 - v1));
+ }
+ else {
+ outputVertices.push_back(v1);
+ }
+ }
+ }
+
+ vStart = vEnd;
+ }
+
+ inputVertices = outputVertices;
+ }
+
+ return outputVertices;
+}
+
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index 315de65c..639e8d3b 100644
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -32,6 +32,7 @@
#include <algorithm>
#include <cassert>
#include <cmath>
+#include <vector>
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -96,8 +97,16 @@ decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB
/// Compute the distance between a point and a line
decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point);
+/// Clip a segment against multiple planes and return the clipped segment vertices
+std::vector<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
+ const std::vector<Vector3>& planesPoints,
+ const std::vector<Vector3>& planesNormals);
+
+/// Clip a polygon against multiple planes and return the clipped polygon vertices
+std::vector<Vector3> clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
+ const std::vector<Vector3>& planesNormals);
+
}
-
#endif
diff --git a/test/Test.h b/test/Test.h
index 37fa9dee..1288813d 100644
--- a/test/Test.h
+++ b/test/Test.h
@@ -35,7 +35,7 @@
namespace reactphysics3d {
// Macros
-#define test(condition) applyTest(condition, #condition, __FILE__, __LINE__)
+#define test(condition) applyTest(condition, #condition, __FILE__, __LINE__);
#define fail(text) applyFail(text, __FILE__, __LINE__);
// Class Test
diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h
index fe8926d5..0f25ab94 100644
--- a/test/tests/collision/TestRaycast.h
+++ b/test/tests/collision/TestRaycast.h
@@ -300,8 +300,8 @@ class TestRaycast : public Test {
mBoxProxyShape->setCollisionCategoryBits(CATEGORY1);
mSphereProxyShape->setCollisionCategoryBits(CATEGORY1);
mCapsuleProxyShape->setCollisionCategoryBits(CATEGORY1);
- mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2);
- mConvexMeshProxyShapeEdgesInfo->setCollisionCategoryBits(CATEGORY2);
+ //mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2);
+ //mConvexMeshProxyShapeEdgesInfo->setCollisionCategoryBits(CATEGORY2);
mCompoundSphereProxyShape->setCollisionCategoryBits(CATEGORY2);
mCompoundCapsuleProxyShape->setCollisionCategoryBits(CATEGORY2);
mTriangleProxyShape->setCollisionCategoryBits(CATEGORY1);
@@ -1787,7 +1787,7 @@ class TestRaycast : public Test {
// ----- Test raycast miss ----- //
test(!mConcaveMeshBody->raycast(ray1, raycastInfo3));
- test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3));
+ //test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray1, &mCallback);
test(!mCallback.isHit);
diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h
index 42e1065c..04a81a69 100644
--- a/test/tests/mathematics/TestMathematicsFunctions.h
+++ b/test/tests/mathematics/TestMathematicsFunctions.h
@@ -27,8 +27,6 @@
#define TEST_MATHEMATICS_FUNCTIONS_H
// Libraries
-#include "Test.h"
-#include "mathematics/mathematics_functions.h"
/// Reactphysics3D namespace
namespace reactphysics3d {
@@ -170,6 +168,94 @@ class TestMathematicsFunctions : public Test {
test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001));
test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001));
test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001));
+
+
+ // Test clipSegmentWithPlanes()
+ std::vector<Vector3> segmentVertices;
+ segmentVertices.push_back(Vector3(-6, 3, 0));
+ segmentVertices.push_back(Vector3(8, 3, 0));
+
+ std::vector<Vector3> planesNormals;
+ std::vector<Vector3> planesPoints;
+ planesNormals.push_back(Vector3(-1, 0, 0));
+ planesPoints.push_back(Vector3(4, 0, 0));
+
+ std::vector<Vector3> clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1],
+ planesPoints, planesNormals);
+ test(clipSegmentVertices.size() == 2);
+ test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001));
+ test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001));
+ test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].x, 4, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].y, 3, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001));
+
+ segmentVertices.clear();
+ segmentVertices.push_back(Vector3(8, 3, 0));
+ segmentVertices.push_back(Vector3(-6, 3, 0));
+
+ clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals);
+ test(clipSegmentVertices.size() == 2);
+ test(approxEqual(clipSegmentVertices[0].x, 4, 0.000001));
+ test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001));
+ test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].x, -6, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].y, 3, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001));
+
+ segmentVertices.clear();
+ segmentVertices.push_back(Vector3(-6, 3, 0));
+ segmentVertices.push_back(Vector3(3, 3, 0));
+
+ clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals);
+ test(clipSegmentVertices.size() == 2);
+ test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001));
+ test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001));
+ test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].x, 3, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].y, 3, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001));
+
+ segmentVertices.clear();
+ segmentVertices.push_back(Vector3(5, 3, 0));
+ segmentVertices.push_back(Vector3(8, 3, 0));
+
+ clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals);
+ test(clipSegmentVertices.size() == 0);
+
+ // Test clipPolygonWithPlanes()
+ std::vector<Vector3> polygonVertices;
+ polygonVertices.push_back(Vector3(-4, 2, 0));
+ polygonVertices.push_back(Vector3(7, 2, 0));
+ polygonVertices.push_back(Vector3(7, 4, 0));
+ polygonVertices.push_back(Vector3(-4, 4, 0));
+
+ planesNormals.clear();
+ planesPoints.clear();
+ planesNormals.push_back(Vector3(1, 0, 0));
+ planesPoints.push_back(Vector3(0, 0, 0));
+ planesNormals.push_back(Vector3(0, 1, 0));
+ planesPoints.push_back(Vector3(0, 0, 0));
+ planesNormals.push_back(Vector3(-1, 0, 0));
+ planesPoints.push_back(Vector3(10, 0, 0));
+ planesNormals.push_back(Vector3(0, -1, 0));
+ planesPoints.push_back(Vector3(10, 5, 0));
+
+ clipSegmentVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
+ test(clipSegmentVertices.size() == 4);
+ test(approxEqual(clipSegmentVertices[0].x, 0, 0.000001));
+ test(approxEqual(clipSegmentVertices[0].y, 2, 0.000001));
+ test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].x, 7, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].y, 2, 0.000001));
+ test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001));
+ test(approxEqual(clipSegmentVertices[2].x, 7, 0.000001));
+ test(approxEqual(clipSegmentVertices[2].y, 4, 0.000001));
+ test(approxEqual(clipSegmentVertices[2].z, 0, 0.000001));
+ test(approxEqual(clipSegmentVertices[3].x, 0, 0.000001));
+ test(approxEqual(clipSegmentVertices[3].y, 4, 0.000001));
+ test(approxEqual(clipSegmentVertices[3].z, 0, 0.000001));
+
}
};
From 7fb6f49149b04f5303735394ab568af726c03cbf Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 28 Apr 2017 21:40:16 +0200
Subject: [PATCH 045/133] Working on capsule vs polyhedron narrow-phase
algorithm
---
src/collision/ContactManifoldInfo.h | 27 ++-
src/collision/PolyhedronMesh.cpp | 15 ++
src/collision/PolyhedronMesh.h | 14 ++
src/collision/ProxyShape.h | 1 -
.../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 7 +-
.../narrowphase/CapsuleVsCapsuleAlgorithm.h | 2 +-
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 71 +++++-
.../CapsuleVsConvexPolyhedronAlgorithm.h | 2 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 218 +++++++++++++++++-
src/collision/narrowphase/SAT/SATAlgorithm.h | 12 +
.../narrowphase/SphereVsCapsuleAlgorithm.cpp | 3 +
.../narrowphase/SphereVsCapsuleAlgorithm.h | 2 +-
.../SphereVsConvexPolyhedronAlgorithm.cpp | 7 +-
.../SphereVsConvexPolyhedronAlgorithm.h | 2 +-
src/collision/shapes/BoxShape.h | 8 +
src/collision/shapes/ConvexMeshShape.h | 8 +
src/collision/shapes/ConvexPolyhedronShape.h | 3 +
src/collision/shapes/ConvexShape.h | 2 +-
src/mathematics/mathematics_functions.cpp | 5 +
src/mathematics/mathematics_functions.h | 3 +
20 files changed, 382 insertions(+), 30 deletions(-)
diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h
index d4afc590..f631b9fc 100644
--- a/src/collision/ContactManifoldInfo.h
+++ b/src/collision/ContactManifoldInfo.h
@@ -64,15 +64,8 @@ class ContactManifoldInfo {
/// Destructor
~ContactManifoldInfo() {
- // Delete all the contact points in the linked list
- ContactPointInfo* element = mContactPointsList;
- while(element != nullptr) {
- ContactPointInfo* elementToDelete = element;
- element = element->next;
-
- // Delete the current element
- mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
- }
+ // Remove all the contact points
+ reset();
}
/// Deleted copy-constructor
@@ -96,6 +89,22 @@ class ContactManifoldInfo {
mContactPointsList = contactPointInfo;
}
+ /// Remove all the contact points
+ void reset() {
+
+ // Delete all the contact points in the linked list
+ ContactPointInfo* element = mContactPointsList;
+ while(element != nullptr) {
+ ContactPointInfo* elementToDelete = element;
+ element = element->next;
+
+ // Delete the current element
+ mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
+ }
+
+ mContactPointsList = nullptr;
+ }
+
/// Get the first contact point info of the linked list of contact points
ContactPointInfo* getFirstContactPointInfo() const {
return mContactPointsList;
diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp
index 60a3be9f..61b1e3c8 100644
--- a/src/collision/PolyhedronMesh.cpp
+++ b/src/collision/PolyhedronMesh.cpp
@@ -42,6 +42,9 @@ PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) {
// Compute the faces normals
computeFacesNormals();
+
+ // Compute the centroid
+ computeCentroid();
}
// Destructor
@@ -126,3 +129,15 @@ void PolyhedronMesh::computeFacesNormals() {
mFacesNormals[f].normalize();
}
}
+
+// Compute the centroid of the polyhedron
+void PolyhedronMesh::computeCentroid() {
+
+ mCentroid.setToZero();
+
+ for (uint v=0; v < getNbVertices(); v++) {
+ mCentroid += getVertex(v);
+ }
+
+ mCentroid /= getNbVertices();
+}
diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h
index 940b2db0..a64db670 100644
--- a/src/collision/PolyhedronMesh.h
+++ b/src/collision/PolyhedronMesh.h
@@ -55,6 +55,9 @@ class PolyhedronMesh {
/// Array with the face normals
Vector3* mFacesNormals;
+ /// Centroid of the polyhedron
+ Vector3 mCentroid;
+
// -------------------- Methods -------------------- //
/// Create the half-edge structure of the mesh
@@ -63,6 +66,9 @@ class PolyhedronMesh {
/// Compute the faces normals
void computeFacesNormals();
+ /// Compute the centroid of the polyhedron
+ void computeCentroid() ;
+
public:
// -------------------- Methods -------------------- //
@@ -84,6 +90,9 @@ class PolyhedronMesh {
/// Return the half-edge structure of the mesh
const HalfEdgeStructure& getHalfEdgeStructure() const;
+
+ /// Return the centroid of the polyhedron
+ Vector3 getCentroid() const;
};
// Return the number of vertices
@@ -102,6 +111,11 @@ inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const {
return mHalfEdgeStructure;
}
+// Return the centroid of the polyhedron
+inline Vector3 PolyhedronMesh::getCentroid() const {
+ return mCentroid;
+}
+
}
#endif
diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h
index 45c0add2..d9575b3d 100644
--- a/src/collision/ProxyShape.h
+++ b/src/collision/ProxyShape.h
@@ -175,7 +175,6 @@ class ProxyShape {
friend class CollisionDetection;
friend class CollisionWorld;
friend class DynamicsWorld;
- friend class EPAAlgorithm;
friend class GJKAlgorithm;
friend class ConvexMeshShape;
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
index 11160b8c..a6c7a020 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
@@ -30,13 +30,14 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
+// Compute the narrow-phase collision detection between two capsules
+// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
+// by Dirk Gregorius.
bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
- const decimal parallelEpsilon = decimal(0.001);
-
// Get the capsule collision shapes
const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape1);
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
@@ -62,7 +63,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
// If the two capsules are parallel (we create two contact points)
- if (seg1.cross(seg2).lengthSquare() < parallelEpsilon * parallelEpsilon) {
+ if (areParallelVectors(seg1, seg2)) {
// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
index 898d6337..9d8393c9 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
@@ -60,7 +60,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
- /// Compute a contact info if the two bounding volume collide
+ /// Compute the narrow-phase collision detection between two capsules
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
};
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index 8deb17e5..5e8dd5f9 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -27,24 +27,86 @@
#include "CapsuleVsConvexPolyhedronAlgorithm.h"
#include "SAT/SATAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
+#include "collision/shapes/CapsuleShape.h"
+#include "collision/shapes/ConvexPolyhedronShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
+// Compute the narrow-phase collision detection between a capsule and a polyhedron
+// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
+// by Dirk Gregorius.
bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) {
- // Get the local-space to world-space transforms
- const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
- const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
-
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
+ SATAlgorithm satAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
// 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)
+
+ // Get the contact point created by GJK
+ ContactPointInfo* contactPoint = contactManifoldInfo.getFirstContactPointInfo();
+
+ 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<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
+ const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(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;
+
+ // 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)) {
+
+ // Remove the previous contact point computed by GJK
+ contactManifoldInfo.reset();
+
+ const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
+ const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
+
+ // Compute the end-points of the inner segment of the capsule
+ const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
+ const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
+
+ // Convert the inner capsule segment points into the polyhedron local-space
+ const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
+ const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
+ const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
+
+ const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
+
+ // Compute and create two contact points
+ satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
+ polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
+ capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
+ contactManifoldInfo, isCapsuleShape1);
+
+ break;
+ }
+
+ }
+
// Return true
return true;
}
@@ -53,7 +115,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
- SATAlgorithm satAlgorithm;
return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
}
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
index e9905596..6b3f504f 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
@@ -60,7 +60,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
- /// Compute a contact info if the two bounding volume collide
+ /// Compute the narrow-phase collision detection between a capsule and a polyhedron
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
};
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 1b57e36f..3637b3dc 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -27,7 +27,7 @@
#include "SATAlgorithm.h"
#include "constraint/ContactPoint.h"
#include "collision/PolyhedronMesh.h"
-#include "collision/shapes/ConvexPolyhedronShape.h"
+#include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/SphereShape.h"
#include "configuration.h"
#include "engine/Profiler.h"
@@ -110,8 +110,220 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
// Test collision between a capsule and a convex mesh
bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
- assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
- assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+ 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<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
+ const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
+
+ const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
+ const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
+
+ const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
+
+ // Minimum penetration depth
+ decimal minPenetrationDepth = DECIMAL_LARGEST;
+ uint minFaceIndex = 0;
+ uint minEdgeIndex = 0;
+ bool isMinPenetrationFaceNormal = false;
+ Vector3 separatingAxisCapsuleSpace;
+ Vector3 separatingPolyhedronEdgeVertex1;
+ Vector3 separatingPolyhedronEdgeVertex2;
+
+ // For each face of the convex mesh
+ for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
+
+ // Get the face
+ HalfEdgeStructure::Face face = polyhedron->getFace(f);
+
+ // Get the face normal
+ const Vector3 faceNormal = polyhedron->getFaceNormal(f);
+
+ // Compute the penetration depth (using the capsule support in the direction opposite to the face normal)
+ const Vector3 faceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
+ const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-faceNormalCapsuleSpace, nullptr);
+ const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]);
+ const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint;
+ const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal);
+
+ // If the penetration depth is negative, we have found a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+ return false;
+ }
+
+ // Check if we have found a new minimum penetration axis
+ if (penetrationDepth < minPenetrationDepth) {
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = f;
+ isMinPenetrationFaceNormal = true;
+ separatingAxisCapsuleSpace = faceNormalCapsuleSpace;
+ }
+ }
+
+ // Compute the end-points of the inner segment of the capsule
+ const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
+ const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
+ const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA;
+
+ // For each direction that is the cross product of the capsule inner segment and
+ // an edge of the polyhedron
+ for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) {
+
+ // Get an edge from the polyhedron (convert it into the capsule local-space)
+ HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e);
+ const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
+ const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
+ const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
+
+ HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
+ const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex);
+ const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex);
+
+ // Check using the Gauss Map if this edge cross product can be as separating axis
+ if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) {
+
+ // Compute the axis to test (cross product between capsule inner segment and polyhedron edge)
+ Vector3 axis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace);
+
+ // Skip separating axis test if polyhedron edge is parallel to the capsule inner segment
+ if (axis.lengthSquare() >= decimal(0.00001)) {
+
+ const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid();
+ const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1;
+
+ // Swap axis direction if necessary such that it points out of the polyhedron
+ if (axis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) {
+ axis = -axis;
+ }
+
+ axis.normalize();
+
+ // Compute the penetration depth
+ const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-axis, nullptr);
+ const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint;
+ const decimal penetrationDepth = capsuleSupportPointToEdgePoint.dot(axis);
+
+ // If the penetration depth is negative, we have found a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+ return false;
+ }
+
+ // Check if we have found a new minimum penetration axis
+ if (penetrationDepth < minPenetrationDepth) {
+ minPenetrationDepth = penetrationDepth;
+ minEdgeIndex = e;
+ isMinPenetrationFaceNormal = false;
+ separatingAxisCapsuleSpace = axis;
+ separatingPolyhedronEdgeVertex1 = edgeVertex1;
+ separatingPolyhedronEdgeVertex2 = edgeVertex2;
+ }
+ }
+ }
+ }
+
+ // Convert the inner capsule segment points into the polyhedron local-space
+ const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
+ const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
+ const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
+
+ const Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace;
+ const decimal capsuleRadius = capsuleShape->getRadius();
+
+ // If the separating axis is a face normal
+ // We need to clip the inner capsule segment with the adjacent faces of the separating face
+ if (isMinPenetrationFaceNormal) {
+
+ computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
+ polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
+ capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
+ contactManifoldInfo, isCapsuleShape1);
+ }
+ else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
+
+ // Compute the closest points between the inner capsule segment and the
+ // edge of the polyhedron in polyhedron local-space
+ Vector3 closestPointPolyhedronEdge, closestPointCapsuleInnerSegment;
+ computeClosestPointBetweenTwoSegments(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
+ separatingPolyhedronEdgeVertex1, separatingPolyhedronEdgeVertex2,
+ closestPointCapsuleInnerSegment, closestPointPolyhedronEdge);
+
+
+ // Project closest capsule inner segment point into the capsule bounds
+ const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
+
+ // Create the contact point
+ contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+ isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
+ isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
+ }
+
+ return true;
+}
+
+// Compute the two contact points between a polyhedron and a capsule when the separating
+// axis is a face normal of the polyhedron
+void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
+ decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
+ const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
+ const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
+ ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const {
+
+ HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
+ uint firstEdgeIndex = face.edgeIndex;
+ uint edgeIndex = firstEdgeIndex;
+
+ std::vector<Vector3> planesPoints;
+ std::vector<Vector3> planesNormals;
+
+ // For each adjacent edge of the separating face of the polyhedron
+ do {
+ HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex);
+ HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
+
+ // Construct a clippling plane for each adjacent edge of the separting face of the polyhedron
+ planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex));
+ planesNormals.push_back(polyhedron->getFaceNormal(twinEdge.faceIndex));
+
+ edgeIndex = edge.nextEdgeIndex;
+
+ } while(edgeIndex != firstEdgeIndex);
+
+ // First we clip the inner segment of the capsule with the four planes of the adjacent faces
+ std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
+ planesPoints, planesNormals);
+
+ // Project the two clipped points into the polyhedron face
+ const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
+ const Vector3 contactPoint1Polyhedron = clipSegment[0] + faceNormal * (penetrationDepth - capsuleRadius);
+ const Vector3 contactPoint2Polyhedron = clipSegment[1] + faceNormal * (penetrationDepth - capsuleRadius);
+
+ // Project the two clipped points into the capsule bounds
+ const Vector3 contactPoint1Capsule = (polyhedronToCapsuleTransform * clipSegment[0]) - separatingAxisCapsuleSpace * capsuleRadius;
+ const Vector3 contactPoint2Capsule = (polyhedronToCapsuleTransform * clipSegment[1]) - separatingAxisCapsuleSpace * capsuleRadius;
+
+ // Create the contact points
+ contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
+ isCapsuleShape1 ? contactPoint1Capsule : contactPoint1Polyhedron,
+ isCapsuleShape1 ? contactPoint1Polyhedron : contactPoint1Capsule);
+ contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
+ isCapsuleShape1 ? contactPoint2Capsule : contactPoint2Polyhedron,
+ isCapsuleShape1 ? contactPoint2Polyhedron : contactPoint2Capsule);
+}
+
+// This method returns true if an edge of a polyhedron and a capsule forms a
+// face of the Minkowski Difference. This test is used to know if two edges
+// (one edge of the polyhedron vs the inner segment of the capsule in this case)
+// have to be test as a possible separating axis
+bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
+ const Vector3& edgeAdjacentFace2Normal) const {
+
+ // Return true if the arc on the Gauss Map corresponding to the polyhedron edge
+ // intersect the unit circle plane corresponding to capsule Gauss Map
+ return capsuleSegment.dot(edgeAdjacentFace1Normal) * capsuleSegment.dot(edgeAdjacentFace2Normal) < decimal(0.0);
}
// Test collision between a triangle and a convex mesh
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index b779d711..42e380f2 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -29,6 +29,7 @@
// Libraries
#include "collision/ContactManifoldInfo.h"
#include "collision/NarrowPhaseInfo.h"
+#include "collision/shapes/ConvexPolyhedronShape.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -68,6 +69,17 @@ class SATAlgorithm {
/// Test collision between a capsule and a convex mesh
bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+ /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
+ void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
+ decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
+ const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
+ const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
+ ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const;
+
+ // This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference
+ bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
+ const Vector3& edgeAdjacentFace2Normal) const;
+
/// Test collision between a triangle and a convex mesh
bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
index a311b2bf..cd437d6e 100644
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
@@ -31,6 +31,9 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
+// Compute the narrow-phase collision detection between a sphere and a capsule
+// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
+// by Dirk Gregorius.
bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
index 784df6f9..154f6e4c 100644
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
@@ -60,7 +60,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
- /// Compute a contact info if the two bounding volume collide
+ /// Compute the narrow-phase collision detection between a sphere and a capsule
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
};
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index 44959971..708d9f8d 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -31,13 +31,12 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
+// Compute the narrow-phase collision detection a sphere and a convex polyhedron
+// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
+// by Dirk Gregorius.
bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) {
- // Get the local-space to world-space transforms
- const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
- const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
-
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
index 591ebd44..c1fd3e55 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
@@ -60,7 +60,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
- /// Compute a contact info if the two bounding volume collide
+ /// Compute the narrow-phase collision detection a sphere and a convex polyhedron
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
};
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index be79e3dd..d2b92dcc 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -128,6 +128,9 @@ class BoxShape : public ConvexPolyhedronShape {
/// Return the normal vector of a given face of the polyhedron
virtual Vector3 getFaceNormal(uint faceIndex) const override;
+
+ /// Return the centroid of the polyhedron
+ virtual Vector3 getCentroid() const override;
};
// Return the extents of the box
@@ -236,6 +239,11 @@ inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const {
}
}
+// Return the centroid of the box
+inline Vector3 BoxShape::getCentroid() const {
+ return Vector3::zero();
+}
+
// Return the number of half-edges of the polyhedron
inline uint BoxShape::getNbHalfEdges() const {
return 24;
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index ff11c232..28ba9c86 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -142,6 +142,9 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
/// Return the normal vector of a given face of the polyhedron
virtual Vector3 getFaceNormal(uint faceIndex) const override;
+
+ /// Return the centroid of the polyhedron
+ virtual Vector3 getCentroid() const override;
};
/// Set the scaling vector of the collision shape
@@ -239,6 +242,11 @@ inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const {
return mPolyhedronMesh->getFaceNormal(faceIndex);
}
+// Return the centroid of the polyhedron
+inline Vector3 ConvexMeshShape::getCentroid() const {
+ return mPolyhedronMesh->getCentroid();
+}
+
}
#endif
diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h
index 059e5a3b..f6d2a34b 100644
--- a/src/collision/shapes/ConvexPolyhedronShape.h
+++ b/src/collision/shapes/ConvexPolyhedronShape.h
@@ -88,6 +88,9 @@ class ConvexPolyhedronShape : public ConvexShape {
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const override;
+
+ /// Return the centroid of the polyhedron
+ virtual Vector3 getCentroid() const=0;
};
// Return true if the collision shape is a polyhedron
diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h
index 9180fefe..4b98c9fa 100644
--- a/src/collision/shapes/ConvexShape.h
+++ b/src/collision/shapes/ConvexShape.h
@@ -81,7 +81,7 @@ class ConvexShape : public CollisionShape {
// -------------------- Friendship -------------------- //
friend class GJKAlgorithm;
- friend class EPAAlgorithm;
+ friend class SATAlgorithm;
};
// Return true if the collision shape is convex, false if it is concave
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 07e3530f..36ba113a 100644
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -60,6 +60,11 @@ Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) {
return vector;
}
+/// Return true if two vectors are parallel
+bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& vector2) {
+ return vector1.cross(vector2).lengthSquare() < 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
index 639e8d3b..44ce9cf4 100644
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -76,6 +76,9 @@ inline bool sameSign(decimal a, decimal b) {
return a * b >= decimal(0.0);
}
+/// Return true if two vectors are parallel
+bool areParallelVectors(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);
From 0ec21e36b9e823aefed5c03949758af41bf240ea Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 16 May 2017 07:10:44 +0200
Subject: [PATCH 046/133] Working on SAT algorithm between two polyhedra
---
.../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 2 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 336 +++++++++++++++++-
src/collision/narrowphase/SAT/SATAlgorithm.h | 24 +-
src/mathematics/mathematics_functions.cpp | 2 +-
src/mathematics/mathematics_functions.h | 2 +-
.../mathematics/TestMathematicsFunctions.h | 13 +-
6 files changed, 349 insertions(+), 30 deletions(-)
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
index a6c7a020..17008622 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
@@ -66,7 +66,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
if (areParallelVectors(seg1, seg2)) {
// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
- const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
+ const decimal segmentsDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
if (segmentsDistance >= sumRadius) {
// The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius.
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 3637b3dc..fd09db27 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -169,8 +169,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA;
- // For each direction that is the cross product of the capsule inner segment and
- // an edge of the polyhedron
+ // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron
for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) {
// Get an edge from the polyhedron (convert it into the capsule local-space)
@@ -326,18 +325,327 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
return capsuleSegment.dot(edgeAdjacentFace1Normal) * capsuleSegment.dot(edgeAdjacentFace2Normal) < decimal(0.0);
}
-// Test collision between a triangle and a convex mesh
-bool SATAlgorithm::testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
-
- assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::TRIANGLE);
- assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
-}
-
-// Test collision between two convex meshes
-bool SATAlgorithm::testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+// Test collision between two convex polyhedrons
+bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+
+ const ConvexPolyhedronShape* polyhedron1 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfo->collisionShape1);
+ const ConvexPolyhedronShape* polyhedron2 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfo->collisionShape2);
+
+ const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform;
+ const Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse();
+
+ decimal minPenetrationDepth = DECIMAL_LARGEST;
+ uint minFaceIndex = 0;
+ bool isMinPenetrationFaceNormal = false;
+ bool isMinPenetrationFaceNormalPolyhedron1 = false;
+ Vector3 separatingEdge1A, separatingEdge1B;
+ Vector3 separatingEdge2A, separatingEdge2B;
+ Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
+
+ // Test all the face normals of the polyhedron 1 for separating axis
+ uint faceIndex;
+ decimal penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
+ if (penetrationDepth <= decimal(0.0)) {
+
+ // We have found a separating axis
+ return false;
+ }
+ if (penetrationDepth < minPenetrationDepth) {
+ isMinPenetrationFaceNormal = true;
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = faceIndex;
+ isMinPenetrationFaceNormalPolyhedron1 = true;
+ }
+
+ // Test all the face normals of the polyhedron 2 for separating axis
+ penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
+ if (penetrationDepth <= decimal(0.0)) {
+
+ // We have found a separating axis
+ return false;
+ }
+ if (penetrationDepth < minPenetrationDepth) {
+ isMinPenetrationFaceNormal = true;
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = faceIndex;
+ isMinPenetrationFaceNormalPolyhedron1 = false;
+ }
+
+ // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis
+ for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) {
+
+ // Get an edge of polyhedron 1
+ HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i);
+
+ const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
+ const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
+ const Vector3 edge1Direction = edge1B - edge1A;
+
+ for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) {
+
+ // Get an edge of polyhedron 2
+ HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j);
+
+ const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
+ const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
+ const Vector3 edge2Direction = edge2B - edge2A;
+
+ // If the two edges build a minkowski face (and the cross product is
+ // therefore a candidate for separating axis
+ if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
+
+ Vector3 separatingAxisPolyhedron2Space;
+
+ // Compute the penetration depth
+ decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
+ edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
+
+ if (penetrationDepth <= decimal(0.0)) {
+
+ // We have found a separating axis
+ return false;
+ }
+
+ if (penetrationDepth < minPenetrationDepth) {
+ minPenetrationDepth = penetrationDepth;
+ isMinPenetrationFaceNormalPolyhedron1 = false;
+ isMinPenetrationFaceNormal = false;
+ separatingEdge1A = edge1A;
+ separatingEdge1B = edge1B;
+ separatingEdge2A = edge2A;
+ separatingEdge2B = edge2B;
+ minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
+ }
+
+ }
+ }
+ }
+
+ assert(minPenetrationDepth > decimal(0.0));
+ assert((isMinPenetrationFaceNormal && minFaceIndex >= 0) || !isMinPenetrationFaceNormal);
+
+ // If the separation axis is a face normal
+ if (isMinPenetrationFaceNormal) {
+
+ const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
+ const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1;
+ const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
+ const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2;
+
+ const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex);
+ const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
+
+ // Compute the world normal
+ const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
+ -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
+
+ // Get the reference face
+ HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex);
+
+ // Find the incident face on the other polyhedron (most anti-parallel face)
+ uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace);
+
+ // Get the incident face
+ HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
+
+ std::vector<Vector3> polygonVertices; // Vertices to clip of the incident face
+ std::vector<Vector3> planesNormals; // Normals of the clipping planes
+ std::vector<Vector3> planesPoints; // Points on the clipping planes
+
+ // Get all the vertices of the incident face (in the reference local-space)
+ std::vector<uint>::const_iterator it;
+ for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) {
+ const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it);
+ polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace);
+ }
+
+ // Get the reference face clipping planes
+ uint currentEdgeIndex = referenceFace.edgeIndex;
+ uint firstEdgeIndex = currentEdgeIndex;
+ do {
+
+ // Get the adjacent edge
+ HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
+
+ // Get the twin edge
+ HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
+
+ // Get the adjacent face normal (and negate it to have a clipping plane)
+ Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex);
+
+ // Get a vertex of the clipping plane (vertex of the adjacent edge)
+ Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex);
+
+ planesNormals.push_back(faceNormal);
+ planesPoints.push_back(faceVertex);
+
+ // Go to the next adjacent edge of the reference face
+ currentEdgeIndex = edge.nextEdgeIndex;
+
+ } while (currentEdgeIndex != firstEdgeIndex);
+
+ // Clip the reference faces with the adjacent planes of the reference face
+ std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
+
+ // We only keep the clipped points that are below the reference face
+ const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(firstEdgeIndex);
+ std::vector<Vector3>::const_iterator itPoints;
+ for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
+
+ // If the clip point is bellow the reference face
+ if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) {
+
+ // Convert the clip incident polyhedron vertex into the incident polyhedron local-space
+ const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
+
+ // Project the contact point onto the reference face
+ Vector3 contactPointReferencePolyhedron = (*itPoints) + axisReferenceSpace * minPenetrationDepth;
+
+ // Create a new contact point
+ contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+ isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
+ isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
+ }
+ }
+ }
+ else { // If we have an edge vs edge contact
+
+ // Compute the closest points between the two edges (in the local-space of poylhedron 2)
+ Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
+ computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B,
+ closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
+
+ // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
+ const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
+
+ // Compute the world normal
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
+
+ // Create the contact point
+ contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+ closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
+ }
+
+ return true;
+}
+
+// Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector
+// This is used to find the incident face on a polyhedron of a given reference face of another polyhedron
+uint SATAlgorithm::findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const {
+
+ decimal minDotProduct = DECIMAL_LARGEST;
+ uint mostAntiParallelFace = 0;
+
+ // For each face of the polyhedron
+ for (uint i=0; i < polyhedron->getNbFaces(); i++) {
+
+ // Get the face normal
+ decimal dotProduct = polyhedron->getFaceNormal(i).dot(direction);
+ if (dotProduct < minDotProduct) {
+ minDotProduct = dotProduct;
+ mostAntiParallelFace = i;
+ }
+ }
+
+ return mostAntiParallelFace;
+}
+
+// Compute and return the distance between the two edges in the direction of the candidate separating axis
+decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid,
+ const Vector3& edge1Direction, const Vector3& edge2Direction,
+ Vector3& outSeparatingAxisPolyhedron2Space) const {
+
+ // If the two edges are parallel
+ if (areParallelVectors(edge1Direction, edge2Direction)) {
+
+ // Return a large penetration depth to skip those edges
+ return DECIMAL_LARGEST;
+ }
+
+ // Compute the candidate separating axis (cross product between two polyhedrons edges)
+ Vector3 axis = edge1Direction.cross(edge2Direction).getUnit();
+
+ // Make sure the axis direction is going from first to second polyhedron
+ if (axis.dot(edge2A - polyhedron2Centroid) > decimal(0.0)) {
+ axis = -axis;
+ }
+
+ outSeparatingAxisPolyhedron2Space = axis;
+
+ // Compute and return the distance between the edges
+ return -axis.dot(edge2A - edge1A);
+}
+
+// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
+decimal SATAlgorithm::testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
+ const ConvexPolyhedronShape* polyhedron2,
+ const Transform& polyhedron1ToPolyhedron2,
+ uint& minFaceIndex) const {
+
+ decimal minPenetrationDepth = DECIMAL_LARGEST;
+
+ // For each face of the first polyhedron
+ for (uint f = 0; f < polyhedron1->getNbFaces(); f++) {
+
+ HalfEdgeStructure::Face face = polyhedron1->getFace(f);
+
+ // Get the face normal
+ const Vector3 faceNormal = polyhedron1->getFaceNormal(f);
+
+ // Convert the face normal into the local-space of polyhedron 2
+ const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal;
+
+ // Get the support point of polyhedron 2 in the inverse direction of face normal
+ const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr);
+
+ // Compute the penetration depth
+ const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]);
+ decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space);
+
+ // If the penetration depth is negative, we have found a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+ return penetrationDepth;
+ }
+
+ // Check if we have found a new minimum penetration axis
+ if (penetrationDepth < minPenetrationDepth) {
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = f;
+ }
+ }
+
+ return minPenetrationDepth;
+}
+
+
+// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis)
+bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* polyhedron1, const HalfEdgeStructure::Edge& edge1,
+ const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2,
+ const Transform& polyhedron1ToPolyhedron2) const {
+
+ const Vector3 a = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(edge1.faceIndex);
+ const Vector3 b = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex);
+
+ const Vector3 c = polyhedron2->getFaceNormal(edge2.faceIndex);
+ const Vector3 d = polyhedron2->getFaceNormal(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).faceIndex);
+
+ // Compute b.cross(a) using the edge direction
+ const Vector3 edge1Vertex1 = polyhedron1->getVertexPosition(edge1.vertexIndex);
+ const Vector3 edge1Vertex2 = polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).vertexIndex);
+ const Vector3 bCrossA = polyhedron1ToPolyhedron2.getOrientation() * (edge1Vertex2 - edge1Vertex1);
+
+ // Compute d.cross(c) using the edge direction
+ const Vector3 edge2Vertex1 = polyhedron2->getVertexPosition(edge2.vertexIndex);
+ const Vector3 edge2Vertex2 = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).vertexIndex);
+ const Vector3 dCrossC = edge2Vertex2 - edge2Vertex1;
+
+ // Test if the two arcs of the Gauss Map intersect (therefore forming a minkowski face)
+ // Note that we negate the normals of the second polyhedron because we are looking at the
+ // Gauss map of the minkowski difference of the polyhedrons
+ return testGaussMapArcsIntersect(a, b, -c, -d, bCrossA, dCrossC);
}
@@ -346,10 +654,8 @@ bool SATAlgorithm::testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* na
/// and edge between faces with normal C and D on second polygon create a face on the Minkowski
/// sum of both polygons. If this is the case, it means that the cross product of both edges
/// might be a separating axis.
-bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b,
- const Vector3& c, const Vector3& d) const {
- const Vector3 bCrossA = b.cross(a);
- const Vector3 dCrossC = d.cross(c);
+bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d,
+ const Vector3& bCrossA, const Vector3& dCrossC) const {
const decimal cba = c.dot(bCrossA);
const decimal dba = d.dot(bCrossA);
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 42e380f2..ae5da16e 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -43,9 +43,23 @@ class SATAlgorithm {
// -------------------- Methods -------------------- //
+ /// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis)
+ bool testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* polyhedron1, const HalfEdgeStructure::Edge& edge1,
+ const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2,
+ const Transform& polyhedron1ToPolyhedron2) const;
+
/// Return true if the arcs AB and CD on the Gauss Map intersect
bool testGaussMapArcsIntersect(const Vector3& a, const Vector3& b,
- const Vector3& c, const Vector3& d) const;
+ const Vector3& c, const Vector3& d,
+ const Vector3& bCrossA, const Vector3& dCrossC) const;
+
+ // Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector
+ uint findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const;
+
+ /// Compute and return the distance between the two edges in the direction of the candidate separating axis
+ decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid,
+ const Vector3& edge1Direction, const Vector3& edge2Direction,
+ Vector3& outSeparatingAxis) const;
public :
@@ -80,12 +94,12 @@ class SATAlgorithm {
bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
const Vector3& edgeAdjacentFace2Normal) const;
- /// Test collision between a triangle and a convex mesh
- bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
-
/// Test collision between two convex meshes
- bool testCollisionConvexMeshVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+ bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+ /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
+ decimal testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
+ const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const;
};
}
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 36ba113a..e01c749a 100644
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -194,7 +194,7 @@ decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, con
}
// Compute the distance between a point "point" and a line given by the points "linePointA" and "linePointB"
-decimal reactphysics3d::computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) {
+decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point) {
decimal distAB = (linePointB - linePointA).length();
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index 44ce9cf4..c695f342 100644
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -98,7 +98,7 @@ void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b,
decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB, const decimal planeD, const Vector3& planeNormal);
/// Compute the distance between a point and a line
-decimal computeDistancePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point);
+decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point);
/// Clip a segment against multiple planes and return the clipped segment vertices
std::vector<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h
index 04a81a69..b45415ee 100644
--- a/test/tests/mathematics/TestMathematicsFunctions.h
+++ b/test/tests/mathematics/TestMathematicsFunctions.h
@@ -162,13 +162,12 @@ class TestMathematicsFunctions : public Test {
decimal tIntersect = computePlaneSegmentIntersection(Vector3(5, 4, 0), Vector3(9, 4, 0), 4, Vector3(0, 1, 0));
test(tIntersect < 0.0 || tIntersect > 1.0);
- // Test computeDistancePointToLineDistance()
- test(approxEqual(computeDistancePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001));
- test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(4, 3, 0)), 8.0, 0.000001));
- test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001));
- test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001));
- test(approxEqual(computeDistancePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001));
-
+ // Test computePointToLineDistance()
+ test(approxEqual(computePointToLineDistance(Vector3(6, 0, 0), Vector3(14, 0, 0), Vector3(5, 3, 0)), 3.0, 0.000001));
+ test(approxEqual(computePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(4, 3, 0)), 8.0, 0.000001));
+ test(approxEqual(computePointToLineDistance(Vector3(6, -5, 0), Vector3(10, -5, 0), Vector3(-43, 254, 0)), 259.0, 0.000001));
+ test(approxEqual(computePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(6, -5, 8)), 0.0, 0.000001));
+ test(approxEqual(computePointToLineDistance(Vector3(6, -5, 8), Vector3(10, -5, -5), Vector3(10, -5, -5)), 0.0, 0.000001));
// Test clipSegmentWithPlanes()
std::vector<Vector3> segmentVertices;
From 678c88d3bd5be850a3ec354e742b8002363120e6 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 16 May 2017 07:11:18 +0200
Subject: [PATCH 047/133] Add box in collision detection scene for the testbed
application
---
.../CollisionDetectionScene.cpp | 20 +++++++++++++++++++
.../CollisionDetectionScene.h | 1 +
2 files changed, 21 insertions(+)
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index 8c0196b3..b96620b6 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -60,6 +60,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mSphere1->setColor(mGreyColorDemo);
mSphere1->setSleepingColor(mRedColorDemo);
+ /*
// ---------- Sphere 2 ---------- //
openglframework::Vector3 position2(4, 0, 0);
@@ -92,6 +93,17 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Set the color
mCapsule2->setColor(mGreyColorDemo);
mCapsule2->setSleepingColor(mRedColorDemo);
+*/
+ // ---------- Box 1 ---------- //
+ openglframework::Vector3 position5(4, 5, 0);
+
+ // Create a box and a corresponding collision body in the dynamics world
+ mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath);
+ mAllShapes.push_back(mBox1);
+
+ // Set the color
+ mBox1->setColor(mGreyColorDemo);
+ mBox1->setSleepingColor(mRedColorDemo);
// ---------- Cone ---------- //
//openglframework::Vector3 position4(0, 0, 0);
@@ -171,6 +183,7 @@ CollisionDetectionScene::~CollisionDetectionScene() {
mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody());
delete mSphere1;
+ /*
mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody());
delete mSphere2;
@@ -179,6 +192,10 @@ CollisionDetectionScene::~CollisionDetectionScene() {
mCollisionWorld->destroyCollisionBody(mCapsule2->getCollisionBody());
delete mCapsule2;
+ */
+
+ mCollisionWorld->destroyCollisionBody(mBox1->getCollisionBody());
+ delete mBox1;
/*
// Destroy the corresponding rigid body from the dynamics world
@@ -300,9 +317,12 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
// Render the shapes
if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ /*
if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ */
+ if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
/*
if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index 2c33b3ac..01df3df8 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -146,6 +146,7 @@ class CollisionDetectionScene : public SceneDemo {
Sphere* mSphere2;
Capsule* mCapsule1;
Capsule* mCapsule2;
+ Box* mBox1;
//Cone* mCone;
//Cylinder* mCylinder;
//Capsule* mCapsule;
From 2af87d48045d97b266d2cddaff5085f58e20dcc6 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 16 May 2017 07:42:04 +0200
Subject: [PATCH 048/133] Add bias to prefer some axis when penetration depths
are the same in SAT algorithm
---
src/collision/narrowphase/SAT/SATAlgorithm.cpp | 10 +++++++---
src/collision/narrowphase/SAT/SATAlgorithm.h | 5 +++++
2 files changed, 12 insertions(+), 3 deletions(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index fd09db27..c82256f3 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -39,6 +39,9 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
+// Static variables initialization
+const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
+
// Test collision between a sphere and a convex mesh
bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
@@ -353,7 +356,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
// We have found a separating axis
return false;
}
- if (penetrationDepth < minPenetrationDepth) {
+ if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
@@ -367,7 +370,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
// We have found a separating axis
return false;
}
- if (penetrationDepth < minPenetrationDepth) {
+ if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
@@ -409,7 +412,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
return false;
}
- if (penetrationDepth < minPenetrationDepth) {
+ if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
+
minPenetrationDepth = penetrationDepth;
isMinPenetrationFaceNormalPolyhedron1 = false;
isMinPenetrationFaceNormal = false;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index ae5da16e..a95abe31 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -41,6 +41,11 @@ class SATAlgorithm {
// -------------------- Attributes -------------------- //
+ /// Bias used to make sure the SAT algorithm returns the same penetration axis between frames
+ /// when there are multiple separating axis with the same penetration depth. The goal is to
+ /// make sure the contact manifold does not change too much between frames.
+ static const decimal SAME_SEPARATING_AXIS_BIAS;
+
// -------------------- Methods -------------------- //
/// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis)
From 9d55255c56aa171654395c4a77b81efdaa88c086 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 17 May 2017 23:40:17 +0200
Subject: [PATCH 049/133] Add contact point normals and constant color shader
in testbed application
---
testbed/common/VisualContactPoint.cpp | 96 ++++++++++++++--
testbed/common/VisualContactPoint.h | 24 +++-
.../CollisionDetectionScene.cpp | 103 +-----------------
.../CollisionDetectionScene.h | 44 ++------
testbed/scenes/raycast/RaycastScene.cpp | 40 ++-----
testbed/scenes/raycast/RaycastScene.h | 13 ++-
testbed/shaders/color.frag | 38 +++++++
testbed/shaders/color.vert | 43 ++++++++
testbed/src/Scene.h | 5 +-
testbed/src/SceneDemo.cpp | 25 ++++-
testbed/src/SceneDemo.h | 3 +
11 files changed, 240 insertions(+), 194 deletions(-)
mode change 100644 => 100755 testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
mode change 100644 => 100755 testbed/scenes/collisiondetection/CollisionDetectionScene.h
create mode 100644 testbed/shaders/color.frag
create mode 100644 testbed/shaders/color.vert
diff --git a/testbed/common/VisualContactPoint.cpp b/testbed/common/VisualContactPoint.cpp
index 7661587e..03ed864b 100644
--- a/testbed/common/VisualContactPoint.cpp
+++ b/testbed/common/VisualContactPoint.cpp
@@ -36,17 +36,24 @@ openglframework::Mesh VisualContactPoint::mMesh;
bool VisualContactPoint::mStaticDataCreated = false;
// Constructor
-VisualContactPoint::VisualContactPoint(const openglframework::Vector3& position,
- const std::string& meshFolderPath)
- : mColor(1.0f, 0.0f, 0.0f, 1.0f) {
+VisualContactPoint::VisualContactPoint(const openglframework::Vector3& position, const std::string& meshFolderPath,
+ const openglframework::Vector3& normalLineEndPointLocal, const openglframework::Color& color)
+ : mColor(color), mVBOVerticesNormalLine(GL_ARRAY_BUFFER) {
+
+ mContactNormalLinePoints[0] = openglframework::Vector3(0, 0, 0);
+ mContactNormalLinePoints[1] = (normalLineEndPointLocal - position) * 0.5f;
// Initialize the position where the mesh will be rendered
translateWorld(position);
+
+ // Create the VBO and VAO to render the contact normal line
+ createContactNormalLineVBOAndVAO();
}
// Destructor
VisualContactPoint::~VisualContactPoint() {
-
+ mVAONormalLine.destroy();
+ mVBOVerticesNormalLine.destroy();
}
// Load and initialize the mesh for all the contact points
@@ -84,8 +91,7 @@ void VisualContactPoint::destroyStaticData() {
}
// Render the sphere at the correct position and with the correct orientation
-void VisualContactPoint::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+void VisualContactPoint::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the VAO
mVAO.bind();
@@ -93,6 +99,10 @@ void VisualContactPoint::render(openglframework::Shader& shader,
// Bind the shader
shader.bind();
+ // Set the vertex color
+ openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a);
+ shader.setVector4Uniform("vertexColor", color, false);
+
mVBOVertices.bind();
// Set the model to camera matrix
@@ -106,10 +116,6 @@ void VisualContactPoint::render(openglframework::Shader& shader,
localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
- // Set the vertex color
- openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a);
- shader.setVector4Uniform("vertexColor", color, false);
-
// Get the location of shader attribute variables
GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false);
@@ -138,9 +144,56 @@ void VisualContactPoint::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
+
+ // Render the contact normal line
+ renderContactNormalLine(shader, worldToCameraMatrix);
}
-// Create the Vertex Buffer Objects used to render with OpenGL.
+void VisualContactPoint::renderContactNormalLine(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
+
+ // Bind the VAO
+ mVAONormalLine.bind();
+
+ // Bind the shader
+ shader.bind();
+
+ mVBOVerticesNormalLine.bind();
+
+ // Set the model to camera matrix
+ const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix;
+ shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix);
+ shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
+
+ // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
+ // model-view matrix)
+ const openglframework::Matrix3 normalMatrix =
+ localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
+ shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
+
+ // Set the vertex color
+ openglframework::Vector4 color(0, 1, 0, 1);
+ shader.setVector4Uniform("vertexColor", color, false);
+
+ // Get the location of shader attribute variables
+ GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
+
+ glEnableVertexAttribArray(vertexPositionLoc);
+ glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL);
+
+ // Draw the lines
+ glDrawArrays(GL_LINES, 0, 2);
+
+ glDisableVertexAttribArray(vertexPositionLoc);
+
+ mVBOVerticesNormalLine.unbind();
+
+ // Unbind the VAO
+ mVAONormalLine.unbind();
+
+ shader.unbind();
+}
+
+// Create the Vertex Buffer Objects used to render the contact point sphere with OpenGL.
/// We create two VBOs (one for vertices and one for indices)
void VisualContactPoint::createVBOAndVAO() {
@@ -181,3 +234,24 @@ void VisualContactPoint::createVBOAndVAO() {
// Unbind the VAO
mVAO.unbind();
}
+
+// Create the Vertex Buffer Objects used to render the contact normal line
+void VisualContactPoint::createContactNormalLineVBOAndVAO() {
+
+ // Create the VBO for the vertices data
+ mVBOVerticesNormalLine.create();
+ mVBOVerticesNormalLine.bind();
+ size_t sizeNormalLineVertices = 2 * sizeof(openglframework::Vector3);
+ mVBOVerticesNormalLine.copyDataIntoVBO(sizeNormalLineVertices, &mContactNormalLinePoints[0], GL_STATIC_DRAW);
+ mVBOVerticesNormalLine.unbind();
+
+ // Create the VAO for both VBOs
+ mVAONormalLine.create();
+ mVAONormalLine.bind();
+
+ // Bind the VBO of vertices
+ mVBOVerticesNormalLine.bind();
+
+ // Unbind the VAO
+ mVAONormalLine.unbind();
+}
diff --git a/testbed/common/VisualContactPoint.h b/testbed/common/VisualContactPoint.h
index 944caf21..b540e751 100644
--- a/testbed/common/VisualContactPoint.h
+++ b/testbed/common/VisualContactPoint.h
@@ -28,6 +28,7 @@
// Libraries
#include "openglframework.h"
+#include "Line.h"
const float VISUAL_CONTACT_POINT_RADIUS = 0.2f;
@@ -56,15 +57,30 @@ class VisualContactPoint : public openglframework::Object3D {
/// Vertex Array Object for the vertex data
static openglframework::VertexArrayObject mVAO;
+ /// Vertex Buffer Object for the vertices data
+ openglframework::VertexBufferObject mVBOVerticesNormalLine;
+
+ /// Vertex Array Object for the vertex data
+ openglframework::VertexArrayObject mVAONormalLine;
+
/// True if static data (VBO, VAO) has been created already
static bool mStaticDataCreated;
+ // Two end-points of the contact normal line
+ openglframework::Vector3 mContactNormalLinePoints[2];
+
/// Color
openglframework::Color mColor;
- // Create the Vertex Buffer Objects used to render with OpenGL.
+ // Create the Vertex Buffer Objects used to render the contact point sphere with OpenGL.
static void createVBOAndVAO();
+ // Create the Vertex Buffer Objects used to render the contact normal line with OpenGL.
+ void createContactNormalLineVBOAndVAO();
+
+ /// Render the contact normal line
+ void renderContactNormalLine(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix);
+
// -------------------- Methods -------------------- //
public :
@@ -72,8 +88,8 @@ class VisualContactPoint : public openglframework::Object3D {
// -------------------- Methods -------------------- //
/// Constructor
- VisualContactPoint(const openglframework::Vector3& position,
- const std::string &meshFolderPath);
+ VisualContactPoint(const openglframework::Vector3& position, const std::string &meshFolderPath,
+ const openglframework::Vector3& normalLineEndPointLocal, const openglframework::Color& color);
/// Destructor
~VisualContactPoint();
@@ -84,7 +100,7 @@ class VisualContactPoint : public openglframework::Object3D {
/// Destroy the mesh for the contact points
static void destroyStaticData();
- /// Render the sphere at the correct position and with the correct orientation
+ /// Render the sphere contact point at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix);
};
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
old mode 100644
new mode 100755
index b96620b6..21a0b51a
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -34,7 +34,7 @@ using namespace collisiondetectionscene;
CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
: SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
mContactManager(mPhongShader, mMeshFolderPath),
- mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) {
+ mAreNormalsDisplayed(false) {
mSelectedShapeIndex = 0;
mIsContactPointsDisplayed = true;
@@ -60,7 +60,6 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mSphere1->setColor(mGreyColorDemo);
mSphere1->setSleepingColor(mRedColorDemo);
- /*
// ---------- Sphere 2 ---------- //
openglframework::Vector3 position2(4, 0, 0);
@@ -93,17 +92,6 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Set the color
mCapsule2->setColor(mGreyColorDemo);
mCapsule2->setSleepingColor(mRedColorDemo);
-*/
- // ---------- Box 1 ---------- //
- openglframework::Vector3 position5(4, 5, 0);
-
- // Create a box and a corresponding collision body in the dynamics world
- mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath);
- mAllShapes.push_back(mBox1);
-
- // Set the color
- mBox1->setColor(mGreyColorDemo);
- mBox1->setSleepingColor(mRedColorDemo);
// ---------- Cone ---------- //
//openglframework::Vector3 position4(0, 0, 0);
@@ -158,9 +146,6 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
//mHeightField->setColor(mGreyColorDemo);
//mHeightField->setSleepingColor(mRedColorDemo);
- // Create the VBO and VAO to render the lines
- //createVBOAndVAO(mPhongShader);
-
mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo);
}
@@ -172,9 +157,6 @@ void CollisionDetectionScene::reset() {
// Destructor
CollisionDetectionScene::~CollisionDetectionScene() {
- // Destroy the shader
- mPhongShader.destroy();
-
// Destroy the box rigid body from the dynamics world
//mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody());
//delete mBox;
@@ -183,7 +165,6 @@ CollisionDetectionScene::~CollisionDetectionScene() {
mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody());
delete mSphere1;
- /*
mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody());
delete mSphere2;
@@ -192,10 +173,6 @@ CollisionDetectionScene::~CollisionDetectionScene() {
mCollisionWorld->destroyCollisionBody(mCapsule2->getCollisionBody());
delete mCapsule2;
- */
-
- mCollisionWorld->destroyCollisionBody(mBox1->getCollisionBody());
- delete mBox1;
/*
// Destroy the corresponding rigid body from the dynamics world
@@ -246,10 +223,6 @@ CollisionDetectionScene::~CollisionDetectionScene() {
// Destroy the collision world
delete mCollisionWorld;
-
- // Destroy the VBOs and VAO
- mVBOVertices.destroy();
- mVAO.destroy();
}
// Update the physics world (take a simulation step)
@@ -272,57 +245,11 @@ void CollisionDetectionScene::update() {
void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix) {
- /*
- // Bind the VAO
- mVAO.bind();
-
- // Bind the shader
- shader.bind();
-
- mVBOVertices.bind();
-
- // Set the model to camera matrix
- const Matrix4 localToCameraMatrix = Matrix4::identity();
- shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix);
- shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
-
- // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
- // model-view matrix)
- const openglframework::Matrix3 normalMatrix =
- localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
- shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
-
- // Set the vertex color
- openglframework::Vector4 color(1, 0, 0, 1);
- shader.setVector4Uniform("vertexColor", color, false);
-
- // Get the location of shader attribute variables
- GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
-
- glEnableVertexAttribArray(vertexPositionLoc);
- glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL);
-
- // Draw the lines
- glDrawArrays(GL_LINES, 0, NB_RAYS);
-
- glDisableVertexAttribArray(vertexPositionLoc);
-
- mVBOVertices.unbind();
-
- // Unbind the VAO
- mVAO.unbind();
-
- shader.unbind();
- */
-
// Render the shapes
if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- /*
if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- */
- if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
/*
if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
@@ -338,34 +265,6 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
shader.unbind();
}
-// Create the Vertex Buffer Objects used to render with OpenGL.
-/// We create two VBOs (one for vertices and one for indices)
-void CollisionDetectionScene::createVBOAndVAO(openglframework::Shader& shader) {
-
- // Bind the shader
- shader.bind();
-
- // Create the VBO for the vertices data
- mVBOVertices.create();
- mVBOVertices.bind();
- size_t sizeVertices = mLinePoints.size() * sizeof(openglframework::Vector3);
- mVBOVertices.copyDataIntoVBO(sizeVertices, &mLinePoints[0], GL_STATIC_DRAW);
- mVBOVertices.unbind();
-
- // Create the VAO for both VBOs
- mVAO.create();
- mVAO.bind();
-
- // Bind the VBO of vertices
- mVBOVertices.bind();
-
- // Unbind the VAO
- mVAO.unbind();
-
- // Unbind the shader
- shader.unbind();
-}
-
void CollisionDetectionScene::selectNextShape() {
int previousIndex = mSelectedShapeIndex;
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
old mode 100644
new mode 100755
index 01df3df8..6eeeb25b
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -58,7 +58,7 @@ const int NB_RAYS = 100;
const float RAY_LENGTH = 30.0f;
const int NB_BODIES = 9;
-// Raycast manager
+// Contact manager
class ContactManager : public rp3d::CollisionCallback {
private:
@@ -66,16 +66,12 @@ class ContactManager : public rp3d::CollisionCallback {
/// All the visual contact points
std::vector<ContactPoint> mContactPoints;
- /// All the normals at contact points
- std::vector<Line*> mNormals;
-
/// Contact point mesh folder path
std::string mMeshFolderPath;
public:
- ContactManager(openglframework::Shader& shader,
- const std::string& meshFolderPath)
+ ContactManager(openglframework::Shader& shader, const std::string& meshFolderPath)
: mMeshFolderPath(meshFolderPath) {
}
@@ -87,22 +83,20 @@ class ContactManager : public rp3d::CollisionCallback {
rp3d::ContactPointInfo* contactPointInfo = collisionCallbackInfo.contactManifold.getFirstContactPointInfo();
while (contactPointInfo != nullptr) {
+ // Contact normal
+ rp3d::Vector3 normal = contactPointInfo->normal;
+ openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z);
+
rp3d::Vector3 point1 = contactPointInfo->localPoint1;
point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
+
openglframework::Vector3 position1(point1.x, point1.y, point1.z);
- mContactPoints.push_back(ContactPoint(position1));
-
+ mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red()));
rp3d::Vector3 point2 = contactPointInfo->localPoint2;
point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
openglframework::Vector3 position2(point2.x, point2.y, point2.z);
- mContactPoints.push_back(ContactPoint(position2));
-
- // Create a line to display the normal at hit point
- rp3d::Vector3 n = contactPointInfo->normal;
- openglframework::Vector3 normal(n.x, n.y, n.z);
- Line* normalLine = new Line(position1, position1 + normal);
- mNormals.push_back(normalLine);
+ mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue()));
contactPointInfo = contactPointInfo->next;
}
@@ -111,13 +105,6 @@ class ContactManager : public rp3d::CollisionCallback {
void resetPoints() {
mContactPoints.clear();
-
- // Destroy all the normals
- for (std::vector<Line*>::iterator it = mNormals.begin();
- it != mNormals.end(); ++it) {
- delete (*it);
- }
- mNormals.clear();
}
std::vector<ContactPoint> getContactPoints() const {
@@ -146,7 +133,6 @@ class CollisionDetectionScene : public SceneDemo {
Sphere* mSphere2;
Capsule* mCapsule1;
Capsule* mCapsule2;
- Box* mBox1;
//Cone* mCone;
//Cylinder* mCylinder;
//Capsule* mCapsule;
@@ -162,18 +148,6 @@ class CollisionDetectionScene : public SceneDemo {
/// Collision world used for the physics simulation
rp3d::CollisionWorld* mCollisionWorld;
- /// All the points to render the lines
- std::vector<openglframework::Vector3> mLinePoints;
-
- /// Vertex Buffer Object for the vertices data
- openglframework::VertexBufferObject mVBOVertices;
-
- /// Vertex Array Object for the vertex data
- openglframework::VertexArrayObject mVAO;
-
- /// Create the Vertex Buffer Objects used to render with OpenGL.
- void createVBOAndVAO(openglframework::Shader& shader);
-
/// Select the next shape
void selectNextShape();
diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp
index c403ebb2..d420073f 100644
--- a/testbed/scenes/raycast/RaycastScene.cpp
+++ b/testbed/scenes/raycast/RaycastScene.cpp
@@ -124,7 +124,7 @@ RaycastScene::RaycastScene(const std::string& name)
createLines();
// Create the VBO and VAO to render the lines
- createVBOAndVAO(mPhongShader);
+ createVBOAndVAO();
changeBody();
}
@@ -199,9 +199,6 @@ void RaycastScene::reset() {
// Destructor
RaycastScene::~RaycastScene() {
- // Destroy the shader
- mPhongShader.destroy();
-
// Destroy the box rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody());
delete mBox;
@@ -293,40 +290,33 @@ void RaycastScene::update() {
}
// Render the scene
-void RaycastScene::renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+void RaycastScene::renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the VAO
mVAO.bind();
// Bind the shader
- shader.bind();
+ mColorShader.bind();
mVBOVertices.bind();
// Set the model to camera matrix
const Matrix4 localToCameraMatrix = Matrix4::identity();
- shader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix);
- shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
-
- // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
- // model-view matrix)
- const openglframework::Matrix3 normalMatrix =
- localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
- shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
+ mColorShader.setMatrix4x4Uniform("localToWorldMatrix", localToCameraMatrix);
+ mColorShader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
// Set the vertex color
- openglframework::Vector4 color(1, 0, 0, 1);
- shader.setVector4Uniform("vertexColor", color, false);
+ openglframework::Vector4 color(1, 0.55, 0, 1);
+ mColorShader.setVector4Uniform("vertexColor", color, false);
// Get the location of shader attribute variables
- GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
+ GLint vertexPositionLoc = mColorShader.getAttribLocation("vertexPosition");
glEnableVertexAttribArray(vertexPositionLoc);
glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL);
// Draw the lines
- glDrawArrays(GL_LINES, 0, NB_RAYS);
+ glDrawArrays(GL_LINES, 0, mLinePoints.size() * 2);
glDisableVertexAttribArray(vertexPositionLoc);
@@ -335,7 +325,7 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader,
// Unbind the VAO
mVAO.unbind();
- shader.unbind();
+ mColorShader.unbind();
// Render the shapes
if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
@@ -345,16 +335,11 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader,
if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
-
- shader.unbind();
}
// Create the Vertex Buffer Objects used to render with OpenGL.
/// We create two VBOs (one for vertices and one for indices)
-void RaycastScene::createVBOAndVAO(openglframework::Shader& shader) {
-
- // Bind the shader
- shader.bind();
+void RaycastScene::createVBOAndVAO() {
// Create the VBO for the vertices data
mVBOVertices.create();
@@ -372,9 +357,6 @@ void RaycastScene::createVBOAndVAO(openglframework::Shader& shader) {
// Unbind the VAO
mVAO.unbind();
-
- // Unbind the shader
- shader.unbind();
}
// Called when a keyboard event occurs
diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h
index be698e53..39b73799 100644
--- a/testbed/scenes/raycast/RaycastScene.h
+++ b/testbed/scenes/raycast/RaycastScene.h
@@ -82,13 +82,16 @@ class RaycastManager : public rp3d::RaycastCallback {
}
virtual rp3d::decimal notifyRaycastHit(const rp3d::RaycastInfo& raycastInfo) override {
+
+ rp3d::Vector3 n = raycastInfo.worldNormal;
+ openglframework::Vector3 normal(n.x, n.y, n.z);
+
rp3d::Vector3 hitPos = raycastInfo.worldPoint;
openglframework::Vector3 position(hitPos.x, hitPos.y, hitPos.z);
- mHitPoints.push_back(ContactPoint(position));
+ mHitPoints.push_back(ContactPoint(position, normal, openglframework::Color::red()));
// Create a line to display the normal at hit point
- rp3d::Vector3 n = raycastInfo.worldNormal;
- openglframework::Vector3 normal(n.x, n.y, n.z);
+ // TODO : Remove the mNormals because the VisualContactPoint is now able to display the contact normal on its own
Line* normalLine = new Line(position, position + normal);
mNormals.push_back(normalLine);
@@ -134,8 +137,6 @@ class RaycastScene : public SceneDemo {
/// True if the hit points normals are displayed
bool mAreNormalsDisplayed;
- /// Raycast manager
-
/// All objects on the scene
Box* mBox;
Sphere* mSphere;
@@ -161,7 +162,7 @@ class RaycastScene : public SceneDemo {
void createLines();
// Create the Vertex Buffer Objects used to render with OpenGL.
- void createVBOAndVAO(openglframework::Shader& shader);
+ void createVBOAndVAO();
public:
diff --git a/testbed/shaders/color.frag b/testbed/shaders/color.frag
new file mode 100644
index 00000000..7ba710f0
--- /dev/null
+++ b/testbed/shaders/color.frag
@@ -0,0 +1,38 @@
+#version 330
+
+/********************************************************************************
+* OpenGL-Framework *
+* Copyright (c) 2015 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Uniform variables
+uniform vec4 vertexColor; // Vertex color
+
+// Out variable
+out vec4 color; // Output color
+
+void main() {
+
+ // Compute the final color
+ color = vertexColor;
+}
diff --git a/testbed/shaders/color.vert b/testbed/shaders/color.vert
new file mode 100644
index 00000000..72083abc
--- /dev/null
+++ b/testbed/shaders/color.vert
@@ -0,0 +1,43 @@
+#version 330
+
+/********************************************************************************
+* OpenGL-Framework *
+* Copyright (c) 2015 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Uniform variables
+uniform mat4 localToWorldMatrix; // Local-space to world-space matrix
+uniform mat4 worldToCameraMatrix; // World-space to camera-space matrix
+uniform mat4 projectionMatrix; // Projection matrix
+
+// In variables
+in vec4 vertexPosition;
+
+void main() {
+
+ // Compute the vertex position
+ vec4 positionCameraSpace = worldToCameraMatrix * localToWorldMatrix * vertexPosition;
+
+ // Compute the clip-space vertex coordinates
+ gl_Position = projectionMatrix * positionCameraSpace;
+}
diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h
index d51ffec1..aa29b9e5 100644
--- a/testbed/src/Scene.h
+++ b/testbed/src/Scene.h
@@ -34,9 +34,12 @@ struct ContactPoint {
public:
openglframework::Vector3 point;
+ openglframework::Vector3 normal;
+ openglframework::Color color;
/// Constructor
- ContactPoint(const openglframework::Vector3& pointWorld) : point(pointWorld) {
+ ContactPoint(const openglframework::Vector3& pointWorld, const openglframework::Vector3& normalWorld, const openglframework::Color colorPoint)
+ : point(pointWorld), normal(normalWorld), color(colorPoint) {
}
};
diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp
index 1906d9f4..2cb3e440 100644
--- a/testbed/src/SceneDemo.cpp
+++ b/testbed/src/SceneDemo.cpp
@@ -46,6 +46,7 @@ SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMa
mDepthShader("shaders/depth.vert", "shaders/depth.frag"),
mPhongShader("shaders/phong.vert", "shaders/phong.frag"),
mQuadShader("shaders/quad.vert", "shaders/quad.frag"),
+ mColorShader("shaders/color.vert", "shaders/color.frag"),
mVBOQuad(GL_ARRAY_BUFFER), mMeshFolderPath("meshes/") {
shadowMapTextureLevel++;
@@ -84,6 +85,11 @@ SceneDemo::~SceneDemo() {
mFBOShadowMap.destroy();
mVBOQuad.destroy();
+ mDepthShader.destroy();
+ mPhongShader.destroy();
+ mQuadShader.destroy();
+ mColorShader.destroy();
+
// Destroy the contact points
removeAllContactPoints();
@@ -156,7 +162,7 @@ void SceneDemo::render() {
if (mIsShadowMappingEnabled) mShadowMapTexture.bind();
const GLuint textureUnit = 0;
- // Set the variables of the shader
+ // Set the variables of the phong shader
mPhongShader.setMatrix4x4Uniform("projectionMatrix", mCamera.getProjectionMatrix());
mPhongShader.setMatrix4x4Uniform("shadowMapProjectionMatrix", mShadowMapBiasMatrix * shadowMapProjMatrix);
mPhongShader.setMatrix4x4Uniform("worldToLight0CameraMatrix", worldToLightCameraMatrix);
@@ -166,6 +172,12 @@ void SceneDemo::render() {
mPhongShader.setIntUniform("shadowMapSampler", textureUnit);
mPhongShader.setIntUniform("isShadowEnabled", mIsShadowMappingEnabled);
mPhongShader.setVector2Uniform("shadowMapDimension", Vector2(SHADOWMAP_WIDTH, SHADOWMAP_HEIGHT));
+ mPhongShader.unbind();
+
+ // Set the variables of the color shader
+ mColorShader.bind();
+ mColorShader.setMatrix4x4Uniform("projectionMatrix", mCamera.getProjectionMatrix());
+ mColorShader.unbind();
// Set the viewport to render the scene
glViewport(mViewportX, mViewportY, mViewportWidth, mViewportHeight);
@@ -289,20 +301,19 @@ void SceneDemo::updateContactPoints() {
for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
// Create a visual contact point for rendering
- VisualContactPoint* point = new VisualContactPoint(it->point, mMeshFolderPath);
+ VisualContactPoint* point = new VisualContactPoint(it->point, mMeshFolderPath, it->point + it->normal, it->color);
mContactPoints.push_back(point);
}
}
}
// Render the contact points
-void SceneDemo::renderContactPoints(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
+void SceneDemo::renderContactPoints(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
// Render all the raycast hit points
for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
it != mContactPoints.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix);
+ (*it)->render(mColorShader, worldToCameraMatrix);
}
}
@@ -335,7 +346,9 @@ std::vector<ContactPoint> SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn
rp3d::ContactPoint* contactPoint = manifold->getContactPoint(i);
rp3d::Vector3 point = contactPoint->getWorldPointOnBody1();
- ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z));
+ rp3d::Vector3 normalWorld = contactPoint->getNormal();
+ openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z);
+ ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red());
contactPoints.push_back(contact);
}
diff --git a/testbed/src/SceneDemo.h b/testbed/src/SceneDemo.h
index c64c8a9b..f7b5f7ce 100644
--- a/testbed/src/SceneDemo.h
+++ b/testbed/src/SceneDemo.h
@@ -73,6 +73,9 @@ class SceneDemo : public Scene {
/// Phong shader
openglframework::Shader mPhongShader;
+ /// Constant color shader
+ openglframework::Shader mColorShader;
+
// TODO : Delete this
openglframework::Shader mQuadShader;
From 730b68787735cf246a23b18c1ed78332520b026c Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 29 May 2017 08:32:10 +0200
Subject: [PATCH 050/133] Working on temporal coherence in SAT (polyhedron vs
polyhedron)
---
CMakeLists.txt | 2 +
src/collision/CollisionDetection.cpp | 13 +-
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 14 +-
...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 48 +++
...vexPolyhedronVsConvexPolyhedronAlgorithm.h | 71 ++++
.../narrowphase/DefaultCollisionDispatch.cpp | 9 +
.../narrowphase/DefaultCollisionDispatch.h | 16 +-
.../narrowphase/GJK/GJKAlgorithm.cpp | 11 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 333 +++++++++++++-----
src/collision/narrowphase/SAT/SATAlgorithm.h | 15 +-
.../SphereVsConvexPolyhedronAlgorithm.cpp | 12 +-
.../SphereVsConvexPolyhedronAlgorithm.h | 2 +-
src/engine/OverlappingPair.cpp | 3 +-
src/engine/OverlappingPair.h | 68 +++-
14 files changed, 502 insertions(+), 115 deletions(-)
create mode 100644 src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
create mode 100644 src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5cacac60..2e3be144 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -88,6 +88,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp"
"src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h"
"src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp"
+ "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
+ "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp"
"src/collision/shapes/AABB.h"
"src/collision/shapes/AABB.cpp"
"src/collision/shapes/ConvexShape.h"
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 908c5bf5..9ce02f9d 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -241,7 +241,7 @@ void CollisionDetection::computeNarrowPhase() {
// If there is no collision algorithm between those two kinds of shapes, skip it
if (narrowPhaseAlgorithm != nullptr) {
-
+
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
@@ -268,7 +268,15 @@ void CollisionDetection::computeNarrowPhase() {
// Trigger a callback event for the new contact
if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo);
+
+ currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true;
}
+ else {
+ currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = false;
+ }
+
+ // The previous frame collision info is now valid
+ currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true;
}
currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
@@ -834,6 +842,9 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Report the contact to the user
callback->notifyContact(collisionInfo);
}
+
+ // The previous frame collision info is now valid
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true;
}
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index 5e8dd5f9..142a7fc9 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -44,6 +44,9 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
SATAlgorithm satAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
+
// If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
@@ -104,9 +107,11 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
break;
}
-
}
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+
// Return true
return true;
}
@@ -115,7 +120,12 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
- return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+ bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
+
+ return isColliding;
}
return false;
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
new file mode 100644
index 00000000..16b9edf5
--- /dev/null
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
@@ -0,0 +1,48 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
+#include "GJK/GJKAlgorithm.h"
+#include "SAT/SATAlgorithm.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Compute the narrow-phase collision detection between two convex polyhedra
+// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
+// by Dirk Gregorius.
+bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactManifoldInfo& contactManifoldInfo) {
+
+ // Run the SAT algorithm to find the separating axis and compute contact point
+ SATAlgorithm satAlgorithm;
+ bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+
+ return isColliding;
+}
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
new file mode 100644
index 00000000..ed76d6fc
--- /dev/null
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
@@ -0,0 +1,71 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H
+#define REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H
+
+// Libraries
+#include "body/Body.h"
+#include "constraint/ContactPoint.h"
+#include "NarrowPhaseAlgorithm.h"
+
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class ConvexPolyhedronVsConvexPolyhedronAlgorithm
+/**
+ * This class is used to compute the narrow-phase collision detection
+ * between two convex polyhedra.
+ */
+class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
+
+ protected :
+
+ public :
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ ConvexPolyhedronVsConvexPolyhedronAlgorithm() = default;
+
+ /// Destructor
+ virtual ~ConvexPolyhedronVsConvexPolyhedronAlgorithm() override = default;
+
+ /// Deleted copy-constructor
+ ConvexPolyhedronVsConvexPolyhedronAlgorithm(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
+
+ /// Deleted assignment operator
+ ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
+
+ /// Compute the narrow-phase collision detection between two convex polyhedra
+ virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactManifoldInfo& contactManifoldInfo) override;
+};
+
+}
+
+#endif
+
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
index 0a1e498c..df1828ad 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
@@ -56,6 +56,15 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
return &mSphereVsConvexPolyhedronAlgorithm;
}
+ // Capsule vs Convex Polyhedron algorithm
+ if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
+ return &mCapsuleVsConvexPolyhedronAlgorithm;
+ }
+ // Convex Polyhedron vs Convex Polyhedron algorithm
+ if (shape1Type == CollisionShapeType::CONVEX_POLYHEDRON &&
+ shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
+ return &mConvexPolyhedronVsConvexPolyhedronAlgorithm;
+ }
return nullptr;
}
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h
index 7c5d77ea..7b3945d3 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.h
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.h
@@ -33,6 +33,8 @@
#include "SphereVsConvexPolyhedronAlgorithm.h"
#include "SphereVsCapsuleAlgorithm.h"
#include "CapsuleVsCapsuleAlgorithm.h"
+#include "CapsuleVsConvexPolyhedronAlgorithm.h"
+#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
namespace reactphysics3d {
@@ -50,14 +52,20 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Sphere vs Sphere collision algorithm
SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
- /// Sphere vs Convex Mesh collision algorithm
- SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm;
+ /// Capsule vs Capsule collision algorithm
+ CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm;
/// Sphere vs Capsule collision algorithm
SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm;
- /// Capsule vs Capsule collision algorithm
- CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm;
+ /// Sphere vs Convex Polyhedron collision algorithm
+ SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm;
+
+ /// Capsule vs Convex Polyhedron collision algorithm
+ CapsuleVsConvexPolyhedronAlgorithm mCapsuleVsConvexPolyhedronAlgorithm;
+
+ /// Convex Polyhedron vs Convex Polyhedron collision algorithm
+ ConvexPolyhedronVsConvexPolyhedronAlgorithm mConvexPolyhedronVsConvexPolyhedronAlgorithm;
public:
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index 6be90769..4f7e1177 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -93,7 +93,14 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro
VoronoiSimplex simplex;
// Get the previous point V (last cached separating axis)
- Vector3 v = narrowPhaseInfo->overlappingPair->getCachedSeparatingAxis();
+ Vector3 v;
+ LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
+ if (lastFrameInfo.isValid && lastFrameInfo.wasUsingGJK) {
+ v = lastFrameInfo.gjkSeparatingAxis;
+ }
+ else {
+ v.setAllValues(0, 1, 0);
+ }
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
@@ -113,7 +120,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro
if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence
- narrowPhaseInfo->overlappingPair->setCachedSeparatingAxis(v);
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().gjkSeparatingAxis = v;
// No intersection, we return
return GJKResult::SEPARATED;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index c82256f3..6e85e303 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -29,6 +29,7 @@
#include "collision/PolyhedronMesh.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/SphereShape.h"
+#include "engine/OverlappingPair.h"
#include "configuration.h"
#include "engine/Profiler.h"
#include <algorithm>
@@ -329,7 +330,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
}
// Test collision between two convex polyhedrons
-bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo,
+ ContactManifoldInfo& contactManifoldInfo) const {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
@@ -344,94 +346,235 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
uint minFaceIndex = 0;
bool isMinPenetrationFaceNormal = false;
bool isMinPenetrationFaceNormalPolyhedron1 = false;
+ uint minSeparatingEdge1Index, minSeparatingEdge2Index;
Vector3 separatingEdge1A, separatingEdge1B;
Vector3 separatingEdge2A, separatingEdge2B;
Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
- // Test all the face normals of the polyhedron 1 for separating axis
- uint faceIndex;
- decimal penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
- if (penetrationDepth <= decimal(0.0)) {
+ LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
- // We have found a separating axis
- return false;
- }
- if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
- isMinPenetrationFaceNormal = true;
- minPenetrationDepth = penetrationDepth;
- minFaceIndex = faceIndex;
- isMinPenetrationFaceNormalPolyhedron1 = true;
- }
+ // True if the shapes were overlapping in the previous frame and are
+ // still overlapping on the same axis in this frame
+ bool isTemporalCoherenceValid = false;
- // Test all the face normals of the polyhedron 2 for separating axis
- penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
- if (penetrationDepth <= decimal(0.0)) {
+ // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
+ // frame collision data per triangle)
+ if (polyhedron1->getType() != CollisionShapeType::TRIANGLE && polyhedron2->getType() != CollisionShapeType::TRIANGLE) {
- // We have found a separating axis
- return false;
- }
- if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
- isMinPenetrationFaceNormal = true;
- minPenetrationDepth = penetrationDepth;
- minFaceIndex = faceIndex;
- isMinPenetrationFaceNormalPolyhedron1 = false;
- }
+ // If the last frame collision info is valid and was also using SAT algorithm
+ if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
- // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis
- for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) {
+ // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
+ // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
+ // the shapes are still separated along this axis, we directly exit with no collision.
- // Get an edge of polyhedron 1
- HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i);
-
- const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
- const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
- const Vector3 edge1Direction = edge1B - edge1A;
-
- for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) {
-
- // Get an edge of polyhedron 2
- HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j);
-
- const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
- const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
- const Vector3 edge2Direction = edge2B - edge2A;
-
- // If the two edges build a minkowski face (and the cross product is
- // therefore a candidate for separating axis
- if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
-
- Vector3 separatingAxisPolyhedron2Space;
-
- // Compute the penetration depth
- decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
- edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
+ // If the previous separating axis (or axis with minimum penetration depth)
+ // was a face normal of polyhedron 1
+ if (lastFrameInfo.satIsAxisFacePolyhedron1) {
+ decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2,
+ lastFrameInfo.satMinAxisFaceIndex);
+ // If the previous axis is a separating axis
if (penetrationDepth <= decimal(0.0)) {
- // We have found a separating axis
+ // Return no collision
return false;
}
- if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+
+ if (isTemporalCoherenceValid) {
minPenetrationDepth = penetrationDepth;
- isMinPenetrationFaceNormalPolyhedron1 = false;
- isMinPenetrationFaceNormal = false;
- separatingEdge1A = edge1A;
- separatingEdge1B = edge1B;
- separatingEdge2A = edge2A;
- separatingEdge2B = edge2B;
- minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
+ minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+ isMinPenetrationFaceNormal = true;
+ isMinPenetrationFaceNormalPolyhedron1 = true;
+ }
+ }
+ else if (lastFrameInfo.satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth)
+ // was a face normal of polyhedron 2
+
+ decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1,
+ lastFrameInfo.satMinAxisFaceIndex);
+ // If the previous axis is a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+
+ // Return no collision
+ return false;
}
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+
+ if (isTemporalCoherenceValid) {
+
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+ isMinPenetrationFaceNormal = true;
+ isMinPenetrationFaceNormalPolyhedron1 = false;
+ }
+ }
+ else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
+
+ HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameInfo.satMinEdge1Index);
+ HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameInfo.satMinEdge2Index);
+
+ // If the two edges build a minkowski face (and the cross product is
+ // therefore a candidate for separating axis
+ if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
+
+ Vector3 separatingAxisPolyhedron2Space;
+
+ const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
+ const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
+ const Vector3 edge1Direction = edge1B - edge1A;
+ const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
+ const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
+ const Vector3 edge2Direction = edge2B - edge2A;
+
+ // Compute the penetration depth
+ decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
+ edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
+
+ // If the previous axis is a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+
+ // Return no collision
+ return false;
+ }
+
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+
+ if (isTemporalCoherenceValid) {
+
+ minPenetrationDepth = penetrationDepth;
+ isMinPenetrationFaceNormal = false;
+ isMinPenetrationFaceNormalPolyhedron1 = false;
+ minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index;
+ minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index;
+ separatingEdge1A = edge1A;
+ separatingEdge1B = edge1B;
+ separatingEdge2A = edge2A;
+ separatingEdge2B = edge2B;
+ minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
+ }
+ }
}
}
}
+ // We the shapes are still overlapping in the same axis as in
+ // the previous frame, we skip the whole SAT algorithm
+ if (isTemporalCoherenceValid) {
+
+ // Test all the face normals of the polyhedron 1 for separating axis
+ uint faceIndex;
+ decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
+ if (penetrationDepth <= decimal(0.0)) {
+
+ lastFrameInfo.satIsAxisFacePolyhedron1 = true;
+ lastFrameInfo.satIsAxisFacePolyhedron2 = false;
+ lastFrameInfo.satMinAxisFaceIndex = faceIndex;
+
+ // We have found a separating axis
+ return false;
+ }
+ if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
+ isMinPenetrationFaceNormal = true;
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = faceIndex;
+ isMinPenetrationFaceNormalPolyhedron1 = true;
+ }
+
+ // Test all the face normals of the polyhedron 2 for separating axis
+ penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
+ if (penetrationDepth <= decimal(0.0)) {
+
+ lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+ lastFrameInfo.satIsAxisFacePolyhedron2 = true;
+ lastFrameInfo.satMinAxisFaceIndex = faceIndex;
+
+ // We have found a separating axis
+ return false;
+ }
+ if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
+ isMinPenetrationFaceNormal = true;
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = faceIndex;
+ isMinPenetrationFaceNormalPolyhedron1 = false;
+ }
+
+ // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis
+ for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) {
+
+ // Get an edge of polyhedron 1
+ HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i);
+
+ const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
+ const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
+ const Vector3 edge1Direction = edge1B - edge1A;
+
+ for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) {
+
+ // Get an edge of polyhedron 2
+ HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j);
+
+ const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
+ const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
+ const Vector3 edge2Direction = edge2B - edge2A;
+
+ // If the two edges build a minkowski face (and the cross product is
+ // therefore a candidate for separating axis
+ if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
+
+ Vector3 separatingAxisPolyhedron2Space;
+
+ // Compute the penetration depth
+ decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
+ edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
+
+ if (penetrationDepth <= decimal(0.0)) {
+
+ lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+ lastFrameInfo.satIsAxisFacePolyhedron2 = false;
+ lastFrameInfo.satMinEdge1Index = i;
+ lastFrameInfo.satMinEdge2Index = j;
+
+ // We have found a separating axis
+ return false;
+ }
+
+ if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
+
+ minPenetrationDepth = penetrationDepth;
+ isMinPenetrationFaceNormalPolyhedron1 = false;
+ isMinPenetrationFaceNormal = false;
+ minSeparatingEdge1Index = i;
+ minSeparatingEdge2Index = j;
+ separatingEdge1A = edge1A;
+ separatingEdge1B = edge1B;
+ separatingEdge2A = edge2A;
+ separatingEdge2B = edge2B;
+ minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
+ }
+
+ }
+ }
+ }
+ }
+
+ // Here we know the shapes are overlapping on a given minimum separating axis.
+ // Now, we will clip the shapes along this axis to find the contact points
+
assert(minPenetrationDepth > decimal(0.0));
assert((isMinPenetrationFaceNormal && minFaceIndex >= 0) || !isMinPenetrationFaceNormal);
- // If the separation axis is a face normal
+ // If the minimum separating axis is a face normal
if (isMinPenetrationFaceNormal) {
const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
@@ -439,6 +582,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2;
+ assert(minPenetrationDepth > decimal(0.0));
+
const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex);
const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
@@ -491,8 +636,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
} while (currentEdgeIndex != firstEdgeIndex);
+ assert(planesNormals.size() > 0);
+ assert(planesNormals.size() == planesPoints.size());
+
// Clip the reference faces with the adjacent planes of the reference face
std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
+ assert(clipPolygonVertices.size() > 0);
// We only keep the clipped points that are below the reference face
const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(firstEdgeIndex);
@@ -514,6 +663,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
}
}
+
+ lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
}
else { // If we have an edge vs edge contact
@@ -531,6 +684,11 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
// Create the contact point
contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
+
+ lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+ lastFrameInfo.satIsAxisFacePolyhedron2 = false;
+ lastFrameInfo.satMinEdge1Index = minSeparatingEdge1Index;
+ lastFrameInfo.satMinEdge2Index = minSeparatingEdge2Index;
}
return true;
@@ -583,34 +741,47 @@ decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const V
return -axis.dot(edge2A - edge1A);
}
+// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron
+decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
+ const ConvexPolyhedronShape* polyhedron2,
+ const Transform& polyhedron1ToPolyhedron2,
+ uint faceIndex) const {
+
+ HalfEdgeStructure::Face face = polyhedron1->getFace(faceIndex);
+
+ // Get the face normal
+ const Vector3 faceNormal = polyhedron1->getFaceNormal(faceIndex);
+
+ // Convert the face normal into the local-space of polyhedron 2
+ const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal;
+
+ // Get the support point of polyhedron 2 in the inverse direction of face normal
+ const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr);
+
+ // Compute the penetration depth
+ const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]);
+ decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space);
+
+ return penetrationDepth;
+}
+
// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
-decimal SATAlgorithm::testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
- const ConvexPolyhedronShape* polyhedron2,
- const Transform& polyhedron1ToPolyhedron2,
- uint& minFaceIndex) const {
+decimal SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
+ const ConvexPolyhedronShape* polyhedron2,
+ const Transform& polyhedron1ToPolyhedron2,
+ uint& minFaceIndex) const {
decimal minPenetrationDepth = DECIMAL_LARGEST;
// For each face of the first polyhedron
for (uint f = 0; f < polyhedron1->getNbFaces(); f++) {
- HalfEdgeStructure::Face face = polyhedron1->getFace(f);
-
- // Get the face normal
- const Vector3 faceNormal = polyhedron1->getFaceNormal(f);
-
- // Convert the face normal into the local-space of polyhedron 2
- const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal;
-
- // Get the support point of polyhedron 2 in the inverse direction of face normal
- const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr);
-
- // Compute the penetration depth
- const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]);
- decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space);
+ decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2,
+ polyhedron1ToPolyhedron2, f);
// If the penetration depth is negative, we have found a separating axis
if (penetrationDepth <= decimal(0.0)) {
+ minFaceIndex = f;
return penetrationDepth;
}
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index a95abe31..3512ef4d 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -66,6 +66,17 @@ class SATAlgorithm {
const Vector3& edge1Direction, const Vector3& edge2Direction,
Vector3& outSeparatingAxis) const;
+ /// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron
+ decimal testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
+ const ConvexPolyhedronShape* polyhedron2,
+ const Transform& polyhedron1ToPolyhedron2,
+ uint faceIndex) const;
+
+
+ /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
+ decimal testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
+ const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const;
+
public :
// -------------------- Methods -------------------- //
@@ -101,10 +112,6 @@ class SATAlgorithm {
/// Test collision between two convex meshes
bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
-
- /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
- decimal testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
- const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const;
};
}
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index 708d9f8d..bb43bd33 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -31,7 +31,7 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-// Compute the narrow-phase collision detection a sphere and a convex polyhedron
+// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
@@ -41,6 +41,9 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
GJKAlgorithm gjkAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
+
// If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
@@ -53,7 +56,12 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
- return satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+ bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+ narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
+
+ return isColliding;
}
return false;
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
index c1fd3e55..f2f8d6e5 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
@@ -60,7 +60,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
- /// Compute the narrow-phase collision detection a sphere and a convex polyhedron
+ /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
};
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index 1f5d894e..ce628b7d 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -32,7 +32,6 @@ using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
int nbMaxContactManifolds, PoolAllocator& memoryAllocator)
- : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds),
- mCachedSeparatingAxis(0.0, 1.0, 0.0) {
+ : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds) {
}
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index c34b91b7..ce028f9c 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -37,6 +37,51 @@ namespace reactphysics3d {
// Type for the overlapping pair ID
using overlappingpairid = std::pair<uint, uint>;
+// Structure LastFrameCollisionInfo
+/**
+ * This structure contains collision info about the last frame.
+ * This is used for temporal coherence between frames.
+ */
+struct LastFrameCollisionInfo {
+
+ /// True if we have information about the previous frame
+ bool isValid;
+
+ /// True if the two shapes were colliding in the previous frame
+ bool wasColliding;
+
+ /// True if we were using GJK algorithm to check for collision in the previous frame
+ bool wasUsingGJK;
+
+ /// True if we were using SAT algorithm to check for collision in the previous frame
+ bool wasUsingSAT;
+
+ /// True if there was a narrow-phase collision
+ /// in the previous frame
+ bool wasCollidingLastFrame;
+
+ // ----- GJK Algorithm -----
+
+ /// Previous separating axis
+ Vector3 gjkSeparatingAxis;
+
+ // SAT Algorithm
+ bool satIsAxisFacePolyhedron1;
+ bool satIsAxisFacePolyhedron2;
+ uint satMinAxisFaceIndex;
+ uint satMinEdge1Index;
+ uint satMinEdge2Index;
+
+ /// Constructor
+ LastFrameCollisionInfo() {
+
+ isValid = false;
+ wasColliding = false;
+ wasUsingSAT = false;
+ wasUsingGJK = false;
+ }
+};
+
// Class OverlappingPair
/**
* This class represents a pair of two proxy collision shapes that are overlapping
@@ -54,8 +99,8 @@ class OverlappingPair {
/// Set of persistent contact manifolds
ContactManifoldSet mContactManifoldSet;
- /// Cached previous separating axis
- Vector3 mCachedSeparatingAxis;
+ /// Collision information about the last frame (for temporal coherence)
+ LastFrameCollisionInfo mLastFrameCollisionInfo;
public:
@@ -83,11 +128,8 @@ class OverlappingPair {
/// Add a contact manifold
void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
- /// Return the cached separating axis
- Vector3 getCachedSeparatingAxis() const;
-
- /// Set the cached separating axis
- void setCachedSeparatingAxis(const Vector3& axis);
+ /// Return the last frame collision info
+ LastFrameCollisionInfo& getLastFrameCollisionInfo();
/// Return the number of contacts in the cache
uint getNbContactPoints() const;
@@ -124,17 +166,11 @@ inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& conta
mContactManifoldSet.addContactManifold(contactManifoldInfo);
}
-// Return the cached separating axis
-inline Vector3 OverlappingPair::getCachedSeparatingAxis() const {
- return mCachedSeparatingAxis;
+// Return the last frame collision info
+inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() {
+ return mLastFrameCollisionInfo;
}
-// Set the cached separating axis
-inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) {
- mCachedSeparatingAxis = axis;
-}
-
-
// Return the number of contact points in the contact manifold
inline uint OverlappingPair::getNbContactPoints() const {
return mContactManifoldSet.getTotalNbContactPoints();
From 6b0ba1cfbb217fd3c90394e48ba25d5cf3e2dc59 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 29 May 2017 22:30:30 +0200
Subject: [PATCH 051/133] Fix issues in collision detection
---
src/collision/CollisionDetection.cpp | 32 +++++++++++++------
src/collision/CollisionDetection.h | 2 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 14 ++++----
.../narrowphase/SphereVsCapsuleAlgorithm.cpp | 2 --
4 files changed, 31 insertions(+), 19 deletions(-)
mode change 100644 => 100755 src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 9ce02f9d..dc78d59e 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -407,10 +407,10 @@ void CollisionDetection::clearContactPoints() {
}
// Compute the middle-phase collision detection between two proxy shapes
-NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape* shape1, ProxyShape* shape2) {
+NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) {
- // Create a temporary overlapping pair
- OverlappingPair pair(shape1, shape2, 0, mMemoryAllocator);
+ ProxyShape* shape1 = pair->getShape1();
+ ProxyShape* shape2 = pair->getShape2();
// -------------------------------------------------------
@@ -424,7 +424,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape
// No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection
- narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(&pair, shape1->getCollisionShape(),
+ narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
shape2->getCachedCollisionData());
@@ -436,7 +436,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(ProxyShape
// Run the middle-phase collision detection algorithm to find the triangles of the concave
// shape we need to use during the narrow-phase collision detection
- computeConvexVsConcaveMiddlePhase(&pair, mMemoryAllocator, &narrowPhaseInfo);
+ computeConvexVsConcaveMiddlePhase(pair, mMemoryAllocator, &narrowPhaseInfo);
}
return narrowPhaseInfo;
@@ -503,8 +503,11 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType();
const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType();
+ // Create a temporary overlapping pair
+ OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator);
+
// Compute the middle-phase collision detection between the two shapes
- NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape);
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
bool isColliding = false;
@@ -591,8 +594,11 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType();
const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
+ // Create a temporary overlapping pair
+ OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator);
+
// Compute the middle-phase collision detection between the two shapes
- NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape);
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
bool isColliding = false;
@@ -666,8 +672,11 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
// Test if the AABBs of the two proxy shapes overlap
if (aabb1.testCollision(aabb2)) {
+ // Create a temporary overlapping pair
+ OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator);
+
// Compute the middle-phase collision detection between the two shapes
- NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(body1ProxyShape, body2ProxyShape);
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType();
const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType();
@@ -747,8 +756,11 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType();
const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
+ // Create a temporary overlapping pair
+ OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator);
+
// Compute the middle-phase collision detection between the two shapes
- NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(bodyProxyShape, proxyShape);
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
// For each narrow-phase info object
while (narrowPhaseInfo != nullptr) {
@@ -816,7 +828,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
// Compute the middle-phase collision detection between the two shapes
- NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(shape1, shape2);
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair);
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 29200ec7..3fe88812 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -136,7 +136,7 @@ class CollisionDetection {
NarrowPhaseInfo** firstNarrowPhaseInfo);
/// Compute the middle-phase collision detection between two proxy shapes
- NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(ProxyShape* shape1, ProxyShape* shape2);
+ NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(OverlappingPair* pair);
public :
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 6e85e303..91db1383 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -48,9 +48,10 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
- assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
- assert(!isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
- assert(!isSphereShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
+ narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
+ narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the capsule collision shapes
const SphereShape* sphere = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
@@ -116,9 +117,10 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
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);
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
+ narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE ||
+ narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
// Get the collision shapes
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
old mode 100644
new mode 100755
index cd437d6e..86255c2b
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
@@ -39,8 +39,6 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
- assert(!isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
- assert(!isSphereShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the collision shapes
const SphereShape* sphereShape = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
From 3ec8dddd9198c976899041a822125c6f5faafa29 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 29 May 2017 22:31:33 +0200
Subject: [PATCH 052/133] Add box shapes in collision detection scene of
testbed application
---
.../CollisionDetectionScene.cpp | 46 +++++++++++++++----
.../CollisionDetectionScene.h | 2 +
2 files changed, 40 insertions(+), 8 deletions(-)
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index 21a0b51a..6351028a 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -50,10 +50,10 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mCollisionWorld = new rp3d::CollisionWorld();
// ---------- Sphere 1 ---------- //
- openglframework::Vector3 position1(0, 0, 0);
+ openglframework::Vector3 position1(12, 0, 0);
// Create a sphere and a corresponding collision body in the dynamics world
- mSphere1 = new Sphere(6, position1, mCollisionWorld, mMeshFolderPath);
+ mSphere1 = new Sphere(4, position1, mCollisionWorld, mMeshFolderPath);
mAllShapes.push_back(mSphere1);
// Set the color
@@ -61,10 +61,10 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mSphere1->setSleepingColor(mRedColorDemo);
// ---------- Sphere 2 ---------- //
- openglframework::Vector3 position2(4, 0, 0);
+ openglframework::Vector3 position2(0, 0, 0);
// Create a sphere and a corresponding collision body in the dynamics world
- mSphere2 = new Sphere(4, position2, mCollisionWorld, mMeshFolderPath);
+ mSphere2 = new Sphere(2, position2, mCollisionWorld, mMeshFolderPath);
mAllShapes.push_back(mSphere2);
// Set the color
@@ -72,7 +72,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mSphere2->setSleepingColor(mRedColorDemo);
// ---------- Capsule 1 ---------- //
- openglframework::Vector3 position3(4, 0, 0);
+ openglframework::Vector3 position3(8, 0, 0);
// Create a cylinder and a corresponding collision body in the dynamics world
mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath);
@@ -83,7 +83,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mCapsule1->setSleepingColor(mRedColorDemo);
// ---------- Capsule 2 ---------- //
- openglframework::Vector3 position4(-4, 0, 0);
+ openglframework::Vector3 position4(-8, 0, 0);
// Create a cylinder and a corresponding collision body in the dynamics world
mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position4, mCollisionWorld, mMeshFolderPath);
@@ -93,6 +93,28 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mCapsule2->setColor(mGreyColorDemo);
mCapsule2->setSleepingColor(mRedColorDemo);
+ // ---------- Box 1 ---------- //
+ openglframework::Vector3 position5(0, -12, 0);
+
+ // Create a cylinder and a corresponding collision body in the dynamics world
+ mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath);
+ mAllShapes.push_back(mBox1);
+
+ // Set the color
+ mBox1->setColor(mGreyColorDemo);
+ mBox1->setSleepingColor(mRedColorDemo);
+
+ // ---------- Box 2 ---------- //
+ openglframework::Vector3 position6(0, 12, 0);
+
+ // Create a cylinder and a corresponding collision body in the dynamics world
+ mBox2 = new Box(openglframework::Vector3(3, 2, 5), position6, mCollisionWorld, mMeshFolderPath);
+ mAllShapes.push_back(mBox2);
+
+ // Set the color
+ mBox2->setColor(mGreyColorDemo);
+ mBox2->setSleepingColor(mRedColorDemo);
+
// ---------- Cone ---------- //
//openglframework::Vector3 position4(0, 0, 0);
@@ -174,6 +196,12 @@ CollisionDetectionScene::~CollisionDetectionScene() {
mCollisionWorld->destroyCollisionBody(mCapsule2->getCollisionBody());
delete mCapsule2;
+ mCollisionWorld->destroyCollisionBody(mBox1->getCollisionBody());
+ delete mBox1;
+
+ mCollisionWorld->destroyCollisionBody(mBox2->getCollisionBody());
+ delete mBox2;
+
/*
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody());
@@ -250,6 +278,8 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mBox2->getCollisionBody()->isActive()) mBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
/*
if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
@@ -286,8 +316,8 @@ bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, i
return true;
}
- float stepDist = 0.5f;
- float stepAngle = 20 * (3.14f / 180.0f);
+ float stepDist = 0.2f;
+ float stepAngle = 15 * (3.14f / 180.0f);
if (key == GLFW_KEY_RIGHT && action == GLFW_PRESS) {
rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index 6eeeb25b..51a05412 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -133,6 +133,8 @@ class CollisionDetectionScene : public SceneDemo {
Sphere* mSphere2;
Capsule* mCapsule1;
Capsule* mCapsule2;
+ Box* mBox1;
+ Box* mBox2;
//Cone* mCone;
//Cylinder* mCylinder;
//Capsule* mCapsule;
From b1597c508fcfae1cd733ece9bfabfb65a1c59694 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 31 May 2017 07:36:39 +0200
Subject: [PATCH 053/133] Working on temporal coherence in SAT algorithm
---
.../narrowphase/SAT/SATAlgorithm.cpp | 385 +++++++++++++-----
src/collision/narrowphase/SAT/SATAlgorithm.h | 19 +
2 files changed, 305 insertions(+), 99 deletions(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 91db1383..767aff8a 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -71,27 +71,64 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint minFaceIndex = 0;
- // For each face of the convex mesh
- for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
+ // True if the shapes were overlapping in the previous frame and are
+ // still overlapping on the same axis in this frame
+ bool isTemporalCoherenceValid = false;
- // Get the face
- HalfEdgeStructure::Face face = polyhedron->getFace(f);
+ LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
- // Get the face normal
- const Vector3 faceNormal = polyhedron->getFaceNormal(f);
+ // If the last frame collision info is valid and was also using SAT algorithm
+ if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
- Vector3 sphereCenterToFacePoint = polyhedron->getVertexPosition(face.faceVertices[0]) - sphereCenter;
- decimal penetrationDepth = sphereCenterToFacePoint.dot(faceNormal) + sphere->getRadius();
+ // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
+ // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
+ // the shapes are still separated along this axis, we directly exit with no collision.
- // If the penetration depth is negative, we have found a separating axis
+ // Compute the penetration depth of the shapes along the face normal direction
+ decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
+ sphere, sphereCenter);
+
+ // If the previous axis is a separating axis
if (penetrationDepth <= decimal(0.0)) {
+
+ // Return no collision
return false;
}
- // Check if we have found a new minimum penetration axis
- if (penetrationDepth < minPenetrationDepth) {
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+
+ if (isTemporalCoherenceValid) {
+
minPenetrationDepth = penetrationDepth;
- minFaceIndex = f;
+ minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+ }
+ }
+
+ // We the shapes are still overlapping in the same axis as in
+ // the previous frame, we skip the whole SAT algorithm
+ if (!isTemporalCoherenceValid) {
+
+ // For each face of the convex mesh
+ for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
+
+ // Compute the penetration depth of the shapes along the face normal direction
+ decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(f, polyhedron, sphere, sphereCenter);
+
+ // If the penetration depth is negative, we have found a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+
+ lastFrameInfo.satMinAxisFaceIndex = f;
+
+ return false;
+ }
+
+ // Check if we have found a new minimum penetration axis
+ if (penetrationDepth < minPenetrationDepth) {
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = f;
+ }
}
}
@@ -109,9 +146,27 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
+ lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
+
return true;
}
+// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction
+decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron,
+ const SphereShape* sphere, const Vector3& sphereCenter) const {
+
+ // Get the face
+ HalfEdgeStructure::Face face = polyhedron->getFace(faceIndex);
+
+ // Get the face normal
+ const Vector3 faceNormal = polyhedron->getFaceNormal(faceIndex);
+
+ Vector3 sphereCenterToFacePoint = polyhedron->getVertexPosition(face.faceVertices[0]) - sphereCenter;
+ decimal penetrationDepth = sphereCenterToFacePoint.dot(faceNormal) + sphere->getRadius();
+
+ return penetrationDepth;
+}
+
// Test collision between a capsule and a convex mesh
bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
@@ -131,6 +186,11 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
+ // Compute the end-points of the inner segment of the capsule
+ const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
+ const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
+ const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA;
+
// Minimum penetration depth
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint minFaceIndex = 0;
@@ -140,80 +200,151 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
Vector3 separatingPolyhedronEdgeVertex1;
Vector3 separatingPolyhedronEdgeVertex2;
- // For each face of the convex mesh
- for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
+ // True if the shapes were overlapping in the previous frame and are
+ // still overlapping on the same axis in this frame
+ bool isTemporalCoherenceValid = false;
- // Get the face
- HalfEdgeStructure::Face face = polyhedron->getFace(f);
+ LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
- // Get the face normal
- const Vector3 faceNormal = polyhedron->getFaceNormal(f);
+ // If the last frame collision info is valid and was also using SAT algorithm
+ if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
- // Compute the penetration depth (using the capsule support in the direction opposite to the face normal)
- const Vector3 faceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
- const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-faceNormalCapsuleSpace, nullptr);
- const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]);
- const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint;
- const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal);
+ // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
+ // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
+ // the shapes are still separated along this axis, we directly exit with no collision.
- // If the penetration depth is negative, we have found a separating axis
- if (penetrationDepth <= decimal(0.0)) {
- return false;
+ // If the previous minimum separation axis was a face normal of the polyhedron
+ if (lastFrameInfo.satIsAxisFacePolyhedron1) {
+
+ Vector3 outFaceNormalCapsuleSpace;
+
+ // Compute the penetration depth along the polyhedron face normal direction
+ const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
+ capsuleShape, polyhedronToCapsuleTransform,
+ outFaceNormalCapsuleSpace);
+
+ // If the previous axis is a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+
+ // Return no collision
+ return false;
+ }
+
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+
+ if (isTemporalCoherenceValid) {
+
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+ isMinPenetrationFaceNormal = true;
+ separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace;
+ }
}
+ else { // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron
- // Check if we have found a new minimum penetration axis
- if (penetrationDepth < minPenetrationDepth) {
- minPenetrationDepth = penetrationDepth;
- minFaceIndex = f;
- isMinPenetrationFaceNormal = true;
- separatingAxisCapsuleSpace = faceNormalCapsuleSpace;
+ // Get an edge from the polyhedron (convert it into the capsule local-space)
+ HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index);
+ const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
+ const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
+ const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
+
+ Vector3 outAxis;
+
+ // Compute the penetration depth along this axis
+ const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
+ capsuleSegmentAxis, edgeVertex1,
+ edgeDirectionCapsuleSpace,
+ polyhedronToCapsuleTransform,
+ outAxis);
+
+ // If the previous axis is a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+
+ // Return no collision
+ return false;
+ }
+
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+
+ if (isTemporalCoherenceValid) {
+
+ minPenetrationDepth = penetrationDepth;
+ minEdgeIndex = lastFrameInfo.satMinEdge1Index;
+ isMinPenetrationFaceNormal = false;
+ separatingAxisCapsuleSpace = outAxis;
+ separatingPolyhedronEdgeVertex1 = edgeVertex1;
+ separatingPolyhedronEdgeVertex2 = edgeVertex2;
+ }
}
}
- // Compute the end-points of the inner segment of the capsule
- const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
- const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
- const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA;
+ // We the shapes are still overlapping in the same axis as in
+ // the previous frame, we skip the whole SAT algorithm
+ if (!isTemporalCoherenceValid) {
- // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron
- for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) {
+ // For each face of the convex mesh
+ for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
- // Get an edge from the polyhedron (convert it into the capsule local-space)
- HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e);
- const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
- const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
- const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
+ Vector3 outFaceNormalCapsuleSpace;
- HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
- const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex);
- const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex);
+ // Compute the penetration depth
+ const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(f, polyhedron, capsuleShape,
+ polyhedronToCapsuleTransform,
+ outFaceNormalCapsuleSpace);
- // Check using the Gauss Map if this edge cross product can be as separating axis
- if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) {
+ // If the penetration depth is negative, we have found a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
- // Compute the axis to test (cross product between capsule inner segment and polyhedron edge)
- Vector3 axis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace);
+ lastFrameInfo.satIsAxisFacePolyhedron1 = true;
+ lastFrameInfo.satMinAxisFaceIndex = f;
- // Skip separating axis test if polyhedron edge is parallel to the capsule inner segment
- if (axis.lengthSquare() >= decimal(0.00001)) {
+ return false;
+ }
- const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid();
- const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1;
+ // Check if we have found a new minimum penetration axis
+ if (penetrationDepth < minPenetrationDepth) {
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = f;
+ isMinPenetrationFaceNormal = true;
+ separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace;
+ }
+ }
- // Swap axis direction if necessary such that it points out of the polyhedron
- if (axis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) {
- axis = -axis;
- }
+ // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron
+ for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) {
- axis.normalize();
+ // Get an edge from the polyhedron (convert it into the capsule local-space)
+ HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e);
+ const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
+ const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
+ const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
+
+ HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
+ const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex);
+ const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex);
+
+ // Check using the Gauss Map if this edge cross product can be as separating axis
+ if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) {
+
+ Vector3 outAxis;
// Compute the penetration depth
- const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-axis, nullptr);
- const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint;
- const decimal penetrationDepth = capsuleSupportPointToEdgePoint.dot(axis);
+ const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
+ capsuleSegmentAxis, edgeVertex1,
+ edgeDirectionCapsuleSpace,
+ polyhedronToCapsuleTransform,
+ outAxis);
// If the penetration depth is negative, we have found a separating axis
if (penetrationDepth <= decimal(0.0)) {
+
+ lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+ lastFrameInfo.satMinEdge1Index = e;
+
return false;
}
@@ -222,12 +353,13 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
minPenetrationDepth = penetrationDepth;
minEdgeIndex = e;
isMinPenetrationFaceNormal = false;
- separatingAxisCapsuleSpace = axis;
+ separatingAxisCapsuleSpace = outAxis;
separatingPolyhedronEdgeVertex1 = edgeVertex1;
separatingPolyhedronEdgeVertex2 = edgeVertex2;
}
}
}
+
}
// Convert the inner capsule segment points into the polyhedron local-space
@@ -246,6 +378,9 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
contactManifoldInfo, isCapsuleShape1);
+
+ lastFrameInfo.satIsAxisFacePolyhedron1 = true;
+ lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
}
else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
@@ -264,11 +399,68 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
+
+ lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+ lastFrameInfo.satMinEdge1Index = minEdgeIndex;
}
return true;
}
+// Compute the penetration depth when the separating axis is the cross product of polyhedron edge and capsule inner segment
+decimal SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const ConvexPolyhedronShape* polyhedron, const CapsuleShape* capsule,
+ const Vector3& capsuleSegmentAxis, const Vector3& edgeVertex1,
+ const Vector3& edgeDirectionCapsuleSpace,
+ const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const {
+
+ decimal penetrationDepth = DECIMAL_LARGEST;
+
+ // Compute the axis to test (cross product between capsule inner segment and polyhedron edge)
+ outAxis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace);
+
+ // Skip separating axis test if polyhedron edge is parallel to the capsule inner segment
+ if (outAxis.lengthSquare() >= decimal(0.00001)) {
+
+ const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid();
+ const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1;
+
+ // Swap axis direction if necessary such that it points out of the polyhedron
+ if (outAxis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) {
+ outAxis = -outAxis;
+ }
+
+ outAxis.normalize();
+
+ // Compute the penetration depth
+ const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outAxis, nullptr);
+ const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint;
+ penetrationDepth = capsuleSupportPointToEdgePoint.dot(outAxis);
+ }
+
+ return penetrationDepth;
+}
+
+// Compute the penetration depth between the face of a polyhedron and a capsule along the polyhedron face normal direction
+decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhedronFaceIndex, const ConvexPolyhedronShape* polyhedron,
+ const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform,
+ Vector3& outFaceNormalCapsuleSpace) const {
+
+ // Get the face
+ HalfEdgeStructure::Face face = polyhedron->getFace(polyhedronFaceIndex);
+
+ // Get the face normal
+ const Vector3 faceNormal = polyhedron->getFaceNormal(polyhedronFaceIndex);
+
+ // Compute the penetration depth (using the capsule support in the direction opposite to the face normal)
+ outFaceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
+ 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);
+
+ return penetrationDepth;
+}
+
// Compute the two contact points between a polyhedron and a capsule when the separating
// axis is a face normal of the polyhedron
void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
@@ -424,47 +616,42 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameInfo.satMinEdge1Index);
HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameInfo.satMinEdge2Index);
- // If the two edges build a minkowski face (and the cross product is
- // therefore a candidate for separating axis
- if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
+ Vector3 separatingAxisPolyhedron2Space;
- Vector3 separatingAxisPolyhedron2Space;
+ const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
+ const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
+ const Vector3 edge1Direction = edge1B - edge1A;
+ const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
+ const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
+ const Vector3 edge2Direction = edge2B - edge2A;
- const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
- const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
- const Vector3 edge1Direction = edge1B - edge1A;
- const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
- const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
- const Vector3 edge2Direction = edge2B - edge2A;
+ // Compute the penetration depth
+ decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
+ edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
- // Compute the penetration depth
- decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
- edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
+ // If the previous axis is a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
- // If the previous axis is a separating axis
- if (penetrationDepth <= decimal(0.0)) {
+ // Return no collision
+ return false;
+ }
- // Return no collision
- return false;
- }
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameInfo.wasColliding;
- // The two shapes are overlapping as in the previous frame and on the same axis, therefore
- // we will skip the entire SAT algorithm because the minimum separating axis did not change
- isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+ if (isTemporalCoherenceValid) {
- if (isTemporalCoherenceValid) {
-
- minPenetrationDepth = penetrationDepth;
- isMinPenetrationFaceNormal = false;
- isMinPenetrationFaceNormalPolyhedron1 = false;
- minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index;
- minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index;
- separatingEdge1A = edge1A;
- separatingEdge1B = edge1B;
- separatingEdge2A = edge2A;
- separatingEdge2B = edge2B;
- minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
- }
+ minPenetrationDepth = penetrationDepth;
+ isMinPenetrationFaceNormal = false;
+ isMinPenetrationFaceNormalPolyhedron1 = false;
+ minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index;
+ minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index;
+ separatingEdge1A = edge1A;
+ separatingEdge1B = edge1B;
+ separatingEdge2A = edge2A;
+ separatingEdge2B = edge2B;
+ minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
}
}
}
@@ -472,7 +659,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
// We the shapes are still overlapping in the same axis as in
// the previous frame, we skip the whole SAT algorithm
- if (isTemporalCoherenceValid) {
+ if (!isTemporalCoherenceValid) {
// Test all the face normals of the polyhedron 1 for separating axis
uint faceIndex;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 3512ef4d..7a002fce 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -31,9 +31,13 @@
#include "collision/NarrowPhaseInfo.h"
#include "collision/shapes/ConvexPolyhedronShape.h"
+
/// ReactPhysics3D namespace
namespace reactphysics3d {
+class CapsuleShape;
+class SphereShape;
+
// Class SATAlgorithm
class SATAlgorithm {
@@ -77,6 +81,21 @@ class SATAlgorithm {
decimal testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const;
+ /// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction
+ decimal computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron,
+ const SphereShape* sphere, const Vector3& sphereCenter) const;
+
+ /// Compute the penetration depth between the face of a polyhedron and a capsule along the polyhedron face normal direction
+ decimal computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhedronFaceIndex, const ConvexPolyhedronShape* polyhedron,
+ const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform,
+ Vector3& outFaceNormalCapsuleSpace) const;
+
+ /// Compute the penetration depth when the separating axis is the cross product of polyhedron edge and capsule inner segment
+ decimal computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const ConvexPolyhedronShape* polyhedron, const CapsuleShape* capsule,
+ const Vector3& capsuleSegmentAxis, const Vector3& edgeVertex1,
+ const Vector3& edgeDirectionCapsuleSpace,
+ const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const;
+
public :
// -------------------- Methods -------------------- //
From 95db87fd624fd2cde6b6f4f750eda8ba7a4bd205 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 5 Jun 2017 00:05:49 +0200
Subject: [PATCH 054/133] Working on contacts reduction
---
CMakeLists.txt | 1 +
src/collision/CollisionDetection.cpp | 13 +-
src/collision/ContactManifold.h | 3 -
src/collision/ContactManifoldInfo.cpp | 278 ++++++++++++++++++++++++++
src/collision/ContactManifoldInfo.h | 72 ++-----
5 files changed, 306 insertions(+), 61 deletions(-)
create mode 100644 src/collision/ContactManifoldInfo.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2e3be144..cba141df 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -62,6 +62,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/body/RigidBody.cpp"
"src/collision/ContactPointInfo.h"
"src/collision/ContactManifoldInfo.h"
+ "src/collision/ContactManifoldInfo.cpp"
"src/collision/broadphase/BroadPhaseAlgorithm.h"
"src/collision/broadphase/BroadPhaseAlgorithm.cpp"
"src/collision/broadphase/DynamicAABBTree.h"
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index dc78d59e..0e7c8078 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -248,8 +248,8 @@ void CollisionDetection::computeNarrowPhase() {
ContactManifoldInfo contactManifoldInfo(mSingleFrameAllocator);
if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactManifoldInfo)) {
- // Reduce the number of points in the contact manifold
- contactManifoldInfo.reduce();
+ // Reduce the number of points in the contact manifold (if necessary)
+ contactManifoldInfo.reduce(currentNarrowPhaseInfo->shape1ToWorldTransform);
// If it is the first contact since the pairs are overlapping
if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) {
@@ -696,6 +696,9 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
+ // Reduce the number of points in the contact manifold (if necessary)
+ contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
+
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body1, body2,
body1ProxyShape, body2ProxyShape);
@@ -777,6 +780,9 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
+ // Reduce the number of points in the contact manifold (if necessary)
+ contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
+
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body,
proxyShape->getBody(), bodyProxyShape,
proxyShape);
@@ -848,6 +854,9 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
+ // Reduce the number of points in the contact manifold (if necessary)
+ contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
+
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, shape1->getBody(),
shape2->getBody(), shape1, shape2);
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index da323c0f..893a84a7 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -37,9 +37,6 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
-// Constants
-const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
-
// Class declarations
class ContactManifold;
diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp
new file mode 100644
index 00000000..c06785ea
--- /dev/null
+++ b/src/collision/ContactManifoldInfo.cpp
@@ -0,0 +1,278 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "ContactManifoldInfo.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator)
+ : mContactPointsList(nullptr), mAllocator(allocator), mNbContactPoints(0) {}
+
+// Destructor
+ContactManifoldInfo::~ContactManifoldInfo() {
+
+ // Remove all the contact points
+ reset();
+}
+
+// Add a new contact point into the manifold
+void ContactManifoldInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth,
+ const Vector3& localPt1, const Vector3& localPt2) {
+
+ assert(penDepth > decimal(0.0));
+
+ // Create the contact point info
+ ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo)))
+ ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
+
+ // Add it into the linked list of contact points
+ contactPointInfo->next = mContactPointsList;
+ mContactPointsList = contactPointInfo;
+
+ mNbContactPoints++;
+}
+
+// Remove all the contact points
+void ContactManifoldInfo::reset() {
+
+ // Delete all the contact points in the linked list
+ ContactPointInfo* element = mContactPointsList;
+ while(element != nullptr) {
+ ContactPointInfo* elementToDelete = element;
+ element = element->next;
+
+ // Delete the current element
+ mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
+ }
+
+ mContactPointsList = nullptr;
+ mNbContactPoints = 0;
+}
+
+// Reduce the number of points in the contact manifold
+// This is based on the technique described by Dirk Gregorius in his
+// "Contacts Creation" GDC presentation
+void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
+
+ assert(mContactPointsList != nullptr);
+
+ // TODO : Implement this (do not forget to deallocate removed points)
+
+ // The following algorithm only works to reduce to 4 contact points
+ assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
+
+ // If there are too many contact points in the manifold
+ if (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
+
+ ContactPointInfo* pointsToKeep[MAX_CONTACT_POINTS_IN_MANIFOLD];
+
+ // Compute the initial contact point we need to keep.
+ // The first point we keep is always the point in a given
+ // constant direction (in order to always have same contact points
+ // between frames for better stability)
+
+ const Transform worldToShape1Transform = shape1ToWorldTransform.getInverse();
+
+ // Compute the contact normal of the manifold (we use the first contact point)
+ // in the local-space of the first collision shape
+ const Vector3 contactNormalShape1Space = worldToShape1Transform.getOrientation() * mContactPointsList->normal;
+
+ // Compute a search direction
+ const Vector3 searchDirection(1, 1, 1);
+ ContactPointInfo* element = mContactPointsList;
+ pointsToKeep[0] = element;
+ decimal maxDotProduct = searchDirection.dot(element->localPoint1);
+ element = element->next;
+ while(element != nullptr) {
+
+ decimal dotProduct = searchDirection.dot(element->localPoint1);
+ if (dotProduct > maxDotProduct) {
+ maxDotProduct = dotProduct;
+ pointsToKeep[0] = element;
+ }
+ element = element->next;
+ }
+
+ // Compute the second contact point we need to keep.
+ // The second point we keep is the one farthest away from the first point.
+
+ decimal maxDistance = decimal(0.0);
+ element = mContactPointsList;
+ while(element != nullptr) {
+
+ if (element == pointsToKeep[0]) {
+ element = element->next;
+ continue;
+ }
+
+ decimal distance = (pointsToKeep[0]->localPoint1 - element->localPoint1).lengthSquare();
+ if (distance >= maxDistance) {
+ maxDistance = distance;
+ pointsToKeep[1] = element;
+ }
+ element = element->next;
+ }
+ assert(pointsToKeep[1] != nullptr);
+
+ // Compute the third contact point we need to keep.
+ // The second point is the one producing the triangle with the larger area
+ // with first and second point.
+
+ // We compute the most positive or most negative triangle area (depending on winding)
+ ContactPointInfo* thirdPointMaxArea = nullptr;
+ ContactPointInfo* thirdPointMinArea = nullptr;
+ decimal minArea = decimal(0.0);
+ decimal maxArea = decimal(0.0);
+ bool isPreviousAreaPositive = true;
+ element = mContactPointsList;
+ while(element != nullptr) {
+
+ if (element == pointsToKeep[0] || element == pointsToKeep[1]) {
+ element = element->next;
+ continue;
+ }
+
+ const Vector3 newToFirst = pointsToKeep[0]->localPoint1 - element->localPoint1;
+ const Vector3 newToSecond = pointsToKeep[1]->localPoint1 - element->localPoint1;
+
+ // Compute the triangle area
+ decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
+
+ if (area >= maxArea) {
+ maxArea = area;
+ thirdPointMaxArea = element;
+ }
+ if (area <= minArea) {
+ minArea = area;
+ thirdPointMinArea = element;
+ }
+ element = element->next;
+ }
+ assert(minArea <= decimal(0.0));
+ assert(maxArea >= decimal(0.0));
+ if (maxArea > (-minArea)) {
+ isPreviousAreaPositive = true;
+ pointsToKeep[2] = thirdPointMaxArea;
+ }
+ else {
+ isPreviousAreaPositive = false;
+ pointsToKeep[2] = thirdPointMinArea;
+ }
+ assert(pointsToKeep[2] != nullptr);
+
+ // Compute the 4th point by choosing the triangle that add the most
+ // triangle area to the previous triangle and has opposite sign area (opposite winding)
+
+ decimal largestArea = decimal(0.0); // Largest area (positive or negative)
+ element = mContactPointsList;
+
+ // For each remaining point
+ while(element != nullptr) {
+
+ if (element == pointsToKeep[0] || element == pointsToKeep[1] || element == pointsToKeep[2]) {
+ element = element->next;
+ continue;
+ }
+
+ // For each edge of the triangle made by the first three points
+ for (uint i=0; i<3; i++) {
+
+ uint edgeVertex1Index = i;
+ uint edgeVertex2Index = i < 2 ? i + 1 : 0;
+
+ const Vector3 newToFirst = pointsToKeep[edgeVertex1Index]->localPoint1 - element->localPoint1;
+ const Vector3 newToSecond = pointsToKeep[edgeVertex2Index]->localPoint1 - element->localPoint1;
+
+ // Compute the triangle area
+ decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
+
+ // We are looking at the triangle with maximal area (positive or negative).
+ // If the previous area is positive, we are looking at negative area now.
+ // If the previous area is negative, we are looking at the positive area now.
+ if (isPreviousAreaPositive && area < largestArea) {
+ largestArea = area;
+ pointsToKeep[3] = element;
+ }
+ else if (!isPreviousAreaPositive && area > largestArea) {
+ largestArea = area;
+ pointsToKeep[3] = element;
+ }
+
+ element = element->next;
+ }
+ }
+ assert(pointsToKeep[3] != nullptr);
+
+ // Delete the contact points we do not want to keep from the linked list
+ element = mContactPointsList;
+ ContactPointInfo* previousElement = nullptr;
+ while(element != nullptr) {
+
+ // Skip the points we want to keep
+ if (element == pointsToKeep[0] || element == pointsToKeep[1] ||
+ element == pointsToKeep[2] || element == pointsToKeep[3]) {
+
+ previousElement = element;
+ element = element->next;
+ continue;
+ }
+
+ ContactPointInfo* elementToDelete = element;
+ if (previousElement != nullptr) {
+ previousElement->next = elementToDelete->next;
+ }
+ else {
+ mContactPointsList = elementToDelete->next;
+ }
+ element = element->next;
+
+ // Delete the current element
+ mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
+ }
+
+ mNbContactPoints = 4;
+ }
+}
+
+/// Return the largest penetration depth among the contact points
+decimal ContactManifoldInfo::getLargestPenetrationDepth() const {
+
+ decimal maxDepth = decimal(0.0);
+ ContactPointInfo* element = mContactPointsList;
+ while(element != nullptr) {
+
+ if (element->penetrationDepth > maxDepth) {
+ maxDepth = element->penetrationDepth;
+ }
+
+ element = element->next;
+ }
+
+ return maxDepth;
+}
+
+
diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h
index f631b9fc..a7e84a98 100644
--- a/src/collision/ContactManifoldInfo.h
+++ b/src/collision/ContactManifoldInfo.h
@@ -33,6 +33,8 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
+// Constants
+const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class ContactManifoldInfo
/**
@@ -51,22 +53,18 @@ class ContactManifoldInfo {
/// Memory allocator used to allocate contact points
Allocator& mAllocator;
- // -------------------- Methods -------------------- //
-
+ /// Number of contact points in the manifold
+ uint mNbContactPoints;
public:
// -------------------- Methods -------------------- //
/// Constructor
- ContactManifoldInfo(Allocator& allocator) : mContactPointsList(nullptr), mAllocator(allocator) {}
+ ContactManifoldInfo(Allocator& allocator);
/// Destructor
- ~ContactManifoldInfo() {
-
- // Remove all the contact points
- reset();
- }
+ ~ContactManifoldInfo();
/// Deleted copy-constructor
ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete;
@@ -76,64 +74,26 @@ class ContactManifoldInfo {
/// Add a new contact point into the manifold
void addContactPoint(const Vector3& contactNormal, decimal penDepth,
- const Vector3& localPt1, const Vector3& localPt2) {
-
- assert(penDepth > decimal(0.0));
-
- // Create the contact point info
- ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo)))
- ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
-
- // Add it into the linked list of contact points
- contactPointInfo->next = mContactPointsList;
- mContactPointsList = contactPointInfo;
- }
+ const Vector3& localPt1, const Vector3& localPt2);
/// Remove all the contact points
- void reset() {
-
- // Delete all the contact points in the linked list
- ContactPointInfo* element = mContactPointsList;
- while(element != nullptr) {
- ContactPointInfo* elementToDelete = element;
- element = element->next;
-
- // Delete the current element
- mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
- }
-
- mContactPointsList = nullptr;
- }
+ void reset();
/// Get the first contact point info of the linked list of contact points
- ContactPointInfo* getFirstContactPointInfo() const {
- return mContactPointsList;
- }
+ ContactPointInfo* getFirstContactPointInfo() const;
/// Reduce the number of points in the contact manifold
- void reduce() {
-
- // TODO : Implement this (do not forget to deallocate removed points)
- }
+ void reduce(const Transform& shape1ToWorldTransform);
/// Return the largest penetration depth among the contact points
- decimal getLargestPenetrationDepth() const {
-
- decimal maxDepth = decimal(0.0);
- ContactPointInfo* element = mContactPointsList;
- while(element != nullptr) {
-
- if (element->penetrationDepth > maxDepth) {
- maxDepth = element->penetrationDepth;
- }
-
- element = element->next;
- }
-
- return maxDepth;
- }
+ decimal getLargestPenetrationDepth() const;
};
+// Get the first contact point info of the linked list of contact points
+inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const {
+ return mContactPointsList;
+}
+
}
#endif
From 2f43e554b54f674f26a22c5e0c5cf4af0453dab8 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 6 Jun 2017 21:12:26 +0200
Subject: [PATCH 055/133] Make TriangleShape inherits from
ConvexPolyhedronShape
---
src/collision/HalfEdgeStructure.h | 5 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 168 ++++++++++--------
src/collision/shapes/ConvexPolyhedronShape.h | 4 -
src/collision/shapes/TriangleShape.cpp | 56 +++++-
src/collision/shapes/TriangleShape.h | 118 +++++++++---
5 files changed, 249 insertions(+), 102 deletions(-)
diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h
index 01e305c5..6b4e3aa1 100644
--- a/src/collision/HalfEdgeStructure.h
+++ b/src/collision/HalfEdgeStructure.h
@@ -53,6 +53,9 @@ class HalfEdgeStructure {
uint edgeIndex; // Index of an half-edge of the face
std::vector<uint> faceVertices; // Index of the vertices of the face
+ /// Constructor
+ Face() {}
+
/// Constructor
Face(std::vector<uint> vertices) : faceVertices(vertices) {}
};
@@ -155,7 +158,7 @@ inline HalfEdgeStructure::Edge HalfEdgeStructure::getHalfEdge(uint index) const
return mEdges[index];
}
-// Retunr a given vertex
+// Return a given vertex
inline HalfEdgeStructure::Vertex HalfEdgeStructure::getVertex(uint index) const {
assert(index < mVertices.size());
return mVertices[index];
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 767aff8a..3bd3e796 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -46,6 +46,8 @@ const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
// Test collision between a sphere and a convex mesh
bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+ PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()");
+
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
@@ -77,32 +79,37 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
- // If the last frame collision info is valid and was also using SAT algorithm
- if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
+ // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
+ // frame collision data per triangle)
+ if (polyhedron->getType() != CollisionShapeType::TRIANGLE) {
- // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
- // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
- // the shapes are still separated along this axis, we directly exit with no collision.
+ // If the last frame collision info is valid and was also using SAT algorithm
+ if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
- // Compute the penetration depth of the shapes along the face normal direction
- decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
- sphere, sphereCenter);
+ // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
+ // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
+ // the shapes are still separated along this axis, we directly exit with no collision.
- // If the previous axis is a separating axis
- if (penetrationDepth <= decimal(0.0)) {
+ // Compute the penetration depth of the shapes along the face normal direction
+ decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
+ sphere, sphereCenter);
- // Return no collision
- return false;
- }
+ // If the previous axis is a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
- // The two shapes are overlapping as in the previous frame and on the same axis, therefore
- // we will skip the entire SAT algorithm because the minimum separating axis did not change
- isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+ // Return no collision
+ return false;
+ }
- if (isTemporalCoherenceValid) {
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameInfo.wasColliding;
- minPenetrationDepth = penetrationDepth;
- minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+ if (isTemporalCoherenceValid) {
+
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+ }
}
}
@@ -170,6 +177,8 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd
// Test collision between a capsule and a convex mesh
bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+ PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()");
+
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
@@ -206,78 +215,83 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
- // If the last frame collision info is valid and was also using SAT algorithm
- if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
+ // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
+ // frame collision data per triangle)
+ if (polyhedron->getType() != CollisionShapeType::TRIANGLE) {
- // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
- // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
- // the shapes are still separated along this axis, we directly exit with no collision.
+ // If the last frame collision info is valid and was also using SAT algorithm
+ if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
- // If the previous minimum separation axis was a face normal of the polyhedron
- if (lastFrameInfo.satIsAxisFacePolyhedron1) {
+ // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
+ // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
+ // the shapes are still separated along this axis, we directly exit with no collision.
- Vector3 outFaceNormalCapsuleSpace;
+ // If the previous minimum separation axis was a face normal of the polyhedron
+ if (lastFrameInfo.satIsAxisFacePolyhedron1) {
- // Compute the penetration depth along the polyhedron face normal direction
- const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
- capsuleShape, polyhedronToCapsuleTransform,
- outFaceNormalCapsuleSpace);
+ Vector3 outFaceNormalCapsuleSpace;
- // If the previous axis is a separating axis
- if (penetrationDepth <= decimal(0.0)) {
+ // Compute the penetration depth along the polyhedron face normal direction
+ const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
+ capsuleShape, polyhedronToCapsuleTransform,
+ outFaceNormalCapsuleSpace);
- // Return no collision
- return false;
+ // If the previous axis is a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+
+ // Return no collision
+ return false;
+ }
+
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+
+ if (isTemporalCoherenceValid) {
+
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
+ isMinPenetrationFaceNormal = true;
+ separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace;
+ }
}
+ else { // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron
- // The two shapes are overlapping as in the previous frame and on the same axis, therefore
- // we will skip the entire SAT algorithm because the minimum separating axis did not change
- isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+ // Get an edge from the polyhedron (convert it into the capsule local-space)
+ HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index);
+ const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
+ const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
+ const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
- if (isTemporalCoherenceValid) {
+ Vector3 outAxis;
- minPenetrationDepth = penetrationDepth;
- minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
- isMinPenetrationFaceNormal = true;
- separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace;
- }
- }
- else { // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron
+ // Compute the penetration depth along this axis
+ const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
+ capsuleSegmentAxis, edgeVertex1,
+ edgeDirectionCapsuleSpace,
+ polyhedronToCapsuleTransform,
+ outAxis);
- // Get an edge from the polyhedron (convert it into the capsule local-space)
- HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index);
- const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
- const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
- const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
+ // If the previous axis is a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
- Vector3 outAxis;
+ // Return no collision
+ return false;
+ }
- // Compute the penetration depth along this axis
- const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
- capsuleSegmentAxis, edgeVertex1,
- edgeDirectionCapsuleSpace,
- polyhedronToCapsuleTransform,
- outAxis);
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameInfo.wasColliding;
- // If the previous axis is a separating axis
- if (penetrationDepth <= decimal(0.0)) {
+ if (isTemporalCoherenceValid) {
- // Return no collision
- return false;
- }
-
- // The two shapes are overlapping as in the previous frame and on the same axis, therefore
- // we will skip the entire SAT algorithm because the minimum separating axis did not change
- isTemporalCoherenceValid = lastFrameInfo.wasColliding;
-
- if (isTemporalCoherenceValid) {
-
- minPenetrationDepth = penetrationDepth;
- minEdgeIndex = lastFrameInfo.satMinEdge1Index;
- isMinPenetrationFaceNormal = false;
- separatingAxisCapsuleSpace = outAxis;
- separatingPolyhedronEdgeVertex1 = edgeVertex1;
- separatingPolyhedronEdgeVertex2 = edgeVertex2;
+ minPenetrationDepth = penetrationDepth;
+ minEdgeIndex = lastFrameInfo.satMinEdge1Index;
+ isMinPenetrationFaceNormal = false;
+ separatingAxisCapsuleSpace = outAxis;
+ separatingPolyhedronEdgeVertex1 = edgeVertex1;
+ separatingPolyhedronEdgeVertex2 = edgeVertex2;
+ }
}
}
}
@@ -527,6 +541,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) const {
+ PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()");
+
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h
index f6d2a34b..6c1de4d5 100644
--- a/src/collision/shapes/ConvexPolyhedronShape.h
+++ b/src/collision/shapes/ConvexPolyhedronShape.h
@@ -42,10 +42,6 @@ class ConvexPolyhedronShape : public ConvexShape {
protected :
- // -------------------- Attributes -------------------- //
-
- // -------------------- Methods -------------------- //
-
public :
// -------------------- Methods -------------------- //
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index 4767435b..9c0deb38 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -40,10 +40,16 @@ using namespace reactphysics3d;
* @param margin The collision margin (in meters) around the collision shape
*/
TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin)
- : ConvexShape(CollisionShapeType::TRIANGLE, margin) {
+ : ConvexPolyhedronShape(margin) {
+
mPoints[0] = point1;
mPoints[1] = point2;
mPoints[2] = point3;
+
+ // Compute the triangle normal
+ mNormal = (point3 - point1).cross(point2 - point1);
+ mNormal.normalize();
+
mRaycastTestType = TriangleRaycastSide::FRONT;
}
@@ -120,3 +126,51 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
return true;
}
+// Return a given half-edge of the polyhedron
+HalfEdgeStructure::Edge TriangleShape::getHalfEdge(uint edgeIndex) const {
+ assert(edgeIndex < getNbHalfEdges());
+
+ HalfEdgeStructure::Edge edge;
+
+ switch(edgeIndex) {
+ case 0:
+ edge.vertexIndex = 0;
+ edge.twinEdgeIndex = 1;
+ edge.faceIndex = 0;
+ edge.nextEdgeIndex = 2;
+ break;
+ case 1:
+ edge.vertexIndex = 1;
+ edge.twinEdgeIndex = 0;
+ edge.faceIndex = 1;
+ edge.nextEdgeIndex = 5;
+ break;
+ case 2:
+ edge.vertexIndex = 1;
+ edge.twinEdgeIndex = 3;
+ edge.faceIndex = 0;
+ edge.nextEdgeIndex = 4;
+ break;
+ case 3:
+ edge.vertexIndex = 2;
+ edge.twinEdgeIndex = 2;
+ edge.faceIndex = 1;
+ edge.nextEdgeIndex = 1;
+ break;
+ case 4:
+ edge.vertexIndex = 2;
+ edge.twinEdgeIndex = 5;
+ edge.faceIndex = 0;
+ edge.nextEdgeIndex = 0;
+ break;
+ case 5:
+ edge.vertexIndex = 0;
+ edge.twinEdgeIndex = 4;
+ edge.faceIndex = 1;
+ edge.nextEdgeIndex = 3;
+ break;
+ }
+
+ return edge;
+
+}
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index 29732f8f..e25086b8 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -28,7 +28,7 @@
// Libraries
#include "mathematics/mathematics.h"
-#include "ConvexShape.h"
+#include "ConvexPolyhedronShape.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -51,7 +51,7 @@ enum class TriangleRaycastSide {
* This class represents a triangle collision shape that is centered
* at the origin and defined three points.
*/
-class TriangleShape : public ConvexShape {
+class TriangleShape : public ConvexPolyhedronShape {
protected:
@@ -60,6 +60,9 @@ class TriangleShape : public ConvexShape {
/// Three points of the triangle
Vector3 mPoints[3];
+ /// Normal of the triangle
+ Vector3 mNormal;
+
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide mRaycastTestType;
@@ -113,11 +116,32 @@ class TriangleShape : public ConvexShape {
// Set the raycast test type (front, back, front-back)
void setRaycastTestType(TriangleRaycastSide testType);
- /// Return the coordinates of a given vertex of the triangle
- Vector3 getVertex(int index) const;
+ /// Return the number of faces of the polyhedron
+ virtual uint getNbFaces() const override;
- /// Return true if the collision shape is a polyhedron
- virtual bool isPolyhedron() const override;
+ /// Return a given face of the polyhedron
+ virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override;
+
+ /// Return the number of vertices of the polyhedron
+ virtual uint getNbVertices() const override;
+
+ /// Return a given vertex of the polyhedron
+ virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override;
+
+ /// Return the position of a given vertex
+ virtual Vector3 getVertexPosition(uint vertexIndex) const override;
+
+ /// Return the normal vector of a given face of the polyhedron
+ virtual Vector3 getFaceNormal(uint faceIndex) const override;
+
+ /// Return the number of half-edges of the polyhedron
+ virtual uint getNbHalfEdges() const override;
+
+ /// Return a given half-edge of the polyhedron
+ virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override;
+
+ /// Return the centroid of the polyhedron
+ virtual Vector3 getCentroid() const override;
// ---------- Friendship ---------- //
@@ -199,6 +223,74 @@ inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape
return false;
}
+// Return the number of faces of the polyhedron
+inline uint TriangleShape::getNbFaces() const {
+ return 2;
+}
+
+// Return a given face of the polyhedron
+inline HalfEdgeStructure::Face TriangleShape::getFace(uint faceIndex) const {
+ assert(faceIndex < 2);
+
+ HalfEdgeStructure::Face face;
+
+ if (faceIndex == 0) {
+ face.faceVertices.push_back(0);
+ face.faceVertices.push_back(1);
+ face.faceVertices.push_back(2);
+ face.edgeIndex = 0;
+
+ }
+ else {
+ face.faceVertices.push_back(0);
+ face.faceVertices.push_back(2);
+ face.faceVertices.push_back(1);
+ face.edgeIndex = 1;
+ }
+
+ return face;
+}
+
+// Return the number of vertices of the polyhedron
+inline uint TriangleShape::getNbVertices() const {
+ return 3;
+}
+
+// Return a given vertex of the polyhedron
+inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) const {
+ assert(vertexIndex < 3);
+
+ HalfEdgeStructure::Vertex vertex(vertexIndex);
+ switch (vertexIndex) {
+ case 0: vertex.edgeIndex = 0; break;
+ case 1: vertex.edgeIndex = 2; break;
+ case 2: vertex.edgeIndex = 4; break;
+ }
+ return vertex;
+}
+
+// Return the position of a given vertex
+inline Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const {
+ assert(vertexIndex < 3);
+ return mPoints[vertexIndex];
+}
+
+// Return the normal vector of a given face of the polyhedron
+inline Vector3 TriangleShape::getFaceNormal(uint faceIndex) const {
+ assert(faceIndex < 2);
+ return faceIndex == 0 ? mNormal : -mNormal;
+}
+
+// Return the centroid of the box
+inline Vector3 TriangleShape::getCentroid() const {
+ return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0);
+}
+
+// Return the number of half-edges of the polyhedron
+inline uint TriangleShape::getNbHalfEdges() const {
+ return 6;
+}
+
// Return the raycast test type (front, back, front-back)
inline TriangleRaycastSide TriangleShape::getRaycastTestType() const {
return mRaycastTestType;
@@ -212,20 +304,6 @@ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
mRaycastTestType = testType;
}
-// Return the coordinates of a given vertex of the triangle
-/**
- * @param index Index (0 to 2) of a vertex of the triangle
- */
-inline Vector3 TriangleShape::getVertex(int index) const {
- assert(index >= 0 && index < 3);
- return mPoints[index];
-}
-
-// Return true if the collision shape is a polyhedron
-inline bool TriangleShape::isPolyhedron() const {
- return true;
-}
-
}
#endif
From 6e9a84823a77c5f1aef7e94e44f654ffb3b17021 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 17 Jul 2017 08:05:40 +0200
Subject: [PATCH 056/133] 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 <cassert>
// 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<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(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<uint> face0;
face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3);
std::vector<uint> 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<uint> 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<uint> 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<uint> 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<uint> 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);
From ddd7f500a659b983c59c31984af074845864ab9f Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 17 Jul 2017 18:35:51 +0200
Subject: [PATCH 057/133] Fix issues in SAT algorithm
---
.../narrowphase/SAT/SATAlgorithm.cpp | 51 ++++++++++---------
.../SphereVsConvexPolyhedronAlgorithm.cpp | 4 +-
2 files changed, 30 insertions(+), 25 deletions(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index d204a88e..4e15eb15 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -141,15 +141,11 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal);
- const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse() * normalWorld * sphere->getRadius();
+ const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius();
const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
- if (!isSphereShape1) {
- normalWorld = -normalWorld;
- }
-
// Create the contact info object
- contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+ contactManifoldInfo.addContactPoint(isSphereShape1 ? normalWorld : -normalWorld, minPenetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
@@ -497,32 +493,41 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// Construct a clippling plane for each adjacent edge of the separting face of the polyhedron
planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex));
- planesNormals.push_back(polyhedron->getFaceNormal(twinEdge.faceIndex));
+ planesNormals.push_back(-polyhedron->getFaceNormal(twinEdge.faceIndex));
edgeIndex = edge.nextEdgeIndex;
} while(edgeIndex != firstEdgeIndex);
// First we clip the inner segment of the capsule with the four planes of the adjacent faces
- std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
- planesPoints, planesNormals);
+ std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals);
+ assert(clipSegment.size() == 2);
- // Project the two clipped points into the polyhedron face
- const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
- const Vector3 contactPoint1Polyhedron = clipSegment[0] + faceNormal * (penetrationDepth - capsuleRadius);
- const Vector3 contactPoint2Polyhedron = clipSegment[1] + faceNormal * (penetrationDepth - capsuleRadius);
+ const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
- // Project the two clipped points into the capsule bounds
- const Vector3 contactPoint1Capsule = (polyhedronToCapsuleTransform * clipSegment[0]) - separatingAxisCapsuleSpace * capsuleRadius;
- const Vector3 contactPoint2Capsule = (polyhedronToCapsuleTransform * clipSegment[1]) - separatingAxisCapsuleSpace * capsuleRadius;
+ // Project the two clipped points into the polyhedron face
+ const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);
- // Create the contact points
- contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
- isCapsuleShape1 ? contactPoint1Capsule : contactPoint1Polyhedron,
- isCapsuleShape1 ? contactPoint1Polyhedron : contactPoint1Capsule);
- contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
- isCapsuleShape1 ? contactPoint2Capsule : contactPoint2Polyhedron,
- isCapsuleShape1 ? contactPoint2Polyhedron : contactPoint2Capsule);
+ // For each of the two clipped points
+ for (int i = 0; i<2; i++) {
+
+ // Compute the penetration depth of the two clipped points (to filter out the points that does not correspond to the penetration depth)
+ const decimal clipPointPenDepth = (planesPoints[0] - clipSegment[i]).dot(faceNormal);
+
+ // If the clipped point is one that produce this penetration depth, we keep it
+ if (clipPointPenDepth > penetrationDepth - capsuleRadius - decimal(0.001)) {
+
+ const Vector3 contactPointPolyhedron = clipSegment[i] + delta;
+
+ // Project the clipped point into the capsule bounds
+ const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius;
+
+ // Create the contact point
+ contactManifoldInfo.addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth,
+ isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
+ isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
+ }
+ }
}
// This method returns true if an edge of a polyhedron and a capsule forms a
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index 068a79ff..d90a29b3 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -43,8 +43,8 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
- assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE ||
- narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
+ narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
From 6eec956eb0afcdf6cf0194da3c520158fd3aa0d4 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 21 Jul 2017 08:09:43 +0200
Subject: [PATCH 058/133] Fix issues in SAT algorithm between two convex
polyhedra
---
src/collision/narrowphase/SAT/SATAlgorithm.cpp | 15 ++++++++-------
src/mathematics/mathematics_functions.cpp | 5 +++++
src/mathematics/mathematics_functions.h | 3 +++
3 files changed, 16 insertions(+), 7 deletions(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 4e15eb15..c838d8ab 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -854,18 +854,19 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
assert(clipPolygonVertices.size() > 0);
// We only keep the clipped points that are below the reference face
- const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(firstEdgeIndex);
+ const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
std::vector<Vector3>::const_iterator itPoints;
for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
// If the clip point is bellow the reference face
- if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) {
+ if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0))
+ {
// Convert the clip incident polyhedron vertex into the incident polyhedron local-space
const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
// Project the contact point onto the reference face
- Vector3 contactPointReferencePolyhedron = (*itPoints) + axisReferenceSpace * minPenetrationDepth;
+ Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
// Create a new contact point
contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
@@ -1011,8 +1012,8 @@ bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* poly
const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2,
const Transform& polyhedron1ToPolyhedron2) const {
- const Vector3 a = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(edge1.faceIndex);
- const Vector3 b = polyhedron1ToPolyhedron2 * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex);
+ const Vector3 a = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(edge1.faceIndex);
+ const Vector3 b = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex);
const Vector3 c = polyhedron2->getFaceNormal(edge2.faceIndex);
const Vector3 d = polyhedron2->getFaceNormal(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).faceIndex);
@@ -1020,12 +1021,12 @@ bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* poly
// Compute b.cross(a) using the edge direction
const Vector3 edge1Vertex1 = polyhedron1->getVertexPosition(edge1.vertexIndex);
const Vector3 edge1Vertex2 = polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).vertexIndex);
- const Vector3 bCrossA = polyhedron1ToPolyhedron2.getOrientation() * (edge1Vertex2 - edge1Vertex1);
+ const Vector3 bCrossA = polyhedron1ToPolyhedron2.getOrientation() * (edge1Vertex1 - edge1Vertex2);
// Compute d.cross(c) using the edge direction
const Vector3 edge2Vertex1 = polyhedron2->getVertexPosition(edge2.vertexIndex);
const Vector3 edge2Vertex2 = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).vertexIndex);
- const Vector3 dCrossC = edge2Vertex2 - edge2Vertex1;
+ const Vector3 dCrossC = edge2Vertex1 - edge2Vertex2;
// Test if the two arcs of the Gauss Map intersect (therefore forming a minkowski face)
// Note that we negate the normals of the second polyhedron because we are looking at the
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 24329732..d627ed7b 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -354,4 +354,9 @@ std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vec
return outputVertices;
}
+// Project a point onto a plane that is given by a point and its unit length normal
+Vector3 reactphysics3d::projectPointOntoPlane(const Vector3& point, const Vector3& unitPlaneNormal, const Vector3& planePoint) {
+ return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal;
+}
+
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index 5ee03a19..78b5ba67 100755
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -112,6 +112,9 @@ std::vector<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& s
std::vector<Vector3> clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
const std::vector<Vector3>& planesNormals);
+/// Project a point onto a plane that is given by a point and its unit length normal
+Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint);
+
}
From 8b82c4ac819998ea2af634a8a5858fd0cada31df Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 30 Jul 2017 22:14:46 +0200
Subject: [PATCH 059/133] Refactor the way to create the contact manifolds and
contact points
---
CMakeLists.txt | 4 +
src/body/CollisionBody.cpp | 6 +-
src/collision/CollisionCallback.cpp | 79 ++++
src/collision/CollisionCallback.h | 89 ++++
src/collision/CollisionDetection.cpp | 425 ++++++++++++------
src/collision/CollisionDetection.h | 17 +-
src/collision/ContactManifold.cpp | 82 ++--
src/collision/ContactManifold.h | 248 ++++++----
src/collision/ContactManifoldInfo.cpp | 60 +--
src/collision/ContactManifoldInfo.h | 45 +-
src/collision/ContactManifoldSet.cpp | 278 +++++++-----
src/collision/ContactManifoldSet.h | 84 ++--
src/collision/NarrowPhaseInfo.cpp | 97 ++++
src/collision/NarrowPhaseInfo.h | 22 +-
src/collision/OverlapCallback.h | 57 +++
.../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 8 +-
.../narrowphase/CapsuleVsCapsuleAlgorithm.h | 3 +-
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 15 +-
.../CapsuleVsConvexPolyhedronAlgorithm.h | 3 +-
.../narrowphase/ConcaveVsConvexAlgorithm.cpp | 178 ++++----
.../narrowphase/ConcaveVsConvexAlgorithm.h | 26 +-
...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 5 +-
...vexPolyhedronVsConvexPolyhedronAlgorithm.h | 3 +-
.../narrowphase/GJK/GJKAlgorithm.cpp | 5 +-
src/collision/narrowphase/GJK/GJKAlgorithm.h | 5 +-
.../narrowphase/NarrowPhaseAlgorithm.h | 4 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 22 +-
src/collision/narrowphase/SAT/SATAlgorithm.h | 8 +-
.../narrowphase/SphereVsCapsuleAlgorithm.cpp | 4 +-
.../narrowphase/SphereVsCapsuleAlgorithm.h | 3 +-
.../SphereVsConvexPolyhedronAlgorithm.cpp | 7 +-
.../SphereVsConvexPolyhedronAlgorithm.h | 3 +-
.../narrowphase/SphereVsSphereAlgorithm.cpp | 5 +-
.../narrowphase/SphereVsSphereAlgorithm.h | 3 +-
src/collision/shapes/BoxShape.h | 2 +
src/collision/shapes/CollisionShape.h | 17 -
src/constraint/ContactPoint.cpp | 9 +-
src/constraint/ContactPoint.h | 70 ++-
src/engine/CollisionWorld.h | 58 ---
src/engine/ContactSolver.cpp | 8 +-
src/engine/DynamicsWorld.cpp | 11 +-
src/engine/EventListener.h | 10 +-
src/engine/OverlappingPair.cpp | 100 ++++-
src/engine/OverlappingPair.h | 69 ++-
src/memory/Allocator.h | 3 +
src/memory/PoolAllocator.h | 8 +
src/memory/SingleFrameAllocator.h | 9 +-
src/reactphysics3d.h | 2 +
.../CollisionDetectionScene.h | 40 +-
testbed/src/SceneDemo.cpp | 6 +-
50 files changed, 1558 insertions(+), 767 deletions(-)
create mode 100644 src/collision/CollisionCallback.cpp
create mode 100644 src/collision/CollisionCallback.h
create mode 100644 src/collision/NarrowPhaseInfo.cpp
create mode 100644 src/collision/OverlapCallback.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index cba141df..1e1d2fcf 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -132,6 +132,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/CollisionDetection.h"
"src/collision/CollisionDetection.cpp"
"src/collision/NarrowPhaseInfo.h"
+ "src/collision/NarrowPhaseInfo.cpp"
"src/collision/ContactManifold.h"
"src/collision/ContactManifold.cpp"
"src/collision/ContactManifoldSet.h"
@@ -167,6 +168,9 @@ SET (REACTPHYSICS3D_SOURCES
"src/engine/Profiler.cpp"
"src/engine/Timer.h"
"src/engine/Timer.cpp"
+ "src/collision/CollisionCallback.h"
+ "src/collision/CollisionCallback.cpp"
+ "src/collision/OverlapCallback.h"
"src/mathematics/mathematics.h"
"src/mathematics/mathematics_functions.h"
"src/mathematics/mathematics_functions.cpp"
diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index db22f606..39d47de4 100644
--- a/src/body/CollisionBody.cpp
+++ b/src/body/CollisionBody.cpp
@@ -177,7 +177,7 @@ void CollisionBody::resetContactManifoldsList() {
// Delete the linked list of contact manifolds of that body
ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != nullptr) {
- ContactManifoldListElement* nextElement = currentElement->next;
+ ContactManifoldListElement* nextElement = currentElement->getNext();
// Delete the current element
currentElement->~ContactManifoldListElement();
@@ -272,8 +272,8 @@ int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
// this body
ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != nullptr) {
- currentElement->contactManifold->mIsAlreadyInIsland = false;
- currentElement = currentElement->next;
+ currentElement->getContactManifold()->mIsAlreadyInIsland = false;
+ currentElement = currentElement->getNext();
nbManifolds++;
}
diff --git a/src/collision/CollisionCallback.cpp b/src/collision/CollisionCallback.cpp
new file mode 100644
index 00000000..f470763d
--- /dev/null
+++ b/src/collision/CollisionCallback.cpp
@@ -0,0 +1,79 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "collision/CollisionCallback.h"
+#include "engine/OverlappingPair.h"
+#include "memory/Allocator.h"
+#include "collision/ContactManifold.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator) :
+ contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()),
+ body2(pair->getShape2()->getBody()),
+ proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()),
+ mMemoryAllocator(allocator) {
+
+ assert(pair != nullptr);
+
+ const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
+
+ // For each contact manifold in the set of manifolds in the pair
+ ContactManifold* contactManifold = manifoldSet.getContactManifolds();
+ while (contactManifold != nullptr) {
+
+ assert(contactManifold->getNbContactPoints() > 0);
+
+ // Add the contact manifold at the beginning of the linked
+ // list of contact manifolds of the first body
+ ContactManifoldListElement* element = new (mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)))
+ ContactManifoldListElement(contactManifold,
+ contactManifoldElements);
+ contactManifoldElements = element;
+
+ contactManifold = contactManifold->getNext();
+ }
+}
+
+// Destructor
+CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() {
+
+ // Release memory allocator for the contact manifold list elements
+ ContactManifoldListElement* element = contactManifoldElements;
+ while (element != nullptr) {
+
+ ContactManifoldListElement* nextElement = element->getNext();
+
+ // Delete and release memory
+ element->~ContactManifoldListElement();
+ mMemoryAllocator.release(element, sizeof(ContactManifoldListElement));
+
+ element = nextElement;
+ }
+}
+
diff --git a/src/collision/CollisionCallback.h b/src/collision/CollisionCallback.h
new file mode 100644
index 00000000..f860b42d
--- /dev/null
+++ b/src/collision/CollisionCallback.h
@@ -0,0 +1,89 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_COLLISION_CALLBACK_H
+#define REACTPHYSICS3D_COLLISION_CALLBACK_H
+
+// Libraries
+#include "collision/ContactManifold.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class CollisionCallback
+/**
+ * This class can be used to register a callback for collision test queries.
+ * You should implement your own class inherited from this one and implement
+ * the notifyContact() method. This method will called each time a contact
+ * point is reported.
+ */
+class CollisionCallback {
+
+ public:
+
+ // Structure CollisionCallbackInfo
+ /**
+ * This structure represents the contact information between two collision
+ * shapes of two bodies
+ */
+ struct CollisionCallbackInfo {
+
+ public:
+
+ /// Pointer to the first element of the linked-list that contains contact manifolds
+ ContactManifoldListElement* contactManifoldElements;
+
+ /// Pointer to the first body of the contact
+ CollisionBody* body1;
+
+ /// Pointer to the second body of the contact
+ CollisionBody* body2;
+
+ /// Pointer to the proxy shape of first body
+ const ProxyShape* proxyShape1;
+
+ /// Pointer to the proxy shape of second body
+ const ProxyShape* proxyShape2;
+
+ /// Memory allocator
+ Allocator& mMemoryAllocator;
+
+ // Constructor
+ CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator);
+
+ // Destructor
+ ~CollisionCallbackInfo();
+ };
+
+ /// Destructor
+ virtual ~CollisionCallback() = default;
+
+ /// This method will be called for each reported contact point
+ virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
+};
+
+}
+
+#endif
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 0e7c8078..57360706 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -26,10 +26,13 @@
// Libraries
#include "CollisionDetection.h"
#include "engine/CollisionWorld.h"
+#include "collision/OverlapCallback.h"
#include "body/Body.h"
#include "collision/shapes/BoxShape.h"
#include "body/RigidBody.h"
#include "configuration.h"
+#include "collision/CollisionCallback.h"
+#include "collision/OverlapCallback.h"
#include <cassert>
#include <complex>
#include <set>
@@ -43,7 +46,7 @@ using namespace std;
// Constructor
CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator)
- : mMemoryAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator),
+ : mPoolAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator),
mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this),
mIsCollisionShapesAdded(false) {
@@ -83,7 +86,7 @@ void CollisionDetection::computeBroadPhase() {
// Ask the broad-phase to recompute the overlapping pairs of collision
// shapes. This call can only add new overlapping pairs in the collision
// detection.
- mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryAllocator);
+ mBroadPhaseAlgorithm.computeOverlappingPairs(mPoolAllocator);
}
}
@@ -101,25 +104,24 @@ void CollisionDetection::computeMiddlePhase() {
OverlappingPair* pair = it->second;
+ // Make all the contact manifolds and contact points of the pair obselete
+ pair->makeContactsObselete();
+
ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2();
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
- // Check if the collision filtering allows collision between the two shapes and
- // that the two shapes are still overlapping. Otherwise, we destroy the
+ // Check if the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair
- if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
- (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) ||
- !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
+ if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
++it;
- // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
-
// Destroy the overlapping pair
itToRemove->second->~OverlappingPair();
+
mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove);
continue;
@@ -128,56 +130,61 @@ void CollisionDetection::computeMiddlePhase() {
++it;
}
- CollisionBody* const body1 = shape1->getBody();
- CollisionBody* const body2 = shape2->getBody();
+ // Check if the collision filtering allows collision between the two shapes
+ if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 &&
+ (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0)) {
- // Check that at least one body is awake and not static
- bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC;
- bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC;
- if (!isBody1Active && !isBody2Active) continue;
+ CollisionBody* const body1 = shape1->getBody();
+ CollisionBody* const body2 = shape2->getBody();
- // Check if the bodies are in the set of bodies that cannot collide between each other
- bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
- if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
+ // Check that at least one body is awake and not static
+ bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC;
+ bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC;
+ if (!isBody1Active && !isBody2Active) continue;
- const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
- const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+ // Check if the bodies are in the set of bodies that cannot collide between each other
+ bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
+ if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
- // If both shapes are convex
- if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) {
+ const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
+ const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
- // No middle-phase is necessary, simply create a narrow phase info
- // for the narrow-phase collision detection
- NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList;
- mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo)))
- NarrowPhaseInfo(pair, shape1->getCollisionShape(),
- shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
- shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
- shape2->getCachedCollisionData());
- mNarrowPhaseInfoList->next = firstNarrowPhaseInfo;
+ // If both shapes are convex
+ if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) {
- }
- // Concave vs Convex algorithm
- else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
- (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
+ // No middle-phase is necessary, simply create a narrow phase info
+ // for the narrow-phase collision detection
+ NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList;
+ mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo)))
+ NarrowPhaseInfo(pair, shape1->getCollisionShape(),
+ shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
+ shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
+ shape2->getCachedCollisionData());
+ mNarrowPhaseInfoList->next = firstNarrowPhaseInfo;
- NarrowPhaseInfo* narrowPhaseInfo = nullptr;
- computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo);
-
- // Add all the narrow-phase info object reported by the callback into the
- // list of all the narrow-phase info object
- while (narrowPhaseInfo != nullptr) {
- NarrowPhaseInfo* next = narrowPhaseInfo->next;
- narrowPhaseInfo->next = mNarrowPhaseInfoList;
- mNarrowPhaseInfoList = narrowPhaseInfo;
-
- narrowPhaseInfo = next;
}
- }
- // Concave vs Concave shape
- else {
- // Not handled
- continue;
+ // Concave vs Convex algorithm
+ else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
+ (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
+
+ NarrowPhaseInfo* narrowPhaseInfo = nullptr;
+ computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo);
+
+ // Add all the narrow-phase info object reported by the callback into the
+ // list of all the narrow-phase info object
+ while (narrowPhaseInfo != nullptr) {
+ NarrowPhaseInfo* next = narrowPhaseInfo->next;
+ narrowPhaseInfo->next = mNarrowPhaseInfoList;
+ mNarrowPhaseInfoList = narrowPhaseInfo;
+
+ narrowPhaseInfo = next;
+ }
+ }
+ // Concave vs Concave shape
+ else {
+ // Not handled
+ continue;
+ }
}
}
}
@@ -231,7 +238,7 @@ void CollisionDetection::computeNarrowPhase() {
PROFILE("CollisionDetection::computeNarrowPhase()");
- const NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList;
+ NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList;
while (currentNarrowPhaseInfo != nullptr) {
// Select the narrow phase algorithm to use according to the two collision shapes
@@ -245,30 +252,16 @@ void CollisionDetection::computeNarrowPhase() {
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactManifoldInfo contactManifoldInfo(mSingleFrameAllocator);
- if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactManifoldInfo)) {
+ if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true)) {
- // Reduce the number of points in the contact manifold (if necessary)
- contactManifoldInfo.reduce(currentNarrowPhaseInfo->shape1ToWorldTransform);
-
- // If it is the first contact since the pairs are overlapping
- if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) {
-
- // Trigger a callback event
- if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactManifoldInfo);
- }
-
- // Add the contact manifold to the corresponding overlapping pair
- currentNarrowPhaseInfo->overlappingPair->addContactManifold(contactManifoldInfo);
+ // Add the contact points as a potential contact manifold into the pair
+ currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold();
// Add the overlapping pair into the set of pairs in contact during narrow-phase
overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(),
currentNarrowPhaseInfo->overlappingPair->getShape2());
mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair;
- // Trigger a callback event for the new contact
- if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo);
-
currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true;
}
else {
@@ -282,8 +275,14 @@ void CollisionDetection::computeNarrowPhase() {
currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
}
+ // Convert the potential contact into actual contacts
+ processAllPotentialContacts();
+
// Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies();
+
+ // Report contacts to the user
+ reportAllContacts();
}
// Allow the broadphase to notify the collision detection about an overlapping pair.
@@ -302,13 +301,9 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
// Check if the overlapping pair already exists
if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
- // Compute the maximum number of contact manifolds for this pair
- int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(),
- shape2->getCollisionShape()->getType());
-
// Create the overlapping pair and add it into the set of overlapping pairs
- OverlappingPair* newPair = new (mWorld->mPoolAllocator.allocate(sizeof(OverlappingPair)))
- OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mPoolAllocator);
+ OverlappingPair* newPair = new (mPoolAllocator.allocate(sizeof(OverlappingPair)))
+ OverlappingPair(shape1, shape2, mPoolAllocator, mSingleFrameAllocator);
assert(newPair != nullptr);
#ifndef NDEBUG
@@ -372,40 +367,173 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
// For each contact manifold in the set of manifolds in the pair
- for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
-
- ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
+ ContactManifold* contactManifold = manifoldSet.getContactManifolds();
+ while (contactManifold != nullptr) {
assert(contactManifold->getNbContactPoints() > 0);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
- void* allocatedMemory1 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement));
- ContactManifoldListElement* listElement1 = new (allocatedMemory1)
+ ContactManifoldListElement* listElement1 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement)))
ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList);
body1->mContactManifoldsList = listElement1;
// Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body
- void* allocatedMemory2 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement));
- ContactManifoldListElement* listElement2 = new (allocatedMemory2)
+ ContactManifoldListElement* listElement2 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement)))
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
+
+ contactManifold = contactManifold->getNext();
}
}
-// Delete all the contact points in the currently overlapping pairs
-void CollisionDetection::clearContactPoints() {
+/// Convert the potential contact into actual contacts
+void CollisionDetection::processAllPotentialContacts() {
- // For each overlapping pair
+ // For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it;
- for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
- it->second->clearContactPoints();
+ for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
+
+ // Process the potential contacts of the overlapping pair
+ processPotentialContacts(it->second);
}
}
+// Process the potential contact manifold of a pair to create actual contact manifold
+void CollisionDetection::processPotentialContacts(OverlappingPair* pair) {
+
+ // Reduce the number of contact points of the manifold
+ pair->reducePotentialContactManifolds();
+
+ // If there is a concave mesh shape in the pair
+ if (pair->hasConcaveShape()) {
+
+ processSmoothMeshContacts(pair);
+ }
+ else { // If both collision shapes are convex
+
+ // Add all the potential contact manifolds as actual contact manifolds to the pair
+ ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds();
+ while (potentialManifold != nullptr) {
+
+ pair->addContactManifold(potentialManifold);
+
+ potentialManifold = potentialManifold->mNext;
+ }
+ }
+
+ // Clear the obselete contact manifolds and contact points
+ pair->clearObseleteManifoldsAndContactPoints();
+
+ // Reset the potential contacts of the pair
+ pair->clearPotentialContactManifolds();
+}
+
+// Report contacts for all the colliding overlapping pairs
+void CollisionDetection::reportAllContacts() {
+
+ // For each overlapping pairs in contact during the narrow-phase
+ std::map<overlappingpairid, OverlappingPair*>::iterator it;
+ for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
+
+ // If there is a user callback
+ if (mWorld->mEventListener != nullptr) {
+
+ CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator);
+
+ // Trigger a callback event to report the new contact to the user
+ mWorld->mEventListener->newContact(collisionInfo);
+ }
+ }
+}
+
+// Process the potential contacts where one collion is a concave shape.
+// This method processes the concave triangle mesh collision using the smooth mesh collision algorithm described
+// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
+// issue with some internal edges.
+void CollisionDetection::processSmoothMeshContacts(OverlappingPair* pair) {
+
+// // Set with the triangle vertices already processed to void further contacts with same triangle
+// std::unordered_multimap<int, Vector3> processTriangleVertices;
+
+// std::vector<SmoothMeshContactInfo*> smoothContactPoints;
+
+// // If the collision shape 1 is the triangle
+// bool isFirstShapeTriangle = pair->getShape1()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE;
+// assert(isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE);
+// assert(!isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() != CollisionShapeType::TRIANGLE);
+
+// const TriangleShape* triangleShape = nullptr;
+// if (isFirstShapeTriangle) {
+// triangleShape = static_cast<const TriangleShape*>(pair->getShape1()->getCollisionShape());
+// }
+// else {
+// triangleShape = static_cast<const TriangleShape*>(pair->getShape2()->getCollisionShape());
+// }
+// assert(triangleShape != nullptr);
+
+// // Get the temporary memory allocator
+// Allocator& allocator = pair->getTemporaryAllocator();
+
+// // For each potential contact manifold of the pair
+// ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds();
+// while (potentialManifold != nullptr) {
+
+// // For each contact point of the potential manifold
+// ContactPointInfo* contactPointInfo = potentialManifold->getFirstContactPointInfo();
+// while (contactPointInfo != nullptr) {
+
+// // Compute the barycentric coordinates of the point in the triangle
+// decimal u, v, w;
+// computeBarycentricCoordinatesInTriangle(triangleShape->getVertexPosition(0),
+// triangleShape->getVertexPosition(1),
+// triangleShape->getVertexPosition(2),
+// isFirstShapeTriangle ? contactPointInfo->localPoint1 : contactPointInfo->localPoint2,
+// u, v, w);
+// int nbZeros = 0;
+// bool isUZero = approxEqual(u, 0, 0.0001);
+// bool isVZero = approxEqual(v, 0, 0.0001);
+// bool isWZero = approxEqual(w, 0, 0.0001);
+// if (isUZero) nbZeros++;
+// if (isVZero) nbZeros++;
+// if (isWZero) nbZeros++;
+
+// // If the triangle contact point is on a triangle vertex of a triangle edge
+// if (nbZeros == 1 || nbZeros == 2) {
+
+
+// // Create a smooth mesh contact info
+// SmoothMeshContactInfo* smoothContactInfo = new (allocator.allocate(sizeof(SmoothMeshContactInfo)))
+// SmoothMeshContactInfo(potentialManifold, contactInfo, isFirstShapeTriangle,
+// triangleShape->getVertexPosition(0),
+// triangleShape->getVertexPosition(1),
+// triangleShape->getVertexPosition(2));
+
+// smoothContactPoints.push_back(smoothContactInfo);
+
+// // Remove the contact point info from the manifold. If the contact point will be kept in the future, we
+// // will put the contact point back in the manifold.
+// ...
+// }
+
+// // Note that we do not remove the contact points that are not on the vertices or edges of the triangle
+// // from the contact manifold because we know we will keep to contact points. We only remove the vertices
+// // and edges contact points for the moment. If those points will be kept in the future, we will have to
+// // put them back again in the contact manifold
+// }
+
+// potentialManifold = potentialManifold->mNext;
+// }
+
+// // Sort the list of narrow-phase contacts according to their penetration depth
+// std::sort(smoothContactPoints.begin(), smoothContactPoints.end(), ContactsDepthCompare());
+
+// ...
+}
+
// Compute the middle-phase collision detection between two proxy shapes
NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) {
@@ -424,7 +552,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
// No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection
- narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(),
+ narrowPhaseInfo = new (mPoolAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
shape2->getCachedCollisionData());
@@ -436,7 +564,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
// Run the middle-phase collision detection algorithm to find the triangles of the concave
// shape we need to use during the narrow-phase collision detection
- computeConvexVsConcaveMiddlePhase(pair, mMemoryAllocator, &narrowPhaseInfo);
+ computeConvexVsConcaveMiddlePhase(pair, mPoolAllocator, &narrowPhaseInfo);
}
return narrowPhaseInfo;
@@ -450,7 +578,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over
std::unordered_set<bodyindex> reportedBodies;
// Ask the broad-phase to get all the overlapping shapes
- LinkedList<int> overlappingNodes(mMemoryAllocator);
+ LinkedList<int> overlappingNodes(mPoolAllocator);
mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes);
// For each overlaping proxy shape
@@ -504,7 +632,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType();
// Create a temporary overlapping pair
- OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator);
+ OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator);
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -526,16 +654,18 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
- isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo);
+ isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
}
}
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
narrowPhaseInfo = narrowPhaseInfo->next;
+ // Call the destructor
+ currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
// Release the allocated memory
- mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Return if we have found a narrow-phase collision
@@ -570,7 +700,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
// Ask the broad-phase to get all the overlapping shapes
- LinkedList<int> overlappingNodes(mMemoryAllocator);
+ LinkedList<int> overlappingNodes(mPoolAllocator);
mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
const bodyindex bodyId = body->getID();
@@ -595,7 +725,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
// Create a temporary overlapping pair
- OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator);
+ OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -617,16 +747,18 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
- isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo);
+ isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
}
}
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
narrowPhaseInfo = narrowPhaseInfo->next;
+ // Call the destructor
+ currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
// Release the allocated memory
- mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Return if we have found a narrow-phase collision
@@ -673,7 +805,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
if (aabb1.testCollision(aabb2)) {
// Create a temporary overlapping pair
- OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator);
+ OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator);
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -693,26 +825,29 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
- // Reduce the number of points in the contact manifold (if necessary)
- contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
-
- CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body1, body2,
- body1ProxyShape, body2ProxyShape);
-
- // Report the contact to the user
- collisionCallback->notifyContact(collisionInfo);
+ // Add the contact points as a potential contact manifold into the pair
+ narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
}
}
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
narrowPhaseInfo = narrowPhaseInfo->next;
+ // Call the destructor
+ currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
// Release the allocated memory
- mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
+
+ // Process the potential contacts
+ processPotentialContacts(&pair);
+
+ // Report the contacts to the user
+ CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
+ collisionCallback->notifyContact(collisionInfo);
}
// Go to the next proxy shape
@@ -737,7 +872,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
// Ask the broad-phase to get all the overlapping shapes
- LinkedList<int> overlappingNodes(mMemoryAllocator);
+ LinkedList<int> overlappingNodes(mPoolAllocator);
mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
const bodyindex bodyId = body->getID();
@@ -760,7 +895,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
// Create a temporary overlapping pair
- OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator);
+ OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -777,17 +912,13 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
- // Reduce the number of points in the contact manifold (if necessary)
- contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
+ // Add the contact points as a potential contact manifold into the pair
+ narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
- CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body,
- proxyShape->getBody(), bodyProxyShape,
- proxyShape);
-
- // Report the contact to the user
+ // Report the contacts to the user
+ CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
callback->notifyContact(collisionInfo);
}
}
@@ -795,9 +926,15 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
narrowPhaseInfo = narrowPhaseInfo->next;
+ // Call the destructor
+ currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
// Release the allocated memory
- mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
+
+ // Process the potential contacts
+ processPotentialContacts(&pair);
}
}
@@ -822,10 +959,14 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
- OverlappingPair* pair = it->second;
+ OverlappingPair* originalPair = it->second;
- ProxyShape* shape1 = pair->getShape1();
- ProxyShape* shape2 = pair->getShape2();
+ // Create a new overlapping pair so that we do not work on the original one
+ OverlappingPair pair(originalPair->getShape1(), originalPair->getShape2(), mPoolAllocator,
+ mPoolAllocator);
+
+ ProxyShape* shape1 = pair.getShape1();
+ ProxyShape* shape2 = pair.getShape2();
// Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping.
@@ -834,14 +975,14 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
// Compute the middle-phase collision detection between the two shapes
- NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair);
-
- const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
- const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
// For each narrow-phase info object
while (narrowPhaseInfo != nullptr) {
+ const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
+ const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
@@ -851,29 +992,29 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
- // Reduce the number of points in the contact manifold (if necessary)
- contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
+ // Add the contact points as a potential contact manifold into the pair
+ narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
- CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, shape1->getBody(),
- shape2->getBody(), shape1, shape2);
-
- // Report the contact to the user
+ // Report the contacts to the user
+ CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
callback->notifyContact(collisionInfo);
}
-
- // The previous frame collision info is now valid
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true;
}
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
narrowPhaseInfo = narrowPhaseInfo->next;
+ // Call the destructor
+ currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
// Release the allocated memory
- mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
+
+ // Process the potential contacts
+ processPotentialContacts(&pair);
}
}
}
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 3fe88812..49f0d56b 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -72,7 +72,7 @@ class CollisionDetection {
NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES];
/// Reference to the memory allocator
- PoolAllocator& mMemoryAllocator;
+ PoolAllocator& mPoolAllocator;
/// Reference to the single frame memory allocator
SingleFrameAllocator& mSingleFrameAllocator;
@@ -118,9 +118,6 @@ class CollisionDetection {
/// involed in the corresponding contact.
void addContactManifoldToBody(OverlappingPair* pair);
- /// Delete all the contact points in the currently overlapping pairs
- void clearContactPoints();
-
/// Fill-in the collision detection matrix
void fillInCollisionMatrix();
@@ -137,6 +134,18 @@ class CollisionDetection {
/// Compute the middle-phase collision detection between two proxy shapes
NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(OverlappingPair* pair);
+
+ /// Convert the potential contact into actual contacts
+ void processAllPotentialContacts();
+
+ /// Process the potential contact manifold of a pair to create actual contact manifold
+ void processPotentialContacts(OverlappingPair* pair);
+
+ /// Report contacts for all the colliding overlapping pairs
+ void reportAllContacts();
+
+ /// Process the potential contacts where one collion is a concave shape
+ void processSmoothMeshContacts(OverlappingPair* pair);
public :
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index 36103642..846a418a 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -30,15 +30,14 @@
using namespace reactphysics3d;
// Constructor
-ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
- PoolAllocator& memoryAllocator, short normalDirectionId)
- : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
+ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, Allocator& memoryAllocator)
+ : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
- mMemoryAllocator(memoryAllocator) {
+ mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObselete(false) {
// For each contact point info in the manifold
- const ContactPointInfo* pointInfo = manifoldInfo.getFirstContactPointInfo();
+ const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
while(pointInfo != nullptr) {
// Create the new contact point
@@ -46,7 +45,8 @@ ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyS
ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
// Add the new contact point into the manifold
- mContactPoints[mNbContactPoints] = contact;
+ contact->setNext(mContactPoints);
+ mContactPoints = contact;
mNbContactPoints++;
pointInfo = pointInfo->next;
@@ -58,49 +58,69 @@ ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyS
// Destructor
ContactManifold::~ContactManifold() {
- clear();
-}
-// Clear the contact manifold
-void ContactManifold::clear() {
- for (uint i=0; i<mNbContactPoints; i++) {
-
- // Call the destructor explicitly and tell the memory allocator that
- // the corresponding memory block is now free
- mContactPoints[i]->~ContactPoint();
- mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint));
+ // Delete all the contact points
+ ContactPoint* contactPoint = mContactPoints;
+ while(contactPoint != nullptr) {
+
+ ContactPoint* nextContactPoint = contactPoint->getNext();
+
+ // TODO : Delete this
+ bool test = mMemoryAllocator.isReleaseNeeded();
+
+ // Delete the contact point
+ contactPoint->~ContactPoint();
+ mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
+
+ contactPoint = nextContactPoint;
}
- mNbContactPoints = 0;
}
// Add a contact point
void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
- assert(mNbContactPoints < MAX_CONTACT_POINTS_IN_MANIFOLD);
-
// Create the new contact point
ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
// Add the new contact point into the manifold
- mContactPoints[mNbContactPoints] = contactPoint;
- mNbContactPoints++;
+ contactPoint->setNext(mContactPoints);
+ mContactPoints = contactPoint;
+ mNbContactPoints++;
}
-// Remove a contact point
-void ContactManifold::removeContactPoint(int index) {
+// Clear the obselete contact points
+void ContactManifold::clearObseleteContactPoints() {
- assert(mNbContactPoints > 0);
- assert(index >= 0 && index < mNbContactPoints);
+ ContactPoint* contactPoint = mContactPoints;
+ ContactPoint* previousContactPoint = nullptr;
+ while (contactPoint != nullptr) {
- // Delete the contact
- mContactPoints[index]->~ContactPoint();
- mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint));
+ ContactPoint* nextContactPoint = contactPoint->getNext();
- for (int i=index; (i+1) < mNbContactPoints; i++) {
- mContactPoints[i] = mContactPoints[i+1];
+ if (contactPoint->getIsObselete()) {
+
+ // Delete the contact point
+ contactPoint->~ContactPoint();
+ mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
+
+ if (previousContactPoint != nullptr) {
+ previousContactPoint->setNext(nextContactPoint);
+ }
+ else {
+ mContactPoints = nextContactPoint;
+ }
+
+ mNbContactPoints--;
+ }
+ else {
+ previousContactPoint = contactPoint;
+ }
+
+ contactPoint = nextContactPoint;
}
- mNbContactPoints--;
+ assert(mNbContactPoints >= 0);
+ assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
}
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index 893a84a7..0704beed 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -46,40 +46,43 @@ class ContactManifold;
*/
struct ContactManifoldListElement {
- public:
+ private:
// -------------------- Attributes -------------------- //
- /// Pointer to the actual contact manifold
- ContactManifold* contactManifold;
+ /// Pointer to a contact manifold with contact points
+ ContactManifold* mContactManifold;
/// Next element of the list
- ContactManifoldListElement* next;
+ ContactManifoldListElement* mNext;
+
+ public:
// -------------------- Methods -------------------- //
/// Constructor
- ContactManifoldListElement(ContactManifold* initContactManifold,
- ContactManifoldListElement* initNext)
- :contactManifold(initContactManifold), next(initNext) {
+ ContactManifoldListElement(ContactManifold* contactManifold,
+ ContactManifoldListElement* next)
+ :mContactManifold(contactManifold), mNext(next) {
}
+
+ /// Return the contact manifold
+ ContactManifold* getContactManifold() {
+ return mContactManifold;
+ }
+
+ /// Return the next element in the linked-list
+ ContactManifoldListElement* getNext() {
+ return mNext;
+ }
};
// Class ContactManifold
/**
* This class represents the set of contact points between two bodies.
* The contact manifold is implemented in a way to cache the contact
- * points among the frames for better stability following the
- * "Contact Generation" presentation of Erwin Coumans at GDC 2010
- * conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf).
- * Some code of this class is based on the implementation of the
- * btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org).
- * The contacts between two bodies are added one after the other in the cache.
- * When the cache is full, we have to remove one point. The idea is to keep
- * the point with the deepest penetration depth and also to keep the
- * points producing the larger area (for a more stable contact manifold).
- * The new added point is always kept.
+ * points among the frames for better stability.
*/
class ContactManifold {
@@ -94,10 +97,10 @@ class ContactManifold {
ProxyShape* mShape2;
/// Contact points in the manifold
- ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
+ ContactPoint* mContactPoints;
/// Normal direction Id (Unique Id representing the normal direction)
- short int mNormalDirectionId;
+ short int mContactNormalId;
/// Number of contacts in the cache
int8 mNbContactPoints;
@@ -124,20 +127,86 @@ class ContactManifold {
bool mIsAlreadyInIsland;
/// Reference to the memory allocator
- PoolAllocator& mMemoryAllocator;
+ Allocator& mMemoryAllocator;
+
+ /// Pointer to the next contact manifold in the linked-list
+ ContactManifold* mNext;
+
+ /// Pointer to the previous contact manifold in linked-list
+ ContactManifold* mPrevious;
+
+ /// True if the contact manifold is obselete
+ bool mIsObselete;
// -------------------- Methods -------------------- //
/// Return true if the contact manifold has already been added into an island
bool isAlreadyInIsland() const;
+
+ /// Set the pointer to the next element in the linked-list
+ void setNext(ContactManifold* nextManifold);
+
+ /// Return true if the manifold is obselete
+ bool getIsObselete() const;
+
+ /// Set to true to make the manifold obselete
+ void setIsObselete(bool isObselete, bool setContactPoints);
+
+ /// Clear the obselete contact points
+ void clearObseleteContactPoints();
+
+ /// Return the contact normal direction Id of the manifold
+ short getContactNormalId() const;
+
+ /// Return the largest depth of all the contact points
+ decimal getLargestContactDepth() const;
+
+ /// set the first friction vector at the center of the contact manifold
+ void setFrictionVector1(const Vector3& mFrictionVector1);
+
+ /// set the second friction vector at the center of the contact manifold
+ void setFrictionVector2(const Vector3& mFrictionVector2);
+
+ /// Set the first friction accumulated impulse
+ void setFrictionImpulse1(decimal frictionImpulse1);
+
+ /// Set the second friction accumulated impulse
+ void setFrictionImpulse2(decimal frictionImpulse2);
+
+ /// Add a contact point
+ void addContactPoint(const ContactPointInfo* contactPointInfo);
+
+ /// Set the friction twist accumulated impulse
+ void setFrictionTwistImpulse(decimal frictionTwistImpulse);
+
+ /// Set the accumulated rolling resistance impulse
+ void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
+
+ /// Set the pointer to the previous element in the linked-list
+ void setPrevious(ContactManifold* previousManifold);
+
+ /// Return the first friction vector at the center of the contact manifold
+ const Vector3& getFrictionVector1() const;
+
+ /// Return the second friction vector at the center of the contact manifold
+ const Vector3& getFrictionVector2() const;
+
+ /// Return the first friction accumulated impulse
+ decimal getFrictionImpulse1() const;
+
+ /// Return the second friction accumulated impulse
+ decimal getFrictionImpulse2() const;
+
+ /// Return the friction twist accumulated impulse
+ decimal getFrictionTwistImpulse() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
- ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
- PoolAllocator& memoryAllocator, short int normalDirectionId);
+ ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
+ Allocator& memoryAllocator);
/// Destructor
~ContactManifold();
@@ -160,68 +229,25 @@ class ContactManifold {
/// Return a pointer to the second body of the contact manifold
CollisionBody* getBody2() const;
- /// Return the normal direction Id
- short int getNormalDirectionId() const;
-
- /// Clear the contact manifold
- void clear();
-
/// Return the number of contact points in the manifold
int8 getNbContactPoints() const;
- /// Return the first friction vector at the center of the contact manifold
- const Vector3& getFrictionVector1() const;
+ /// Return a pointer to the first contact point of the manifold
+ ContactPoint* getContactPoints() const;
- /// set the first friction vector at the center of the contact manifold
- void setFrictionVector1(const Vector3& mFrictionVector1);
+ /// Return a pointer to the previous element in the linked-list
+ ContactManifold* getPrevious() const;
- /// Return the second friction vector at the center of the contact manifold
- const Vector3& getFrictionVector2() const;
-
- /// set the second friction vector at the center of the contact manifold
- void setFrictionVector2(const Vector3& mFrictionVector2);
-
- /// Return the first friction accumulated impulse
- decimal getFrictionImpulse1() const;
-
- /// Set the first friction accumulated impulse
- void setFrictionImpulse1(decimal frictionImpulse1);
-
- /// Return the second friction accumulated impulse
- decimal getFrictionImpulse2() const;
-
- /// Set the second friction accumulated impulse
- void setFrictionImpulse2(decimal frictionImpulse2);
-
- /// Return the friction twist accumulated impulse
- decimal getFrictionTwistImpulse() const;
-
- /// Set the friction twist accumulated impulse
- void setFrictionTwistImpulse(decimal frictionTwistImpulse);
-
- /// Set the accumulated rolling resistance impulse
- void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
-
- /// Return a contact point of the manifold
- ContactPoint* getContactPoint(uint index) const;
-
- /// Return the normalized averaged normal vector
- Vector3 getAverageContactNormal() const;
-
- /// Return the largest depth of all the contact points
- decimal getLargestContactDepth() const;
-
- /// Add a contact point
- void addContactPoint(const ContactPointInfo* contactPointInfo);
-
- /// Remove a contact point
- void removeContactPoint(int index);
+ /// Return a pointer to the next element in the linked-list
+ ContactManifold* getNext() const;
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class Island;
friend class CollisionBody;
+ friend class ContactManifoldSet;
+ friend class ContactSolver;
};
// Return a pointer to the first proxy shape of the contact
@@ -244,11 +270,6 @@ inline CollisionBody* ContactManifold::getBody2() const {
return mShape2->getBody();
}
-// Return the normal direction Id
-inline short int ContactManifold::getNormalDirectionId() const {
- return mNormalDirectionId;
-}
-
// Return the number of contact points in the manifold
inline int8 ContactManifold::getNbContactPoints() const {
return mNbContactPoints;
@@ -309,10 +330,9 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR
mRollingResistanceImpulse = rollingResistanceImpulse;
}
-// Return a contact point of the manifold
-inline ContactPoint* ContactManifold::getContactPoint(uint index) const {
- assert(index < mNbContactPoints);
- return mContactPoints[index];
+// Return a pointer to the first contact point of the manifold
+inline ContactPoint* ContactManifold::getContactPoints() const {
+ return mContactPoints;
}
// Return true if the contact manifold has already been added into an island
@@ -320,31 +340,69 @@ inline bool ContactManifold::isAlreadyInIsland() const {
return mIsAlreadyInIsland;
}
-// Return the normalized averaged normal vector
-inline Vector3 ContactManifold::getAverageContactNormal() const {
- Vector3 averageNormal;
-
- for (uint i=0; i<mNbContactPoints; i++) {
- averageNormal += mContactPoints[i]->getNormal();
- }
-
- return averageNormal.getUnit();
-}
-
// Return the largest depth of all the contact points
inline decimal ContactManifold::getLargestContactDepth() const {
decimal largestDepth = 0.0f;
- for (uint i=0; i<mNbContactPoints; i++) {
- decimal depth = mContactPoints[i]->getPenetrationDepth();
+ assert(mNbContactPoints > 0);
+
+ ContactPoint* contactPoint = mContactPoints;
+ while(contactPoint != nullptr){
+ decimal depth = contactPoint->getPenetrationDepth();
if (depth > largestDepth) {
largestDepth = depth;
}
+
+ contactPoint = contactPoint->getNext();
}
return largestDepth;
}
+// Return a pointer to the previous element in the linked-list
+inline ContactManifold* ContactManifold::getPrevious() const {
+ return mPrevious;
+}
+
+// Set the pointer to the previous element in the linked-list
+inline void ContactManifold::setPrevious(ContactManifold* previousManifold) {
+ mPrevious = previousManifold;
+}
+
+// Return a pointer to the next element in the linked-list
+inline ContactManifold* ContactManifold::getNext() const {
+ return mNext;
+}
+
+// Set the pointer to the next element in the linked-list
+inline void ContactManifold::setNext(ContactManifold* nextManifold) {
+ mNext = nextManifold;
+}
+
+// Return true if the manifold is obselete
+inline bool ContactManifold::getIsObselete() const {
+ return mIsObselete;
+}
+
+// Set to true to make the manifold obselete
+inline void ContactManifold::setIsObselete(bool isObselete, bool setContactPoints) {
+ mIsObselete = isObselete;
+
+ if (setContactPoints) {
+ ContactPoint* contactPoint = mContactPoints;
+ while (contactPoint != nullptr) {
+ contactPoint->setIsObselete(isObselete);
+
+ contactPoint = contactPoint->getNext();
+ }
+ }
+}
+
+// Return the contact normal direction Id of the manifold
+inline short ContactManifold::getContactNormalId() const {
+ return mContactNormalId;
+}
+
}
#endif
diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp
index c06785ea..a6b8c6df 100644
--- a/src/collision/ContactManifoldInfo.cpp
+++ b/src/collision/ContactManifoldInfo.cpp
@@ -30,7 +30,8 @@ using namespace reactphysics3d;
// Constructor
ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator)
- : mContactPointsList(nullptr), mAllocator(allocator), mNbContactPoints(0) {}
+ : mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator),
+ mContactNormalId(-1) {}
// Destructor
ContactManifoldInfo::~ContactManifoldInfo() {
@@ -40,14 +41,13 @@ ContactManifoldInfo::~ContactManifoldInfo() {
}
// Add a new contact point into the manifold
-void ContactManifoldInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth,
- const Vector3& localPt1, const Vector3& localPt2) {
+void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId) {
- assert(penDepth > decimal(0.0));
+ assert(contactPointInfo->penetrationDepth > decimal(0.0));
+ assert(contactNormalId >= 0);
+ assert(mContactNormalId == -1 || contactNormalId == mContactNormalId);
- // Create the contact point info
- ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo)))
- ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
+ mContactNormalId = contactNormalId;
// Add it into the linked list of contact points
contactPointInfo->next = mContactPointsList;
@@ -73,15 +73,31 @@ void ContactManifoldInfo::reset() {
mNbContactPoints = 0;
}
-// Reduce the number of points in the contact manifold
+// Return the largest penetration depth among its contact points
+decimal ContactManifoldInfo::getLargestPenetrationDepth() const {
+
+ ContactPointInfo* contactPoint = mContactPointsList;
+ assert(contactPoint != nullptr);
+ decimal maxDepth = decimal(0.0);
+ while (contactPoint != nullptr) {
+
+ if (contactPoint->penetrationDepth > maxDepth) {
+ maxDepth = contactPoint->penetrationDepth;
+ }
+
+ contactPoint = contactPoint->next;
+ }
+
+ return maxDepth;
+}
+
+// Reduce the number of contact points of the currently computed manifold
// This is based on the technique described by Dirk Gregorius in his
// "Contacts Creation" GDC presentation
void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
assert(mContactPointsList != nullptr);
- // TODO : Implement this (do not forget to deallocate removed points)
-
// The following algorithm only works to reduce to 4 contact points
assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
@@ -221,9 +237,9 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
largestArea = area;
pointsToKeep[3] = element;
}
-
- element = element->next;
}
+
+ element = element->next;
}
assert(pointsToKeep[3] != nullptr);
@@ -250,6 +266,9 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
}
element = element->next;
+ // Call the destructor
+ elementToDelete->~ContactPointInfo();
+
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
@@ -258,21 +277,4 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
}
}
-/// Return the largest penetration depth among the contact points
-decimal ContactManifoldInfo::getLargestPenetrationDepth() const {
-
- decimal maxDepth = decimal(0.0);
- ContactPointInfo* element = mContactPointsList;
- while(element != nullptr) {
-
- if (element->penetrationDepth > maxDepth) {
- maxDepth = element->penetrationDepth;
- }
-
- element = element->next;
- }
-
- return maxDepth;
-}
-
diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h
index a7e84a98..5851c375 100644
--- a/src/collision/ContactManifoldInfo.h
+++ b/src/collision/ContactManifoldInfo.h
@@ -34,7 +34,7 @@
namespace reactphysics3d {
// Constants
-const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
+const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class ContactManifoldInfo
/**
@@ -50,12 +50,18 @@ class ContactManifoldInfo {
/// Linked list with all the contact points
ContactPointInfo* mContactPointsList;
- /// Memory allocator used to allocate contact points
- Allocator& mAllocator;
-
/// Number of contact points in the manifold
uint mNbContactPoints;
+ /// Next element in the linked-list of contact manifold info
+ ContactManifoldInfo* mNext;
+
+ /// Reference the the memory allocator where the contact point infos have been allocated
+ Allocator& mAllocator;
+
+ /// Contact normal direction Id (Identify the contact normal direction of points in manifold)
+ short mContactNormalId;
+
public:
// -------------------- Methods -------------------- //
@@ -66,15 +72,14 @@ class ContactManifoldInfo {
/// Destructor
~ContactManifoldInfo();
- /// Deleted copy-constructor
+ /// Deleted Copy-constructor
ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete;
/// Deleted assignment operator
ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete;
/// Add a new contact point into the manifold
- void addContactPoint(const Vector3& contactNormal, decimal penDepth,
- const Vector3& localPt1, const Vector3& localPt2);
+ void addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId);
/// Remove all the contact points
void reset();
@@ -82,11 +87,21 @@ class ContactManifoldInfo {
/// Get the first contact point info of the linked list of contact points
ContactPointInfo* getFirstContactPointInfo() const;
- /// Reduce the number of points in the contact manifold
+ /// Return the largest penetration depth among its contact points
+ decimal getLargestPenetrationDepth() const;
+
+ /// Return the pointer to the next manifold info in the linked-list
+ ContactManifoldInfo* getNext();
+
+ /// Return the contact normal Id
+ short getContactNormalId() const;
+
+ /// Reduce the number of contact points of the currently computed manifold
void reduce(const Transform& shape1ToWorldTransform);
- /// Return the largest penetration depth among the contact points
- decimal getLargestPenetrationDepth() const;
+ // Friendship
+ friend class OverlappingPair;
+ friend class CollisionDetection;
};
// Get the first contact point info of the linked list of contact points
@@ -94,6 +109,16 @@ inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const {
return mContactPointsList;
}
+// Return the pointer to the next manifold info in the linked-list
+inline ContactManifoldInfo* ContactManifoldInfo::getNext() {
+ return mNext;
+}
+
+// Return the contact normal Id
+inline short ContactManifoldInfo::getContactNormalId() const {
+ return mContactNormalId;
+}
+
}
#endif
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index c9b0d5bb..b9b7f3e8 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -30,10 +30,12 @@ using namespace reactphysics3d;
// Constructor
ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
- PoolAllocator& memoryAllocator, int nbMaxManifolds)
- : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
- mShape2(shape2), mMemoryAllocator(memoryAllocator) {
+ Allocator& memoryAllocator)
+ : mNbManifolds(0), mShape1(shape1),
+ mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr) {
+ // Compute the maximum number of manifolds allowed between the two shapes
+ mNbMaxManifolds = computeNbMaxContactManifolds(shape1->getCollisionShape(), shape2->getCollisionShape());
}
// Destructor
@@ -43,167 +45,131 @@ ContactManifoldSet::~ContactManifoldSet() {
clear();
}
-void ContactManifoldSet::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) {
+void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) {
- assert(contactManifoldInfo.getFirstContactPointInfo() != nullptr);
+ assert(contactManifoldInfo->getFirstContactPointInfo() != nullptr);
- // If there is no contact manifold yet
- if (mNbManifolds == 0) {
+ // Try to find an existing contact manifold with similar contact normal
+ ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo->getContactNormalId());
- // If the maximum number of manifold is 1
- if (mNbMaxManifolds == 1) {
- createManifold(contactManifoldInfo, 0);
- }
- else {
+ // If a similar manifold has been found
+ if (similarManifold != nullptr) {
- // Compute an Id corresponding to the normal direction (using a cubemap)
- short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal);
-
- createManifold(contactManifoldInfo, normalDirectionId);
- }
+ // Update the old manifold with the new one
+ updateManifoldWithNewOne(similarManifold, contactManifoldInfo);
}
- else { // If there is already at least one contact manifold in the set
+ else {
- // If the maximum number of manifold is 1
- if (mNbMaxManifolds == 1) {
+ // If there are too much contact manifolds in the set
+ if (mNbManifolds >= mNbMaxManifolds) {
- // Replace the old manifold with the new one
- updateManifoldWithNewOne(0, contactManifoldInfo);
- }
- else {
+ // We need to remove a manifold from the set.
+ // We seach for the one with the smallest maximum penetration depth among its contact points
+ ContactManifold* minDepthManifold = getManifoldWithSmallestContactPenetrationDepth(contactManifoldInfo->getLargestPenetrationDepth());
- // Compute an Id corresponding to the normal direction (using a cubemap)
- short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal);
+ // If the manifold with the minimum penetration depth is an existing one
+ if (minDepthManifold != nullptr) {
- // Select the manifold with the most similar normal (if exists)
- int similarManifoldIndex = 0;
- if (mNbMaxManifolds > 1) {
- similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
- }
+ // Remove the manifold
+ removeManifold(minDepthManifold);
- // If a similar manifold has been found
- if (similarManifoldIndex != -1) {
-
- // Replace the old manifold with the new one
- updateManifoldWithNewOne(similarManifoldIndex, contactManifoldInfo);
- }
- else {
-
- // If we have not reach the maximum number of manifolds
- if (mNbManifolds < mNbMaxManifolds) {
-
- // Create the new contact manifold
- createManifold(contactManifoldInfo, normalDirectionId);
- }
- else {
-
- decimal newManifoldPenDepth = contactManifoldInfo.getLargestPenetrationDepth();
-
- // We have reached the maximum number of manifold, we do not
- // want to keep the manifold with the smallest penetration detph
- int smallestPenDepthManifoldIndex = getManifoldWithSmallestContactPenetrationDepth(newManifoldPenDepth);
-
- // If the manifold with the smallest penetration depth is not the new one,
- // we have to keep the new manifold and remove the one with the smallest depth
- if (smallestPenDepthManifoldIndex >= 0) {
- removeManifold(smallestPenDepthManifoldIndex);
- createManifold(contactManifoldInfo, normalDirectionId);
- }
- }
+ // Create a new contact manifold
+ createManifold(contactManifoldInfo);
}
}
+
+ // Create a new contact manifold
+ createManifold(contactManifoldInfo);
}
}
// Update a previous similar manifold with a new one
-void ContactManifoldSet::updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold) {
+void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) {
- // For each contact point of the previous manifold
- for (int i=0; i<mManifolds[oldManifoldIndex]->getNbContactPoints(); ) {
+ assert(oldManifold != nullptr);
+ assert(newManifold != nullptr);
- ContactPoint* contactPoint = mManifolds[oldManifoldIndex]->getContactPoint(i);
+ // For each contact point of the new manifold
+ ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo();
+ assert(contactPointInfo != nullptr);
+ while (contactPointInfo != nullptr) {
- // For each contact point in the new manifold
- ContactPointInfo* newPoint = newManifold.getFirstContactPointInfo();
- bool needToRemovePoint = true;
- while (newPoint != nullptr) {
+ // For each contact point in the old manifold
+ bool isSimilarPointFound = false;
+ ContactPoint* oldContactPoint = oldManifold->getContactPoints();
+ while (oldContactPoint != nullptr) {
+
+ assert(oldContactPoint != nullptr);
// If the new contact point is similar (very close) to the old contact point
- if (!newPoint->isUsed && contactPoint->isSimilarWithContactPoint(newPoint)) {
+ if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) {
// Replace (update) the old contact point with the new one
- contactPoint->update(newPoint, mManifolds[oldManifoldIndex]->getShape1()->getLocalToWorldTransform(),
- mManifolds[oldManifoldIndex]->getShape2()->getLocalToWorldTransform());
- needToRemovePoint = false;
- newPoint->isUsed = true;
+ oldContactPoint->update(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
+ isSimilarPointFound = true;
break;
}
- newPoint = newPoint->next;
+ oldContactPoint = oldContactPoint->getNext();
}
- // If no new contact point is similar to the old one
- if (needToRemovePoint) {
+ // If we have not found a similar contact point
+ if (!isSimilarPointFound) {
- // Remove the old contact point
- mManifolds[oldManifoldIndex]->removeContactPoint(i);
- }
- else {
- i++;
+ // Add the contact point to the manifold
+ oldManifold->addContactPoint(contactPointInfo);
}
+
+ contactPointInfo = contactPointInfo->next;
}
- // For each point of the new manifold that have not been used yet (to update
- // an existing point in the previous manifold), add it into the previous manifold
- const ContactPointInfo* newPointInfo = newManifold.getFirstContactPointInfo();
- while (newPointInfo != nullptr) {
-
- if (!newPointInfo->isUsed) {
- mManifolds[oldManifoldIndex]->addContactPoint(newPointInfo);
- }
-
- newPointInfo = newPointInfo->next;
- }
+ // The old manifold is no longer obselete
+ oldManifold->setIsObselete(false, false);
}
-// Return the manifold with the smallest contact penetration depth
-int ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const {
+// Return the manifold with the smallest contact penetration depth among its points
+ContactManifold* ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const {
- // The contact point will be in a new contact manifold, we now have too much
- // manifolds condidates. We need to remove one. We choose to remove the manifold
- // with the smallest contact depth among their points
- int smallestDepthManifoldIndex = -1;
- decimal minDepth = initDepth;
assert(mNbManifolds == mNbMaxManifolds);
- for (int i=0; i<mNbManifolds; i++) {
- decimal depth = mManifolds[i]->getLargestContactDepth();
+
+ ContactManifold* minDepthManifold = nullptr;
+ decimal minDepth = initDepth;
+ ContactManifold* manifold = mManifolds;
+ while (manifold != nullptr) {
+ decimal depth = manifold->getLargestContactDepth();
if (depth < minDepth) {
minDepth = depth;
- smallestDepthManifoldIndex = i;
+ minDepthManifold = manifold;
}
+
+ manifold = manifold->getNext();
}
- return smallestDepthManifoldIndex;
+ return minDepthManifold;
}
-// Return the index of the contact manifold with a similar average normal.
-// If no manifold has close enough average normal, it returns -1
-int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
+// Return the contact manifold with a similar average normal.
+// If no manifold has close enough average normal, it returns nullptr
+ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
+
+ ContactManifold* manifold = mManifolds;
// Return the Id of the manifold with the same normal direction id (if exists)
- for (int i=0; i<mNbManifolds; i++) {
- if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) {
- return i;
+ while (manifold != nullptr) {
+ if (normalDirectionId == manifold->getContactNormalId()) {
+ return manifold;
}
+
+ manifold = manifold->getNext();
}
- return -1;
+ return nullptr;
}
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket
-short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const {
+short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) {
assert(normal.lengthSquare() > MACHINE_EPSILON);
@@ -240,36 +206,100 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
// Clear the contact manifold set
void ContactManifoldSet::clear() {
- // Destroy all the contact manifolds
- for (int i=mNbManifolds-1; i>=0; i--) {
- removeManifold(i);
+ ContactManifold* manifold = mManifolds;
+ while(manifold != nullptr) {
+
+ ContactManifold* nextManifold = manifold->getNext();
+
+ // Delete the contact manifold
+ manifold->~ContactManifold();
+ mMemoryAllocator.release(manifold, sizeof(ContactManifold));
+
+ manifold = nextManifold;
+
+ mNbManifolds--;
}
assert(mNbManifolds == 0);
}
// Create a new contact manifold and add it to the set
-void ContactManifoldSet::createManifold(const ContactManifoldInfo& manifoldInfo, short int normalDirectionId) {
+void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) {
assert(mNbManifolds < mNbMaxManifolds);
- mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
- ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator, normalDirectionId);
+ ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
+ ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator);
+ manifold->setPrevious(nullptr);
+ manifold->setNext(mManifolds);
+ mManifolds = manifold;
+
mNbManifolds++;
}
// Remove a contact manifold from the set
-void ContactManifoldSet::removeManifold(int index) {
+void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
assert(mNbManifolds > 0);
- assert(index >= 0 && index < mNbManifolds);
- // Delete the new contact
- mManifolds[index]->~ContactManifold();
- mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
+ ContactManifold* previous = manifold->getPrevious();
+ ContactManifold* next = manifold->getNext();
- for (int i=index; (i+1) < mNbManifolds; i++) {
- mManifolds[i] = mManifolds[i+1];
+ if (previous != nullptr) {
+ previous->setNext(manifold->getNext());
}
+ if (next != nullptr) {
+ next->setPrevious(manifold->getPrevious());
+ }
+
+ // Delete the contact manifold
+ manifold->~ContactManifold();
+ mMemoryAllocator.release(manifold, sizeof(ContactManifold));
mNbManifolds--;
}
+
+// Make all the contact manifolds and contact points obselete
+void ContactManifoldSet::makeContactsObselete() {
+
+ ContactManifold* manifold = mManifolds;
+ while (manifold != nullptr) {
+
+ manifold->setIsObselete(true, true);
+
+ manifold = manifold->getNext();
+ }
+}
+
+// Clear the obselete contact manifolds and contact points
+void ContactManifoldSet::clearObseleteManifoldsAndContactPoints() {
+
+ ContactManifold* manifold = mManifolds;
+ ContactManifold* previousManifold = nullptr;
+ while (manifold != nullptr) {
+ ContactManifold* nextManifold = manifold->getNext();
+
+ if (manifold->getIsObselete()) {
+
+ if (previousManifold != nullptr) {
+ previousManifold->setNext(nextManifold);
+
+ if (nextManifold != nullptr) {
+ nextManifold->setPrevious(previousManifold);
+ }
+ }
+ else {
+ mManifolds = nextManifold;
+ }
+
+ // Delete the contact manifold
+ removeManifold(manifold);
+
+ }
+ else {
+ manifold->clearObseleteContactPoints();
+ previousManifold = manifold;
+ }
+
+ manifold = nextManifold;
+ }
+}
diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h
index 1a2bac97..0990277c 100644
--- a/src/collision/ContactManifoldSet.h
+++ b/src/collision/ContactManifoldSet.h
@@ -60,33 +60,34 @@ class ContactManifoldSet {
/// Pointer to the second proxy shape of the contact
ProxyShape* mShape2;
- /// Reference to the memory allocator
- PoolAllocator& mMemoryAllocator;
+ /// Reference to the memory allocator for the contact manifolds
+ Allocator& mMemoryAllocator;
/// Contact manifolds of the set
- ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET];
+ ContactManifold* mManifolds;
// -------------------- Methods -------------------- //
/// Create a new contact manifold and add it to the set
- void createManifold(const ContactManifoldInfo& manifoldInfo, short normalDirectionId);
+ void createManifold(const ContactManifoldInfo* manifoldInfo);
- /// Remove a contact manifold from the set
- void removeManifold(int index);
-
- // Return the index of the contact manifold with a similar average normal.
- int selectManifoldWithSimilarNormal(short int normalDirectionId) const;
-
- // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
- // Each face of the cube is divided into 4x4 buckets. This method maps the
- // normal vector into of the of the bucket and returns a unique Id for the bucket
- short int computeCubemapNormalId(const Vector3& normal) const;
+ // Return the contact manifold with a similar average normal.
+ ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const;
/// Return the manifold with the smallest contact penetration depth
- int getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const;
+ ContactManifold* getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const;
/// Update a previous similar manifold with a new one
- void updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold);
+ void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold);
+
+ /// Return the maximum number of contact manifolds allowed between to collision shapes
+ int computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2);
+
+ /// Clear the contact manifold set
+ void clear();
+
+ /// Delete a contact manifold
+ void removeManifold(ContactManifold* manifold);
public:
@@ -94,13 +95,13 @@ class ContactManifoldSet {
/// Constructor
ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
- PoolAllocator& memoryAllocator, int nbMaxManifolds);
+ Allocator& memoryAllocator);
/// Destructor
~ContactManifoldSet();
/// Add a contact manifold in the set
- void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
+ void addContactManifold(const ContactManifoldInfo* contactManifoldInfo);
/// Return the first proxy shape
ProxyShape* getShape1() const;
@@ -108,17 +109,25 @@ class ContactManifoldSet {
/// Return the second proxy shape
ProxyShape* getShape2() const;
- /// Clear the contact manifold set
- void clear();
-
/// Return the number of manifolds in the set
int getNbContactManifolds() const;
- /// Return a given contact manifold
- ContactManifold* getContactManifold(int index) const;
+ /// Return a pointer to the first element of the linked-list of contact manifolds
+ ContactManifold* getContactManifolds() const;
+
+ /// Make all the contact manifolds and contact points obselete
+ void makeContactsObselete();
/// Return the total number of contact points in the set of manifolds
int getTotalNbContactPoints() const;
+
+ /// Clear the obselete contact manifolds and contact points
+ void clearObseleteManifoldsAndContactPoints();
+
+ // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
+ // Each face of the cube is divided into 4x4 buckets. This method maps the
+ // normal vector into of the of the bucket and returns a unique Id for the bucket
+ static short int computeCubemapNormalId(const Vector3& normal);
};
// Return the first proxy shape
@@ -136,21 +145,38 @@ inline int ContactManifoldSet::getNbContactManifolds() const {
return mNbManifolds;
}
-// Return a given contact manifold
-inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const {
- assert(index >= 0 && index < mNbManifolds);
- return mManifolds[index];
+// Return a pointer to the first element of the linked-list of contact manifolds
+inline ContactManifold* ContactManifoldSet::getContactManifolds() const {
+ return mManifolds;
}
// Return the total number of contact points in the set of manifolds
inline int ContactManifoldSet::getTotalNbContactPoints() const {
int nbPoints = 0;
- for (int i=0; i<mNbManifolds; i++) {
- nbPoints += mManifolds[i]->getNbContactPoints();
+
+ ContactManifold* manifold = mManifolds;
+ while (manifold != nullptr) {
+ nbPoints += manifold->getNbContactPoints();
+
+ manifold = manifold->getNext();
}
+
return nbPoints;
}
+// Return the maximum number of contact manifolds allowed between to collision shapes
+inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2) {
+
+ // If both shapes are convex
+ if (shape1->isConvex() && shape2->isConvex()) {
+ return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
+
+ } // If there is at least one concave shape
+ else {
+ return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
+ }
+}
+
}
#endif
diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp
new file mode 100644
index 00000000..22ee6666
--- /dev/null
+++ b/src/collision/NarrowPhaseInfo.cpp
@@ -0,0 +1,97 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include <iostream>
+#include "NarrowPhaseInfo.h"
+#include "ContactPointInfo.h"
+#include "engine/OverlappingPair.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1,
+ const CollisionShape* shape2, const Transform& shape1Transform,
+ const Transform& shape2Transform, void** cachedData1, void** cachedData2)
+ : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
+ shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
+ contactPoints(nullptr), cachedCollisionData1(cachedData1),
+ cachedCollisionData2(cachedData2), next(nullptr) {
+
+}
+
+// Destructor
+NarrowPhaseInfo::~NarrowPhaseInfo() {
+
+ assert(contactPoints == nullptr);
+}
+
+// Add a new contact point
+void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth,
+ const Vector3& localPt1, const Vector3& localPt2) {
+
+ assert(penDepth > decimal(0.0));
+
+ // Get the memory allocator
+ Allocator& allocator = overlappingPair->getTemporaryAllocator();
+
+ // Create the contact point info
+ ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo)))
+ ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
+
+ // Add it into the linked list of contact points
+ contactPointInfo->next = contactPoints;
+ contactPoints = contactPointInfo;
+}
+
+/// Take all the generated contact points and create a new potential
+/// contact manifold into the overlapping pair
+void NarrowPhaseInfo::addContactPointsAsPotentialContactManifold() {
+ overlappingPair->addPotentialContactPoints(this);
+}
+
+// Reset the remaining contact points
+void NarrowPhaseInfo::resetContactPoints() {
+
+ // Get the memory allocator
+ Allocator& allocator = overlappingPair->getTemporaryAllocator();
+
+ // For each remaining contact point info
+ ContactPointInfo* element = contactPoints;
+ while(element != nullptr) {
+
+ ContactPointInfo* elementToDelete = element;
+
+ element = element->next;
+
+ // Call the destructor
+ elementToDelete->~ContactPointInfo();
+
+ // Delete the current element
+ allocator.release(elementToDelete, sizeof(ContactPointInfo));
+ }
+
+ contactPoints = nullptr;
+}
diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h
index b97a71be..29bc6c9d 100644
--- a/src/collision/NarrowPhaseInfo.h
+++ b/src/collision/NarrowPhaseInfo.h
@@ -28,6 +28,7 @@
// Libraries
#include "shapes/CollisionShape.h"
+#include "collision/ContactManifoldInfo.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@@ -58,6 +59,9 @@ struct NarrowPhaseInfo {
/// Transform that maps from collision shape 2 local-space to world-space
Transform shape2ToWorldTransform;
+ /// Linked-list of contact points created during the narrow-phase
+ ContactPointInfo* contactPoints;
+
/// Cached collision data of the proxy shape
// TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
void** cachedCollisionData1;
@@ -72,12 +76,20 @@ struct NarrowPhaseInfo {
/// Constructor
NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1,
const CollisionShape* shape2, const Transform& shape1Transform,
- const Transform& shape2Transform, void** cachedData1, void** cachedData2)
- : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
- shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
- cachedCollisionData1(cachedData1), cachedCollisionData2(cachedData2), next(nullptr) {
+ const Transform& shape2Transform, void** cachedData1, void** cachedData2);
- }
+ /// Destructor
+ ~NarrowPhaseInfo();
+
+ /// Add a new contact point
+ void addContactPoint(const Vector3& contactNormal, decimal penDepth,
+ const Vector3& localPt1, const Vector3& localPt2);
+
+ /// Create a new potential contact manifold into the overlapping pair using current contact points
+ void addContactPointsAsPotentialContactManifold();
+
+ /// Reset the remaining contact points
+ void resetContactPoints();
};
}
diff --git a/src/collision/OverlapCallback.h b/src/collision/OverlapCallback.h
new file mode 100644
index 00000000..bbba73d1
--- /dev/null
+++ b/src/collision/OverlapCallback.h
@@ -0,0 +1,57 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_OVERLAP_CALLBACK_H
+#define REACTPHYSICS3D_OVERLAP_CALLBACK_H
+
+// Libraries
+#include "body/CollisionBody.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class OverlapCallback
+/**
+ * This class can be used to register a callback for collision overlap queries.
+ * You should implement your own class inherited from this one and implement
+ * the notifyOverlap() method. This method will called each time a contact
+ * point is reported.
+ */
+class OverlapCallback {
+
+ public:
+
+ /// Destructor
+ virtual ~OverlapCallback() {
+
+ }
+
+ /// This method will be called for each reported overlapping bodies
+ virtual void notifyOverlap(CollisionBody* collisionBody)=0;
+};
+
+}
+
+#endif
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
index 17008622..f98e4d20 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
@@ -33,7 +33,7 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two capsules
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
-bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
+bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
@@ -116,8 +116,8 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
decimal penetrationDepth = sumRadius - segmentsDistance;
// Create the contact info object
- contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
- contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
return true;
}
@@ -148,7 +148,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
decimal penetrationDepth = sumRadius - closestPointsDistance;
// Create the contact info object
- contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
return true;
}
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
index 9d8393c9..0b19338f 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
@@ -61,8 +61,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two capsules
- virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index b538dfa7..88bd9ace 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -37,13 +37,12 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a capsule and a polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
-bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) {
+bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
SATAlgorithm satAlgorithm;
- GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
+ GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
@@ -61,8 +60,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
// 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);
+ ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
+ assert(contactPoint != nullptr);
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
@@ -96,7 +95,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
// Remove the previous contact point computed by GJK
- contactManifoldInfo.reset();
+ narrowPhaseInfo->resetContactPoints();
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
@@ -116,7 +115,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
- contactManifoldInfo, isCapsuleShape1);
+ narrowPhaseInfo, isCapsuleShape1);
break;
}
@@ -133,7 +132,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
- bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+ bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
index 6b3f504f..c0dd46d5 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
@@ -61,8 +61,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a capsule and a polyhedron
- virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}
diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
index c0da7611..7f5f439b 100644
--- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
+++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
@@ -141,114 +141,114 @@ void ConcaveVsConvexAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
// Process the concave triangle mesh collision using the smooth mesh collision algorithm described
// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
// issue with some internal edges.
-void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
- std::vector<SmoothMeshContactInfo> contactPoints,
- NarrowPhaseCallback* narrowPhaseCallback) {
+//void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
+// std::vector<SmoothMeshContactInfo> contactPoints,
+// NarrowPhaseCallback* narrowPhaseCallback) {
- // Set with the triangle vertices already processed to void further contacts with same triangle
- std::unordered_multimap<int, Vector3> processTriangleVertices;
+// // Set with the triangle vertices already processed to void further contacts with same triangle
+// std::unordered_multimap<int, Vector3> processTriangleVertices;
- // Sort the list of narrow-phase contacts according to their penetration depth
- std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
+// // Sort the list of narrow-phase contacts according to their penetration depth
+// std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
- // For each contact point (from smaller penetration depth to larger)
- std::vector<SmoothMeshContactInfo>::const_iterator it;
- for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
+// // For each contact point (from smaller penetration depth to larger)
+// std::vector<SmoothMeshContactInfo>::const_iterator it;
+// for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
- const SmoothMeshContactInfo info = *it;
- const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+// const SmoothMeshContactInfo info = *it;
+// const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
- // Compute the barycentric coordinates of the point in the triangle
- decimal u, v, w;
- computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
- info.triangleVertices[1],
- info.triangleVertices[2],
- contactPoint, u, v, w);
- int nbZeros = 0;
- bool isUZero = approxEqual(u, 0, 0.0001);
- bool isVZero = approxEqual(v, 0, 0.0001);
- bool isWZero = approxEqual(w, 0, 0.0001);
- if (isUZero) nbZeros++;
- if (isVZero) nbZeros++;
- if (isWZero) nbZeros++;
+// // Compute the barycentric coordinates of the point in the triangle
+// decimal u, v, w;
+// computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
+// info.triangleVertices[1],
+// info.triangleVertices[2],
+// contactPoint, u, v, w);
+// int nbZeros = 0;
+// bool isUZero = approxEqual(u, 0, 0.0001);
+// bool isVZero = approxEqual(v, 0, 0.0001);
+// bool isWZero = approxEqual(w, 0, 0.0001);
+// if (isUZero) nbZeros++;
+// if (isVZero) nbZeros++;
+// if (isWZero) nbZeros++;
- // If it is a vertex contact
- if (nbZeros == 2) {
+// // If it is a vertex contact
+// if (nbZeros == 2) {
- Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
+// Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
- // Check that this triangle vertex has not been processed yet
- if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
+// // Check that this triangle vertex has not been processed yet
+// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
- // Keep the contact as it is and report it
- narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
- }
- }
- else if (nbZeros == 1) { // If it is an edge contact
+// // Keep the contact as it is and report it
+// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
+// }
+// }
+// else if (nbZeros == 1) { // If it is an edge contact
- Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
- Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
+// Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
+// Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
- // Check that this triangle edge has not been processed yet
- if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
- !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
+// // Check that this triangle edge has not been processed yet
+// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
+// !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
- // Keep the contact as it is and report it
- narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
- }
+// // Keep the contact as it is and report it
+// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
+// }
- }
- else { // If it is a face contact
+// }
+// else { // If it is a face contact
- ContactPointInfo newContactInfo(info.contactInfo);
+// ContactPointInfo newContactInfo(info.contactInfo);
- ProxyShape* firstShape;
- ProxyShape* secondShape;
- if (info.isFirstShapeTriangle) {
- firstShape = overlappingPair->getShape1();
- secondShape = overlappingPair->getShape2();
- }
- else {
- firstShape = overlappingPair->getShape2();
- secondShape = overlappingPair->getShape1();
- }
+// ProxyShape* firstShape;
+// ProxyShape* secondShape;
+// if (info.isFirstShapeTriangle) {
+// firstShape = overlappingPair->getShape1();
+// secondShape = overlappingPair->getShape2();
+// }
+// else {
+// firstShape = overlappingPair->getShape2();
+// secondShape = overlappingPair->getShape1();
+// }
- // We use the triangle normal as the contact normal
- Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
- Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
- Vector3 localNormal = a.cross(b);
- newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
- Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
- Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
- newContactInfo.normal.normalize();
- if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
- newContactInfo.normal = -newContactInfo.normal;
- }
+// // We use the triangle normal as the contact normal
+// Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
+// Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
+// Vector3 localNormal = a.cross(b);
+// newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
+// Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+// Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
+// newContactInfo.normal.normalize();
+// if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
+// newContactInfo.normal = -newContactInfo.normal;
+// }
- // We recompute the contact point on the second body with the new normal as described in
- // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
- // Dirk Gregorius) to avoid adding torque
- Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
- if (info.isFirstShapeTriangle) {
- Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
- newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
- }
- else {
- Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
- newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
- }
+// // We recompute the contact point on the second body with the new normal as described in
+// // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
+// // Dirk Gregorius) to avoid adding torque
+// Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
+// if (info.isFirstShapeTriangle) {
+// Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
+// newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
+// }
+// else {
+// Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
+// newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
+// }
- // Report the contact
- narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
- }
+// // Report the contact
+// narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
+// }
- // Add the three vertices of the triangle to the set of processed
- // triangle vertices
- addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
- addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
- addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
- }
-}
+// // Add the three vertices of the triangle to the set of processed
+// // triangle vertices
+// addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
+// addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
+// addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
+// }
+//}
// Return true if the vertex is in the set of already processed vertices
bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) const {
diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
index 531615da..4619e480 100644
--- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
+++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
@@ -83,25 +83,35 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
// Class SmoothMeshContactInfo
/**
- * This class is used to store data about a contact with a triangle for the smooth
- * mesh algorithm.
+ * Contains data for of potential smooth contact during the smooth mesh
+ * contacts computation.
*/
-class SmoothMeshContactInfo {
+struct SmoothMeshContactInfo {
public:
- ContactPointInfo contactInfo;
+ ContactManifoldInfo* contactManifoldInfo;
+ ContactPointInfo* contactInfo;
bool isFirstShapeTriangle;
Vector3 triangleVertices[3];
+ bool isUVWZero[3];
/// Constructor
- SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1,
- const Vector3& trianglePoint2, const Vector3& trianglePoint3)
- : contactInfo(contact) {
+ SmoothMeshContactInfo(ContactManifoldInfo* manifoldInfo, ContactPointInfo* contactPointInfo,
+ bool firstShapeTriangle,
+ const Vector3& trianglePoint1, const Vector3& trianglePoint2,
+ const Vector3& trianglePoint3, bool isUZero, bool isVZero, bool isWZero)
+ : contactManifoldInfo(manifoldInfo), contactInfo(contactPointInfo) {
+
isFirstShapeTriangle = firstShapeTriangle;
+
triangleVertices[0] = trianglePoint1;
triangleVertices[1] = trianglePoint2;
triangleVertices[2] = trianglePoint3;
+
+ isUVWZero[0] = isUZero;
+ isUVWZero[1] = isVZero;
+ isUVWZero[2] = isWZero;
}
};
@@ -109,7 +119,7 @@ class SmoothMeshContactInfo {
struct ContactsDepthCompare {
bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2)
{
- return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
+ return contact1.contactInfo->penetrationDepth < contact2.contactInfo->penetrationDepth;
}
};
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
index 16b9edf5..c6297fb2 100644
--- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
@@ -34,12 +34,11 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two convex polyhedra
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
-bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) {
+bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
- bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+ bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
index ed76d6fc..1e0c9748 100644
--- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
@@ -61,8 +61,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm
ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two convex polyhedra
- virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index 4f7e1177..ff81782f 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -46,8 +46,7 @@ using namespace reactphysics3d;
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects.
-GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) {
+GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
PROFILE("GJKAlgorithm::testCollision()");
@@ -209,7 +208,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro
}
// Add a new contact point
- contactManifoldInfo.addContactPoint(normal, penetrationDepth, pA, pB);
+ narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB);
return GJKResult::COLLIDE_IN_MARGIN;
}
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h
index f40592c4..b8c7319c 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.h
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h
@@ -87,16 +87,13 @@ class GJKAlgorithm {
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volumes collide.
- GJKResult testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo);
+ GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
-
-
};
}
diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
index f665181b..035bd8e5 100644
--- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h
+++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
@@ -83,8 +83,10 @@ class NarrowPhaseAlgorithm {
/// Deleted assignment operator
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
+ // TODO : Use the following reportContacts variable in all narrow-phase algorithms
+
/// Compute a contact info if the two bounding volume collide
- virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo)=0;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0;
};
}
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index c838d8ab..4974d367 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -44,7 +44,7 @@ using namespace reactphysics3d;
const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
// Test collision between a sphere and a convex mesh
-bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()");
@@ -145,7 +145,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
// Create the contact info object
- contactManifoldInfo.addContactPoint(isSphereShape1 ? normalWorld : -normalWorld, minPenetrationDepth,
+ narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
@@ -171,7 +171,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd
}
// Test collision between a capsule and a convex mesh
-bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()");
@@ -387,7 +387,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
- contactManifoldInfo, isCapsuleShape1);
+ narrowPhaseInfo, isCapsuleShape1);
lastFrameInfo.satIsAxisFacePolyhedron1 = true;
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
@@ -406,7 +406,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
// Create the contact point
- contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+ narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
@@ -477,7 +477,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
- ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const {
+ NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const {
HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
uint firstEdgeIndex = face.edgeIndex;
@@ -523,7 +523,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius;
// Create the contact point
- contactManifoldInfo.addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth,
+ narrowPhaseInfo->addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth,
isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
}
@@ -543,8 +543,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
}
// Test collision between two convex polyhedrons
-bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) const {
+bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo,
+ bool reportContacts) const {
PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()");
@@ -869,7 +869,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
// Create a new contact point
- contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+ narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
}
@@ -893,7 +893,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
// Create the contact point
- contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+ narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
lastFrameInfo.satIsAxisFacePolyhedron1 = false;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 7a002fce..49cd2626 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -113,24 +113,24 @@ class SATAlgorithm {
SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete;
/// Test collision between a sphere and a convex mesh
- bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+ bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
/// Test collision between a capsule and a convex mesh
- bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+ bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
/// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
- ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const;
+ NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const;
// This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference
bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
const Vector3& edgeAdjacentFace2Normal) const;
/// Test collision between two convex meshes
- bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+ bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
};
}
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
index 86255c2b..e291fd83 100755
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
@@ -34,7 +34,7 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a capsule
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
-bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
+bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
@@ -86,7 +86,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
}
// Create the contact info object
- contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
index 154f6e4c..bb9e9866 100644
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
@@ -61,8 +61,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a capsule
- virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index d90a29b3..049ce449 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -34,12 +34,11 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
-bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) {
+bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
- GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
+ GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
@@ -61,7 +60,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
- bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+ bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
index f2f8d6e5..2d9fa801 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
@@ -61,8 +61,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
- virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
index 54e51e52..94211ac0 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
@@ -30,8 +30,7 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) {
+bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
@@ -62,7 +61,7 @@ bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseIn
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
- contactManifoldInfo.addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
+ narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
return true;
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
index b70befd2..1fd885a5 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
@@ -61,8 +61,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
- virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- ContactManifoldInfo& contactManifoldInfo) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
};
}
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index d2b92dcc..5a07b0cb 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -237,6 +237,8 @@ inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const {
case 4: return Vector3(0, -1, 0);
case 5: return Vector3(0, 1, 0);
}
+
+ assert(false);
}
// Return the centroid of the box
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index 2b1fa576..78b82ae0 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -118,10 +118,6 @@ class CollisionShape {
/// Return true if the collision shape type is a convex shape
static bool isConvex(CollisionShapeType shapeType);
- /// Return the maximum number of contact manifolds in an overlapping pair given two shape types
- static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
- CollisionShapeType shapeType2);
-
// -------------------- Friendship -------------------- //
friend class ProxyShape;
@@ -151,19 +147,6 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
mScaling = scaling;
}
-// Return the maximum number of contact manifolds allowed in an overlapping
-// pair wit the given two collision shape types
-inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
- CollisionShapeType shapeType2) {
- // If both shapes are convex
- if (isConvex(shapeType1) && isConvex(shapeType2)) {
- return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
- } // If there is at least one concave shape
- else {
- return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
- }
-}
-
}
#endif
diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp
index f420bbff..fa6838ec 100644
--- a/src/constraint/ContactPoint.cpp
+++ b/src/constraint/ContactPoint.cpp
@@ -37,19 +37,20 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform&
mPenetrationDepth(contactInfo->penetrationDepth),
mLocalPointOnBody1(contactInfo->localPoint1),
mLocalPointOnBody2(contactInfo->localPoint2),
- mIsRestingContact(false) {
+ mIsRestingContact(false), mIsObselete(false), mNext(nullptr) {
assert(mPenetrationDepth > decimal(0.0));
// Compute the world position of the contact points
mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
+
+ mIsObselete = false;
}
// Update the contact point with a new one that is similar (very close)
/// The idea is to keep the cache impulse (for warm starting the contact solver)
-void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
- const Transform& body2Transform) {
+void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, const Transform& body2Transform) {
assert(isSimilarWithContactPoint(contactInfo));
assert(contactInfo->penetrationDepth > decimal(0.0));
@@ -62,4 +63,6 @@ void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform&
// Compute the world position of the contact points
mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
+
+ mIsObselete = false;
}
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index ec4c6556..41cc35e4 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -71,6 +71,37 @@ class ContactPoint {
/// Cached penetration impulse
decimal mPenetrationImpulse;
+ /// True if the contact point is obselete
+ bool mIsObselete;
+
+ /// Pointer to the next contact point in the linked-list
+ ContactPoint* mNext;
+
+ // -------------------- Methods -------------------- //
+
+ /// Update the contact point with a new one that is similar (very close)
+ void update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
+ const Transform& body2Transform);
+
+ /// Return true if the contact point is similar (close enougth) to another given contact point
+ bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
+
+ /// Set the cached penetration impulse
+ void setPenetrationImpulse(decimal impulse);
+
+
+ /// Set the mIsRestingContact variable
+ void setIsRestingContact(bool isRestingContact);
+
+ /// Set to true to make the contact point obselete
+ void setIsObselete(bool isObselete);
+
+ /// Set the next contact point in the linked list
+ void setNext(ContactPoint* next);
+
+ /// Return true if the contact point is obselete
+ bool getIsObselete() const;
+
public :
// -------------------- Methods -------------------- //
@@ -88,10 +119,6 @@ class ContactPoint {
/// Deleted assignment operator
ContactPoint& operator=(const ContactPoint& contact) = delete;
- /// Update the contact point with a new one that is similar (very close)
- void update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
- const Transform& body2Transform);
-
/// Return the normal vector of the contact
Vector3 getNormal() const;
@@ -110,23 +137,22 @@ class ContactPoint {
/// Return the cached penetration impulse
decimal getPenetrationImpulse() const;
- /// Return true if the contact point is similar (close enougth) to another given contact point
- bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
-
- /// Set the cached penetration impulse
- void setPenetrationImpulse(decimal impulse);
-
/// Return true if the contact is a resting contact
bool getIsRestingContact() const;
- /// Set the mIsRestingContact variable
- void setIsRestingContact(bool isRestingContact);
+ /// Return the next contact point in the linked list
+ ContactPoint* getNext() const;
/// Return the penetration depth
decimal getPenetrationDepth() const;
/// Return the number of bytes used by the contact point
size_t getSizeInBytes() const;
+
+ // Friendship
+ friend class ContactManifold;
+ friend class ContactManifoldSet;
+ friend class ContactSolver;
};
// Return the normal vector of the contact
@@ -180,6 +206,26 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
mIsRestingContact = isRestingContact;
}
+// Return true if the contact point is obselete
+inline bool ContactPoint::getIsObselete() const {
+ return mIsObselete;
+}
+
+// Set to true to make the contact point obselete
+inline void ContactPoint::setIsObselete(bool isObselete) {
+ mIsObselete = isObselete;
+}
+
+// Return the next contact point in the linked list
+inline ContactPoint* ContactPoint::getNext() const {
+ return mNext;
+}
+
+// Set the next contact point in the linked list
+inline void ContactPoint::setNext(ContactPoint* next) {
+ mNext = next;
+}
+
// Return the penetration depth of the contact
inline decimal ContactPoint::getPenetrationDepth() const {
return mPenetrationDepth;
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index 0e24f0c8..82d2601f 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -233,64 +233,6 @@ inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* ov
mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits);
}
-// Class CollisionCallback
-/**
- * This class can be used to register a callback for collision test queries.
- * You should implement your own class inherited from this one and implement
- * the notifyContact() method. This method will called each time a contact
- * point is reported.
- */
-class CollisionCallback {
-
- public:
-
- struct CollisionCallbackInfo {
-
- public:
- const ContactManifoldInfo& contactManifold;
- CollisionBody* body1;
- CollisionBody* body2;
- const ProxyShape* proxyShape1;
- const ProxyShape* proxyShape2;
-
- // Constructor
- CollisionCallbackInfo(const ContactManifoldInfo& manifold, CollisionBody* b1, CollisionBody* b2,
- const ProxyShape* proxShape1, const ProxyShape* proxShape2) :
- contactManifold(manifold), body1(b1), body2(b2),
- proxyShape1(proxShape1), proxyShape2(proxShape2) {
-
- }
- };
-
- /// Destructor
- virtual ~CollisionCallback() {
-
- }
-
- /// This method will be called for each reported contact point
- virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
-};
-
-// Class OverlapCallback
-/**
- * This class can be used to register a callback for collision overlap queries.
- * You should implement your own class inherited from this one and implement
- * the notifyOverlap() method. This method will called each time a contact
- * point is reported.
- */
-class OverlapCallback {
-
- public:
-
- /// Destructor
- virtual ~OverlapCallback() {
-
- }
-
- /// This method will be called for each reported overlapping bodies
- virtual void notifyOverlap(CollisionBody* collisionBody)=0;
-};
-
}
#endif
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index d5825d5c..94a82b14 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -148,10 +148,8 @@ void ContactSolver::initializeForIsland(Island* island) {
const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
// For each contact point of the contact manifold
- for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
-
- // Get a contact point
- ContactPoint* externalContact = externalManifold->getContactPoint(c);
+ ContactPoint* externalContact = externalManifold->getContactPoints();
+ while (externalContact != nullptr) {
// Get the contact point on the two bodies
Vector3 p1 = externalContact->getWorldPointOnBody1();
@@ -200,6 +198,8 @@ void ContactSolver::initializeForIsland(Island* island) {
mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal;
mNbContactPoints++;
+
+ externalContact = externalContact->getNext();
}
mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 4f7bd15a..136d34f6 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -683,9 +683,9 @@ void DynamicsWorld::computeIslands() {
// For each contact manifold in which the current body is involded
ContactManifoldListElement* contactElement;
for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != nullptr;
- contactElement = contactElement->next) {
+ contactElement = contactElement->getNext()) {
- ContactManifold* contactManifold = contactElement->contactManifold;
+ ContactManifold* contactManifold = contactElement->getContactManifold();
assert(contactManifold->getNbContactPoints() > 0);
@@ -842,12 +842,13 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
// For each contact manifold of the pair
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
- for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
-
- ContactManifold* manifold = manifoldSet.getContactManifold(i);
+ ContactManifold* manifold = manifoldSet.getContactManifolds();
+ while (manifold != nullptr) {
// Get the contact manifold
contactManifolds.push_back(manifold);
+
+ manifold = manifold->getNext();
}
}
diff --git a/src/engine/EventListener.h b/src/engine/EventListener.h
index e8594255..061724a0 100644
--- a/src/engine/EventListener.h
+++ b/src/engine/EventListener.h
@@ -27,7 +27,7 @@
#define REACTPHYSICS3D_EVENT_LISTENER_H
// Libraries
-#include "collision/ContactManifoldInfo.h"
+#include "collision/CollisionCallback.h"
namespace reactphysics3d {
@@ -49,17 +49,11 @@ class EventListener {
/// Destructor
virtual ~EventListener() = default;
- /// Called when a new contact point is found between two bodies that were separated before
- /**
- * @param contact Information about the contact
- */
- virtual void beginContact(const ContactManifoldInfo& contactManifold) {}
-
/// Called when a new contact point is found between two bodies
/**
* @param contact Information about the contact
*/
- virtual void newContact(const ContactManifoldInfo& contactManifold) {}
+ virtual void newContact(const CollisionCallback::CollisionCallbackInfo& collisionInfo) {}
/// Called at the beginning of an internal tick of the simulation step.
/// Each time the DynamicsWorld::update() method is called, the physics
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index ce628b7d..a23857e3 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -26,12 +26,108 @@
// Libraries
#include <cassert>
#include "OverlappingPair.h"
+#include "collision/ContactManifoldInfo.h"
using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
- int nbMaxContactManifolds, PoolAllocator& memoryAllocator)
- : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds) {
+ Allocator& manifoldsAllocator, Allocator& temporaryMemoryAllocator)
+ : mContactManifoldSet(shape1, shape2, manifoldsAllocator), mPotentialContactManifolds(nullptr),
+ mTempMemoryAllocator(temporaryMemoryAllocator) {
}
+
+// Create a new potential contact manifold using contact-points from narrow-phase
+void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) {
+
+ assert(narrowPhaseInfo->contactPoints != nullptr);
+
+ // For each potential contact point to add
+ ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
+ while (contactPoint != nullptr) {
+
+ ContactPointInfo* nextContactPoint = contactPoint->next;
+
+ // Compute the contact normal id
+ short contactNormalId = ContactManifoldSet::computeCubemapNormalId(contactPoint->normal);
+
+ // Look if the contact point correspond to an existing potential manifold
+ // (if the contact point normal is similar to the normal of an existing manifold)
+ ContactManifoldInfo* manifold = mPotentialContactManifolds;
+ bool similarManifoldFound = false;
+ while(manifold != nullptr) {
+
+ // If we have found a corresponding manifold for the new contact point
+ // (a manifold with a similar contact normal direction)
+ if (manifold->getContactNormalId() == contactNormalId) {
+
+ // Add the contact point to the manifold
+ manifold->addContactPoint(contactPoint, contactNormalId);
+
+ similarManifoldFound = true;
+
+ break;
+ }
+
+ manifold = manifold->getNext();
+ }
+
+ // If we have not found an existing manifold with a similar contact normal
+ if (!similarManifoldFound) {
+
+ // Create a new potential contact manifold
+ ContactManifoldInfo* manifoldInfo = new (mTempMemoryAllocator.allocate(sizeof(ContactManifoldInfo)))
+ ContactManifoldInfo(mTempMemoryAllocator);
+
+ // Add the manifold into the linked-list of potential contact manifolds
+ manifoldInfo->mNext = mPotentialContactManifolds;
+ mPotentialContactManifolds = manifoldInfo;
+
+ // Add the contact point to the manifold
+ manifoldInfo->addContactPoint(contactPoint, contactNormalId);
+ }
+
+ contactPoint = nextContactPoint;
+ }
+
+ // All the contact point info of the narrow-phase info have been moved
+ // into the potential contacts of the overlapping pair
+ narrowPhaseInfo->contactPoints = nullptr;
+}
+
+// Clear all the potential contact manifolds
+void OverlappingPair::clearPotentialContactManifolds() {
+
+ // Do we need to release memory
+ if (mTempMemoryAllocator.isReleaseNeeded()) {
+
+ ContactManifoldInfo* element = mPotentialContactManifolds;
+ while(element != nullptr) {
+
+ // Remove the proxy collision shape
+ ContactManifoldInfo* elementToRemove = element;
+ element = element->getNext();
+
+ // Delete the element
+ elementToRemove->~ContactManifoldInfo();
+ mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo));
+ }
+ }
+
+ mPotentialContactManifolds = nullptr;
+}
+
+// Reduce the number of contact points of all the potential contact manifolds
+void OverlappingPair::reducePotentialContactManifolds() {
+
+ // For each potential contact manifold
+ ContactManifoldInfo* manifold = mPotentialContactManifolds;
+ while (manifold != nullptr) {
+
+ // Reduce the number of contact points of the manifold
+ manifold->reduce(mContactManifoldSet.getShape1()->getLocalToWorldTransform());
+
+ manifold = manifold->getNext();
+ }
+}
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index ce028f9c..a55df081 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -102,13 +102,19 @@ class OverlappingPair {
/// Collision information about the last frame (for temporal coherence)
LastFrameCollisionInfo mLastFrameCollisionInfo;
+ /// Linked-list of potential contact manifold
+ ContactManifoldInfo* mPotentialContactManifolds;
+
+ /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo
+ Allocator& mTempMemoryAllocator;
+
public:
// -------------------- Methods -------------------- //
/// Constructor
OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
- int nbMaxContactManifolds, PoolAllocator& memoryAllocator);
+ Allocator& memoryAllocator, Allocator& temporaryMemoryAllocator);
/// Destructor
~OverlappingPair() = default;
@@ -125,9 +131,6 @@ class OverlappingPair {
/// Return the pointer to second body
ProxyShape* getShape2() const;
- /// Add a contact manifold
- void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
-
/// Return the last frame collision info
LastFrameCollisionInfo& getLastFrameCollisionInfo();
@@ -137,8 +140,32 @@ class OverlappingPair {
/// Return the a reference to the contact manifold set
const ContactManifoldSet& getContactManifoldSet();
- /// Clear the contact points of the contact manifold
- void clearContactPoints();
+ /// Clear all the potential contact manifolds
+ void clearPotentialContactManifolds();
+
+ /// Add potential contact-points from narrow-phase into potential contact manifolds
+ void addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo);
+
+ /// Add a contact to the contact manifold
+ void addContactManifold(const ContactManifoldInfo* contactManifoldInfo);
+
+ /// Return a reference to the temporary memory allocator
+ Allocator& getTemporaryAllocator();
+
+ /// Return true if one of the shapes of the pair is a concave shape
+ bool hasConcaveShape() const;
+
+ /// Return a pointer to the first potential contact manifold in the linked-list
+ ContactManifoldInfo* getPotentialContactManifolds();
+
+ /// Reduce the number of contact points of all the potential contact manifolds
+ void reducePotentialContactManifolds();
+
+ /// Make the contact manifolds and contact points obselete
+ void makeContactsObselete();
+
+ /// Clear the obselete contact manifold and contact points
+ void clearObseleteManifoldsAndContactPoints();
/// Return the pair of bodies index
static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
@@ -162,7 +189,7 @@ inline ProxyShape* OverlappingPair::getShape2() const {
}
// Add a contact to the contact manifold
-inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) {
+inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) {
mContactManifoldSet.addContactManifold(contactManifoldInfo);
}
@@ -181,6 +208,12 @@ inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() {
return mContactManifoldSet;
}
+// Make the contact manifolds and contact points obselete
+inline void OverlappingPair::makeContactsObselete() {
+
+ mContactManifoldSet.makeContactsObselete();
+}
+
// Return the pair of bodies index
inline overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) {
assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0);
@@ -205,9 +238,25 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body
return indexPair;
}
-// Clear the contact points of the contact manifold
-inline void OverlappingPair::clearContactPoints() {
- mContactManifoldSet.clear();
+// Return a reference to the temporary memory allocator
+inline Allocator& OverlappingPair::getTemporaryAllocator() {
+ return mTempMemoryAllocator;
+}
+
+// Return true if one of the shapes of the pair is a concave shape
+inline bool OverlappingPair::hasConcaveShape() const {
+ return !getShape1()->getCollisionShape()->isConvex() ||
+ !getShape2()->getCollisionShape()->isConvex();
+}
+
+// Return a pointer to the first potential contact manifold in the linked-list
+inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() {
+ return mPotentialContactManifolds;
+}
+
+// Clear the obselete contact manifold and contact points
+inline void OverlappingPair::clearObseleteManifoldsAndContactPoints() {
+ mContactManifoldSet.clearObseleteManifoldsAndContactPoints();
}
}
diff --git a/src/memory/Allocator.h b/src/memory/Allocator.h
index f7b9ac73..0440a5c9 100644
--- a/src/memory/Allocator.h
+++ b/src/memory/Allocator.h
@@ -49,6 +49,9 @@ class Allocator {
/// Release previously allocated memory.
virtual void release(void* pointer, size_t size)=0;
+
+ /// Return true if memory needs to be release with this allocator
+ virtual bool isReleaseNeeded() const=0;
};
}
diff --git a/src/memory/PoolAllocator.h b/src/memory/PoolAllocator.h
index 71e11714..0664e559 100644
--- a/src/memory/PoolAllocator.h
+++ b/src/memory/PoolAllocator.h
@@ -138,8 +138,16 @@ class PoolAllocator : public Allocator {
/// Release previously allocated memory.
virtual void release(void* pointer, size_t size) override;
+ /// Return true if memory needs to be release with this allocator
+ virtual bool isReleaseNeeded() const override;
+
};
+// Return true if memory needs to be release with this allocator
+inline bool PoolAllocator::isReleaseNeeded() const {
+ return true;
+}
+
}
#endif
diff --git a/src/memory/SingleFrameAllocator.h b/src/memory/SingleFrameAllocator.h
index cc030daa..426c2a2d 100644
--- a/src/memory/SingleFrameAllocator.h
+++ b/src/memory/SingleFrameAllocator.h
@@ -72,10 +72,15 @@ class SingleFrameAllocator : public Allocator {
/// Reset the marker of the current allocated memory
virtual void reset();
-
-
+ /// Return true if memory needs to be release with this allocator
+ virtual bool isReleaseNeeded() const override;
};
+// Return true if memory needs to be release with this allocator
+inline bool SingleFrameAllocator::isReleaseNeeded() const {
+ return false;
+}
+
}
#endif
diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h
index f6b8b8bf..dd2a6265 100644
--- a/src/reactphysics3d.h
+++ b/src/reactphysics3d.h
@@ -58,6 +58,8 @@
#include "collision/PolyhedronMesh.h"
#include "collision/TriangleVertexArray.h"
#include "collision/PolygonVertexArray.h"
+#include "collision/CollisionCallback.h"
+#include "collision/OverlapCallback.h"
#include "constraint/BallAndSocketJoint.h"
#include "constraint/SliderJoint.h"
#include "constraint/HingeJoint.h"
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index 51a05412..14ceab1f 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -79,26 +79,34 @@ class ContactManager : public rp3d::CollisionCallback {
/// This method will be called for each reported contact point
virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
- // For each contact point
- rp3d::ContactPointInfo* contactPointInfo = collisionCallbackInfo.contactManifold.getFirstContactPointInfo();
- while (contactPointInfo != nullptr) {
+ // For each contact manifold
+ rp3d::ContactManifoldListElement* manifoldElement = collisionCallbackInfo.contactManifoldElements;
+ while (manifoldElement != nullptr) {
- // Contact normal
- rp3d::Vector3 normal = contactPointInfo->normal;
- openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z);
+ // Get the contact manifold
+ rp3d::ContactManifold* contactManifold = manifoldElement->getContactManifold();
- rp3d::Vector3 point1 = contactPointInfo->localPoint1;
- point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
-
- openglframework::Vector3 position1(point1.x, point1.y, point1.z);
- mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red()));
+ // For each contact point
+ rp3d::ContactPoint* contactPoint = contactManifold->getContactPoints();
+ while (contactPoint != nullptr) {
- rp3d::Vector3 point2 = contactPointInfo->localPoint2;
- point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
- openglframework::Vector3 position2(point2.x, point2.y, point2.z);
- mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue()));
+ // Contact normal
+ rp3d::Vector3 normal = contactPoint->getNormal();
+ openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z);
- contactPointInfo = contactPointInfo->next;
+ rp3d::Vector3 point1 = contactPoint->getLocalPointOnBody1();
+ point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
+
+ openglframework::Vector3 position1(point1.x, point1.y, point1.z);
+ mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red()));
+
+ rp3d::Vector3 point2 = contactPoint->getLocalPointOnBody2();
+ point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
+ openglframework::Vector3 position2(point2.x, point2.y, point2.z);
+ mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue()));
+
+ contactPoint = contactPoint->getNext();
+ }
}
}
diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp
index 2cb3e440..85e8a66f 100644
--- a/testbed/src/SceneDemo.cpp
+++ b/testbed/src/SceneDemo.cpp
@@ -342,14 +342,16 @@ std::vector<ContactPoint> SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn
const rp3d::ContactManifold* manifold = *it;
// For each contact point of the manifold
- for (uint i=0; i<manifold->getNbContactPoints(); i++) {
+ rp3d::ContactPoint* contactPoint = manifold->getContactPoints();
+ while (contactPoint != nullptr) {
- rp3d::ContactPoint* contactPoint = manifold->getContactPoint(i);
rp3d::Vector3 point = contactPoint->getWorldPointOnBody1();
rp3d::Vector3 normalWorld = contactPoint->getNormal();
openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z);
ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red());
contactPoints.push_back(contact);
+
+ contactPoint = contactPoint->getNext();
}
}
From b6ad69b2786111c965a8560516fc5d12c1abb815 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 30 Jul 2017 23:56:20 +0200
Subject: [PATCH 060/133] Do not compute contacts if not necessary
---
.../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 66 +++---
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 100 ++++----
.../narrowphase/NarrowPhaseAlgorithm.h | 2 -
.../narrowphase/SAT/SATAlgorithm.cpp | 214 +++++++++---------
.../narrowphase/SphereVsCapsuleAlgorithm.cpp | 33 +--
.../narrowphase/SphereVsSphereAlgorithm.cpp | 26 ++-
6 files changed, 235 insertions(+), 206 deletions(-)
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
index f98e4d20..c98329bf 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
@@ -91,33 +91,37 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
// If the segments were overlapping (the clip segment is valid)
if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
- // Clip the inner segment of capsule 2
- if (t1 > decimal(1.0)) t1 = decimal(1.0);
- const Vector3 clipPointA = capsule2SegB - t1 * seg2;
- if (t2 > decimal(1.0)) t2 = decimal(1.0);
- const Vector3 clipPointB = capsule2SegA + t2 * seg2;
+ if (reportContacts) {
- // Project point capsule2SegA onto line of innner segment of capsule 1
- const Vector3 seg1Normalized = seg1.getUnit();
- Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
+ // Clip the inner segment of capsule 2
+ if (t1 > decimal(1.0)) t1 = decimal(1.0);
+ const Vector3 clipPointA = capsule2SegB - t1 * seg2;
+ if (t2 > decimal(1.0)) t2 = decimal(1.0);
+ const Vector3 clipPointB = capsule2SegA + t2 * seg2;
- // Compute a perpendicular vector from segment 1 to segment 2
- Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
- Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
+ // Project point capsule2SegA onto line of innner segment of capsule 1
+ const Vector3 seg1Normalized = seg1.getUnit();
+ Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
- Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
- const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
- const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
- const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius();
- const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius();
+ // Compute a perpendicular vector from segment 1 to segment 2
+ Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
+ Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
- const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized;
+ Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
+ const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
+ const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
+ const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius();
+ const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius();
- decimal penetrationDepth = sumRadius - segmentsDistance;
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized;
- // Create the contact info object
- narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
- narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
+ decimal penetrationDepth = sumRadius - segmentsDistance;
+
+ // Create the contact info object
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
+
+ }
return true;
}
@@ -137,18 +141,22 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
// If the collision shapes overlap
if (closestPointsDistanceSquare < sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) {
- decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
- closestPointsSeg1ToSeg2 /= closestPointsDistance;
+ if (reportContacts) {
- const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
- const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
+ decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
+ closestPointsSeg1ToSeg2 /= closestPointsDistance;
- const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
+ const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
+ const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
- decimal penetrationDepth = sumRadius - closestPointsDistance;
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
- // Create the contact info object
- narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
+ decimal penetrationDepth = sumRadius - closestPointsDistance;
+
+ // Create the contact info object
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
+
+ }
return true;
}
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index 88bd9ace..be9cc05f 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -55,70 +55,74 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
// 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 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)
+ if (reportContacts) {
- // Get the contact point created by GJK
- ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
- assert(contactPoint != nullptr);
+ // 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)
- bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
+ // Get the contact point created by GJK
+ ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
+ assert(contactPoint != nullptr);
- // Get the collision shapes
- const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
- const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
+ bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
- // For each face of the polyhedron
- for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
+ // Get the collision shapes
+ const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
+ const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
- // Get the face
- HalfEdgeStructure::Face face = polyhedron->getFace(f);
+ // For each face of the polyhedron
+ for (uint f = 0; f < polyhedron->getNbFaces(); 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;
-
- 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
- narrowPhaseInfo->resetContactPoints();
+ // 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;
- const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
- // Compute the end-points of the inner segment of the capsule
+ // Get the face normal
+ const Vector3 faceNormal = polyhedron->getFaceNormal(f);
+ const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
+
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);
- // Convert the inner capsule segment points into the polyhedron local-space
- const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
- const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
- const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
+ bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0);
+ bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal);
- const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
+ // 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)) {
- // Compute and create two contact points
- satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
- polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
- capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
- narrowPhaseInfo, isCapsuleShape1);
+ // Remove the previous contact point computed by GJK
+ narrowPhaseInfo->resetContactPoints();
- break;
+ const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
+ const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
+
+ // Compute the end-points of the inner segment of the capsule
+ const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
+ const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
+
+ // Convert the inner capsule segment points into the polyhedron local-space
+ const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
+ const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
+ const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
+
+ const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
+
+ // Compute and create two contact points
+ satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
+ polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
+ capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
+ narrowPhaseInfo, isCapsuleShape1);
+
+ break;
+ }
}
+
}
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
index 035bd8e5..6a8746c6 100644
--- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h
+++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
@@ -83,8 +83,6 @@ class NarrowPhaseAlgorithm {
/// Deleted assignment operator
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
- // TODO : Use the following reportContacts variable in all narrow-phase algorithms
-
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0;
};
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 4974d367..98686311 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -139,15 +139,18 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
}
}
- const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
- Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal);
- const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius();
- const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
+ if (reportContacts) {
- // Create the contact info object
- narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
- isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
- isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
+ const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
+ Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal);
+ const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius();
+ const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
+
+ // Create the contact info object
+ narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
+ isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
+ isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
+ }
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
@@ -384,34 +387,39 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
// We need to clip the inner capsule segment with the adjacent faces of the separating face
if (isMinPenetrationFaceNormal) {
- computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
- polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
- capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
- narrowPhaseInfo, isCapsuleShape1);
+ if (reportContacts) {
+
+ computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
+ polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
+ capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
+ narrowPhaseInfo, isCapsuleShape1);
+ }
lastFrameInfo.satIsAxisFacePolyhedron1 = true;
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
}
else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
- // Compute the closest points between the inner capsule segment and the
- // edge of the polyhedron in polyhedron local-space
- Vector3 closestPointPolyhedronEdge, closestPointCapsuleInnerSegment;
- computeClosestPointBetweenTwoSegments(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
- separatingPolyhedronEdgeVertex1, separatingPolyhedronEdgeVertex2,
- closestPointCapsuleInnerSegment, closestPointPolyhedronEdge);
+ if (reportContacts) {
+ // Compute the closest points between the inner capsule segment and the
+ // edge of the polyhedron in polyhedron local-space
+ Vector3 closestPointPolyhedronEdge, closestPointCapsuleInnerSegment;
+ computeClosestPointBetweenTwoSegments(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
+ separatingPolyhedronEdgeVertex1, separatingPolyhedronEdgeVertex2,
+ closestPointCapsuleInnerSegment, closestPointPolyhedronEdge);
- // Project closest capsule inner segment point into the capsule bounds
- const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
+ // Project closest capsule inner segment point into the capsule bounds
+ const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
- // Create the contact point
- narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
- isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
- isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
+ // Create the contact point
+ narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
+ isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
+ isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
+ }
- lastFrameInfo.satIsAxisFacePolyhedron1 = false;
- lastFrameInfo.satMinEdge1Index = minEdgeIndex;
+ lastFrameInfo.satIsAxisFacePolyhedron1 = false;
+ lastFrameInfo.satMinEdge1Index = minEdgeIndex;
}
return true;
@@ -543,8 +551,7 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
}
// Test collision between two convex polyhedrons
-bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo,
- bool reportContacts) const {
+bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()");
@@ -787,91 +794,93 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// If the minimum separating axis is a face normal
if (isMinPenetrationFaceNormal) {
- const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
- const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1;
- const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
- const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2;
+ if (reportContacts) {
- assert(minPenetrationDepth > decimal(0.0));
+ const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
+ const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1;
+ const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
+ const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2;
- const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex);
- const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
+ assert(minPenetrationDepth > decimal(0.0));
- // Compute the world normal
- const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
- -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
+ const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex);
+ const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
- // Get the reference face
- HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex);
+ // Compute the world normal
+ const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
+ -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
- // Find the incident face on the other polyhedron (most anti-parallel face)
- uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace);
+ // Get the reference face
+ HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex);
- // Get the incident face
- HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
+ // Find the incident face on the other polyhedron (most anti-parallel face)
+ uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace);
- std::vector<Vector3> polygonVertices; // Vertices to clip of the incident face
- std::vector<Vector3> planesNormals; // Normals of the clipping planes
- std::vector<Vector3> planesPoints; // Points on the clipping planes
+ // Get the incident face
+ HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
- // Get all the vertices of the incident face (in the reference local-space)
- std::vector<uint>::const_iterator it;
- for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) {
- const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it);
- polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace);
- }
+ std::vector<Vector3> polygonVertices; // Vertices to clip of the incident face
+ std::vector<Vector3> planesNormals; // Normals of the clipping planes
+ std::vector<Vector3> planesPoints; // Points on the clipping planes
- // Get the reference face clipping planes
- uint currentEdgeIndex = referenceFace.edgeIndex;
- uint firstEdgeIndex = currentEdgeIndex;
- do {
+ // Get all the vertices of the incident face (in the reference local-space)
+ std::vector<uint>::const_iterator it;
+ for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) {
+ const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it);
+ polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace);
+ }
- // Get the adjacent edge
- HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
+ // Get the reference face clipping planes
+ uint currentEdgeIndex = referenceFace.edgeIndex;
+ uint firstEdgeIndex = currentEdgeIndex;
+ do {
- // Get the twin edge
- HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
+ // Get the adjacent edge
+ HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
- // Get the adjacent face normal (and negate it to have a clipping plane)
- Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex);
+ // Get the twin edge
+ HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
- // Get a vertex of the clipping plane (vertex of the adjacent edge)
- Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex);
+ // Get the adjacent face normal (and negate it to have a clipping plane)
+ Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex);
- planesNormals.push_back(faceNormal);
- planesPoints.push_back(faceVertex);
+ // Get a vertex of the clipping plane (vertex of the adjacent edge)
+ Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex);
- // Go to the next adjacent edge of the reference face
- currentEdgeIndex = edge.nextEdgeIndex;
+ planesNormals.push_back(faceNormal);
+ planesPoints.push_back(faceVertex);
- } while (currentEdgeIndex != firstEdgeIndex);
+ // Go to the next adjacent edge of the reference face
+ currentEdgeIndex = edge.nextEdgeIndex;
- assert(planesNormals.size() > 0);
- assert(planesNormals.size() == planesPoints.size());
+ } while (currentEdgeIndex != firstEdgeIndex);
- // Clip the reference faces with the adjacent planes of the reference face
- std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
- assert(clipPolygonVertices.size() > 0);
+ assert(planesNormals.size() > 0);
+ assert(planesNormals.size() == planesPoints.size());
- // We only keep the clipped points that are below the reference face
- const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
- std::vector<Vector3>::const_iterator itPoints;
- for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
+ // Clip the reference faces with the adjacent planes of the reference face
+ std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
+ assert(clipPolygonVertices.size() > 0);
- // If the clip point is bellow the reference face
- if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0))
- {
+ // We only keep the clipped points that are below the reference face
+ const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
+ std::vector<Vector3>::const_iterator itPoints;
+ for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
- // Convert the clip incident polyhedron vertex into the incident polyhedron local-space
- const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
+ // If the clip point is bellow the reference face
+ if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) {
- // Project the contact point onto the reference face
- Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
+ // Convert the clip incident polyhedron vertex into the incident polyhedron local-space
+ const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
- // Create a new contact point
- narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
- isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
- isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
+ // Project the contact point onto the reference face
+ Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
+
+ // Create a new contact point
+ narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
+ isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
+ isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
+ }
}
}
@@ -881,20 +890,23 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
}
else { // If we have an edge vs edge contact
- // Compute the closest points between the two edges (in the local-space of poylhedron 2)
- Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
- computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B,
- closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
+ if (reportContacts) {
- // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
- const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
+ // Compute the closest points between the two edges (in the local-space of poylhedron 2)
+ Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
+ computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B,
+ closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
- // Compute the world normal
- const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
+ // Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
+ const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
- // Create the contact point
- narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
- closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
+ // Compute the world normal
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
+
+ // Create the contact point
+ narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
+ closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
+ }
lastFrameInfo.satIsAxisFacePolyhedron1 = false;
lastFrameInfo.satIsAxisFacePolyhedron2 = false;
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
index e291fd83..52016f2f 100755
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
@@ -71,24 +71,27 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, b
// If the collision shapes overlap
if (sphereSegmentDistanceSquare < sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) {
- decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
- sphereCenterToSegment /= sphereSegmentDistance;
+ if (reportContacts) {
- const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
- const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
-
- Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
-
- decimal penetrationDepth = sumRadius - sphereSegmentDistance;
+ decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
+ sphereCenterToSegment /= sphereSegmentDistance;
- if (!isSphereShape1) {
- normalWorld = -normalWorld;
+ const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
+ const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
+
+ Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
+
+ decimal penetrationDepth = sumRadius - sphereSegmentDistance;
+
+ if (!isSphereShape1) {
+ normalWorld = -normalWorld;
+ }
+
+ // Create the contact info object
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
+ isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
+ isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
}
-
- // Create the contact info object
- narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
- isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
- isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
return true;
}
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
index 94211ac0..1335c911 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
@@ -52,17 +52,21 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bo
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters < sumRadius * sumRadius) {
- Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
- Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
- Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
- centerSphere2InBody1LocalSpace.getUnit();
- Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
- centerSphere1InBody2LocalSpace.getUnit();
- decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
-
- // Create the contact info object
- narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
- intersectionOnBody1, intersectionOnBody2);
+
+ if (reportContacts) {
+
+ Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
+ Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
+ Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
+ centerSphere2InBody1LocalSpace.getUnit();
+ Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
+ centerSphere1InBody2LocalSpace.getUnit();
+ decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
+
+ // Create the contact info object
+ narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
+ intersectionOnBody1, intersectionOnBody2);
+ }
return true;
}
From 2f601909423be819bf1d67e4535fe31cf8edb009 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 1 Aug 2017 15:57:46 +0200
Subject: [PATCH 061/133] Do not generate contact in GJK algorithm if not
needed
---
.../narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp | 1 -
src/collision/narrowphase/GJK/GJKAlgorithm.cpp | 7 +++++--
2 files changed, 5 insertions(+), 3 deletions(-)
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index be9cc05f..73a1dd13 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -122,7 +122,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
break;
}
}
-
}
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index ff81782f..10d344f1 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -207,8 +207,11 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
return GJKResult::INTERPENETRATE;
}
- // Add a new contact point
- narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB);
+ if (reportContacts) {
+
+ // Add a new contact point
+ narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB);
+ }
return GJKResult::COLLIDE_IN_MARGIN;
}
From 319cc72cde3052239b62cbb0f51a1f8bf8db252c Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 18 Aug 2017 17:50:27 +0200
Subject: [PATCH 062/133] Fix issues in collision detection
---
src/collision/CollisionCallback.cpp | 1 +
src/collision/CollisionDetection.cpp | 33 ++--
src/collision/ContactManifoldSet.cpp | 4 +
.../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 167 ++++++++++++------
.../narrowphase/SAT/SATAlgorithm.cpp | 19 +-
.../narrowphase/SphereVsCapsuleAlgorithm.cpp | 57 ++++--
.../narrowphase/SphereVsSphereAlgorithm.cpp | 28 ++-
src/collision/shapes/ConvexMeshShape.h | 4 +-
src/engine/OverlappingPair.h | 8 +
9 files changed, 227 insertions(+), 94 deletions(-)
mode change 100644 => 100755 src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
mode change 100644 => 100755 src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
diff --git a/src/collision/CollisionCallback.cpp b/src/collision/CollisionCallback.cpp
index f470763d..db820db4 100644
--- a/src/collision/CollisionCallback.cpp
+++ b/src/collision/CollisionCallback.cpp
@@ -45,6 +45,7 @@ CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair*
// For each contact manifold in the set of manifolds in the pair
ContactManifold* contactManifold = manifoldSet.getContactManifolds();
+ assert(contactManifold != nullptr);
while (contactManifold != nullptr) {
assert(contactManifold->getNbContactPoints() > 0);
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 57360706..0abd82a3 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -440,7 +440,7 @@ void CollisionDetection::reportAllContacts() {
for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
// If there is a user callback
- if (mWorld->mEventListener != nullptr) {
+ if (mWorld->mEventListener != nullptr && it->second->hasContacts()) {
CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator);
@@ -845,9 +845,12 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
// Process the potential contacts
processPotentialContacts(&pair);
- // Report the contacts to the user
- CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
- collisionCallback->notifyContact(collisionInfo);
+ if (pair.hasContacts()) {
+
+ // Report the contacts to the user
+ CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
+ collisionCallback->notifyContact(collisionInfo);
+ }
}
// Go to the next proxy shape
@@ -916,10 +919,6 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
-
- // Report the contacts to the user
- CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
- callback->notifyContact(collisionInfo);
}
}
@@ -935,6 +934,13 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// Process the potential contacts
processPotentialContacts(&pair);
+
+ if (pair.hasContacts()) {
+
+ // Report the contacts to the user
+ CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
+ callback->notifyContact(collisionInfo);
+ }
}
}
@@ -996,10 +1002,6 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
-
- // Report the contacts to the user
- CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
- callback->notifyContact(collisionInfo);
}
}
@@ -1015,6 +1017,13 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Process the potential contacts
processPotentialContacts(&pair);
+
+ if (pair.hasContacts()) {
+
+ // Report the contacts to the user
+ CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
+ callback->notifyContact(collisionInfo);
+ }
}
}
}
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index b9b7f3e8..23602dda 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -247,6 +247,10 @@ void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
if (previous != nullptr) {
previous->setNext(manifold->getNext());
}
+ else {
+ mManifolds = next;
+ }
+
if (next != nullptr) {
next->setPrevious(manifold->getPrevious());
}
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
old mode 100644
new mode 100755
index c98329bf..b15cb186
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
@@ -63,68 +63,88 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
// If the two capsules are parallel (we create two contact points)
- if (areParallelVectors(seg1, seg2)) {
+ bool areCapsuleInnerSegmentsParralel = areParallelVectors(seg1, seg2);
+ if (areCapsuleInnerSegmentsParralel) {
// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
- const decimal segmentsDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
- if (segmentsDistance >= sumRadius) {
+ const decimal segmentsPerpendicularDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
+ if (segmentsPerpendicularDistance >= sumRadius) {
// The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius.
// Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision.
return false;
}
- // If the distance between the two segments is larger than zero (inner segments of capsules are not overlapping)
- // If the inner segments are overlapping, we cannot compute a contact normal (unknown direction). In this case,
- // we skip the parallel contact points calculation (there might still be contact in the spherical caps of the capsules)
- if (segmentsDistance > MACHINE_EPSILON) {
+ // Compute the planes that goes through the extreme points of the inner segment of capsule 1
+ decimal d1 = seg1.dot(capsule1SegA);
+ decimal d2 = -seg1.dot(capsule1SegB);
- // Compute the planes that goes through the extreme points of the inner segment of capsule 1
- decimal d1 = seg1.dot(capsule1SegA);
- decimal d2 = -seg1.dot(capsule1SegB);
+ // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
+ // segment of capsule 1
+ decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
+ decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
- // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
- // segment of capsule 1
- decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
- decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
+ // If the segments were overlapping (the clip segment is valid)
+ if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
- // If the segments were overlapping (the clip segment is valid)
- if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
+ if (reportContacts) {
- if (reportContacts) {
+ // Clip the inner segment of capsule 2
+ if (t1 > decimal(1.0)) t1 = decimal(1.0);
+ const Vector3 clipPointA = capsule2SegB - t1 * seg2;
+ if (t2 > decimal(1.0)) t2 = decimal(1.0);
+ const Vector3 clipPointB = capsule2SegA + t2 * seg2;
- // Clip the inner segment of capsule 2
- if (t1 > decimal(1.0)) t1 = decimal(1.0);
- const Vector3 clipPointA = capsule2SegB - t1 * seg2;
- if (t2 > decimal(1.0)) t2 = decimal(1.0);
- const Vector3 clipPointB = capsule2SegA + t2 * seg2;
+ // Project point capsule2SegA onto line of innner segment of capsule 1
+ const Vector3 seg1Normalized = seg1.getUnit();
+ Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
- // Project point capsule2SegA onto line of innner segment of capsule 1
- const Vector3 seg1Normalized = seg1.getUnit();
- Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
+ Vector3 normalCapsule2SpaceNormalized;
+ Vector3 segment1ToSegment2;
- // Compute a perpendicular vector from segment 1 to segment 2
- Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
- Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
+ // If the inner capsule segments perpendicular distance is not zero (the inner segments are not overlapping)
+ if (segmentsPerpendicularDistance > MACHINE_EPSILON) {
- Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
- const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
- const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
- const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius();
- const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius();
+ // Compute a perpendicular vector from segment 1 to segment 2
+ segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
+ normalCapsule2SpaceNormalized = segment1ToSegment2.getUnit();
+ }
+ else { // If the capsule inner segments are overlapping (degenerate case)
- const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized;
+ // We cannot use the vector between segments as a contact normal. To generate a contact normal, we take
+ // any vector that is orthogonal to the inner capsule segments.
- decimal penetrationDepth = sumRadius - segmentsDistance;
+ Vector3 vec1(1, 0, 0);
+ Vector3 vec2(0, 1, 0);
- // Create the contact info object
- narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
- narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
+ Vector3 seg2Normalized = seg2.getUnit();
- }
+ // Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule 2 inner segment (smallest absolute dot product)
+ decimal cosA1 = std::abs(seg2Normalized.x); // abs(vec1.dot(seg2))
+ decimal cosA2 = std::abs(seg2Normalized.y); // abs(vec2.dot(seg2))
- return true;
- }
+ segment1ToSegment2.setToZero();
+
+ // We choose as a contact normal, any direction that is perpendicular to the inner capsules segments
+ normalCapsule2SpaceNormalized = cosA1 < cosA2 ? seg2Normalized.cross(vec1) : seg2Normalized.cross(vec2);
+ }
+
+ Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
+ const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
+ const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
+ const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
+ const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
+
+ decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance;
+
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized;
+
+ // Create the contact info object
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
+ }
+
+ return true;
}
}
@@ -132,31 +152,72 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
Vector3 closestPointCapsule1Seg;
Vector3 closestPointCapsule2Seg;
computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB,
- closestPointCapsule1Seg, closestPointCapsule2Seg);
+ closestPointCapsule1Seg, closestPointCapsule2Seg);
// Compute the distance between the sphere center and the closest point on the segment
Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg);
const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
// If the collision shapes overlap
- if (closestPointsDistanceSquare < sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) {
+ if (closestPointsDistanceSquare < sumRadius * sumRadius) {
+
+ if (reportContacts) {
- if (reportContacts) {
+ // If the distance between the inner segments is not zero
+ if (closestPointsDistanceSquare > MACHINE_EPSILON) {
- decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
- closestPointsSeg1ToSeg2 /= closestPointsDistance;
+ decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
+ closestPointsSeg1ToSeg2 /= closestPointsDistance;
- const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
- const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
+ const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
+ const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
- const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
- decimal penetrationDepth = sumRadius - closestPointsDistance;
+ decimal penetrationDepth = sumRadius - closestPointsDistance;
- // Create the contact info object
- narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
+ // Create the contact info object
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
+ }
+ else { // The segment are overlapping (degenerate case)
- }
+ // If the capsule segments are parralel
+ if (areCapsuleInnerSegmentsParralel) {
+
+ // The segment are parallel, not overlapping and their distance is zero.
+ // Therefore, the capsules are just touching at the top of their inner segments
+ decimal squareDistCapsule2PointToCapsuleSegA = (capsule1SegA - closestPointCapsule2Seg).lengthSquare();
+
+ Vector3 capsule1SegmentMostExtremePoint = squareDistCapsule2PointToCapsuleSegA > MACHINE_EPSILON ? capsule1SegA : capsule1SegB;
+ Vector3 normalCapsuleSpace2 = (closestPointCapsule2Seg - capsule1SegmentMostExtremePoint);
+ normalCapsuleSpace2.normalize();
+
+ const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
+ const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
+
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
+
+ // Create the contact info object
+ narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
+ }
+ else { // If the capsules inner segments are not parallel
+
+ // We cannot use a vector between the segments as contact normal. We need to compute a new contact normal with the cross
+ // product between the two segments.
+ Vector3 normalCapsuleSpace2 = seg1.cross(seg2);
+ normalCapsuleSpace2.normalize();
+
+ // Compute the contact points on both shapes
+ const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
+ const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
+
+ const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
+
+ // Create the contact info object
+ narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
+ }
+ }
+ }
return true;
}
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 98686311..cebec9a6 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -838,17 +838,20 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// Get the adjacent edge
HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
- // Get the twin edge
- HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
+ // Get the twin edge
+ HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
- // Get the adjacent face normal (and negate it to have a clipping plane)
- Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex);
+ // Compute the edge vertices and edge direction
+ Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex);
+ Vector3 edgeV2 = referencePolyhedron->getVertexPosition(twinEdge.vertexIndex);
+ Vector3 edgeDirection = edgeV2 - edgeV1;
- // Get a vertex of the clipping plane (vertex of the adjacent edge)
- Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex);
+ // Compute the normal of the clipping plane for this edge
+ // The clipping plane is perpendicular to the edge direction and the reference face normal
+ Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection);
- planesNormals.push_back(faceNormal);
- planesPoints.push_back(faceVertex);
+ planesNormals.push_back(clipPlaneNormal);
+ planesPoints.push_back(edgeV1);
// Go to the next adjacent edge of the reference face
currentEdgeIndex = edge.nextEdgeIndex;
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
index 52016f2f..a5d7a6ac 100755
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
@@ -69,28 +69,61 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, b
decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius();
// If the collision shapes overlap
- if (sphereSegmentDistanceSquare < sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) {
+ if (sphereSegmentDistanceSquare < sumRadius * sumRadius) {
+
+ decimal penetrationDepth;
+ Vector3 normalWorld;
+ Vector3 contactPointSphereLocal;
+ Vector3 contactPointCapsuleLocal;
if (reportContacts) {
- decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
- sphereCenterToSegment /= sphereSegmentDistance;
+ // If the sphere center is not on the capsule inner segment
+ if (sphereSegmentDistanceSquare > MACHINE_EPSILON) {
- const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
- const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
+ decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
+ sphereCenterToSegment /= sphereSegmentDistance;
- Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
+ contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
+ contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
- decimal penetrationDepth = sumRadius - sphereSegmentDistance;
+ normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
- if (!isSphereShape1) {
- normalWorld = -normalWorld;
- }
+ penetrationDepth = sumRadius - sphereSegmentDistance;
+
+ if (!isSphereShape1) {
+ normalWorld = -normalWorld;
+ }
+ }
+ else { // If the sphere center is on the capsule inner segment (degenerate case)
+
+ // We take any direction that is orthogonal to the inner capsule segment as a contact normal
+
+ // Capsule inner segment
+ Vector3 capsuleSegment = (capsuleSegB - capsuleSegA).getUnit();
+
+ Vector3 vec1(1, 0, 0);
+ Vector3 vec2(0, 1, 0);
+
+ // Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule inner segment (smallest absolute dot product)
+ decimal cosA1 = std::abs(capsuleSegment.x); // abs(vec1.dot(seg2))
+ decimal cosA2 = std::abs(capsuleSegment.y); // abs(vec2.dot(seg2))
+
+ penetrationDepth = sumRadius;
+
+ // We choose as a contact normal, any direction that is perpendicular to the inner capsule segment
+ Vector3 normalCapsuleSpace = cosA1 < cosA2 ? capsuleSegment.cross(vec1) : capsuleSegment.cross(vec2);
+ normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace;
+
+ // Compute the two local contact points
+ contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereShape->getRadius());
+ contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius();
+ }
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
- isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
- isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
+ isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
+ isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
}
return true;
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
old mode 100644
new mode 100755
index 1335c911..0b520213
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
@@ -57,15 +57,29 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bo
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
- Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
- centerSphere2InBody1LocalSpace.getUnit();
- Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
- centerSphere1InBody2LocalSpace.getUnit();
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
+ Vector3 intersectionOnBody1;
+ Vector3 intersectionOnBody2;
+ Vector3 normal;
- // Create the contact info object
- narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
- intersectionOnBody1, intersectionOnBody2);
+ // If the two sphere centers are not at the same position
+ if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
+
+ intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.getUnit();
+ intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.getUnit();
+ normal = vectorBetweenCenters.getUnit();
+ }
+ else { // If the sphere centers are at the same position (degenerate case)
+
+ // Take any contact normal direction
+ normal.setAllValues(0, 1, 0);
+
+ intersectionOnBody1 = sphereShape1->getRadius() * (transform1.getInverse().getOrientation() * normal);
+ intersectionOnBody2 = sphereShape2->getRadius() * (transform2.getInverse().getOrientation() * normal);
+ }
+
+ // Create the contact info object
+ narrowPhaseInfo->addContactPoint(normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2);
}
return true;
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index 28ba9c86..dd2b7021 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -233,7 +233,7 @@ inline HalfEdgeStructure::Edge ConvexMeshShape::getHalfEdge(uint edgeIndex) cons
// Return the position of a given vertex
inline Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const {
assert(vertexIndex < getNbVertices());
- return mPolyhedronMesh->getVertex(vertexIndex);
+ return mPolyhedronMesh->getVertex(vertexIndex) * mScaling;
}
// Return the normal vector of a given face of the polyhedron
@@ -244,7 +244,7 @@ inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const {
// Return the centroid of the polyhedron
inline Vector3 ConvexMeshShape::getCentroid() const {
- return mPolyhedronMesh->getCentroid();
+ return mPolyhedronMesh->getCentroid() * mScaling;
}
}
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index a55df081..906cddb7 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -155,6 +155,9 @@ class OverlappingPair {
/// Return true if one of the shapes of the pair is a concave shape
bool hasConcaveShape() const;
+ /// Return true if the overlapping pair has contact manifolds with contacts
+ bool hasContacts() const;
+
/// Return a pointer to the first potential contact manifold in the linked-list
ContactManifoldInfo* getPotentialContactManifolds();
@@ -249,6 +252,11 @@ inline bool OverlappingPair::hasConcaveShape() const {
!getShape2()->getCollisionShape()->isConvex();
}
+// Return true if the overlapping pair has contact manifolds with contacts
+inline bool OverlappingPair::hasContacts() const {
+ return mContactManifoldSet.getContactManifolds() != nullptr;
+}
+
// Return a pointer to the first potential contact manifold in the linked-list
inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() {
return mPotentialContactManifolds;
From 11589dbb2cc5e8048260f90c548ed464e833fd5e Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 18 Aug 2017 17:51:10 +0200
Subject: [PATCH 063/133] Edit collision detection scene
---
.../CollisionDetectionScene.cpp | 49 +++++--------------
.../CollisionDetectionScene.h | 7 ++-
2 files changed, 15 insertions(+), 41 deletions(-)
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index e8f09670..a99d494d 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -72,7 +72,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mSphere2->setSleepingColor(mRedColorDemo);
// ---------- Capsule 1 ---------- //
- openglframework::Vector3 position3(0, -12, 0);
+ openglframework::Vector3 position3(-6, 7, 0);
// Create a cylinder and a corresponding collision body in the dynamics world
mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath);
@@ -83,7 +83,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mCapsule1->setSleepingColor(mRedColorDemo);
// ---------- Capsule 2 ---------- //
- openglframework::Vector3 position4(-8, 0, 0);
+ openglframework::Vector3 position4(11, -8, 0);
// Create a cylinder and a corresponding collision body in the dynamics world
mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position4, mCollisionWorld, mMeshFolderPath);
@@ -115,38 +115,16 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mBox2->setColor(mGreyColorDemo);
mBox2->setSleepingColor(mRedColorDemo);
- // ---------- Cone ---------- //
- //openglframework::Vector3 position4(0, 0, 0);
-
- // Create a cone and a corresponding collision body in the dynamics world
- //mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld,
- // mMeshFolderPath);
-
- // Set the color
- //mCone->setColor(mGreyColorDemo);
- //mCone->setSleepingColor(mRedColorDemo);
-
- // ---------- Cylinder ---------- //
- //openglframework::Vector3 position5(0, 0, 0);
-
- // Create a cylinder and a corresponding collision body in the dynamics world
- //mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5,
- // mCollisionWorld, mMeshFolderPath);
-
- // Set the color
- //mCylinder->setColor(mGreyColorDemo);
- //mCylinder->setSleepingColor(mRedColorDemo);
-
-
// ---------- Convex Mesh ---------- //
- //openglframework::Vector3 position7(0, 0, 0);
+ openglframework::Vector3 position7(-5, 0, 0);
// Create a convex mesh and a corresponding collision body in the dynamics world
- //mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj");
+ mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj");
+ mAllShapes.push_back(mConvexMesh);
// Set the color
- //mConvexMesh->setColor(mGreyColorDemo);
- //mConvexMesh->setSleepingColor(mRedColorDemo);
+ mConvexMesh->setColor(mGreyColorDemo);
+ mConvexMesh->setSleepingColor(mRedColorDemo);
// ---------- Concave Mesh ---------- //
//openglframework::Vector3 position8(0, 0, 0);
@@ -202,6 +180,9 @@ CollisionDetectionScene::~CollisionDetectionScene() {
mCollisionWorld->destroyCollisionBody(mBox2->getCollisionBody());
delete mBox2;
+ mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody());
+ delete mConvexMesh;
+
/*
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody());
@@ -217,13 +198,7 @@ CollisionDetectionScene::~CollisionDetectionScene() {
mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody());
// Destroy the sphere
- delete mCapsule;
-
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody());
-
- // Destroy the convex mesh
- delete mConvexMesh;
+ delete mCapsule;
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody());
@@ -280,13 +255,13 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mBox2->getCollisionBody()->isActive()) mBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
/*
if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix);
if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix);
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);
if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix);
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index 14ceab1f..d0f1b569 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -107,6 +107,8 @@ class ContactManager : public rp3d::CollisionCallback {
contactPoint = contactPoint->getNext();
}
+
+ manifoldElement = manifoldElement->getNext();
}
}
@@ -143,10 +145,7 @@ class CollisionDetectionScene : public SceneDemo {
Capsule* mCapsule2;
Box* mBox1;
Box* mBox2;
- //Cone* mCone;
- //Cylinder* mCylinder;
- //Capsule* mCapsule;
- //ConvexMesh* mConvexMesh;
+ ConvexMesh* mConvexMesh;
//Dumbbell* mDumbbell;
//ConcaveMesh* mConcaveMesh;
//HeightField* mHeightField;
From 624e01b595e8723295caa13f6c6a13c84033c2e8 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 21 Aug 2017 07:35:08 +0200
Subject: [PATCH 064/133] Working on ConcaveMeshShape and HeightFieldShape
collision detection
---
CMakeLists.txt | 2 +
src/collision/CollisionDetection.cpp | 2 +
src/collision/ContactManifold.h | 9 +-
src/collision/MiddlePhaseTriangleCallback.cpp | 49 ++++
src/collision/MiddlePhaseTriangleCallback.h | 84 +++++++
src/collision/PolyhedronMesh.cpp | 4 +
src/collision/TriangleVertexArray.cpp | 238 +++++++++++++++++-
src/collision/TriangleVertexArray.h | 71 +++++-
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 2 +-
.../narrowphase/ConcaveVsConvexAlgorithm.cpp | 9 +-
.../narrowphase/ConcaveVsConvexAlgorithm.h | 26 +-
.../narrowphase/GJK/GJKAlgorithm.cpp | 9 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 75 ++++--
src/collision/narrowphase/SAT/SATAlgorithm.h | 2 +-
.../SphereVsConvexPolyhedronAlgorithm.cpp | 10 +-
src/collision/shapes/ConcaveMeshShape.cpp | 112 ++-------
src/collision/shapes/ConcaveMeshShape.h | 22 +-
src/collision/shapes/ConcaveShape.h | 3 +-
src/collision/shapes/ConvexShape.h | 2 +
src/collision/shapes/HeightFieldShape.cpp | 42 +++-
src/collision/shapes/HeightFieldShape.h | 3 +-
src/collision/shapes/TriangleShape.cpp | 104 +++++++-
src/collision/shapes/TriangleShape.h | 52 +++-
23 files changed, 770 insertions(+), 162 deletions(-)
create mode 100644 src/collision/MiddlePhaseTriangleCallback.cpp
create mode 100644 src/collision/MiddlePhaseTriangleCallback.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1e1d2fcf..dcc2e4bf 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -137,6 +137,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/ContactManifold.cpp"
"src/collision/ContactManifoldSet.h"
"src/collision/ContactManifoldSet.cpp"
+ "src/collision/MiddlePhaseTriangleCallback.h"
+ "src/collision/MiddlePhaseTriangleCallback.cpp"
"src/constraint/BallAndSocketJoint.h"
"src/constraint/BallAndSocketJoint.cpp"
"src/constraint/ContactPoint.h"
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 0abd82a3..0c21240a 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -29,9 +29,11 @@
#include "collision/OverlapCallback.h"
#include "body/Body.h"
#include "collision/shapes/BoxShape.h"
+#include "collision/shapes/ConcaveShape.h"
#include "body/RigidBody.h"
#include "configuration.h"
#include "collision/CollisionCallback.h"
+#include "collision/MiddlePhaseTriangleCallback.h"
#include "collision/OverlapCallback.h"
#include <cassert>
#include <complex>
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index 0704beed..ada20003 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -80,9 +80,14 @@ struct ContactManifoldListElement {
// Class ContactManifold
/**
- * This class represents the set of contact points between two bodies.
+ * This class represents a set of contact points between two bodies that
+ * all have a similar contact normal direction. Usually, there is a single
+ * contact manifold when two convex shapes are in contact. However, when
+ * a convex shape collides with a concave shape, there can be several
+ * contact manifolds with different normal directions.
* The contact manifold is implemented in a way to cache the contact
- * points among the frames for better stability.
+ * points among the frames for better stability (warm starting of the
+ * contact solver)
*/
class ContactManifold {
diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp
new file mode 100644
index 00000000..e99da461
--- /dev/null
+++ b/src/collision/MiddlePhaseTriangleCallback.cpp
@@ -0,0 +1,49 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "collision/MiddlePhaseTriangleCallback.h"
+
+using namespace reactphysics3d;
+
+// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase)
+void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
+ const Vector3* verticesNormals) {
+
+ // Create a triangle collision shape
+ decimal margin = mConcaveShape->getTriangleMargin();
+ TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
+ TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
+ verticesNormals, meshSubPart, triangleIndex, margin);
+
+ // Create a narrow phase info for the narrow-phase collision detection
+ NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList;
+ narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo)))
+ NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(),
+ triangleShape, mConvexProxyShape->getLocalToWorldTransform(),
+ mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(),
+ mConcaveProxyShape->getCachedCollisionData());
+ narrowPhaseInfoList->next = firstNarrowPhaseInfo;
+}
diff --git a/src/collision/MiddlePhaseTriangleCallback.h b/src/collision/MiddlePhaseTriangleCallback.h
new file mode 100644
index 00000000..36bfac24
--- /dev/null
+++ b/src/collision/MiddlePhaseTriangleCallback.h
@@ -0,0 +1,84 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_MIDDLE_PHASE_TRIANGLE_CALLBACK_H
+#define REACTPHYSICS3D_MIDDLE_PHASE_TRIANGLE_CALLBACK_H
+
+// Libraries
+#include "collision/shapes/ConcaveShape.h"
+#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class ConvexVsTriangleCallback
+/**
+ * This class is used to report a collision between the triangle
+ * of a concave mesh shape and a convex shape during the
+ * middle-phase algorithm.
+ */
+class MiddlePhaseTriangleCallback : public TriangleCallback {
+
+ protected:
+
+ /// Broadphase overlapping pair
+ OverlappingPair* mOverlappingPair;
+
+ /// Pointer to the concave proxy shape
+ ProxyShape* mConcaveProxyShape;
+
+ /// Pointer to the convex proxy shape
+ ProxyShape* mConvexProxyShape;
+
+ /// Pointer to the concave collision shape
+ const ConcaveShape* mConcaveShape;
+
+ /// Reference to the single-frame memory allocator
+ Allocator& mAllocator;
+
+ public:
+
+ /// Pointer to the first element of the linked-list of narrow-phase info
+ NarrowPhaseInfo* narrowPhaseInfoList;
+
+ /// Constructor
+ MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair,
+ ProxyShape* concaveProxyShape,
+ ProxyShape* convexProxyShape, const ConcaveShape* concaveShape,
+ Allocator& allocator)
+ :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape),
+ mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape),
+ mAllocator(allocator), narrowPhaseInfoList(nullptr) {
+
+ }
+
+ /// Test collision between a triangle and the convex mesh shape
+ virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints,
+ const Vector3* verticesNormals) override;
+};
+
+}
+
+#endif
diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp
index 61b1e3c8..b4f82fad 100644
--- a/src/collision/PolyhedronMesh.cpp
+++ b/src/collision/PolyhedronMesh.cpp
@@ -30,6 +30,10 @@ using namespace reactphysics3d;
// Constructor
+/*
+ * Create a polyhedron mesh given an array of polygons.
+ * @param polygonVertexArray Pointer to the array of polygons and their vertices
+ */
PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) {
mPolygonVertexArray = polygonVertexArray;
diff --git a/src/collision/TriangleVertexArray.cpp b/src/collision/TriangleVertexArray.cpp
index 883b0ab7..ad59d37a 100644
--- a/src/collision/TriangleVertexArray.cpp
+++ b/src/collision/TriangleVertexArray.cpp
@@ -25,13 +25,19 @@
// Libraries
#include "TriangleVertexArray.h"
+#include "mathematics/Vector3.h"
+#include <cassert>
using namespace reactphysics3d;
-// Constructor
+// Constructor without vertices normals
/// Note that your data will not be copied into the TriangleVertexArray and
/// therefore, you need to make sure that those data are always valid during
-/// the lifetime of the TriangleVertexArray.
+/// the lifetime of the TriangleVertexArray. With this constructor, you do not
+/// need to provide vertices normals for smooth mesh collision. Therefore, the
+/// vertices normals will be computed automatically. The vertices normals are
+/// computed with weighted average of the associated triangle face normal. The
+/// weights are the angle between the associated edges of neighbor triangle face.
/**
* @param nbVertices Number of vertices in the array
* @param verticesStart Pointer to the first vertices of the array
@@ -48,9 +54,237 @@ TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, i
mNbVertices = nbVertices;
mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
mVerticesStride = verticesStride;
+ mVerticesNormalsStart = nullptr;
+ mVerticesNormalsStride = 0;
mNbTriangles = nbTriangles;
mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
mIndicesStride = indexesStride;
mVertexDataType = vertexDataType;
+ mVertexNormaldDataType = NormalDataType::NORMAL_FLOAT_TYPE;
mIndexDataType = indexDataType;
+ mAreVerticesNormalsProvidedByUser = false;
+
+ // Compute the vertices normals because they are not provided by the user
+ computeVerticesNormals();
+}
+
+// Constructor with vertices normals
+/// Note that your data will not be copied into the TriangleVertexArray and
+/// therefore, you need to make sure that those data are always valid during
+/// the lifetime of the TriangleVertexArray. With this constructor, you need
+/// to provide the vertices normals that will be used for smooth mesh collision.
+/**
+ * @param nbVertices Number of vertices in the array
+ * @param verticesStart Pointer to the first vertices of the array
+ * @param verticesStride Number of bytes between the beginning of two consecutive vertices
+ * @param verticesNormalsStart Pointer to the first vertex normal of the array
+ * @param verticesNormalsStride Number of bytes between the beginning of two consecutive vertex normals
+ * @param nbTriangles Number of triangles in the array
+ * @param indexesStart Pointer to the first triangle index
+ * @param indexesStride Number of bytes between the beginning of two consecutive triangle indices
+ * @param vertexDataType Type of data for the vertices (float, double)
+ * @param indexDataType Type of data for the indices (short, int)
+ */
+TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
+ void* verticesNormalsStart, int verticesNormalsStride,
+ uint nbTriangles, void* indexesStart, int indexesStride,
+ VertexDataType vertexDataType, NormalDataType normalDataType,
+ IndexDataType indexDataType) {
+
+ mNbVertices = nbVertices;
+ mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
+ mVerticesStride = verticesStride;
+ mVerticesNormalsStart = reinterpret_cast<unsigned char*>(verticesNormalsStart);
+ mVerticesNormalsStride = verticesNormalsStride;
+ mNbTriangles = nbTriangles;
+ mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
+ mIndicesStride = indexesStride;
+ mVertexDataType = vertexDataType;
+ mVertexNormaldDataType = normalDataType;
+ mIndexDataType = indexDataType;
+ mAreVerticesNormalsProvidedByUser = true;
+
+ assert(mVerticesNormalsStart != nullptr);
+}
+
+// Destructor
+TriangleVertexArray::~TriangleVertexArray() {
+
+ // If the vertices normals have not been provided by the user
+ if (!mAreVerticesNormalsProvidedByUser) {
+
+ // Release the allocated memory
+ float* verticesNormals = reinterpret_cast<float*>(mVerticesNormalsStart);
+ delete[] verticesNormals;
+ }
+}
+
+// Compute the vertices normals when they are not provided by the user
+/// The vertices normals are computed with weighted average of the associated
+/// triangle face normal. The weights are the angle between the associated edges
+/// of neighbor triangle face.
+void TriangleVertexArray::computeVerticesNormals() {
+
+ // Allocate memory for the vertices normals
+ float* verticesNormals = new float[mNbVertices * 3];
+
+ // Init vertices normals to zero
+ for (uint i=0; i<mNbVertices * 3; i++) {
+ verticesNormals[i] = 0.0f;
+ }
+
+ // For each triangle face in the array
+ for (uint f=0; f < mNbTriangles; f++) {
+
+ // Get the indices of the three vertices of the triangle in the array
+ uint verticesIndices[3];
+ getTriangleVerticesIndices(f, verticesIndices);
+
+ // Get the triangle vertices
+ Vector3 triangleVertices[3];
+ getTriangleVertices(f, triangleVertices);
+
+ // Edges lengths
+ decimal edgesLengths[3];
+ edgesLengths[0] = (triangleVertices[1] - triangleVertices[0]).length();
+ edgesLengths[1] = (triangleVertices[2] - triangleVertices[1]).length();
+ edgesLengths[2] = (triangleVertices[0] - triangleVertices[2]).length();
+
+ // For each vertex of the face
+ for (uint v=0; v < 3; v++) {
+
+ uint previousVertex = (v == 0) ? 2 : v-1;
+ uint nextVertex = (v == 2) ? 0 : v+1;
+ Vector3 a = triangleVertices[nextVertex] - triangleVertices[v];
+ Vector3 b = triangleVertices[previousVertex] - triangleVertices[v];
+
+ Vector3 crossProduct = a.cross(b);
+ decimal sinA = crossProduct.length() / (edgesLengths[previousVertex] * edgesLengths[v]);
+ Vector3 normalComponent = std::asin(sinA) * crossProduct;
+
+ // Add the normal component of this vertex into the normals array
+ verticesNormals[verticesIndices[v]] = normalComponent.x;
+ verticesNormals[verticesIndices[v] + 1] = normalComponent.y;
+ verticesNormals[verticesIndices[v] + 2] = normalComponent.z;
+ }
+ }
+
+ // Normalize the computed vertices normals
+ for (uint v=0; v<mNbVertices * 3; v += 3) {
+
+ // Normalize the normal
+ Vector3 normal(verticesNormals[v], verticesNormals[v + 1], verticesNormals[v + 2]);
+ normal.normalize();
+
+ verticesNormals[v] = normal.x;
+ verticesNormals[v + 1] = normal.y;
+ verticesNormals[v + 2] = normal.z;
+ }
+
+ mVerticesNormalsStart = reinterpret_cast<unsigned char*>(verticesNormals);
+}
+
+// Return the indices of the three vertices of a given triangle in the array
+void TriangleVertexArray::getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const {
+
+ assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
+
+ void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride);
+
+ // For each vertex of the triangle
+ for (int i=0; i < 3; i++) {
+
+ // Get the index of the current vertex in the triangle
+ if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
+ outVerticesIndices[i] = ((uint*)vertexIndexPointer)[i];
+ }
+ else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
+ outVerticesIndices[i] = ((unsigned short*)vertexIndexPointer)[i];
+ }
+ else {
+ assert(false);
+ }
+ }
+}
+
+// Return the vertices coordinates of a triangle
+void TriangleVertexArray::getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const {
+
+ assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
+
+ void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride);
+
+ // For each vertex of the triangle
+ for (int k=0; k < 3; k++) {
+
+ // Get the index of the current vertex in the triangle
+ int vertexIndex = 0;
+ if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
+ vertexIndex = ((uint*)vertexIndexPointer)[k];
+ }
+ else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
+ vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
+ }
+ else {
+ assert(false);
+ }
+
+ // Get the vertices components of the triangle
+ if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
+ const float* vertices = (float*)(mVerticesStart + vertexIndex * mVerticesStride);
+ outTriangleVertices[k][0] = decimal(vertices[0]);
+ outTriangleVertices[k][1] = decimal(vertices[1]);
+ outTriangleVertices[k][2] = decimal(vertices[2]);
+ }
+ else if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
+ const double* vertices = (double*)(mVerticesStart + vertexIndex * mVerticesStride);
+ outTriangleVertices[k][0] = decimal(vertices[0]);
+ outTriangleVertices[k][1] = decimal(vertices[1]);
+ outTriangleVertices[k][2] = decimal(vertices[2]);
+ }
+ else {
+ assert(false);
+ }
+ }
+}
+
+// Return the three vertices normals of a triangle
+void TriangleVertexArray::getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const {
+
+ assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
+
+ void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride);
+
+ // For each vertex of the triangle
+ for (int k=0; k < 3; k++) {
+
+ // Get the index of the current vertex in the triangle
+ int vertexIndex = 0;
+ if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
+ vertexIndex = ((uint*)vertexIndexPointer)[k];
+ }
+ else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
+ vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
+ }
+ else {
+ assert(false);
+ }
+
+ // Get the normals from the array
+ if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE) {
+ const float* normal = (float*)(mVerticesNormalsStart + vertexIndex * mVerticesNormalsStride);
+ outTriangleVerticesNormals[k][0] = decimal(normal[0]);
+ outTriangleVerticesNormals[k][1] = decimal(normal[1]);
+ outTriangleVerticesNormals[k][2] = decimal(normal[2]);
+ }
+ else if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_DOUBLE_TYPE) {
+ const double* normal = (double*)(mVerticesNormalsStart + vertexIndex * mVerticesNormalsStride);
+ outTriangleVerticesNormals[k][0] = decimal(normal[0]);
+ outTriangleVerticesNormals[k][1] = decimal(normal[1]);
+ outTriangleVerticesNormals[k][2] = decimal(normal[2]);
+ }
+ else {
+ assert(false);
+ }
+ }
}
diff --git a/src/collision/TriangleVertexArray.h b/src/collision/TriangleVertexArray.h
index 8e434919..20ae9387 100644
--- a/src/collision/TriangleVertexArray.h
+++ b/src/collision/TriangleVertexArray.h
@@ -31,6 +31,8 @@
namespace reactphysics3d {
+struct Vector3;
+
// Class TriangleVertexArray
/**
* This class is used to describe the vertices and faces of a triangular mesh.
@@ -48,11 +50,16 @@ class TriangleVertexArray {
/// Data type for the vertices in the array
enum class VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
+ /// Data type for the vertex normals in the array
+ enum class NormalDataType {NORMAL_FLOAT_TYPE, NORMAL_DOUBLE_TYPE};
+
/// Data type for the indices in the array
enum class IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
protected:
+ // -------------------- Attributes -------------------- //
+
/// Number of vertices in the array
uint mNbVertices;
@@ -63,6 +70,13 @@ class TriangleVertexArray {
/// values in the array
int mVerticesStride;
+ /// Pointer to the first vertex normal value in the array
+ unsigned char* mVerticesNormalsStart;
+
+ /// Stride (number of bytes) between the beginning of two vertex normals
+ /// values in the array
+ int mVerticesNormalsStride;
+
/// Number of triangles in the array
uint mNbTriangles;
@@ -76,22 +90,45 @@ class TriangleVertexArray {
/// Data type of the vertices in the array
VertexDataType mVertexDataType;
+ /// Data type of the vertex normals in the array
+ NormalDataType mVertexNormaldDataType;
+
/// Data type of the indices in the array
IndexDataType mIndexDataType;
+ /// True if the vertices normals are provided by the user
+ bool mAreVerticesNormalsProvidedByUser;
+
+ // -------------------- Methods -------------------- //
+
+ /// Compute the vertices normals when they are not provided by the user
+ void computeVerticesNormals();
+
public:
- /// Constructor
+ // -------------------- Methods -------------------- //
+
+ /// Constructor without vertices normals
TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType);
+ /// Constructor with vertices normals
+ TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
+ void* verticesNormalsStart, int verticesNormalsStride,
+ uint nbTriangles, void* indexesStart, int indexesStride,
+ VertexDataType vertexDataType, NormalDataType normalDataType,
+ IndexDataType indexDataType);
+
/// Destructor
- ~TriangleVertexArray() = default;
+ ~TriangleVertexArray();
/// Return the vertex data type
VertexDataType getVertexDataType() const;
+ /// Return the vertex normal data type
+ NormalDataType getVertexNormalDataType() const;
+
/// Return the index data type
IndexDataType getIndexDataType() const;
@@ -104,14 +141,29 @@ class TriangleVertexArray {
/// Return the vertices stride (number of bytes)
int getVerticesStride() const;
+ /// Return the vertex normals stride (number of bytes)
+ int getVerticesNormlasStride() const;
+
/// Return the indices stride (number of bytes)
int getIndicesStride() const;
/// Return the pointer to the start of the vertices array
unsigned char* getVerticesStart() const;
+ /// Return the pointer to the start of the vertex normals array
+ unsigned char* getVerticesNormalsStart() const;
+
/// Return the pointer to the start of the indices array
unsigned char* getIndicesStart() const;
+
+ /// Return the vertices coordinates of a triangle
+ void getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const;
+
+ /// Return the three vertices normals of a triangle
+ void getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const;
+
+ /// Return the indices of the three vertices of a given triangle in the array
+ void getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const;
};
// Return the vertex data type
@@ -119,6 +171,11 @@ inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataTyp
return mVertexDataType;
}
+// Return the vertex normal data type
+inline TriangleVertexArray::NormalDataType TriangleVertexArray::getVertexNormalDataType() const {
+ return mVertexNormaldDataType;
+}
+
// Return the index data type
inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const {
return mIndexDataType;
@@ -139,6 +196,11 @@ inline int TriangleVertexArray::getVerticesStride() const {
return mVerticesStride;
}
+// Return the vertex normals stride (number of bytes)
+inline int TriangleVertexArray::getVerticesNormlasStride() const {
+ return mVerticesNormalsStride;
+}
+
// Return the indices stride (number of bytes)
inline int TriangleVertexArray::getIndicesStride() const {
return mIndicesStride;
@@ -149,6 +211,11 @@ inline unsigned char* TriangleVertexArray::getVerticesStart() const {
return mVerticesStart;
}
+// Return the pointer to the start of the vertex normals array
+inline unsigned char* TriangleVertexArray::getVerticesNormalsStart() const {
+ return mVerticesNormalsStart;
+}
+
// Return the pointer to the start of the indices array
inline unsigned char* TriangleVertexArray::getIndicesStart() const {
return mIndicesStart;
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index 73a1dd13..72ba2e06 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -82,7 +82,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(f);
- const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
+ Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
index 7f5f439b..8a2475a2 100644
--- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
+++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
@@ -22,7 +22,7 @@
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
-
+/*
// Libraries
#include "collision/shapes/ConcaveShape.h"
#include "collision/shapes/TriangleShape.h"
@@ -34,12 +34,14 @@
using namespace reactphysics3d;
// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase)
-void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints) {
+void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
+ const Vector3* verticesNormals) {
// Create a triangle collision shape
decimal margin = mConcaveShape->getTriangleMargin();
TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
- TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+ TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
+ verticesNormals, meshSubPart, triangleIndex, margin);
// Create a narrow phase info for the narrow-phase collision detection
NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList;
@@ -297,3 +299,4 @@ bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multi
// // smooth mesh collision
// mContactPoints.push_back(smoothContactInfo);
//}
+*/
diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
index 4619e480..70cd3dbc 100644
--- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
+++ b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
@@ -22,7 +22,7 @@
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
-
+/*
#ifndef REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
#define REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
@@ -37,11 +37,6 @@
namespace reactphysics3d {
// Class ConvexVsTriangleCallback
-/**
- * This class is used to report a collision between the triangle
- * of a concave mesh shape and a convex shape during the
- * middle-phase algorithm
- */
class MiddlePhaseTriangleCallback : public TriangleCallback {
protected:
@@ -78,14 +73,11 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
}
/// Test collision between a triangle and the convex mesh shape
- virtual void testTriangle(const Vector3* trianglePoints) override;
+ virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints,
+ const Vector3* verticesNormals) override;
};
// Class SmoothMeshContactInfo
-/**
- * Contains data for of potential smooth contact during the smooth mesh
- * contacts computation.
- */
struct SmoothMeshContactInfo {
public:
@@ -131,11 +123,6 @@ struct ContactsDepthCompare {
// TODO : Delete this
// Class SmoothCollisionNarrowPhaseCallback
-/**
- * This class is used as a narrow-phase callback to get narrow-phase contacts
- * of the concave triangle mesh to temporary store them in order to be used in
- * the smooth mesh collision algorithm if this one is enabled.
- */
class SmoothCollisionNarrowPhaseCallback {
private:
@@ -155,12 +142,6 @@ class SmoothCollisionNarrowPhaseCallback {
// TODO : Delete this
// Class ConcaveVsConvexAlgorithm
-/**
- * This class is used to compute the narrow-phase collision detection
- * between a concave collision shape and a convex collision shape. The idea is
- * to use the GJK collision detection algorithm to compute the collision between
- * the convex shape and each of the triangles in the concave shape.
- */
class ConcaveVsConvexAlgorithm {
protected :
@@ -212,3 +193,4 @@ inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap
#endif
+*/
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index 10d344f1..609c415a 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -27,6 +27,7 @@
#include "GJKAlgorithm.h"
#include "constraint/ContactPoint.h"
#include "engine/OverlappingPair.h"
+#include "collision/shapes/TriangleShape.h"
#include "configuration.h"
#include "engine/Profiler.h"
#include <algorithm>
@@ -76,7 +77,8 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
- Transform body2Tobody1 = transform1.getInverse() * transform2;
+ Transform transform1Inverse = transform1.getInverse();
+ Transform body2Tobody1 = transform1Inverse * transform2;
// Matrix that transform a direction from local
// space of body 1 into local space of body 2
@@ -209,6 +211,10 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
if (reportContacts) {
+ // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+ TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2,
+ penetrationDepth, normal);
+
// Add a new contact point
narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB);
}
@@ -219,6 +225,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
return GJKResult::SEPARATED;
}
+
// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) {
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index cebec9a6..23ce86e0 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -30,6 +30,7 @@
#include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/SphereShape.h"
#include "engine/OverlappingPair.h"
+#include "collision/shapes/TriangleShape.h"
#include "configuration.h"
#include "engine/Profiler.h"
#include <algorithm>
@@ -143,13 +144,21 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal);
- const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius();
- const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
+ Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius();
+ Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
+
+ // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+ TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
+ isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
+ isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal,
+ narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
+ minPenetrationDepth, normalWorld);
+
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
- isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
- isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
+ isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
+ isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
}
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
@@ -380,7 +389,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
- const Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace;
+ Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace;
const decimal capsuleRadius = capsuleShape->getRadius();
// If the separating axis is a face normal
@@ -410,7 +419,14 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
closestPointCapsuleInnerSegment, closestPointPolyhedronEdge);
// Project closest capsule inner segment point into the capsule bounds
- const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
+ Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
+
+ // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+ TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
+ isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
+ isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule,
+ narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
+ minPenetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
@@ -483,7 +499,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe
// axis is a face normal of the polyhedron
void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
- const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
+ Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const {
@@ -525,15 +541,27 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// If the clipped point is one that produce this penetration depth, we keep it
if (clipPointPenDepth > penetrationDepth - capsuleRadius - decimal(0.001)) {
- const Vector3 contactPointPolyhedron = clipSegment[i] + delta;
+ Vector3 contactPointPolyhedron = clipSegment[i] + delta;
// Project the clipped point into the capsule bounds
- const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius;
+ Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius;
+
+ if (isCapsuleShape1) {
+ normalWorld = -normalWorld;
+ }
+
+ // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+ TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
+ isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
+ isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule,
+ narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
+ penetrationDepth, normalWorld);
+
// Create the contact point
- narrowPhaseInfo->addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth,
- isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
- isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
+ narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
+ isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
+ isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
}
}
}
@@ -807,7 +835,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
// Compute the world normal
- const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
+ Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
-(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
// Get the reference face
@@ -874,11 +902,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) {
// Convert the clip incident polyhedron vertex into the incident polyhedron local-space
- const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
+ Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
// Project the contact point onto the reference face
Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
+ // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+ TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
+ isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
+ isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron,
+ narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
+ minPenetrationDepth, normalWorld);
+
// Create a new contact point
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
@@ -901,14 +936,20 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
// Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
- const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
+ Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal
- const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
+ Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
+
+ // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+ TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
+ closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
+ narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
+ minPenetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
- closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
+ closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
}
lastFrameInfo.satIsAxisFacePolyhedron1 = false;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 49cd2626..73838d95 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -121,7 +121,7 @@ class SATAlgorithm {
/// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
- const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
+ Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const;
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index 049ce449..7ecda325 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -36,15 +36,15 @@ using namespace reactphysics3d;
// by Dirk Gregorius.
bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
+ narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
+ assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
+ narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
+
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
- assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
- narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
- assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
- narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
-
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp
index a0bd1094..8cb5235a 100644
--- a/src/collision/shapes/ConcaveMeshShape.cpp
+++ b/src/collision/shapes/ConcaveMeshShape.cpp
@@ -49,51 +49,13 @@ void ConcaveMeshShape::initBVHTree() {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
- TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
- TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
- unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
- unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
- int vertexStride = triangleVertexArray->getVerticesStride();
- int indexStride = triangleVertexArray->getIndicesStride();
-
// For each triangle of the concave mesh
for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
- void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
Vector3 trianglePoints[3];
- // For each vertex of the triangle
- for (int k=0; k < 3; k++) {
-
- // Get the index of the current vertex in the triangle
- int vertexIndex = 0;
- if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
- vertexIndex = ((uint*)vertexIndexPointer)[k];
- }
- else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
- vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
- }
- else {
- assert(false);
- }
-
- // Get the vertices components of the triangle
- if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
- const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
- trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
- trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
- trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
- }
- else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
- const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
- trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
- trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
- trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
- }
- else {
- assert(false);
- }
- }
+ // Get the triangle vertices
+ triangleVertexArray->getTriangleVertices(triangleIndex, trianglePoints);
// Create the AABB for the triangle
AABB aabb = AABB::createAABBForTriangle(trianglePoints);
@@ -106,56 +68,32 @@ void ConcaveMeshShape::initBVHTree() {
}
// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
-// given the start vertex index pointer of the triangle
-void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
- Vector3* outTriangleVertices) const {
+void ConcaveMeshShape::getTriangleVertices(uint subPart, uint triangleIndex,
+ Vector3* outTriangleVertices) const {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
- TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
- TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
- unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
- unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
- int vertexStride = triangleVertexArray->getVerticesStride();
- int indexStride = triangleVertexArray->getIndicesStride();
+ // Get the vertices coordinates of the triangle
+ triangleVertexArray->getTriangleVertices(triangleIndex, outTriangleVertices);
- void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
-
- // For each vertex of the triangle
- for (int k=0; k < 3; k++) {
-
- // Get the index of the current vertex in the triangle
- int vertexIndex = 0;
- if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
- vertexIndex = ((uint*)vertexIndexPointer)[k];
- }
- else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
- vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
- }
- else {
- assert(false);
- }
-
- // Get the vertices components of the triangle
- if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
- const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
- outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
- outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
- outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
- }
- else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
- const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
- outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
- outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
- outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
- }
- else {
- assert(false);
- }
- }
+ // Apply the scaling factor to the vertices
+ outTriangleVertices[0] *= mScaling.x;
+ outTriangleVertices[1] *= mScaling.x;
+ outTriangleVertices[2] *= mScaling.x;
}
+// Return the three vertex normals (in the array outVerticesNormals) of a triangle
+void ConcaveMeshShape::getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const {
+
+ // Get the triangle vertex array of the current sub-part
+ TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
+
+ // Get the vertices normals of the triangle
+ triangleVertexArray->getTriangleVerticesNormals(triangleIndex, outVerticesNormals);
+}
+
+
// Use a callback method on all triangles of the concave shape inside a given AABB
void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
@@ -208,11 +146,15 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
// Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3];
- mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
+ mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints);
+ // Get the vertices normals of the triangle
+ Vector3 verticesNormals[3];
+ mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
// Create a triangle collision shape
decimal margin = mConcaveMeshShape.getTriangleMargin();
- TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+ TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
+ verticesNormals, data[0], data[1], margin);
triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
// Ray casting test against the collision shape
diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h
index 83e5103a..309ec27c 100644
--- a/src/collision/shapes/ConcaveMeshShape.h
+++ b/src/collision/shapes/ConcaveMeshShape.h
@@ -118,6 +118,10 @@ class ConcaveMeshShape : public ConcaveShape {
/// Dynamic AABB tree to accelerate collision with the triangles
DynamicAABBTree mDynamicAABBTree;
+ /// Array with computed vertices normals for each TriangleVertexArray of the triangle mesh (only
+ /// if the user did not provide its own vertices normals)
+ Vector3** mComputedVerticesNormals;
+
// -------------------- Methods -------------------- //
/// Raycast method with feedback information
@@ -130,9 +134,13 @@ class ConcaveMeshShape : public ConcaveShape {
void initBVHTree();
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
- /// given the start vertex index pointer of the triangle.
- void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
- Vector3* outTriangleVertices) const;
+ void getTriangleVertices(uint subPart, uint triangleIndex, Vector3* outTriangleVertices) const;
+
+ /// Return the three vertex normals (in the array outVerticesNormals) of a triangle
+ void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const;
+
+ /// Get a smooth contact normal for collision for a triangle of the mesh
+ Vector3 computeSmoothLocalContactNormalForTriangle(TriangleShape* triangleShape, const Vector3& localContactPoint) const;
public:
@@ -224,10 +232,14 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId)
// Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3];
- mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
+ mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints);
+
+ // Get the vertices normals of the triangle
+ Vector3 verticesNormals[3];
+ mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
// Call the callback to test narrow-phase collision with this triangle
- mTriangleTestCallback.testTriangle(trianglePoints);
+ mTriangleTestCallback.testTriangle(data[0], data[1], trianglePoints, verticesNormals);
}
}
diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h
index c5657b2b..6e07187f 100644
--- a/src/collision/shapes/ConcaveShape.h
+++ b/src/collision/shapes/ConcaveShape.h
@@ -46,7 +46,8 @@ class TriangleCallback {
virtual ~TriangleCallback() = default;
/// Report a triangle
- virtual void testTriangle(const Vector3* trianglePoints)=0;
+ virtual void testTriangle(uint meshSubPart, uint triangleIndex,
+ const Vector3* trianglePoints, const Vector3* verticesNormals)=0;
};
diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h
index 4b98c9fa..94acc7f9 100644
--- a/src/collision/shapes/ConvexShape.h
+++ b/src/collision/shapes/ConvexShape.h
@@ -49,10 +49,12 @@ class ConvexShape : public CollisionShape {
// -------------------- Methods -------------------- //
// Return a local support point in a given direction with the object margin
+ // TODO : Try to remove cachedCollisionData parameter
Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
+ // TODO : Try to remove cachedCollisionData parameter
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const=0;
diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp
index 02ef852f..c8fc97f2 100644
--- a/src/collision/shapes/HeightFieldShape.cpp
+++ b/src/collision/shapes/HeightFieldShape.cpp
@@ -133,24 +133,48 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
for (int j = jMin; j < jMax; j++) {
// Compute the four point of the current quad
- Vector3 p1 = getVertexAt(i, j);
- Vector3 p2 = getVertexAt(i, j + 1);
- Vector3 p3 = getVertexAt(i + 1, j);
- Vector3 p4 = getVertexAt(i + 1, j + 1);
+ const Vector3 p1 = getVertexAt(i, j);
+ const Vector3 p2 = getVertexAt(i, j + 1);
+ const Vector3 p3 = getVertexAt(i + 1, j);
+ const Vector3 p4 = getVertexAt(i + 1, j + 1);
// Generate the first triangle for the current grid rectangle
Vector3 trianglePoints[3] = {p1, p2, p3};
+ // Compute the triangle normal
+ Vector3 triangle1Normal = (p2 - p1).cross(p3 - p1).getUnit();
+
+ // Use the triangle face normal as vertices normals (this is an aproximation. The correct
+ // solution would be to compute all the normals of the neighbor triangles and use their
+ // weighted average (with incident angle as weight) at the vertices. However, this solution
+ // seems too expensive (it requires to compute the normal of all neighbor triangles instead
+ // and compute the angle of incident edges with asin(). Maybe we could also precompute the
+ // vertices normal at the HeightFieldShape constructor but it will require extra memory to
+ // store them.
+ Vector3 verticesNormals1[3] = {triangle1Normal, triangle1Normal, triangle1Normal};
+
// Test collision against the first triangle
- callback.testTriangle(trianglePoints);
+ callback.testTriangle(0, 0, trianglePoints, verticesNormals1);
// Generate the second triangle for the current grid rectangle
trianglePoints[0] = p3;
trianglePoints[1] = p2;
trianglePoints[2] = p4;
+ // Compute the triangle normal
+ Vector3 triangle2Normal = (p2 - p3).cross(p4 - p3).getUnit();
+
+ // Use the triangle face normal as vertices normals (this is an aproximation. The correct
+ // solution would be to compute all the normals of the neighbor triangles and use their
+ // weighted average (with incident angle as weight) at the vertices. However, this solution
+ // seems too expensive (it requires to compute the normal of all neighbor triangles instead
+ // and compute the angle of incident edges with asin(). Maybe we could also precompute the
+ // vertices normal at the HeightFieldShape constructor but it will require extra memory to
+ // store them.
+ Vector3 verticesNormals2[3] = {triangle2Normal, triangle2Normal, triangle2Normal};
+
// Test collision against the second triangle
- callback.testTriangle(trianglePoints);
+ callback.testTriangle(0, 0, trianglePoints, verticesNormals2);
}
}
}
@@ -232,11 +256,13 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
}
// Raycast test between a ray and a triangle of the heightfield
-void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
+void TriangleOverlapCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
+ const Vector3* verticesNormals) {
// Create a triangle collision shape
decimal margin = mHeightFieldShape.getTriangleMargin();
- TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+ TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
+ verticesNormals, meshSubPart, triangleIndex, margin);
triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
// Ray casting test against the collision shape
diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h
index 6f2b8459..aa0cd787 100644
--- a/src/collision/shapes/HeightFieldShape.h
+++ b/src/collision/shapes/HeightFieldShape.h
@@ -64,7 +64,8 @@ class TriangleOverlapCallback : public TriangleCallback {
bool getIsHit() const {return mIsHit;}
/// Raycast test between a ray and a triangle of the heightfield
- virtual void testTriangle(const Vector3* trianglePoints) override;
+ virtual void testTriangle(uint meshSubPart, uint triangleIndex,
+ const Vector3* trianglePoints, const Vector3* verticesNormals) override;
};
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index 9c0deb38..72eb93d3 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -26,6 +26,7 @@
// Libraries
#include "TriangleShape.h"
#include "collision/ProxyShape.h"
+#include "mathematics/mathematics_functions.h"
#include "engine/Profiler.h"
#include "configuration.h"
#include <cassert>
@@ -34,13 +35,17 @@ using namespace reactphysics3d;
// Constructor
/**
+ * Do not use this constructor. It is supposed to be used internally only.
+ * Use a ConcaveMeshShape instead.
* @param point1 First point of the triangle
* @param point2 Second point of the triangle
* @param point3 Third point of the triangle
+ * @param verticesNormals The three vertices normals for smooth mesh collision
* @param margin The collision margin (in meters) around the collision shape
*/
-TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin)
- : ConvexPolyhedronShape(margin) {
+TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
+ const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin)
+ : ConvexPolyhedronShape(margin), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) {
mPoints[0] = point1;
mPoints[1] = point2;
@@ -50,9 +55,104 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const
mNormal = (point3 - point1).cross(point2 - point1);
mNormal.normalize();
+ mVerticesNormals[0] = verticesNormals[0];
+ mVerticesNormals[1] = verticesNormals[1];
+ mVerticesNormals[2] = verticesNormals[2];
+
mRaycastTestType = TriangleRaycastSide::FRONT;
}
+// This method compute the smooth mesh contact with a triangle in case one of the two collision
+// shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh
+// at the contact point instead of the triangle normal to avoid the internal edge collision issue.
+// This method will return the new smooth world contact
+// normal of the triangle and the the local contact point on the other shape.
+void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2,
+ Vector3& localContactPointShape1, Vector3 localContactPointShape2,
+ const Transform& shape1ToWorld, const Transform& shape2ToWorld,
+ decimal penetrationDepth, Vector3& outSmoothVertexNormal) {
+
+ assert(shape1->getType() != CollisionShapeType::TRIANGLE || shape2->getType() != CollisionShapeType::TRIANGLE);
+
+ // If one the shape is a triangle
+ bool isShape1Triangle = shape1->getType() == CollisionShapeType::TRIANGLE;
+ if (isShape1Triangle || shape2->getType() == CollisionShapeType::TRIANGLE) {
+
+ const TriangleShape* triangleShape = isShape1Triangle ? static_cast<const TriangleShape*>(shape1):
+ static_cast<const TriangleShape*>(shape2);
+
+ // Compute the smooth triangle mesh contact normal and recompute the local contact point on the other shape
+ triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2,
+ isShape1Triangle ? shape1ToWorld : shape2ToWorld,
+ isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(),
+ penetrationDepth,
+ isShape1Triangle ? localContactPointShape2 : localContactPointShape1,
+ outSmoothVertexNormal);
+
+ // Make sure the direction of the contact normal is from shape1 to shape2
+ if (!isShape1Triangle) {
+ outSmoothVertexNormal = -outSmoothVertexNormal;
+ }
+ }
+}
+
+
+// This method implements the technique described in Game Physics Pearl book
+// by Gino van der Bergen and Dirk Gregorius to get smooth triangle mesh collision. The idea is
+// to replace the contact normal of the triangle shape with the precomputed normal of the triangle
+// mesh at this point. Then, we need to recompute the contact point on the other shape in order to
+// stay aligned with the new contact normal. This method will return the new smooth world contact
+// normal of the triangle and the the local contact point on the other shape.
+void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform,
+ const Transform& worldToOtherShapeTransform, decimal penetrationDepth,
+ Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const {
+
+ // Get the smooth contact normal of the mesh at the contact point on the triangle
+ Vector3 localNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle);
+
+ // Convert the local contact normal into world-space
+ outSmoothWorldContactTriangleNormal = triangleShapeToWorldTransform.getOrientation() * localNormal;
+
+ // Convert the contact normal into the local-space of the other shape
+ Vector3 normalOtherShape = worldToOtherShapeTransform.getOrientation() * outSmoothWorldContactTriangleNormal;
+
+ // Convert the local contact point of the triangle into the local-space of the other shape
+ Vector3 trianglePointOtherShape = worldToOtherShapeTransform * triangleShapeToWorldTransform *
+ localContactPointTriangle;
+
+ // Re-align the local contact point on the other shape such that it is aligned along
+ // the new contact normal
+ Vector3 otherShapePoint = trianglePointOtherShape - normalOtherShape * penetrationDepth;
+ outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z);
+}
+
+// Get a smooth contact normal for collision for a triangle of the mesh
+/// This is used to avoid the internal edges issue that occurs when a shape is colliding with
+/// several triangles of a concave mesh. If the shape collide with an edge of the triangle for instance,
+/// the computed contact normal from this triangle edge is not necessarily in the direction of the surface
+/// normal of the mesh at this point. The idea to solve this problem is to use the real (smooth) surface
+/// normal of the mesh at this point as the contact normal. This technique is described in the chapter 5
+/// of the Game Physics Pearl book by Gino van der Bergen and Dirk Gregorius. The vertices normals of the
+/// mesh are either provided by the user or precomputed if the user did not provide them.
+Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const {
+
+ // Compute the barycentric coordinates of the point in the triangle
+ decimal u, v, w;
+ computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w);
+
+ int nbZeros = 0;
+ bool isUZero = approxEqual(u, decimal(0), decimal(0.0001));
+ bool isVZero = approxEqual(v, decimal(0), decimal(0.0001));
+ bool isWZero = approxEqual(w, decimal(0), decimal(0.0001));
+ if (isUZero) nbZeros++;
+ if (isVZero) nbZeros++;
+ if (isWZero) nbZeros++;
+
+ // We compute the contact normal as the barycentric interpolation of the three vertices normals
+ return (u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]).getUnit();
+}
+
+
// Raycast method with feedback information
/// This method use the line vs triangle raycasting technique described in
/// Real-time Collision Detection by Christer Ericson.
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index e25086b8..900b71db 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -49,7 +49,10 @@ enum class TriangleRaycastSide {
// Class TriangleShape
/**
* This class represents a triangle collision shape that is centered
- * at the origin and defined three points.
+ * at the origin and defined three points. A user cannot instanciate
+ * an object of this class. This class is for internal use only. Instances
+ * of this class are created when the user creates an HeightFieldShape and
+ * a ConcaveMeshShape
*/
class TriangleShape : public ConvexPolyhedronShape {
@@ -63,15 +66,27 @@ class TriangleShape : public ConvexPolyhedronShape {
/// Normal of the triangle
Vector3 mNormal;
+ /// Three vertices normals for smooth collision with triangle mesh
+ Vector3 mVerticesNormals[3];
+
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide mRaycastTestType;
+ /// Index of the mesh sub part in the original mesh
+ uint mMeshSubPart;
+
+ /// Triangle index of the triangle in the sub mesh
+ uint mTriangleIndex;
+
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const override;
+ /// Get a smooth contact normal for collision for a triangle of the mesh
+ Vector3 computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const;
+
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
@@ -81,13 +96,20 @@ class TriangleShape : public ConvexPolyhedronShape {
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
+ // -------------------- Methods -------------------- //
+
+ /// This method implements the technique described in Game Physics Pearl book
+ void computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform,
+ const Transform& worldToOtherShapeTransform, decimal penetrationDepth,
+ Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const;
+
public:
// -------------------- Methods -------------------- //
/// Constructor
TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
- decimal margin = OBJECT_MARGIN);
+ const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin = OBJECT_MARGIN);
/// Destructor
virtual ~TriangleShape() override = default;
@@ -143,10 +165,23 @@ class TriangleShape : public ConvexPolyhedronShape {
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const override;
+ /// Return the index of the sub part mesh of the original mesh
+ uint getMeshSubPart() const;
+
+ /// Return the triangle index in the original mesh
+ uint getTriangleIndex() const;
+
+ /// This method compute the smooth mesh contact with a triangle in case one of the two collision shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh
+ static void computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2,
+ Vector3& localContactPointShape1, Vector3 localContactPointShape2,
+ const Transform& shape1ToWorld, const Transform& shape2ToWorld,
+ decimal penetrationDepth, Vector3& outSmoothVertexNormal);
+
// ---------- Friendship ---------- //
friend class ConcaveMeshRaycastCallback;
friend class TriangleOverlapCallback;
+ friend class MiddlePhaseTriangleCallback;
};
// Return the number of bytes used by the collision shape
@@ -155,8 +190,7 @@ inline size_t TriangleShape::getSizeInBytes() const {
}
// Return a local support point in a given direction without the object margin
-inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const {
+inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const {
Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2]));
return mPoints[dotProducts.getMaxAxis()];
}
@@ -286,6 +320,16 @@ inline Vector3 TriangleShape::getCentroid() const {
return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0);
}
+// Return the index of the sub part mesh of the original mesh
+inline uint TriangleShape::getMeshSubPart() const {
+ return mMeshSubPart;
+}
+
+// Return the triangle index in the original mesh
+inline uint TriangleShape::getTriangleIndex() const {
+ return mTriangleIndex;
+}
+
// Return the number of half-edges of the polyhedron
inline uint TriangleShape::getNbHalfEdges() const {
return 6;
From e725af80b6bc9da177f01ca1892a65007db1ffa4 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 22 Aug 2017 07:38:22 +0200
Subject: [PATCH 065/133] Update raycasting test code for convex mesh and
remove commented code
---
test/tests/collision/TestRaycast.h | 147 +++++++++--------------------
testbed/common/ConvexMesh.cpp | 7 --
2 files changed, 46 insertions(+), 108 deletions(-)
diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h
index 0f25ab94..93860a1c 100644
--- a/test/tests/collision/TestRaycast.h
+++ b/test/tests/collision/TestRaycast.h
@@ -110,7 +110,6 @@ class TestRaycast : public Test {
CollisionBody* mSphereBody;
CollisionBody* mCapsuleBody;
CollisionBody* mConvexMeshBody;
- CollisionBody* mConvexMeshBodyEdgesInfo;
CollisionBody* mCylinderBody;
CollisionBody* mCompoundBody;
CollisionBody* mTriangleBody;
@@ -128,7 +127,6 @@ class TestRaycast : public Test {
SphereShape* mSphereShape;
CapsuleShape* mCapsuleShape;
ConvexMeshShape* mConvexMeshShape;
- ConvexMeshShape* mConvexMeshShapeEdgesInfo;
TriangleShape* mTriangleShape;
ConcaveShape* mConcaveMeshShape;
HeightFieldShape* mHeightFieldShape;
@@ -138,7 +136,6 @@ class TestRaycast : public Test {
ProxyShape* mSphereProxyShape;
ProxyShape* mCapsuleProxyShape;
ProxyShape* mConvexMeshProxyShape;
- ProxyShape* mConvexMeshProxyShapeEdgesInfo;
ProxyShape* mCompoundSphereProxyShape;
ProxyShape* mCompoundCapsuleProxyShape;
ProxyShape* mTriangleProxyShape;
@@ -152,6 +149,11 @@ class TestRaycast : public Test {
std::vector<uint> mConcaveMeshIndices;
TriangleVertexArray* mConcaveMeshVertexArray;
float mHeightFieldData[100];
+ PolygonVertexArray::PolygonFace mPolygonFaces[6];
+ PolygonVertexArray* mPolygonVertexArray;
+ PolyhedronMesh* mPolyhedronMesh;
+ Vector3 mPolyhedronVertices[8];
+ int mPolyhedronIndices[4 * 6];
public :
@@ -175,7 +177,6 @@ class TestRaycast : public Test {
mSphereBody = mWorld->createCollisionBody(mBodyTransform);
mCapsuleBody = mWorld->createCollisionBody(mBodyTransform);
mConvexMeshBody = mWorld->createCollisionBody(mBodyTransform);
- mConvexMeshBodyEdgesInfo = mWorld->createCollisionBody(mBodyTransform);
mCylinderBody = mWorld->createCollisionBody(mBodyTransform);
mCompoundBody = mWorld->createCollisionBody(mBodyTransform);
mTriangleBody = mWorld->createCollisionBody(mBodyTransform);
@@ -200,51 +201,47 @@ class TestRaycast : public Test {
const Vector3 triangleVertex1(100, 100, 0);
const Vector3 triangleVertex2(105, 100, 0);
const Vector3 triangleVertex3(100, 103, 0);
- mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3);
+ Vector3 triangleVerticesNormals[3] = {Vector3(0, 0, 1), Vector3(0, 0, 1), Vector3(0, 0, 1)};
+ mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3, triangleVerticesNormals, 0, 0);
mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform);
mCapsuleShape = new CapsuleShape(2, 5);
mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform);
// TODO : Create convex mesh shape with new way (polyhedron mesh) to add test again
- // Box of dimension (2, 3, 4)
- /*mConvexMeshShape = new ConvexMeshShape(0.0);
- mConvexMeshShape->addVertex(Vector3(-2, -3, -4));
- mConvexMeshShape->addVertex(Vector3(2, -3, -4));
- mConvexMeshShape->addVertex(Vector3(2, -3, 4));
- mConvexMeshShape->addVertex(Vector3(-2, -3, 4));
- mConvexMeshShape->addVertex(Vector3(-2, 3, -4));
- mConvexMeshShape->addVertex(Vector3(2, 3, -4));
- mConvexMeshShape->addVertex(Vector3(2, 3, 4));
- mConvexMeshShape->addVertex(Vector3(-2, 3, 4));
- mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform);
+ // Box of extents (2, 3, 4)
+ mPolyhedronVertices[0] = Vector3(-2, -3, -4);
+ mPolyhedronVertices[1] = Vector3(2, -3, -4);
+ mPolyhedronVertices[2] = Vector3(2, -3, 4);
+ mPolyhedronVertices[3] = Vector3(-2, -3, 4);
+ mPolyhedronVertices[4] = Vector3(-2, 3, -4);
+ mPolyhedronVertices[5] = Vector3(2, 3, -4);
+ mPolyhedronVertices[6] = Vector3(2, 3, 4);
+ mPolyhedronVertices[7] = Vector3(-2, 3, 4);
- mConvexMeshShapeEdgesInfo = new ConvexMeshShape(0.0);
- mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, -4));
- mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, -4));
- mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, -3, 4));
- mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, -3, 4));
- mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, -4));
- mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, -4));
- mConvexMeshShapeEdgesInfo->addVertex(Vector3(2, 3, 4));
- mConvexMeshShapeEdgesInfo->addVertex(Vector3(-2, 3, 4));
- mConvexMeshShapeEdgesInfo->addEdge(0, 1);
- mConvexMeshShapeEdgesInfo->addEdge(1, 2);
- mConvexMeshShapeEdgesInfo->addEdge(2, 3);
- mConvexMeshShapeEdgesInfo->addEdge(0, 3);
- mConvexMeshShapeEdgesInfo->addEdge(4, 5);
- mConvexMeshShapeEdgesInfo->addEdge(5, 6);
- mConvexMeshShapeEdgesInfo->addEdge(6, 7);
- mConvexMeshShapeEdgesInfo->addEdge(4, 7);
- mConvexMeshShapeEdgesInfo->addEdge(0, 4);
- mConvexMeshShapeEdgesInfo->addEdge(1, 5);
- mConvexMeshShapeEdgesInfo->addEdge(2, 6);
- mConvexMeshShapeEdgesInfo->addEdge(3, 7);
- mConvexMeshShapeEdgesInfo->setIsEdgesInformationUsed(true);
- mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape(
- mConvexMeshShapeEdgesInfo,
- mShapeTransform);
- */
+ mPolyhedronIndices[0] = 0; mPolyhedronIndices[1] = 1; mPolyhedronIndices[2] = 2; mPolyhedronIndices[3] = 3;
+ mPolyhedronIndices[4] = 1; mPolyhedronIndices[5] = 5; mPolyhedronIndices[6] = 6; mPolyhedronIndices[7] = 2;
+ mPolyhedronIndices[8] = 0; mPolyhedronIndices[9] = 4; mPolyhedronIndices[10] = 5; mPolyhedronIndices[11] = 1;
+ mPolyhedronIndices[12] = 0; mPolyhedronIndices[13] = 3; mPolyhedronIndices[14] = 7; mPolyhedronIndices[15] = 4;
+ mPolyhedronIndices[16] = 3; mPolyhedronIndices[17] = 2; mPolyhedronIndices[18] = 6; mPolyhedronIndices[19] = 7;
+ mPolyhedronIndices[20] = 2; mPolyhedronIndices[21] = 5; mPolyhedronIndices[22] = 4; mPolyhedronIndices[23] = 7;
+
+ // Polygon faces descriptions for the polyhedron
+ for (int f=0; f < 8; f++) {
+ PolygonVertexArray::PolygonFace& face = mPolygonFaces[f];
+ face.indexBase = f * 4;
+ face.nbVertices = 4;
+ }
+
+ // Create the polygon vertex array
+ mPolygonVertexArray = new PolygonVertexArray(8, mPolyhedronVertices, sizeof(Vector3),
+ mPolyhedronIndices, sizeof(int), 6, mPolygonFaces,
+ PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
+ PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
+
+ mPolyhedronMesh = new PolyhedronMesh(mPolygonVertexArray);
+ mConvexMeshShape = new ConvexMeshShape(mPolyhedronMesh);
+ mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform);
// Compound shape is a cylinder and a sphere
Vector3 positionShape2(Vector3(4, 2, -3));
@@ -300,8 +297,7 @@ class TestRaycast : public Test {
mBoxProxyShape->setCollisionCategoryBits(CATEGORY1);
mSphereProxyShape->setCollisionCategoryBits(CATEGORY1);
mCapsuleProxyShape->setCollisionCategoryBits(CATEGORY1);
- //mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2);
- //mConvexMeshProxyShapeEdgesInfo->setCollisionCategoryBits(CATEGORY2);
+ mConvexMeshProxyShape->setCollisionCategoryBits(CATEGORY2);
mCompoundSphereProxyShape->setCollisionCategoryBits(CATEGORY2);
mCompoundCapsuleProxyShape->setCollisionCategoryBits(CATEGORY2);
mTriangleProxyShape->setCollisionCategoryBits(CATEGORY1);
@@ -314,13 +310,15 @@ class TestRaycast : public Test {
delete mBoxShape;
delete mSphereShape;
delete mCapsuleShape;
- //delete mConvexMeshShape;
- //delete mConvexMeshShapeEdgesInfo;
+ delete mConvexMeshShape;
delete mTriangleShape;
delete mConcaveMeshShape;
delete mHeightFieldShape;
delete mConcaveMeshVertexArray;
+
+ delete mPolygonVertexArray;
+ delete mPolyhedronMesh;
}
/// Run the tests
@@ -1299,7 +1297,6 @@ class TestRaycast : public Test {
/// CollisionWorld::raycast() methods.
void testConvexMesh() {
- /*
// ----- Test feedback data ----- //
Vector3 point1 = mLocalShapeToWorld * Vector3(1 , 2, 6);
Vector3 point2 = mLocalShapeToWorld * Vector3(1, 2, -4);
@@ -1339,16 +1336,6 @@ class TestRaycast : public Test {
test(approxEqual(raycastInfo2.worldPoint.y, hitPoint.y, epsilon));
test(approxEqual(raycastInfo2.worldPoint.z, hitPoint.z, epsilon));
- // ProxyCollisionShape::raycast()
- RaycastInfo raycastInfo3;
- test(mConvexMeshBodyEdgesInfo->raycast(ray, raycastInfo3));
- test(raycastInfo3.body == mConvexMeshBodyEdgesInfo);
- test(raycastInfo3.proxyShape == mConvexMeshProxyShapeEdgesInfo);
- test(approxEqual(raycastInfo3.hitFraction, decimal(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));
-
// ProxyCollisionShape::raycast()
RaycastInfo raycastInfo4;
test(mConvexMeshProxyShape->raycast(ray, raycastInfo4));
@@ -1359,16 +1346,6 @@ class TestRaycast : public Test {
test(approxEqual(raycastInfo4.worldPoint.y, hitPoint.y, epsilon));
test(approxEqual(raycastInfo4.worldPoint.z, hitPoint.z, epsilon));
- // ProxyCollisionShape::raycast()
- RaycastInfo raycastInfo5;
- test(mConvexMeshProxyShapeEdgesInfo->raycast(ray, raycastInfo5));
- test(raycastInfo5.body == mConvexMeshBodyEdgesInfo);
- test(raycastInfo5.proxyShape == mConvexMeshProxyShapeEdgesInfo);
- test(approxEqual(raycastInfo5.hitFraction, decimal(0.2), epsilon));
- test(approxEqual(raycastInfo5.worldPoint.x, hitPoint.x, epsilon));
- test(approxEqual(raycastInfo5.worldPoint.y, hitPoint.y, epsilon));
- test(approxEqual(raycastInfo5.worldPoint.z, hitPoint.z, epsilon));
-
Ray ray1(mLocalShapeToWorld * Vector3(0, 0, 0), mLocalShapeToWorld * Vector3(5, 7, -1));
Ray ray2(mLocalShapeToWorld * Vector3(5, 11, 7), mLocalShapeToWorld * Vector3(17, 29, 28));
Ray ray3(mLocalShapeToWorld * Vector3(1, 2, 3), mLocalShapeToWorld * Vector3(-11, 2, 24));
@@ -1387,10 +1364,9 @@ class TestRaycast : public Test {
Ray ray16(mLocalShapeToWorld * Vector3(-1, 2, -7), mLocalShapeToWorld * Vector3(-1, 2, 30));
// ----- Test raycast miss ----- //
+ RaycastInfo raycastInfo3;
test(!mConvexMeshBody->raycast(ray1, raycastInfo3));
- test(!mConvexMeshBodyEdgesInfo->raycast(ray1, raycastInfo3));
test(!mConvexMeshProxyShape->raycast(ray1, raycastInfo3));
- test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray1, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray1, &mCallback);
test(!mCallback.isHit);
@@ -1402,73 +1378,55 @@ class TestRaycast : public Test {
test(!mCallback.isHit);
test(!mConvexMeshBody->raycast(ray2, raycastInfo3));
- test(!mConvexMeshBodyEdgesInfo->raycast(ray2, raycastInfo3));
test(!mConvexMeshProxyShape->raycast(ray2, raycastInfo3));
- test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray2, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray2, &mCallback);
test(!mCallback.isHit);
test(!mConvexMeshBody->raycast(ray3, raycastInfo3));
- test(!mConvexMeshBodyEdgesInfo->raycast(ray3, raycastInfo3));
test(!mConvexMeshProxyShape->raycast(ray3, raycastInfo3));
- test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray3, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray3, &mCallback);
test(!mCallback.isHit);
test(!mConvexMeshBody->raycast(ray4, raycastInfo3));
- test(!mConvexMeshBodyEdgesInfo->raycast(ray4, raycastInfo3));
test(!mConvexMeshProxyShape->raycast(ray4, raycastInfo3));
- test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray4, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray4, &mCallback);
test(!mCallback.isHit);
test(!mConvexMeshBody->raycast(ray5, raycastInfo3));
- test(!mConvexMeshBodyEdgesInfo->raycast(ray5, raycastInfo3));
test(!mConvexMeshProxyShape->raycast(ray5, raycastInfo3));
- test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray5, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray5, &mCallback);
test(!mCallback.isHit);
test(!mConvexMeshBody->raycast(ray6, raycastInfo3));
- test(!mConvexMeshBodyEdgesInfo->raycast(ray6, raycastInfo3));
test(!mConvexMeshProxyShape->raycast(ray6, raycastInfo3));
- test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray6, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray6, &mCallback);
test(!mCallback.isHit);
test(!mConvexMeshBody->raycast(ray7, raycastInfo3));
- test(!mConvexMeshBodyEdgesInfo->raycast(ray7, raycastInfo3));
test(!mConvexMeshProxyShape->raycast(ray7, raycastInfo3));
- test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray7, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray7, &mCallback);
test(!mCallback.isHit);
test(!mConvexMeshBody->raycast(ray8, raycastInfo3));
- test(!mConvexMeshBodyEdgesInfo->raycast(ray8, raycastInfo3));
test(!mConvexMeshProxyShape->raycast(ray8, raycastInfo3));
- test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray8, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray8, &mCallback);
test(!mCallback.isHit);
test(!mConvexMeshBody->raycast(ray9, raycastInfo3));
- test(!mConvexMeshBodyEdgesInfo->raycast(ray9, raycastInfo3));
test(!mConvexMeshProxyShape->raycast(ray9, raycastInfo3));
- test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray9, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray9, &mCallback);
test(!mCallback.isHit);
test(!mConvexMeshBody->raycast(ray10, raycastInfo3));
- test(!mConvexMeshBodyEdgesInfo->raycast(ray10, raycastInfo3));
test(!mConvexMeshProxyShape->raycast(ray10, raycastInfo3));
- test(!mConvexMeshProxyShapeEdgesInfo->raycast(ray10, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray10, &mCallback);
test(!mCallback.isHit);
@@ -1494,10 +1452,8 @@ class TestRaycast : public Test {
// ----- Test raycast hits ----- //
test(mConvexMeshBody->raycast(ray11, raycastInfo3));
- test(mConvexMeshBodyEdgesInfo->raycast(ray11, raycastInfo3));
test(mConvexMeshProxyShape->raycast(ray11, raycastInfo3));
- test(mConvexMeshProxyShapeEdgesInfo->raycast(ray11, raycastInfo3));
- mCallback.reset();
+ mCallback.reset();
mWorld->raycast(ray11, &mCallback);
test(mCallback.isHit);
mCallback.reset();
@@ -1505,9 +1461,7 @@ class TestRaycast : public Test {
test(mCallback.isHit);
test(mConvexMeshBody->raycast(ray12, raycastInfo3));
- test(mConvexMeshBodyEdgesInfo->raycast(ray12, raycastInfo3));
test(mConvexMeshProxyShape->raycast(ray12, raycastInfo3));
- test(mConvexMeshProxyShapeEdgesInfo->raycast(ray12, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray12, &mCallback);
test(mCallback.isHit);
@@ -1516,9 +1470,7 @@ class TestRaycast : public Test {
test(mCallback.isHit);
test(mConvexMeshBody->raycast(ray13, raycastInfo3));
- test(mConvexMeshBodyEdgesInfo->raycast(ray13, raycastInfo3));
test(mConvexMeshProxyShape->raycast(ray13, raycastInfo3));
- test(mConvexMeshProxyShapeEdgesInfo->raycast(ray13, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray13, &mCallback);
test(mCallback.isHit);
@@ -1527,9 +1479,7 @@ class TestRaycast : public Test {
test(mCallback.isHit);
test(mConvexMeshBody->raycast(ray14, raycastInfo3));
- test(mConvexMeshBodyEdgesInfo->raycast(ray14, raycastInfo3));
test(mConvexMeshProxyShape->raycast(ray14, raycastInfo3));
- test(mConvexMeshProxyShapeEdgesInfo->raycast(ray14, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray14, &mCallback);
test(mCallback.isHit);
@@ -1538,9 +1488,7 @@ class TestRaycast : public Test {
test(mCallback.isHit);
test(mConvexMeshBody->raycast(ray15, raycastInfo3));
- test(mConvexMeshBodyEdgesInfo->raycast(ray15, raycastInfo3));
test(mConvexMeshProxyShape->raycast(ray15, raycastInfo3));
- test(mConvexMeshProxyShapeEdgesInfo->raycast(ray15, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray15, &mCallback);
test(mCallback.isHit);
@@ -1549,16 +1497,13 @@ class TestRaycast : public Test {
test(mCallback.isHit);
test(mConvexMeshBody->raycast(ray16, raycastInfo3));
- test(mConvexMeshBodyEdgesInfo->raycast(ray16, raycastInfo3));
test(mConvexMeshProxyShape->raycast(ray16, raycastInfo3));
- test(mConvexMeshProxyShapeEdgesInfo->raycast(ray16, raycastInfo3));
mCallback.reset();
mWorld->raycast(ray16, &mCallback);
test(mCallback.isHit);
mCallback.reset();
mWorld->raycast(Ray(ray16.point1, ray16.point2, decimal(0.8)), &mCallback);
test(mCallback.isHit);
- */
}
/// Test the CollisionBody::raycast() and
diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp
index 11be5732..b3cb3b95 100644
--- a/testbed/common/ConvexMesh.cpp
+++ b/testbed/common/ConvexMesh.cpp
@@ -40,13 +40,6 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
- // Vertex and Indices array for the triangle mesh (data in shared and not copied)
- /*mPolygonVertexArray =
- new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3),
- getNbFaces(0), &(mIndices[0][0]), sizeof(int),
- rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
- rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);*/
-
// Polygon faces descriptions for the polyhedron
mPolygonFaces = new rp3d::PolygonVertexArray::PolygonFace[getNbFaces(0)];
rp3d::PolygonVertexArray::PolygonFace* face = mPolygonFaces;
From a655ffb4623a84f386be4427f60215fe3a7758c0 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 31 Aug 2017 22:42:19 +0200
Subject: [PATCH 066/133] Fix issue in ContactManifoldSet.cpp
---
src/collision/ContactManifoldSet.cpp | 16 ++++++++++++----
1 file changed, 12 insertions(+), 4 deletions(-)
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index 23602dda..fb3beda2 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -60,6 +60,8 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa
}
else {
+ bool addNewManifold = true;
+
// If there are too much contact manifolds in the set
if (mNbManifolds >= mNbMaxManifolds) {
@@ -72,14 +74,20 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa
// Remove the manifold
removeManifold(minDepthManifold);
+ }
+ else {
- // Create a new contact manifold
- createManifold(contactManifoldInfo);
+ // The manifold we do not want to get is the new one. Therefore, we do not add it to the set
+ addNewManifold = false;
}
}
- // Create a new contact manifold
- createManifold(contactManifoldInfo);
+ // If we need to add the new contact manifold
+ if (addNewManifold) {
+
+ // Create a new contact manifold
+ createManifold(contactManifoldInfo);
+ }
}
}
From 9b89f66667806fbca2f3390cf909b9ea8d4c5b68 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 31 Aug 2017 23:11:00 +0200
Subject: [PATCH 067/133] Fix issues and refactor collision shape type and
collision shape name
---
src/collision/CollisionDetection.cpp | 70 ++++++++-----------
src/collision/MiddlePhaseTriangleCallback.cpp | 5 +-
src/collision/NarrowPhaseInfo.cpp | 18 +++--
src/collision/NarrowPhaseInfo.h | 13 ++--
src/collision/ProxyShape.h | 15 ++++
.../narrowphase/DefaultCollisionDispatch.cpp | 1 -
.../narrowphase/SAT/SATAlgorithm.cpp | 6 +-
src/collision/shapes/BoxShape.cpp | 2 +-
src/collision/shapes/CapsuleShape.cpp | 2 +-
src/collision/shapes/CollisionShape.cpp | 2 +-
src/collision/shapes/CollisionShape.h | 37 ++++++----
src/collision/shapes/ConcaveMeshShape.cpp | 6 +-
src/collision/shapes/ConcaveMeshShape.h | 3 -
src/collision/shapes/ConcaveShape.cpp | 5 +-
src/collision/shapes/ConcaveShape.h | 24 +------
src/collision/shapes/ConvexMeshShape.cpp | 2 +-
.../shapes/ConvexPolyhedronShape.cpp | 4 +-
src/collision/shapes/ConvexPolyhedronShape.h | 2 +-
src/collision/shapes/ConvexShape.cpp | 4 +-
src/collision/shapes/ConvexShape.h | 2 +-
src/collision/shapes/HeightFieldShape.cpp | 2 +-
src/collision/shapes/SphereShape.cpp | 2 +-
src/collision/shapes/TriangleShape.cpp | 18 ++---
src/collision/shapes/TriangleShape.h | 2 +-
src/engine/OverlappingPair.cpp | 7 +-
src/engine/OverlappingPair.h | 2 +-
testbed/common/ConcaveMesh.cpp | 2 -
.../CollisionDetectionScene.cpp | 22 +++---
.../CollisionDetectionScene.h | 2 +-
29 files changed, 138 insertions(+), 144 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 0c21240a..3c4b771b 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -148,11 +148,11 @@ void CollisionDetection::computeMiddlePhase() {
bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
- const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
- const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+ bool isShape1Convex = shape1->getCollisionShape()->isConvex();
+ bool isShape2Convex = shape2->getCollisionShape()->isConvex();
// If both shapes are convex
- if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) {
+ if (isShape1Convex && isShape2Convex) {
// No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection
@@ -161,13 +161,12 @@ void CollisionDetection::computeMiddlePhase() {
NarrowPhaseInfo(pair, shape1->getCollisionShape(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
- shape2->getCachedCollisionData());
+ shape2->getCachedCollisionData(), mSingleFrameAllocator);
mNarrowPhaseInfoList->next = firstNarrowPhaseInfo;
}
// Concave vs Convex algorithm
- else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
- (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
+ else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) {
NarrowPhaseInfo* narrowPhaseInfo = nullptr;
computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo);
@@ -410,22 +409,14 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) {
// Reduce the number of contact points of the manifold
pair->reducePotentialContactManifolds();
- // If there is a concave mesh shape in the pair
- if (pair->hasConcaveShape()) {
+ // Add all the potential contact manifolds as actual contact manifolds to the pair
+ ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds();
+ while (potentialManifold != nullptr) {
- processSmoothMeshContacts(pair);
- }
- else { // If both collision shapes are convex
+ pair->addContactManifold(potentialManifold);
- // Add all the potential contact manifolds as actual contact manifolds to the pair
- ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds();
- while (potentialManifold != nullptr) {
-
- pair->addContactManifold(potentialManifold);
-
- potentialManifold = potentialManifold->mNext;
- }
- }
+ potentialManifold = potentialManifold->mNext;
+ }
// Clear the obselete contact manifolds and contact points
pair->clearObseleteManifoldsAndContactPoints();
@@ -544,25 +535,24 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
// -------------------------------------------------------
- const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
- const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+ const bool isShape1Convex = shape1->getCollisionShape()->isConvex();
+ const bool isShape2Convex = shape2->getCollisionShape()->isConvex();
NarrowPhaseInfo* narrowPhaseInfo = nullptr;
// If both shapes are convex
- if ((CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type))) {
+ if ((isShape1Convex && isShape2Convex)) {
// No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection
narrowPhaseInfo = new (mPoolAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
- shape2->getCachedCollisionData());
+ shape2->getCachedCollisionData(), mPoolAllocator);
}
// Concave vs Convex algorithm
- else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
- (!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
+ else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) {
// Run the middle-phase collision detection algorithm to find the triangles of the concave
// shape we need to use during the narrow-phase collision detection
@@ -630,9 +620,6 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
// Test if the AABBs of the two proxy shapes overlap
if (aabb1.testCollision(aabb2)) {
- const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType();
- const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType();
-
// Create a temporary overlapping pair
OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator);
@@ -647,6 +634,9 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
// If we have not found a collision yet
if (!isColliding) {
+ const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
+ const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
+
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
@@ -723,9 +713,6 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
// Check if the collision filtering allows collision between the two shapes
if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
- const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType();
- const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
-
// Create a temporary overlapping pair
OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
@@ -740,6 +727,9 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
// If we have not found a collision yet
if (!isColliding) {
+ const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
+ const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
+
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
@@ -812,12 +802,12 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
- const CollisionShapeType shape1Type = body1ProxyShape->getCollisionShape()->getType();
- const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType();
-
// For each narrow-phase info object
while (narrowPhaseInfo != nullptr) {
+ const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
+ const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
+
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
@@ -896,9 +886,6 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// Check if the collision filtering allows collision between the two shapes
if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
- const CollisionShapeType shape1Type = bodyProxyShape->getCollisionShape()->getType();
- const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
-
// Create a temporary overlapping pair
OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
@@ -908,6 +895,9 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// For each narrow-phase info object
while (narrowPhaseInfo != nullptr) {
+ const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
+ const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
+
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
@@ -988,8 +978,8 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// For each narrow-phase info object
while (narrowPhaseInfo != nullptr) {
- const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
- const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+ const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
+ const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp
index e99da461..a1a183ac 100644
--- a/src/collision/MiddlePhaseTriangleCallback.cpp
+++ b/src/collision/MiddlePhaseTriangleCallback.cpp
@@ -32,7 +32,8 @@ using namespace reactphysics3d;
void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
const Vector3* verticesNormals) {
- // Create a triangle collision shape
+ // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the
+ // destructor of the corresponding NarrowPhaseInfo.
decimal margin = mConcaveShape->getTriangleMargin();
TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
@@ -44,6 +45,6 @@ void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIn
NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(),
triangleShape, mConvexProxyShape->getLocalToWorldTransform(),
mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(),
- mConcaveProxyShape->getCachedCollisionData());
+ mConcaveProxyShape->getCachedCollisionData(), mAllocator);
narrowPhaseInfoList->next = firstNarrowPhaseInfo;
}
diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp
index 22ee6666..b4a92a38 100644
--- a/src/collision/NarrowPhaseInfo.cpp
+++ b/src/collision/NarrowPhaseInfo.cpp
@@ -27,18 +27,19 @@
#include <iostream>
#include "NarrowPhaseInfo.h"
#include "ContactPointInfo.h"
+#include "collision/shapes/TriangleShape.h"
#include "engine/OverlappingPair.h"
using namespace reactphysics3d;
// Constructor
-NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1,
- const CollisionShape* shape2, const Transform& shape1Transform,
- const Transform& shape2Transform, void** cachedData1, void** cachedData2)
+NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
+ CollisionShape* shape2, const Transform& shape1Transform,
+ const Transform& shape2Transform, void** cachedData1, void** cachedData2, Allocator& shapeAllocator)
: overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
contactPoints(nullptr), cachedCollisionData1(cachedData1),
- cachedCollisionData2(cachedData2), next(nullptr) {
+ cachedCollisionData2(cachedData2), collisionShapeAllocator(shapeAllocator), next(nullptr) {
}
@@ -46,6 +47,15 @@ NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* sh
NarrowPhaseInfo::~NarrowPhaseInfo() {
assert(contactPoints == nullptr);
+
+ // Release the memory of the TriangleShape (this memory was allocated in the
+ // MiddlePhaseTriangleCallback::testTriangle() method)
+ if (collisionShape1->getName() == CollisionShapeName::TRIANGLE) {
+ collisionShapeAllocator.release(collisionShape1, sizeof(TriangleShape));
+ }
+ if (collisionShape2->getName() == CollisionShapeName::TRIANGLE) {
+ collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape));
+ }
}
// Add a new contact point
diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h
index 29bc6c9d..c9b18094 100644
--- a/src/collision/NarrowPhaseInfo.h
+++ b/src/collision/NarrowPhaseInfo.h
@@ -48,10 +48,10 @@ struct NarrowPhaseInfo {
OverlappingPair* overlappingPair;
/// Pointer to the first collision shape to test collision with
- const CollisionShape* collisionShape1;
+ CollisionShape* collisionShape1;
/// Pointer to the second collision shape to test collision with
- const CollisionShape* collisionShape2;
+ CollisionShape* collisionShape2;
/// Transform that maps from collision shape 1 local-space to world-space
Transform shape1ToWorldTransform;
@@ -70,13 +70,16 @@ struct NarrowPhaseInfo {
// TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
void** cachedCollisionData2;
+ /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor)
+ Allocator& collisionShapeAllocator;
+
/// Pointer to the next element in the linked list
NarrowPhaseInfo* next;
/// Constructor
- NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1,
- const CollisionShape* shape2, const Transform& shape1Transform,
- const Transform& shape2Transform, void** cachedData1, void** cachedData2);
+ NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
+ CollisionShape* shape2, const Transform& shape1Transform,
+ const Transform& shape2Transform, void** cachedData1, void** cachedData2, Allocator& shapeAllocator);
/// Destructor
~NarrowPhaseInfo();
diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h
index d9575b3d..36bec740 100644
--- a/src/collision/ProxyShape.h
+++ b/src/collision/ProxyShape.h
@@ -85,6 +85,11 @@ class ProxyShape {
/// proxy shape will collide with every collision categories by default.
unsigned short mCollideWithMaskBits;
+ // -------------------- Methods -------------------- //
+
+ /// Return the collision shape
+ CollisionShape* getCollisionShape();
+
public:
// -------------------- Methods -------------------- //
@@ -177,6 +182,8 @@ class ProxyShape {
friend class DynamicsWorld;
friend class GJKAlgorithm;
friend class ConvexMeshShape;
+ friend class ContactManifoldSet;
+ friend class MiddlePhaseTriangleCallback;
};
@@ -193,6 +200,14 @@ inline const CollisionShape* ProxyShape::getCollisionShape() const {
return mCollisionShape;
}
+// Return the collision shape
+/**
+* @return Pointer to the internal collision shape
+*/
+inline CollisionShape* ProxyShape::getCollisionShape() {
+ return mCollisionShape;
+}
+
// Return the parent body
/**
* @return Pointer to the parent body
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
index df1828ad..05616b27 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp
@@ -36,7 +36,6 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
- // Convex vs Convex algorithm (GJK algorithm)
if (type1 > type2) {
return nullptr;
}
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 23ce86e0..c5210c64 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -82,7 +82,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
// If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
// frame collision data per triangle)
- if (polyhedron->getType() != CollisionShapeType::TRIANGLE) {
+ if (polyhedron->getName() != CollisionShapeName::TRIANGLE) {
// If the last frame collision info is valid and was also using SAT algorithm
if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
@@ -225,7 +225,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
// If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
// frame collision data per triangle)
- if (polyhedron->getType() != CollisionShapeType::TRIANGLE) {
+ if (polyhedron->getName() != CollisionShapeName::TRIANGLE) {
// If the last frame collision info is valid and was also using SAT algorithm
if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
@@ -609,7 +609,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
// frame collision data per triangle)
- if (polyhedron1->getType() != CollisionShapeType::TRIANGLE && polyhedron2->getType() != CollisionShapeType::TRIANGLE) {
+ if (polyhedron1->getName() != CollisionShapeName::TRIANGLE && polyhedron2->getName() != CollisionShapeName::TRIANGLE) {
// If the last frame collision info is valid and was also using SAT algorithm
if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp
index af6a30b8..54666136 100644
--- a/src/collision/shapes/BoxShape.cpp
+++ b/src/collision/shapes/BoxShape.cpp
@@ -38,7 +38,7 @@ using namespace reactphysics3d;
* @param margin The collision margin (in meters) around the collision shape
*/
BoxShape::BoxShape(const Vector3& extent, decimal margin)
- : ConvexPolyhedronShape(margin), mExtent(extent - Vector3(margin, margin, margin)) {
+ : ConvexPolyhedronShape(CollisionShapeName::BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) {
assert(extent.x > decimal(0.0) && extent.x > margin);
assert(extent.y > decimal(0.0) && extent.y > margin);
assert(extent.z > decimal(0.0) && extent.z > margin);
diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp
index f9f442ce..32ea146c 100644
--- a/src/collision/shapes/CapsuleShape.cpp
+++ b/src/collision/shapes/CapsuleShape.cpp
@@ -37,7 +37,7 @@ using namespace reactphysics3d;
* @param height The height of the capsule (in meters)
*/
CapsuleShape::CapsuleShape(decimal radius, decimal height)
- : ConvexShape(CollisionShapeType::CAPSULE, radius), mHalfHeight(height * decimal(0.5)) {
+ : ConvexShape(CollisionShapeName::CAPSULE, CollisionShapeType::CAPSULE, radius), mHalfHeight(height * decimal(0.5)) {
assert(radius > decimal(0.0));
assert(height > decimal(0.0));
}
diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp
index 301c97d2..d03b4a1c 100644
--- a/src/collision/shapes/CollisionShape.cpp
+++ b/src/collision/shapes/CollisionShape.cpp
@@ -32,7 +32,7 @@
using namespace reactphysics3d;
// Constructor
-CollisionShape::CollisionShape(CollisionShapeType type) : mType(type), mScaling(1.0, 1.0, 1.0) {
+CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type) : mName(name), mType(type), mScaling(1.0, 1.0, 1.0) {
}
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index 78b82ae0..77f96dfb 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -39,9 +39,12 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
-/// Type of the collision shape
-enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_MESH, HEIGHTFIELD};
-const int NB_COLLISION_SHAPE_TYPES = 6;
+/// Type of collision shapes
+enum class CollisionShapeType {SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_SHAPE};
+const int NB_COLLISION_SHAPE_TYPES = 4;
+
+/// Names of collision shapes
+enum class CollisionShapeName { TRIANGLE, SPHERE, CAPSULE, BOX, CONVEX_MESH, TRIANGLE_MESH, HEIGHTFIELD };
// Declarations
class ProxyShape;
@@ -61,6 +64,9 @@ class CollisionShape {
/// Type of the collision shape
CollisionShapeType mType;
+ /// Name of the colision shape
+ CollisionShapeName mName;
+
/// Scaling vector of the collision shape
Vector3 mScaling;
@@ -80,7 +86,7 @@ class CollisionShape {
// -------------------- Methods -------------------- //
/// Constructor
- CollisionShape(CollisionShapeType type);
+ CollisionShape(CollisionShapeName name, CollisionShapeType type);
/// Destructor
virtual ~CollisionShape() = default;
@@ -91,7 +97,10 @@ class CollisionShape {
/// Deleted assignment operator
CollisionShape& operator=(const CollisionShape& shape) = delete;
- /// Return the type of the collision shapes
+ /// Return the name of the collision shape
+ CollisionShapeName getName() const;
+
+ /// Return the type of the collision shape
CollisionShapeType getType() const;
/// Return true if the collision shape is convex, false if it is concave
@@ -115,28 +124,28 @@ class CollisionShape {
/// Compute the world-space AABB of the collision shape given a transform
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
- /// Return true if the collision shape type is a convex shape
- static bool isConvex(CollisionShapeType shapeType);
-
// -------------------- Friendship -------------------- //
friend class ProxyShape;
friend class CollisionWorld;
};
+// Return the name of the collision shape
+/**
+* @return The name of the collision shape (box, sphere, triangle, ...)
+*/
+inline CollisionShapeName CollisionShape::getName() const {
+ return mName;
+}
+
// Return the type of the collision shape
/**
- * @return The type of the collision shape (box, sphere, cylinder, ...)
+ * @return The type of the collision shape (sphere, capsule, convex polyhedron, concave mesh)
*/
inline CollisionShapeType CollisionShape::getType() const {
return mType;
}
-// Return true if the collision shape type is a convex shape
-inline bool CollisionShape::isConvex(CollisionShapeType shapeType) {
- return shapeType != CollisionShapeType::CONCAVE_MESH && shapeType != CollisionShapeType::HEIGHTFIELD;
-}
-
// Return the scaling vector of the collision shape
inline Vector3 CollisionShape::getScaling() const {
return mScaling;
diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp
index 8cb5235a..0a9e1fd6 100644
--- a/src/collision/shapes/ConcaveMeshShape.cpp
+++ b/src/collision/shapes/ConcaveMeshShape.cpp
@@ -30,7 +30,7 @@ using namespace reactphysics3d;
// Constructor
ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh)
- : ConcaveShape(CollisionShapeType::CONCAVE_MESH) {
+ : ConcaveShape(CollisionShapeName::TRIANGLE_MESH) {
mTriangleMesh = triangleMesh;
mRaycastTestType = TriangleRaycastSide::FRONT;
@@ -79,8 +79,8 @@ void ConcaveMeshShape::getTriangleVertices(uint subPart, uint triangleIndex,
// Apply the scaling factor to the vertices
outTriangleVertices[0] *= mScaling.x;
- outTriangleVertices[1] *= mScaling.x;
- outTriangleVertices[2] *= mScaling.x;
+ outTriangleVertices[1] *= mScaling.y;
+ outTriangleVertices[2] *= mScaling.z;
}
// Return the three vertex normals (in the array outVerticesNormals) of a triangle
diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h
index 309ec27c..b99f2b4a 100644
--- a/src/collision/shapes/ConcaveMeshShape.h
+++ b/src/collision/shapes/ConcaveMeshShape.h
@@ -139,9 +139,6 @@ class ConcaveMeshShape : public ConcaveShape {
/// Return the three vertex normals (in the array outVerticesNormals) of a triangle
void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const;
- /// Get a smooth contact normal for collision for a triangle of the mesh
- Vector3 computeSmoothLocalContactNormalForTriangle(TriangleShape* triangleShape, const Vector3& localContactPoint) const;
-
public:
/// Constructor
diff --git a/src/collision/shapes/ConcaveShape.cpp b/src/collision/shapes/ConcaveShape.cpp
index 3f691468..e8c032d2 100644
--- a/src/collision/shapes/ConcaveShape.cpp
+++ b/src/collision/shapes/ConcaveShape.cpp
@@ -31,8 +31,7 @@
using namespace reactphysics3d;
// Constructor
-ConcaveShape::ConcaveShape(CollisionShapeType type)
- : CollisionShape(type), mIsSmoothMeshCollisionEnabled(false),
- mTriangleMargin(0), mRaycastTestType(TriangleRaycastSide::FRONT) {
+ConcaveShape::ConcaveShape(CollisionShapeName name)
+ : CollisionShape(name, CollisionShapeType::CONCAVE_SHAPE), mTriangleMargin(0), mRaycastTestType(TriangleRaycastSide::FRONT) {
}
diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h
index 6e07187f..34e347e0 100644
--- a/src/collision/shapes/ConcaveShape.h
+++ b/src/collision/shapes/ConcaveShape.h
@@ -63,9 +63,6 @@ class ConcaveShape : public CollisionShape {
// -------------------- Attributes -------------------- //
- /// True if the smooth mesh collision algorithm is enabled
- bool mIsSmoothMeshCollisionEnabled;
-
// Margin use for collision detection for each triangle
decimal mTriangleMargin;
@@ -82,7 +79,7 @@ class ConcaveShape : public CollisionShape {
// -------------------- Methods -------------------- //
/// Constructor
- ConcaveShape(CollisionShapeType type);
+ ConcaveShape(CollisionShapeName name);
/// Destructor
virtual ~ConcaveShape() override = default;
@@ -110,12 +107,6 @@ class ConcaveShape : public CollisionShape {
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0;
-
- /// Return true if the smooth mesh collision is enabled
- bool getIsSmoothMeshCollisionEnabled() const;
-
- /// Enable/disable the smooth mesh collision algorithm
- void setIsSmoothMeshCollisionEnabled(bool isEnabled);
};
// Return the triangle margin
@@ -138,19 +129,6 @@ inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape*
return false;
}
-// Return true if the smooth mesh collision is enabled
-inline bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const {
- return mIsSmoothMeshCollisionEnabled;
-}
-
-// Enable/disable the smooth mesh collision algorithm
-/// Smooth mesh collision is used to avoid collisions against some internal edges
-/// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother
-/// but collisions computation is a bit more expensive.
-inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) {
- mIsSmoothMeshCollisionEnabled = isEnabled;
-}
-
// Return the raycast test type (front, back, front-back)
inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const {
return mRaycastTestType;
diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp
index 263bb736..c8f03fa3 100644
--- a/src/collision/shapes/ConvexMeshShape.cpp
+++ b/src/collision/shapes/ConvexMeshShape.cpp
@@ -42,7 +42,7 @@ using namespace reactphysics3d;
* @param margin Collision margin (in meters) around the collision shape
*/
ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin)
- : ConvexPolyhedronShape(margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) {
+ : ConvexPolyhedronShape(CollisionShapeName::CONVEX_MESH, margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) {
// Recalculate the bounds of the mesh
recalculateBounds();
diff --git a/src/collision/shapes/ConvexPolyhedronShape.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp
index e73a7cd7..0db54a85 100644
--- a/src/collision/shapes/ConvexPolyhedronShape.cpp
+++ b/src/collision/shapes/ConvexPolyhedronShape.cpp
@@ -31,7 +31,7 @@
using namespace reactphysics3d;
// Constructor
-ConvexPolyhedronShape::ConvexPolyhedronShape(decimal margin)
- : ConvexShape(CollisionShapeType::CONVEX_POLYHEDRON, margin) {
+ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name, decimal margin)
+ : ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON, margin) {
}
diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h
index 6c1de4d5..fe1f07a9 100644
--- a/src/collision/shapes/ConvexPolyhedronShape.h
+++ b/src/collision/shapes/ConvexPolyhedronShape.h
@@ -47,7 +47,7 @@ class ConvexPolyhedronShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Constructor
- ConvexPolyhedronShape(decimal margin);
+ ConvexPolyhedronShape(CollisionShapeName name, decimal margin);
/// Destructor
virtual ~ConvexPolyhedronShape() override = default;
diff --git a/src/collision/shapes/ConvexShape.cpp b/src/collision/shapes/ConvexShape.cpp
index f077cdcb..649e50f5 100644
--- a/src/collision/shapes/ConvexShape.cpp
+++ b/src/collision/shapes/ConvexShape.cpp
@@ -31,8 +31,8 @@
using namespace reactphysics3d;
// Constructor
-ConvexShape::ConvexShape(CollisionShapeType type, decimal margin)
- : CollisionShape(type), mMargin(margin) {
+ConvexShape::ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin)
+ : CollisionShape(name, type), mMargin(margin) {
}
diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h
index 94acc7f9..4bf4b718 100644
--- a/src/collision/shapes/ConvexShape.h
+++ b/src/collision/shapes/ConvexShape.h
@@ -63,7 +63,7 @@ class ConvexShape : public CollisionShape {
// -------------------- Methods -------------------- //
/// Constructor
- ConvexShape(CollisionShapeType type, decimal margin);
+ ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin);
/// Destructor
virtual ~ConvexShape() override = default;
diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp
index c8fc97f2..b13cf69b 100644
--- a/src/collision/shapes/HeightFieldShape.cpp
+++ b/src/collision/shapes/HeightFieldShape.cpp
@@ -42,7 +42,7 @@ using namespace reactphysics3d;
HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight,
const void* heightFieldData, HeightDataType dataType, int upAxis,
decimal integerHeightScale)
- : ConcaveShape(CollisionShapeType::HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows),
+ : ConcaveShape(CollisionShapeName::HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows),
mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight),
mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale),
mHeightDataType(dataType) {
diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp
index 73fe49aa..8583e80d 100644
--- a/src/collision/shapes/SphereShape.cpp
+++ b/src/collision/shapes/SphereShape.cpp
@@ -35,7 +35,7 @@ using namespace reactphysics3d;
/**
* @param radius Radius of the sphere (in meters)
*/
-SphereShape::SphereShape(decimal radius) : ConvexShape(CollisionShapeType::SPHERE, radius) {
+SphereShape::SphereShape(decimal radius) : ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) {
assert(radius > decimal(0.0));
}
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index 72eb93d3..c72dd133 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -45,7 +45,7 @@ using namespace reactphysics3d;
*/
TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin)
- : ConvexPolyhedronShape(margin), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) {
+ : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE, margin), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) {
mPoints[0] = point1;
mPoints[1] = point2;
@@ -68,15 +68,15 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const
// This method will return the new smooth world contact
// normal of the triangle and the the local contact point on the other shape.
void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2,
- Vector3& localContactPointShape1, Vector3 localContactPointShape2,
+ Vector3& localContactPointShape1, Vector3& localContactPointShape2,
const Transform& shape1ToWorld, const Transform& shape2ToWorld,
decimal penetrationDepth, Vector3& outSmoothVertexNormal) {
- assert(shape1->getType() != CollisionShapeType::TRIANGLE || shape2->getType() != CollisionShapeType::TRIANGLE);
+ assert(shape1->getName() != CollisionShapeName::TRIANGLE || shape2->getName() != CollisionShapeName::TRIANGLE);
// If one the shape is a triangle
- bool isShape1Triangle = shape1->getType() == CollisionShapeType::TRIANGLE;
- if (isShape1Triangle || shape2->getType() == CollisionShapeType::TRIANGLE) {
+ bool isShape1Triangle = shape1->getName() == CollisionShapeName::TRIANGLE;
+ if (isShape1Triangle || shape2->getName() == CollisionShapeName::TRIANGLE) {
const TriangleShape* triangleShape = isShape1Triangle ? static_cast<const TriangleShape*>(shape1):
static_cast<const TriangleShape*>(shape2);
@@ -140,14 +140,6 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3&
decimal u, v, w;
computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w);
- int nbZeros = 0;
- bool isUZero = approxEqual(u, decimal(0), decimal(0.0001));
- bool isVZero = approxEqual(v, decimal(0), decimal(0.0001));
- bool isWZero = approxEqual(w, decimal(0), decimal(0.0001));
- if (isUZero) nbZeros++;
- if (isVZero) nbZeros++;
- if (isWZero) nbZeros++;
-
// We compute the contact normal as the barycentric interpolation of the three vertices normals
return (u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]).getUnit();
}
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index 900b71db..609d5892 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -173,7 +173,7 @@ class TriangleShape : public ConvexPolyhedronShape {
/// This method compute the smooth mesh contact with a triangle in case one of the two collision shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh
static void computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2,
- Vector3& localContactPointShape1, Vector3 localContactPointShape2,
+ Vector3& localContactPointShape1, Vector3& localContactPointShape2,
const Transform& shape1ToWorld, const Transform& shape2ToWorld,
decimal penetrationDepth, Vector3& outSmoothVertexNormal);
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index a23857e3..f3f85fa1 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -36,7 +36,12 @@ OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
: mContactManifoldSet(shape1, shape2, manifoldsAllocator), mPotentialContactManifolds(nullptr),
mTempMemoryAllocator(temporaryMemoryAllocator) {
-}
+}
+
+// Destructor
+OverlappingPair::~OverlappingPair() {
+ assert(mPotentialContactManifolds == nullptr);
+}
// Create a new potential contact manifold using contact-points from narrow-phase
void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) {
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index 906cddb7..e8c89dda 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -117,7 +117,7 @@ class OverlappingPair {
Allocator& memoryAllocator, Allocator& temporaryMemoryAllocator);
/// Destructor
- ~OverlappingPair() = default;
+ ~OverlappingPair();
/// Deleted copy-constructor
OverlappingPair(const OverlappingPair& pair) = delete;
diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp
index 608e0020..d6a6bbe3 100644
--- a/testbed/common/ConcaveMesh.cpp
+++ b/testbed/common/ConcaveMesh.cpp
@@ -109,8 +109,6 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass,
// do not forget to delete it at the end
mConcaveShape = new rp3d::ConcaveMeshShape(&mPhysicsTriangleMesh);
- mConcaveShape->setIsSmoothMeshCollisionEnabled(false);
-
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index a99d494d..7606a61e 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -94,7 +94,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mCapsule2->setSleepingColor(mRedColorDemo);
// ---------- Box 1 ---------- //
- openglframework::Vector3 position5(0, -0, 0);
+ openglframework::Vector3 position5(-4, -7, 0);
// Create a cylinder and a corresponding collision body in the dynamics world
mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath);
@@ -127,14 +127,15 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mConvexMesh->setSleepingColor(mRedColorDemo);
// ---------- Concave Mesh ---------- //
- //openglframework::Vector3 position8(0, 0, 0);
+ 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 + "city.obj");
+ mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj");
+ mAllShapes.push_back(mConcaveMesh);
// Set the color
- //mConcaveMesh->setColor(mGreyColorDemo);
- //mConcaveMesh->setSleepingColor(mRedColorDemo);
+ mConcaveMesh->setColor(mGreyColorDemo);
+ mConcaveMesh->setSleepingColor(mRedColorDemo);
// ---------- Heightfield ---------- //
//openglframework::Vector3 position9(0, 0, 0);
@@ -183,6 +184,9 @@ CollisionDetectionScene::~CollisionDetectionScene() {
mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody());
delete mConvexMesh;
+ mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody());
+ delete mConcaveMesh;
+
/*
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody());
@@ -206,12 +210,6 @@ CollisionDetectionScene::~CollisionDetectionScene() {
// Destroy the dumbbell
delete mDumbbell;
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody());
-
- // Destroy the convex mesh
- delete mConcaveMesh;
-
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody());
@@ -256,6 +254,7 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mBox2->getCollisionBody()->isActive()) mBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
/*
if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
@@ -263,7 +262,6 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix);
if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix);
if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix);
- if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix);
if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix);
*/
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index d0f1b569..8e94eaf5 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -147,7 +147,7 @@ class CollisionDetectionScene : public SceneDemo {
Box* mBox2;
ConvexMesh* mConvexMesh;
//Dumbbell* mDumbbell;
- //ConcaveMesh* mConcaveMesh;
+ ConcaveMesh* mConcaveMesh;
//HeightField* mHeightField;
std::vector<PhysicsObject*> mAllShapes;
From 673e487f141948fece1a1dbd0a35d6d78b33bb04 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 1 Sep 2017 07:37:45 +0200
Subject: [PATCH 068/133] Remove temporal coherence from SAT for sphere vs
polyhedron and capsule vs polyhedron
---
.../narrowphase/SAT/SATAlgorithm.cpp | 258 ++++--------------
1 file changed, 53 insertions(+), 205 deletions(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index c5210c64..b551161d 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -74,69 +74,23 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint minFaceIndex = 0;
- // True if the shapes were overlapping in the previous frame and are
- // still overlapping on the same axis in this frame
- bool isTemporalCoherenceValid = false;
- LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
+ // For each face of the convex mesh
+ for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
- // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
- // frame collision data per triangle)
- if (polyhedron->getName() != CollisionShapeName::TRIANGLE) {
+ // Compute the penetration depth of the shapes along the face normal direction
+ decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(f, polyhedron, sphere, sphereCenter);
- // If the last frame collision info is valid and was also using SAT algorithm
- if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
+ // If the penetration depth is negative, we have found a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
- // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
- // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
- // the shapes are still separated along this axis, we directly exit with no collision.
-
- // Compute the penetration depth of the shapes along the face normal direction
- decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
- sphere, sphereCenter);
-
- // If the previous axis is a separating axis
- if (penetrationDepth <= decimal(0.0)) {
-
- // Return no collision
- return false;
- }
-
- // The two shapes are overlapping as in the previous frame and on the same axis, therefore
- // we will skip the entire SAT algorithm because the minimum separating axis did not change
- isTemporalCoherenceValid = lastFrameInfo.wasColliding;
-
- if (isTemporalCoherenceValid) {
-
- minPenetrationDepth = penetrationDepth;
- minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
- }
+ return false;
}
- }
- // We the shapes are still overlapping in the same axis as in
- // the previous frame, we skip the whole SAT algorithm
- if (!isTemporalCoherenceValid) {
-
- // For each face of the convex mesh
- for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
-
- // Compute the penetration depth of the shapes along the face normal direction
- decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(f, polyhedron, sphere, sphereCenter);
-
- // If the penetration depth is negative, we have found a separating axis
- if (penetrationDepth <= decimal(0.0)) {
-
- lastFrameInfo.satMinAxisFaceIndex = f;
-
- return false;
- }
-
- // Check if we have found a new minimum penetration axis
- if (penetrationDepth < minPenetrationDepth) {
- minPenetrationDepth = penetrationDepth;
- minFaceIndex = f;
- }
+ // Check if we have found a new minimum penetration axis
+ if (penetrationDepth < minPenetrationDepth) {
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = f;
}
}
@@ -161,8 +115,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
}
- lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
-
return true;
}
@@ -217,171 +169,72 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
Vector3 separatingPolyhedronEdgeVertex1;
Vector3 separatingPolyhedronEdgeVertex2;
- // True if the shapes were overlapping in the previous frame and are
- // still overlapping on the same axis in this frame
- bool isTemporalCoherenceValid = false;
+ // For each face of the convex mesh
+ for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
- LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
+ Vector3 outFaceNormalCapsuleSpace;
- // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
- // frame collision data per triangle)
- if (polyhedron->getName() != CollisionShapeName::TRIANGLE) {
+ // Compute the penetration depth
+ const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(f, polyhedron, capsuleShape,
+ polyhedronToCapsuleTransform,
+ outFaceNormalCapsuleSpace);
- // If the last frame collision info is valid and was also using SAT algorithm
- if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
+ // If the penetration depth is negative, we have found a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
- // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
- // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
- // the shapes are still separated along this axis, we directly exit with no collision.
+ return false;
+ }
- // If the previous minimum separation axis was a face normal of the polyhedron
- if (lastFrameInfo.satIsAxisFacePolyhedron1) {
-
- Vector3 outFaceNormalCapsuleSpace;
-
- // Compute the penetration depth along the polyhedron face normal direction
- const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(lastFrameInfo.satMinAxisFaceIndex, polyhedron,
- capsuleShape, polyhedronToCapsuleTransform,
- outFaceNormalCapsuleSpace);
-
- // If the previous axis is a separating axis
- if (penetrationDepth <= decimal(0.0)) {
-
- // Return no collision
- return false;
- }
-
- // The two shapes are overlapping as in the previous frame and on the same axis, therefore
- // we will skip the entire SAT algorithm because the minimum separating axis did not change
- isTemporalCoherenceValid = lastFrameInfo.wasColliding;
-
- if (isTemporalCoherenceValid) {
-
- minPenetrationDepth = penetrationDepth;
- minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
- isMinPenetrationFaceNormal = true;
- separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace;
- }
- }
- else { // If the previous minimum separation axis the cross product of the capsule inner segment and an edge of the polyhedron
-
- // Get an edge from the polyhedron (convert it into the capsule local-space)
- HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(lastFrameInfo.satMinEdge1Index);
- const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
- const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
- const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
-
- Vector3 outAxis;
-
- // Compute the penetration depth along this axis
- const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
- capsuleSegmentAxis, edgeVertex1,
- edgeDirectionCapsuleSpace,
- polyhedronToCapsuleTransform,
- outAxis);
-
- // If the previous axis is a separating axis
- if (penetrationDepth <= decimal(0.0)) {
-
- // Return no collision
- return false;
- }
-
- // The two shapes are overlapping as in the previous frame and on the same axis, therefore
- // we will skip the entire SAT algorithm because the minimum separating axis did not change
- isTemporalCoherenceValid = lastFrameInfo.wasColliding;
-
- if (isTemporalCoherenceValid) {
-
- minPenetrationDepth = penetrationDepth;
- minEdgeIndex = lastFrameInfo.satMinEdge1Index;
- isMinPenetrationFaceNormal = false;
- separatingAxisCapsuleSpace = outAxis;
- separatingPolyhedronEdgeVertex1 = edgeVertex1;
- separatingPolyhedronEdgeVertex2 = edgeVertex2;
- }
- }
+ // Check if we have found a new minimum penetration axis
+ if (penetrationDepth < minPenetrationDepth) {
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = f;
+ isMinPenetrationFaceNormal = true;
+ separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace;
}
}
- // 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) {
+ // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron
+ for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) {
- // For each face of the convex mesh
- for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
+ // Get an edge from the polyhedron (convert it into the capsule local-space)
+ HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e);
+ const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
+ const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
+ const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
- Vector3 outFaceNormalCapsuleSpace;
+ HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
+ const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex);
+ const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex);
+
+ // Check using the Gauss Map if this edge cross product can be as separating axis
+ if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) {
+
+ Vector3 outAxis;
// Compute the penetration depth
- const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(f, polyhedron, capsuleShape,
- polyhedronToCapsuleTransform,
- outFaceNormalCapsuleSpace);
+ const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
+ capsuleSegmentAxis, edgeVertex1,
+ edgeDirectionCapsuleSpace,
+ polyhedronToCapsuleTransform,
+ outAxis);
// If the penetration depth is negative, we have found a separating axis
if (penetrationDepth <= decimal(0.0)) {
- lastFrameInfo.satIsAxisFacePolyhedron1 = true;
- lastFrameInfo.satMinAxisFaceIndex = f;
-
return false;
}
// Check if we have found a new minimum penetration axis
if (penetrationDepth < minPenetrationDepth) {
minPenetrationDepth = penetrationDepth;
- minFaceIndex = f;
- isMinPenetrationFaceNormal = true;
- separatingAxisCapsuleSpace = outFaceNormalCapsuleSpace;
+ minEdgeIndex = e;
+ isMinPenetrationFaceNormal = false;
+ separatingAxisCapsuleSpace = outAxis;
+ separatingPolyhedronEdgeVertex1 = edgeVertex1;
+ separatingPolyhedronEdgeVertex2 = edgeVertex2;
}
}
-
- // For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron
- for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) {
-
- // Get an edge from the polyhedron (convert it into the capsule local-space)
- HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e);
- const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
- const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
- const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
-
- HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
- const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex);
- const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex);
-
- // Check using the Gauss Map if this edge cross product can be as separating axis
- if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) {
-
- Vector3 outAxis;
-
- // Compute the penetration depth
- const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
- capsuleSegmentAxis, edgeVertex1,
- edgeDirectionCapsuleSpace,
- polyhedronToCapsuleTransform,
- outAxis);
-
- // If the penetration depth is negative, we have found a separating axis
- if (penetrationDepth <= decimal(0.0)) {
-
- lastFrameInfo.satIsAxisFacePolyhedron1 = false;
- lastFrameInfo.satMinEdge1Index = e;
-
- return false;
- }
-
- // Check if we have found a new minimum penetration axis
- if (penetrationDepth < minPenetrationDepth) {
- minPenetrationDepth = penetrationDepth;
- minEdgeIndex = e;
- isMinPenetrationFaceNormal = false;
- separatingAxisCapsuleSpace = outAxis;
- separatingPolyhedronEdgeVertex1 = edgeVertex1;
- separatingPolyhedronEdgeVertex2 = edgeVertex2;
- }
- }
- }
-
}
// Convert the inner capsule segment points into the polyhedron local-space
@@ -404,8 +257,6 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
narrowPhaseInfo, isCapsuleShape1);
}
- lastFrameInfo.satIsAxisFacePolyhedron1 = true;
- lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
}
else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
@@ -433,9 +284,6 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
}
-
- lastFrameInfo.satIsAxisFacePolyhedron1 = false;
- lastFrameInfo.satMinEdge1Index = minEdgeIndex;
}
return true;
From 6a22b3a81d5a128f42fa08333cef9c041fef8697 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 3 Sep 2017 10:48:39 +0200
Subject: [PATCH 069/133] Fix temporal coherence in SAT algorithm between two
convex polyhedra
---
.../narrowphase/SAT/SATAlgorithm.cpp | 253 +++++++++++-------
src/collision/narrowphase/SAT/SATAlgorithm.h | 7 +
2 files changed, 164 insertions(+), 96 deletions(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index b551161d..87be645b 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -489,6 +489,24 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = true;
+
+ // Compute the contact points between two faces of two convex polyhedra.
+ // If contact points have been found, we report them without running the whole SAT algorithm
+ if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
+ polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
+ narrowPhaseInfo, minPenetrationDepth)) {
+
+ lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
+
+ return true;
+ }
+ else { // Contact points have not been found (the set of clipped points was empty)
+
+ // Therefore, we need to run the whole SAT algorithm again
+ isTemporalCoherenceValid = false;
+ }
}
}
else if (lastFrameInfo.satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth)
@@ -513,6 +531,24 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = false;
+
+ // Compute the contact points between two faces of two convex polyhedra.
+ // If contact points have been found, we report them without running the whole SAT algorithm
+ if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
+ polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
+ narrowPhaseInfo, minPenetrationDepth)) {
+
+ lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
+
+ return true;
+ }
+ else { // Contact points have not been found (the set of clipped points was empty)
+
+ // Therefore, we need to run the whole SAT algorithm again
+ isTemporalCoherenceValid = false;
+ }
}
}
else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
@@ -544,6 +580,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// we will skip the entire SAT algorithm because the minimum separating axis did not change
isTemporalCoherenceValid = lastFrameInfo.wasColliding;
+ // Temporal coherence is valid only if the two edges build a minkowski
+ // face (and the cross product is therefore a candidate for separating axis
+ if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
+ isTemporalCoherenceValid = false;
+ }
+
if (isTemporalCoherenceValid) {
minPenetrationDepth = penetrationDepth;
@@ -672,102 +714,11 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
if (reportContacts) {
- const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
- const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1;
- const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
- const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2;
-
- assert(minPenetrationDepth > decimal(0.0));
-
- const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex);
- const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
-
- // Compute the world normal
- Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
- -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
-
- // Get the reference face
- HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex);
-
- // Find the incident face on the other polyhedron (most anti-parallel face)
- uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace);
-
- // Get the incident face
- HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
-
- std::vector<Vector3> polygonVertices; // Vertices to clip of the incident face
- std::vector<Vector3> planesNormals; // Normals of the clipping planes
- std::vector<Vector3> planesPoints; // Points on the clipping planes
-
- // Get all the vertices of the incident face (in the reference local-space)
- std::vector<uint>::const_iterator it;
- for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) {
- const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it);
- polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace);
- }
-
- // Get the reference face clipping planes
- uint currentEdgeIndex = referenceFace.edgeIndex;
- uint firstEdgeIndex = currentEdgeIndex;
- do {
-
- // Get the adjacent edge
- HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
-
- // Get the twin edge
- HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
-
- // Compute the edge vertices and edge direction
- Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex);
- Vector3 edgeV2 = referencePolyhedron->getVertexPosition(twinEdge.vertexIndex);
- Vector3 edgeDirection = edgeV2 - edgeV1;
-
- // Compute the normal of the clipping plane for this edge
- // The clipping plane is perpendicular to the edge direction and the reference face normal
- Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection);
-
- planesNormals.push_back(clipPlaneNormal);
- planesPoints.push_back(edgeV1);
-
- // Go to the next adjacent edge of the reference face
- currentEdgeIndex = edge.nextEdgeIndex;
-
- } while (currentEdgeIndex != firstEdgeIndex);
-
- assert(planesNormals.size() > 0);
- assert(planesNormals.size() == planesPoints.size());
-
- // Clip the reference faces with the adjacent planes of the reference face
- std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
- assert(clipPolygonVertices.size() > 0);
-
- // We only keep the clipped points that are below the reference face
- const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
- std::vector<Vector3>::const_iterator itPoints;
- for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
-
- // If the clip point is bellow the reference face
- if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) {
-
- // Convert the clip incident polyhedron vertex into the incident polyhedron local-space
- Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
-
- // Project the contact point onto the reference face
- Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
-
- // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
- TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
- isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
- isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron,
- narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
- minPenetrationDepth, normalWorld);
-
- // Create a new contact point
- narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
- isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
- isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
- }
- }
+ // Compute the contact points between two faces of two convex polyhedra.
+ bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1,
+ polyhedron2, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1,
+ minFaceIndex, narrowPhaseInfo, minPenetrationDepth);
+ assert(contactsFound);
}
lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
@@ -809,6 +760,116 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
return true;
}
+// Compute the contact points between two faces of two convex polyhedra.
+/// The method returns true if contact points have been found
+bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1,
+ const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
+ const Transform& polyhedron1ToPolyhedron2, const Transform& polyhedron2ToPolyhedron1,
+ uint minFaceIndex, NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const {
+
+ const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
+ const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1;
+ const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
+ const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2;
+
+ assert(minPenetrationDepth > decimal(0.0));
+
+ const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex);
+ const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
+
+ // Compute the world normal
+ Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
+ -(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
+
+ // Get the reference face
+ HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex);
+
+ // Find the incident face on the other polyhedron (most anti-parallel face)
+ uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace);
+
+ // Get the incident face
+ HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
+
+ std::vector<Vector3> polygonVertices; // Vertices to clip of the incident face
+ std::vector<Vector3> planesNormals; // Normals of the clipping planes
+ std::vector<Vector3> planesPoints; // Points on the clipping planes
+
+ // Get all the vertices of the incident face (in the reference local-space)
+ std::vector<uint>::const_iterator it;
+ for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) {
+ const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it);
+ polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace);
+ }
+
+ // Get the reference face clipping planes
+ uint currentEdgeIndex = referenceFace.edgeIndex;
+ uint firstEdgeIndex = currentEdgeIndex;
+ do {
+
+ // Get the adjacent edge
+ HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
+
+ // Get the twin edge
+ HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
+
+ // Compute the edge vertices and edge direction
+ Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex);
+ Vector3 edgeV2 = referencePolyhedron->getVertexPosition(twinEdge.vertexIndex);
+ Vector3 edgeDirection = edgeV2 - edgeV1;
+
+ // Compute the normal of the clipping plane for this edge
+ // The clipping plane is perpendicular to the edge direction and the reference face normal
+ Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection);
+
+ planesNormals.push_back(clipPlaneNormal);
+ planesPoints.push_back(edgeV1);
+
+ // Go to the next adjacent edge of the reference face
+ currentEdgeIndex = edge.nextEdgeIndex;
+
+ } while (currentEdgeIndex != firstEdgeIndex);
+
+ assert(planesNormals.size() > 0);
+ assert(planesNormals.size() == planesPoints.size());
+
+ // Clip the reference faces with the adjacent planes of the reference face
+ std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
+ assert(clipPolygonVertices.size() > 0);
+
+ // We only keep the clipped points that are below the reference face
+ const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
+ std::vector<Vector3>::const_iterator itPoints;
+ bool contactPointsFound = false;
+ for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
+
+ // If the clip point is bellow the reference face
+ if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) {
+
+ contactPointsFound = true;
+
+ // Convert the clip incident polyhedron vertex into the incident polyhedron local-space
+ Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
+
+ // Project the contact point onto the reference face
+ Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
+
+ // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
+ TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
+ isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
+ isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron,
+ narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
+ minPenetrationDepth, normalWorld);
+
+ // Create a new contact point
+ narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
+ isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
+ isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
+ }
+ }
+
+ return contactPointsFound;
+}
+
// Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector
// This is used to find the incident face on a polyhedron of a given reference face of another polyhedron
uint SATAlgorithm::findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const {
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 73838d95..37e4f7d2 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -96,6 +96,13 @@ class SATAlgorithm {
const Vector3& edgeDirectionCapsuleSpace,
const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const;
+ /// Compute the contact points between two faces of two convex polyhedra.
+ bool computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, const ConvexPolyhedronShape* polyhedron1,
+ const ConvexPolyhedronShape* polyhedron2, const Transform& polyhedron1ToPolyhedron2,
+ const Transform& polyhedron2ToPolyhedron1, uint minFaceIndex,
+ NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const;
+
+
public :
// -------------------- Methods -------------------- //
From e1602f2b272c571e68d234fcf1e340e811071300 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 3 Sep 2017 17:35:09 +0200
Subject: [PATCH 070/133] Fix issues with normals computation in
TriangleVertexArray
---
src/collision/TriangleVertexArray.cpp | 14 +++++++-------
1 file changed, 7 insertions(+), 7 deletions(-)
diff --git a/src/collision/TriangleVertexArray.cpp b/src/collision/TriangleVertexArray.cpp
index ad59d37a..1e500001 100644
--- a/src/collision/TriangleVertexArray.cpp
+++ b/src/collision/TriangleVertexArray.cpp
@@ -163,9 +163,9 @@ void TriangleVertexArray::computeVerticesNormals() {
Vector3 normalComponent = std::asin(sinA) * crossProduct;
// Add the normal component of this vertex into the normals array
- verticesNormals[verticesIndices[v]] = normalComponent.x;
- verticesNormals[verticesIndices[v] + 1] = normalComponent.y;
- verticesNormals[verticesIndices[v] + 2] = normalComponent.z;
+ verticesNormals[verticesIndices[v] * 3] = normalComponent.x;
+ verticesNormals[verticesIndices[v] * 3 + 1] = normalComponent.y;
+ verticesNormals[verticesIndices[v] * 3 + 2] = normalComponent.z;
}
}
@@ -173,12 +173,12 @@ void TriangleVertexArray::computeVerticesNormals() {
for (uint v=0; v<mNbVertices * 3; v += 3) {
// Normalize the normal
- Vector3 normal(verticesNormals[v], verticesNormals[v + 1], verticesNormals[v + 2]);
+ Vector3 normal(verticesNormals[v * 3], verticesNormals[v * 3 + 1], verticesNormals[v * 3 + 2]);
normal.normalize();
- verticesNormals[v] = normal.x;
- verticesNormals[v + 1] = normal.y;
- verticesNormals[v + 2] = normal.z;
+ verticesNormals[v * 3] = normal.x;
+ verticesNormals[v * 3 + 1] = normal.y;
+ verticesNormals[v * 3 + 2] = normal.z;
}
mVerticesNormalsStart = reinterpret_cast<unsigned char*>(verticesNormals);
From 8bab9c13483210a865bd96bbdf30b1e2db05f4c2 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 3 Sep 2017 18:05:23 +0200
Subject: [PATCH 071/133] Remove unused cachedCollisionData parameter
---
src/collision/narrowphase/GJK/GJKAlgorithm.cpp | 17 +++++------------
src/collision/narrowphase/SAT/SATAlgorithm.cpp | 6 +++---
src/collision/shapes/BoxShape.h | 6 ++----
src/collision/shapes/CapsuleShape.h | 6 ++----
src/collision/shapes/ConvexMeshShape.cpp | 5 +----
src/collision/shapes/ConvexMeshShape.h | 3 +--
src/collision/shapes/ConvexShape.cpp | 5 ++---
src/collision/shapes/ConvexShape.h | 8 ++------
src/collision/shapes/SphereShape.h | 6 ++----
src/collision/shapes/TriangleShape.h | 5 ++---
10 files changed, 22 insertions(+), 45 deletions(-)
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index 609c415a..9bc1d00d 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -66,9 +66,6 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
- void** shape1CachedCollisionData = narrowPhaseInfo->cachedCollisionData1;
- void** shape2CachedCollisionData = narrowPhaseInfo->cachedCollisionData2;
-
bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
// Get the local-space to world-space transforms
@@ -109,8 +106,8 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
do {
// Compute the support points for original objects (without margins) A and B
- suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData);
- suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
+ suppA = shape1->getLocalSupportPointWithoutMargin(-v);
+ suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@@ -237,8 +234,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
- void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
-
// Support point of object B (object B is a single point)
const Vector3 suppB(localPoint);
@@ -254,7 +249,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
do {
// Compute the support points for original objects (without margins) A and B
- suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData);
+ suppA = shape->getLocalSupportPointWithoutMargin(-v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@@ -300,8 +295,6 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
- void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
-
Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin)
Vector3 suppB; // Support point on the collision shape
const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
@@ -321,7 +314,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
decimal lambda = decimal(0.0);
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
- suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
+ suppB = shape->getLocalSupportPointWithoutMargin(rayDirection);
Vector3 v = suppA - suppB;
decimal vDotW, vDotR;
decimal distSquare = v.lengthSquare();
@@ -331,7 +324,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
// Compute the support points
- suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData);
+ suppB = shape->getLocalSupportPointWithoutMargin(v);
w = suppA - suppB;
vDotW = v.dot(w);
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 87be645b..d1062704 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -314,7 +314,7 @@ decimal SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const Con
outAxis.normalize();
// Compute the penetration depth
- const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outAxis, nullptr);
+ const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outAxis);
const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint;
penetrationDepth = capsuleSupportPointToEdgePoint.dot(outAxis);
}
@@ -335,7 +335,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe
// Compute the penetration depth (using the capsule support in the direction opposite to the face normal)
outFaceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
- const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outFaceNormalCapsuleSpace, nullptr);
+ const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outFaceNormalCapsuleSpace);
const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]);
const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint;
const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(outFaceNormalCapsuleSpace);
@@ -932,7 +932,7 @@ decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const Convex
const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal;
// Get the support point of polyhedron 2 in the inverse direction of face normal
- const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr);
+ const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space);
// Compute the penetration depth
const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]);
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index 5a07b0cb..d64222ff 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -65,8 +65,7 @@ class BoxShape : public ConvexPolyhedronShape {
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
- virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const override;
+ virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
@@ -170,8 +169,7 @@ inline size_t BoxShape::getSizeInBytes() const {
}
// Return a local support point in a given direction without the objec margin
-inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const {
+inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x,
direction.y < 0.0 ? -mExtent.y : mExtent.y,
diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h
index 9c0a4545..8aebff58 100644
--- a/src/collision/shapes/CapsuleShape.h
+++ b/src/collision/shapes/CapsuleShape.h
@@ -56,8 +56,7 @@ class CapsuleShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
- virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const override;
+ virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
@@ -169,8 +168,7 @@ inline bool CapsuleShape::isPolyhedron() const {
/// Therefore, in this method, we compute the support points of both top and bottom spheres of
/// the capsule and return the point with the maximum dot product with the direction vector. Note
/// that the object margin is implicitly the radius and height of the capsule.
-inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const {
+inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
// Support point top sphere
decimal dotProductTop = mHalfHeight * direction.y;
diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp
index c8f03fa3..995ef9bb 100644
--- a/src/collision/shapes/ConvexMeshShape.cpp
+++ b/src/collision/shapes/ConvexMeshShape.cpp
@@ -56,10 +56,7 @@ ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin)
/// it as a start in a hill-climbing (local search) process to find the new support vertex which
/// will be in most of the cases very close to the previous one. Using hill-climbing, this method
/// runs in almost constant time.
-Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const {
-
- // TODO : Do we still need to have cachedCollisionData or we can remove it from everywhere ?
+Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
double maxDotProduct = DECIMAL_SMALLEST;
uint indexMaxDotProduct = 0;
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index dd2b7021..b42c02e2 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -83,8 +83,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
virtual void setLocalScaling(const Vector3& scaling) override;
/// Return a local support point in a given direction without the object margin.
- virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const override;
+ virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
diff --git a/src/collision/shapes/ConvexShape.cpp b/src/collision/shapes/ConvexShape.cpp
index 649e50f5..90c5356f 100644
--- a/src/collision/shapes/ConvexShape.cpp
+++ b/src/collision/shapes/ConvexShape.cpp
@@ -37,11 +37,10 @@ ConvexShape::ConvexShape(CollisionShapeName name, CollisionShapeType type, decim
}
// Return a local support point in a given direction with the object margin
-Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction,
- void** cachedCollisionData) const {
+Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
// Get the support point without margin
- Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
+ Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction);
if (mMargin != decimal(0.0)) {
diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h
index 4bf4b718..9a2b7e00 100644
--- a/src/collision/shapes/ConvexShape.h
+++ b/src/collision/shapes/ConvexShape.h
@@ -49,14 +49,10 @@ class ConvexShape : public CollisionShape {
// -------------------- Methods -------------------- //
// Return a local support point in a given direction with the object margin
- // TODO : Try to remove cachedCollisionData parameter
- Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
- void** cachedCollisionData) const;
+ Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
/// Return a local support point in a given direction without the object margin
- // TODO : Try to remove cachedCollisionData parameter
- virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const=0;
+ virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const=0;
public :
diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h
index fe55a7a8..70f651de 100644
--- a/src/collision/shapes/SphereShape.h
+++ b/src/collision/shapes/SphereShape.h
@@ -49,8 +49,7 @@ class SphereShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
- virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const override;
+ virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
@@ -123,8 +122,7 @@ inline size_t SphereShape::getSizeInBytes() const {
}
// Return a local support point in a given direction without the object margin
-inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const {
+inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
// Return the center of the sphere (the radius is taken into account in the object margin)
return Vector3(0.0, 0.0, 0.0);
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index 609d5892..19cedfef 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -81,8 +81,7 @@ class TriangleShape : public ConvexPolyhedronShape {
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
- virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
- void** cachedCollisionData) const override;
+ virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Get a smooth contact normal for collision for a triangle of the mesh
Vector3 computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const;
@@ -190,7 +189,7 @@ inline size_t TriangleShape::getSizeInBytes() const {
}
// Return a local support point in a given direction without the object margin
-inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction, void** cachedCollisionData) const {
+inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2]));
return mPoints[dotProducts.getMaxAxis()];
}
From 946e62dd4bdf5d934564830a055fcbd92aaa1eaa Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 3 Sep 2017 19:06:02 +0200
Subject: [PATCH 072/133] Remove unnecessary collision margin for some shapes
---
src/collision/ContactManifold.cpp | 3 ---
src/collision/MiddlePhaseTriangleCallback.cpp | 3 +--
src/collision/shapes/BoxShape.cpp | 10 +++++-----
src/collision/shapes/BoxShape.h | 2 +-
src/collision/shapes/ConcaveMeshShape.cpp | 4 +---
src/collision/shapes/ConcaveShape.cpp | 2 +-
src/collision/shapes/ConcaveShape.h | 11 -----------
src/collision/shapes/ConvexMeshShape.cpp | 4 ++--
src/collision/shapes/ConvexMeshShape.h | 4 +---
src/collision/shapes/ConvexPolyhedronShape.cpp | 4 ++--
src/collision/shapes/ConvexPolyhedronShape.h | 2 +-
src/collision/shapes/ConvexShape.h | 2 +-
src/collision/shapes/HeightFieldShape.cpp | 3 +--
src/collision/shapes/TriangleShape.cpp | 4 ++--
src/collision/shapes/TriangleShape.h | 2 +-
src/configuration.h | 3 ---
test/tests/collision/TestPointInside.h | 2 +-
test/tests/collision/TestRaycast.h | 2 +-
18 files changed, 22 insertions(+), 45 deletions(-)
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index 846a418a..803a9e82 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -65,9 +65,6 @@ ContactManifold::~ContactManifold() {
ContactPoint* nextContactPoint = contactPoint->getNext();
- // TODO : Delete this
- bool test = mMemoryAllocator.isReleaseNeeded();
-
// Delete the contact point
contactPoint->~ContactPoint();
mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp
index a1a183ac..de6c22ea 100644
--- a/src/collision/MiddlePhaseTriangleCallback.cpp
+++ b/src/collision/MiddlePhaseTriangleCallback.cpp
@@ -34,10 +34,9 @@ void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIn
// Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the
// destructor of the corresponding NarrowPhaseInfo.
- decimal margin = mConcaveShape->getTriangleMargin();
TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
- verticesNormals, meshSubPart, triangleIndex, margin);
+ verticesNormals, meshSubPart, triangleIndex);
// Create a narrow phase info for the narrow-phase collision detection
NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList;
diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp
index 54666136..01c4d965 100644
--- a/src/collision/shapes/BoxShape.cpp
+++ b/src/collision/shapes/BoxShape.cpp
@@ -37,11 +37,11 @@ using namespace reactphysics3d;
* @param extent The vector with the three extents of the box (in meters)
* @param margin The collision margin (in meters) around the collision shape
*/
-BoxShape::BoxShape(const Vector3& extent, decimal margin)
- : ConvexPolyhedronShape(CollisionShapeName::BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) {
- assert(extent.x > decimal(0.0) && extent.x > margin);
- assert(extent.y > decimal(0.0) && extent.y > margin);
- assert(extent.z > decimal(0.0) && extent.z > margin);
+BoxShape::BoxShape(const Vector3& extent)
+ : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent) {
+ assert(extent.x > decimal(0.0));
+ assert(extent.y > decimal(0.0));
+ assert(extent.z > decimal(0.0));
// Vertices
mHalfEdgeStructure.addVertex(0);
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index d64222ff..56c14797 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -81,7 +81,7 @@ class BoxShape : public ConvexPolyhedronShape {
// -------------------- Methods -------------------- //
/// Constructor
- BoxShape(const Vector3& extent, decimal margin = OBJECT_MARGIN);
+ BoxShape(const Vector3& extent);
/// Destructor
virtual ~BoxShape() override = default;
diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp
index 0a9e1fd6..84e95e2c 100644
--- a/src/collision/shapes/ConcaveMeshShape.cpp
+++ b/src/collision/shapes/ConcaveMeshShape.cpp
@@ -59,7 +59,6 @@ void ConcaveMeshShape::initBVHTree() {
// Create the AABB for the triangle
AABB aabb = AABB::createAABBForTriangle(trianglePoints);
- aabb.inflate(mTriangleMargin, mTriangleMargin, mTriangleMargin);
// Add the AABB with the index of the triangle into the dynamic AABB tree
mDynamicAABBTree.addObject(aabb, subPart, triangleIndex);
@@ -152,9 +151,8 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
Vector3 verticesNormals[3];
mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
// Create a triangle collision shape
- decimal margin = mConcaveMeshShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
- verticesNormals, data[0], data[1], margin);
+ verticesNormals, data[0], data[1]);
triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
// Ray casting test against the collision shape
diff --git a/src/collision/shapes/ConcaveShape.cpp b/src/collision/shapes/ConcaveShape.cpp
index e8c032d2..cac82e45 100644
--- a/src/collision/shapes/ConcaveShape.cpp
+++ b/src/collision/shapes/ConcaveShape.cpp
@@ -32,6 +32,6 @@ using namespace reactphysics3d;
// Constructor
ConcaveShape::ConcaveShape(CollisionShapeName name)
- : CollisionShape(name, CollisionShapeType::CONCAVE_SHAPE), mTriangleMargin(0), mRaycastTestType(TriangleRaycastSide::FRONT) {
+ : CollisionShape(name, CollisionShapeType::CONCAVE_SHAPE), mRaycastTestType(TriangleRaycastSide::FRONT) {
}
diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h
index 34e347e0..abc3759b 100644
--- a/src/collision/shapes/ConcaveShape.h
+++ b/src/collision/shapes/ConcaveShape.h
@@ -63,9 +63,6 @@ class ConcaveShape : public CollisionShape {
// -------------------- Attributes -------------------- //
- // Margin use for collision detection for each triangle
- decimal mTriangleMargin;
-
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide mRaycastTestType;
@@ -90,9 +87,6 @@ class ConcaveShape : public CollisionShape {
/// Deleted assignment operator
ConcaveShape& operator=(const ConcaveShape& shape) = delete;
- /// Return the triangle margin
- decimal getTriangleMargin() const;
-
/// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const;
@@ -109,11 +103,6 @@ class ConcaveShape : public CollisionShape {
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0;
};
-// Return the triangle margin
-inline decimal ConcaveShape::getTriangleMargin() const {
- return mTriangleMargin;
-}
-
// Return true if the collision shape is convex, false if it is concave
inline bool ConcaveShape::isConvex() const {
return false;
diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp
index 995ef9bb..f6011375 100644
--- a/src/collision/shapes/ConvexMeshShape.cpp
+++ b/src/collision/shapes/ConvexMeshShape.cpp
@@ -41,8 +41,8 @@ using namespace reactphysics3d;
* @param stride Stride between the beginning of two elements in the vertices array
* @param margin Collision margin (in meters) around the collision shape
*/
-ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin)
- : ConvexPolyhedronShape(CollisionShapeName::CONVEX_MESH, margin), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) {
+ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh)
+ : ConvexPolyhedronShape(CollisionShapeName::CONVEX_MESH), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) {
// Recalculate the bounds of the mesh
recalculateBounds();
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index b42c02e2..4ceabe11 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -99,9 +99,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
// -------------------- Methods -------------------- //
/// Constructor
- // TODO : Do we really need to use the margin anymore ? Maybe for raycasting ? If not, remove all the
- // comments documentation about margin
- ConvexMeshShape(PolyhedronMesh* polyhedronMesh, decimal margin = OBJECT_MARGIN);
+ ConvexMeshShape(PolyhedronMesh* polyhedronMesh);
/// Destructor
virtual ~ConvexMeshShape() override = default;
diff --git a/src/collision/shapes/ConvexPolyhedronShape.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp
index 0db54a85..ab93584d 100644
--- a/src/collision/shapes/ConvexPolyhedronShape.cpp
+++ b/src/collision/shapes/ConvexPolyhedronShape.cpp
@@ -31,7 +31,7 @@
using namespace reactphysics3d;
// Constructor
-ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name, decimal margin)
- : ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON, margin) {
+ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name)
+ : ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON) {
}
diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h
index fe1f07a9..96707d69 100644
--- a/src/collision/shapes/ConvexPolyhedronShape.h
+++ b/src/collision/shapes/ConvexPolyhedronShape.h
@@ -47,7 +47,7 @@ class ConvexPolyhedronShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Constructor
- ConvexPolyhedronShape(CollisionShapeName name, decimal margin);
+ ConvexPolyhedronShape(CollisionShapeName name);
/// Destructor
virtual ~ConvexPolyhedronShape() override = default;
diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h
index 9a2b7e00..04f8aad6 100644
--- a/src/collision/shapes/ConvexShape.h
+++ b/src/collision/shapes/ConvexShape.h
@@ -59,7 +59,7 @@ class ConvexShape : public CollisionShape {
// -------------------- Methods -------------------- //
/// Constructor
- ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin);
+ ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin = decimal(0.0));
/// Destructor
virtual ~ConvexShape() override = default;
diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp
index b13cf69b..2afba401 100644
--- a/src/collision/shapes/HeightFieldShape.cpp
+++ b/src/collision/shapes/HeightFieldShape.cpp
@@ -260,9 +260,8 @@ void TriangleOverlapCallback::testTriangle(uint meshSubPart, uint triangleIndex,
const Vector3* verticesNormals) {
// Create a triangle collision shape
- decimal margin = mHeightFieldShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
- verticesNormals, meshSubPart, triangleIndex, margin);
+ verticesNormals, meshSubPart, triangleIndex);
triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
// Ray casting test against the collision shape
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index c72dd133..48baa225 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -44,8 +44,8 @@ using namespace reactphysics3d;
* @param margin The collision margin (in meters) around the collision shape
*/
TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
- const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin)
- : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE, margin), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) {
+ const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex)
+ : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) {
mPoints[0] = point1;
mPoints[1] = point2;
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index 19cedfef..c31a594c 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -108,7 +108,7 @@ class TriangleShape : public ConvexPolyhedronShape {
/// Constructor
TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
- const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex, decimal margin = OBJECT_MARGIN);
+ const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex);
/// Destructor
virtual ~TriangleShape() override = default;
diff --git a/src/configuration.h b/src/configuration.h
index 04cb0956..b2c53021 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -100,9 +100,6 @@ constexpr decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0);
/// True if the spleeping technique is enabled
constexpr bool SPLEEPING_ENABLED = true;
-/// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
-constexpr decimal OBJECT_MARGIN = decimal(0.04);
-
/// Distance threshold for two contact points for a valid persistent contact (in meters)
constexpr decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h
index abb54791..608b439b 100644
--- a/test/tests/collision/TestPointInside.h
+++ b/test/tests/collision/TestPointInside.h
@@ -115,7 +115,7 @@ class TestPointInside : public Test {
mLocalShapeToWorld = mBodyTransform * mShapeTransform;
// Create collision shapes
- mBoxShape = new BoxShape(Vector3(2, 3, 4), 0);
+ mBoxShape = new BoxShape(Vector3(2, 3, 4));
mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, mShapeTransform);
mSphereShape = new SphereShape(3);
diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h
index 93860a1c..81013570 100644
--- a/test/tests/collision/TestRaycast.h
+++ b/test/tests/collision/TestRaycast.h
@@ -192,7 +192,7 @@ class TestRaycast : public Test {
mLocalShapeToWorld = mBodyTransform * mShapeTransform;
// Create collision shapes
- mBoxShape = new BoxShape(Vector3(2, 3, 4), 0);
+ mBoxShape = new BoxShape(Vector3(2, 3, 4));
mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, mShapeTransform);
mSphereShape = new SphereShape(3);
From 501bca5e3d7ee7b6a0b7b918eeb4e179cd15711a Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 4 Sep 2017 07:26:01 +0200
Subject: [PATCH 073/133] Fix issue in TriangleVertexArray
---
src/collision/TriangleVertexArray.cpp | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/src/collision/TriangleVertexArray.cpp b/src/collision/TriangleVertexArray.cpp
index 1e500001..fa87e684 100644
--- a/src/collision/TriangleVertexArray.cpp
+++ b/src/collision/TriangleVertexArray.cpp
@@ -173,12 +173,12 @@ void TriangleVertexArray::computeVerticesNormals() {
for (uint v=0; v<mNbVertices * 3; v += 3) {
// Normalize the normal
- Vector3 normal(verticesNormals[v * 3], verticesNormals[v * 3 + 1], verticesNormals[v * 3 + 2]);
+ Vector3 normal(verticesNormals[v], verticesNormals[v + 1], verticesNormals[v + 2]);
normal.normalize();
- verticesNormals[v * 3] = normal.x;
- verticesNormals[v * 3 + 1] = normal.y;
- verticesNormals[v * 3 + 2] = normal.z;
+ verticesNormals[v] = normal.x;
+ verticesNormals[v + 1] = normal.y;
+ verticesNormals[v + 2] = normal.z;
}
mVerticesNormalsStart = reinterpret_cast<unsigned char*>(verticesNormals);
From 95ade79af5d6e8ceb9427ff5d15fe49678cda7a3 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 4 Sep 2017 21:23:07 +0200
Subject: [PATCH 074/133] Fix issue with obsolete contact points that were not
removed
---
src/collision/CollisionDetection.cpp | 14 +++-----------
src/collision/CollisionDetection.h | 3 ---
2 files changed, 3 insertions(+), 14 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 3c4b771b..3f9a45f6 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -97,9 +97,6 @@ void CollisionDetection::computeMiddlePhase() {
PROFILE("CollisionDetection::computeMiddlePhase()");
- // Clear the set of overlapping pairs in narrow-phase contact
- mContactOverlappingPairs.clear();
-
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
@@ -258,11 +255,6 @@ void CollisionDetection::computeNarrowPhase() {
// Add the contact points as a potential contact manifold into the pair
currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold();
- // Add the overlapping pair into the set of pairs in contact during narrow-phase
- overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(),
- currentNarrowPhaseInfo->overlappingPair->getShape2());
- mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair;
-
currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true;
}
else {
@@ -349,7 +341,7 @@ void CollisionDetection::addAllContactManifoldsToBodies() {
// For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it;
- for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
+ for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
// Add all the contact manifolds of the pair into the list of contact manifolds
// of the two bodies involved in the contact
@@ -396,7 +388,7 @@ void CollisionDetection::processAllPotentialContacts() {
// For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it;
- for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
+ for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
// Process the potential contacts of the overlapping pair
processPotentialContacts(it->second);
@@ -430,7 +422,7 @@ void CollisionDetection::reportAllContacts() {
// For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it;
- for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
+ for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
// If there is a user callback
if (mWorld->mEventListener != nullptr && it->second->hasContacts()) {
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 49f0d56b..848e7d09 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -86,9 +86,6 @@ class CollisionDetection {
/// Broad-phase overlapping pairs
std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
- /// Overlapping pairs in contact (during the current Narrow-phase collision detection)
- std::map<overlappingpairid, OverlappingPair*> mContactOverlappingPairs;
-
/// Broad-phase algorithm
BroadPhaseAlgorithm mBroadPhaseAlgorithm;
From dd91f6dcbf49eaba97d15b603cf3fcf36ba60426 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 4 Sep 2017 22:23:29 +0200
Subject: [PATCH 075/133] Fix typo
---
src/collision/CollisionDetection.cpp | 6 +++---
src/collision/ContactManifold.cpp | 6 +++---
src/collision/ContactManifold.h | 28 ++++++++++++++--------------
src/collision/ContactManifoldSet.cpp | 16 ++++++++--------
src/collision/ContactManifoldSet.h | 8 ++++----
src/constraint/ContactPoint.cpp | 6 +++---
src/constraint/ContactPoint.h | 24 ++++++++++++------------
src/engine/OverlappingPair.h | 16 ++++++++--------
8 files changed, 55 insertions(+), 55 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 3f9a45f6..b284c034 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -103,7 +103,7 @@ void CollisionDetection::computeMiddlePhase() {
OverlappingPair* pair = it->second;
- // Make all the contact manifolds and contact points of the pair obselete
+ // Make all the contact manifolds and contact points of the pair obsolete
pair->makeContactsObselete();
ProxyShape* shape1 = pair->getShape1();
@@ -410,8 +410,8 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) {
potentialManifold = potentialManifold->mNext;
}
- // Clear the obselete contact manifolds and contact points
- pair->clearObseleteManifoldsAndContactPoints();
+ // Clear the obsolete contact manifolds and contact points
+ pair->clearObsoleteManifoldsAndContactPoints();
// Reset the potential contacts of the pair
pair->clearPotentialContactManifolds();
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index 803a9e82..62ca7a0c 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -34,7 +34,7 @@ ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyS
: mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
- mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObselete(false) {
+ mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) {
// For each contact point info in the manifold
const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
@@ -87,7 +87,7 @@ void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo)
mNbContactPoints++;
}
-// Clear the obselete contact points
+// Clear the obsolete contact points
void ContactManifold::clearObseleteContactPoints() {
ContactPoint* contactPoint = mContactPoints;
@@ -96,7 +96,7 @@ void ContactManifold::clearObseleteContactPoints() {
ContactPoint* nextContactPoint = contactPoint->getNext();
- if (contactPoint->getIsObselete()) {
+ if (contactPoint->getIsObsolete()) {
// Delete the contact point
contactPoint->~ContactPoint();
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index ada20003..e34c4543 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -140,8 +140,8 @@ class ContactManifold {
/// Pointer to the previous contact manifold in linked-list
ContactManifold* mPrevious;
- /// True if the contact manifold is obselete
- bool mIsObselete;
+ /// True if the contact manifold is obsolete
+ bool mIsObsolete;
// -------------------- Methods -------------------- //
@@ -151,13 +151,13 @@ class ContactManifold {
/// Set the pointer to the next element in the linked-list
void setNext(ContactManifold* nextManifold);
- /// Return true if the manifold is obselete
- bool getIsObselete() const;
+ /// Return true if the manifold is obsolete
+ bool getIsObsolete() const;
- /// Set to true to make the manifold obselete
- void setIsObselete(bool isObselete, bool setContactPoints);
+ /// Set to true to make the manifold obsolete
+ void setIsObsolete(bool isObselete, bool setContactPoints);
- /// Clear the obselete contact points
+ /// Clear the obsolete contact points
void clearObseleteContactPoints();
/// Return the contact normal direction Id of the manifold
@@ -384,19 +384,19 @@ inline void ContactManifold::setNext(ContactManifold* nextManifold) {
mNext = nextManifold;
}
-// Return true if the manifold is obselete
-inline bool ContactManifold::getIsObselete() const {
- return mIsObselete;
+// Return true if the manifold is obsolete
+inline bool ContactManifold::getIsObsolete() const {
+ return mIsObsolete;
}
-// Set to true to make the manifold obselete
-inline void ContactManifold::setIsObselete(bool isObselete, bool setContactPoints) {
- mIsObselete = isObselete;
+// Set to true to make the manifold obsolete
+inline void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoints) {
+ mIsObsolete = isObsolete;
if (setContactPoints) {
ContactPoint* contactPoint = mContactPoints;
while (contactPoint != nullptr) {
- contactPoint->setIsObselete(isObselete);
+ contactPoint->setIsObsolete(isObsolete);
contactPoint = contactPoint->getNext();
}
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index fb3beda2..5ad8ce71 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -131,8 +131,8 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold,
contactPointInfo = contactPointInfo->next;
}
- // The old manifold is no longer obselete
- oldManifold->setIsObselete(false, false);
+ // The old manifold is no longer obsolete
+ oldManifold->setIsObsolete(false, false);
}
// Return the manifold with the smallest contact penetration depth among its points
@@ -270,27 +270,27 @@ void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
mNbManifolds--;
}
-// Make all the contact manifolds and contact points obselete
-void ContactManifoldSet::makeContactsObselete() {
+// Make all the contact manifolds and contact points obsolete
+void ContactManifoldSet::makeContactsObsolete() {
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
- manifold->setIsObselete(true, true);
+ manifold->setIsObsolete(true, true);
manifold = manifold->getNext();
}
}
-// Clear the obselete contact manifolds and contact points
-void ContactManifoldSet::clearObseleteManifoldsAndContactPoints() {
+// Clear the obsolete contact manifolds and contact points
+void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() {
ContactManifold* manifold = mManifolds;
ContactManifold* previousManifold = nullptr;
while (manifold != nullptr) {
ContactManifold* nextManifold = manifold->getNext();
- if (manifold->getIsObselete()) {
+ if (manifold->getIsObsolete()) {
if (previousManifold != nullptr) {
previousManifold->setNext(nextManifold);
diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h
index 0990277c..1676f5a2 100644
--- a/src/collision/ContactManifoldSet.h
+++ b/src/collision/ContactManifoldSet.h
@@ -115,14 +115,14 @@ class ContactManifoldSet {
/// Return a pointer to the first element of the linked-list of contact manifolds
ContactManifold* getContactManifolds() const;
- /// Make all the contact manifolds and contact points obselete
- void makeContactsObselete();
+ /// Make all the contact manifolds and contact points obsolete
+ void makeContactsObsolete();
/// Return the total number of contact points in the set of manifolds
int getTotalNbContactPoints() const;
- /// Clear the obselete contact manifolds and contact points
- void clearObseleteManifoldsAndContactPoints();
+ /// Clear the obsolete contact manifolds and contact points
+ void clearObsoleteManifoldsAndContactPoints();
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp
index fa6838ec..2cd951b4 100644
--- a/src/constraint/ContactPoint.cpp
+++ b/src/constraint/ContactPoint.cpp
@@ -37,7 +37,7 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform&
mPenetrationDepth(contactInfo->penetrationDepth),
mLocalPointOnBody1(contactInfo->localPoint1),
mLocalPointOnBody2(contactInfo->localPoint2),
- mIsRestingContact(false), mIsObselete(false), mNext(nullptr) {
+ mIsRestingContact(false), mIsObsolete(false), mNext(nullptr) {
assert(mPenetrationDepth > decimal(0.0));
@@ -45,7 +45,7 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform&
mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
- mIsObselete = false;
+ mIsObsolete = false;
}
// Update the contact point with a new one that is similar (very close)
@@ -64,5 +64,5 @@ void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform&
mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
- mIsObselete = false;
+ mIsObsolete = false;
}
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index 41cc35e4..288abdcc 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -71,8 +71,8 @@ class ContactPoint {
/// Cached penetration impulse
decimal mPenetrationImpulse;
- /// True if the contact point is obselete
- bool mIsObselete;
+ /// True if the contact point is obsolete
+ bool mIsObsolete;
/// Pointer to the next contact point in the linked-list
ContactPoint* mNext;
@@ -93,14 +93,14 @@ class ContactPoint {
/// Set the mIsRestingContact variable
void setIsRestingContact(bool isRestingContact);
- /// Set to true to make the contact point obselete
- void setIsObselete(bool isObselete);
+ /// Set to true to make the contact point obsolete
+ void setIsObsolete(bool isObselete);
/// Set the next contact point in the linked list
void setNext(ContactPoint* next);
- /// Return true if the contact point is obselete
- bool getIsObselete() const;
+ /// Return true if the contact point is obsolete
+ bool getIsObsolete() const;
public :
@@ -206,14 +206,14 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
mIsRestingContact = isRestingContact;
}
-// Return true if the contact point is obselete
-inline bool ContactPoint::getIsObselete() const {
- return mIsObselete;
+// Return true if the contact point is obsolete
+inline bool ContactPoint::getIsObsolete() const {
+ return mIsObsolete;
}
-// Set to true to make the contact point obselete
-inline void ContactPoint::setIsObselete(bool isObselete) {
- mIsObselete = isObselete;
+// Set to true to make the contact point obsolete
+inline void ContactPoint::setIsObsolete(bool isObsolete) {
+ mIsObsolete = isObsolete;
}
// Return the next contact point in the linked list
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index e8c89dda..5141b8cc 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -164,11 +164,11 @@ class OverlappingPair {
/// Reduce the number of contact points of all the potential contact manifolds
void reducePotentialContactManifolds();
- /// Make the contact manifolds and contact points obselete
+ /// Make the contact manifolds and contact points obsolete
void makeContactsObselete();
- /// Clear the obselete contact manifold and contact points
- void clearObseleteManifoldsAndContactPoints();
+ /// Clear the obsolete contact manifold and contact points
+ void clearObsoleteManifoldsAndContactPoints();
/// Return the pair of bodies index
static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
@@ -211,10 +211,10 @@ inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() {
return mContactManifoldSet;
}
-// Make the contact manifolds and contact points obselete
+// Make the contact manifolds and contact points obsolete
inline void OverlappingPair::makeContactsObselete() {
- mContactManifoldSet.makeContactsObselete();
+ mContactManifoldSet.makeContactsObsolete();
}
// Return the pair of bodies index
@@ -262,9 +262,9 @@ inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() {
return mPotentialContactManifolds;
}
-// Clear the obselete contact manifold and contact points
-inline void OverlappingPair::clearObseleteManifoldsAndContactPoints() {
- mContactManifoldSet.clearObseleteManifoldsAndContactPoints();
+// Clear the obsolete contact manifold and contact points
+inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() {
+ mContactManifoldSet.clearObsoleteManifoldsAndContactPoints();
}
}
From 1b82a3e22891ce863e94c70b8ecc842a7ed75b8c Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 7 Sep 2017 22:23:00 +0200
Subject: [PATCH 076/133] Fix issue in GJK algorithm when numerical issue
occurs
---
src/collision/narrowphase/GJK/GJKAlgorithm.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index 9bc1d00d..7979f1df 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -198,7 +198,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
// If the penetration depth is negative (due too numerical errors), there is no contact
if (penetrationDepth <= decimal(0.0)) {
- return GJKResult::INTERPENETRATE;
+ return GJKResult::SEPARATED;
}
// Do not generate a contact point with zero normal length
From b8907730537bd34ea5cbff8007b1c4a7ec3f7006 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 7 Sep 2017 22:24:30 +0200
Subject: [PATCH 077/133] Fix issue when computing clipping planes in SAT
algorithm
---
.../narrowphase/SAT/SATAlgorithm.cpp | 23 ++++++++++++++-----
1 file changed, 17 insertions(+), 6 deletions(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index d1062704..7a11fbf7 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -256,7 +256,6 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1);
}
-
}
else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
@@ -352,6 +351,10 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const {
HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
+
+ // Get the face normal
+ Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
+
uint firstEdgeIndex = face.edgeIndex;
uint edgeIndex = firstEdgeIndex;
@@ -360,12 +363,22 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// For each adjacent edge of the separating face of the polyhedron
do {
+
HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex);
HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
- // Construct a clippling plane for each adjacent edge of the separting face of the polyhedron
+ // Compute the edge vertices and edge direction
+ Vector3 edgeV1 = polyhedron->getVertexPosition(edge.vertexIndex);
+ Vector3 edgeV2 = polyhedron->getVertexPosition(twinEdge.vertexIndex);
+ Vector3 edgeDirection = edgeV2 - edgeV1;
+
+ // Compute the normal of the clipping plane for this edge
+ // The clipping plane is perpendicular to the edge direction and the reference face normal
+ Vector3 clipPlaneNormal = faceNormal.cross(edgeDirection);
+
+ // Construct a clipping plane for each adjacent edge of the separating face of the polyhedron
planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex));
- planesNormals.push_back(-polyhedron->getFaceNormal(twinEdge.faceIndex));
+ planesNormals.push_back(clipPlaneNormal);
edgeIndex = edge.nextEdgeIndex;
@@ -373,9 +386,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// First we clip the inner segment of the capsule with the four planes of the adjacent faces
std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals);
- assert(clipSegment.size() == 2);
-
- const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
+ assert(clipSegment.size() == 2);
// Project the two clipped points into the polyhedron face
const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);
From 8cb2ec7e177453c3fa990d1ab6461416bbf4ea9b Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 8 Sep 2017 07:38:57 +0200
Subject: [PATCH 078/133] Remove wrong assert
---
src/collision/narrowphase/SAT/SATAlgorithm.cpp | 1 -
1 file changed, 1 deletion(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 7a11fbf7..2f345738 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -845,7 +845,6 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
// Clip the reference faces with the adjacent planes of the reference face
std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
- assert(clipPolygonVertices.size() > 0);
// We only keep the clipped points that are below the reference face
const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
From 709bed3cec3cc5f6ec3e8ee77fc5d29bc24f8fa3 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 9 Sep 2017 15:37:55 +0200
Subject: [PATCH 079/133] Fix issues with GJK algorithm
---
.../narrowphase/GJK/GJKAlgorithm.cpp | 22 +++++--------------
src/engine/OverlappingPair.h | 2 ++
2 files changed, 8 insertions(+), 16 deletions(-)
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index 7979f1df..629811eb 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -66,8 +66,6 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
- bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
-
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
@@ -95,6 +93,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
if (lastFrameInfo.isValid && lastFrameInfo.wasUsingGJK) {
v = lastFrameInfo.gjkSeparatingAxis;
+ assert(v.lengthSquare() > decimal(0.000001));
}
else {
v.setAllValues(0, 1, 0);
@@ -140,8 +139,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
// Contact point has been found
contactFound = true;
-
- return GJKResult::INTERPENETRATE;
+ break;
}
// Compute the point of the simplex closest to the origin
@@ -150,14 +148,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
// Contact point has been found
contactFound = true;
-
- return GJKResult::INTERPENETRATE;
- }
-
- // Closest point is almost the origin, go to EPA algorithm
- // Vector v to small to continue computing support points
- if (v.lengthSquare() < MACHINE_EPSILON) {
- return GJKResult::INTERPENETRATE;
+ break;
}
// Store and update the squared distance of the closest point
@@ -177,8 +168,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
break;
}
- } while((isPolytopeShape && !simplex.isFull()) || (!isPolytopeShape && !simplex.isFull() &&
- distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()));
+ } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
if (contactFound && distSquare > MACHINE_EPSILON) {
@@ -203,7 +193,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
// Do not generate a contact point with zero normal length
if (normal.lengthSquare() < MACHINE_EPSILON) {
- return GJKResult::INTERPENETRATE;
+ return GJKResult::SEPARATED;
}
if (reportContacts) {
@@ -219,7 +209,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
return GJKResult::COLLIDE_IN_MARGIN;
}
- return GJKResult::SEPARATED;
+ return GJKResult::INTERPENETRATE;
}
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index 5141b8cc..bb7c1048 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -79,6 +79,8 @@ struct LastFrameCollisionInfo {
wasColliding = false;
wasUsingSAT = false;
wasUsingGJK = false;
+
+ gjkSeparatingAxis = Vector3(0, 1, 0);
}
};
From 63833621a046d58e95a99b73ed35e2beb1ed7658 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 12 Sep 2017 23:25:21 +0200
Subject: [PATCH 080/133] Fix issue with triangle shape normal computation
---
src/collision/shapes/TriangleShape.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index 48baa225..254bc5a1 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -52,7 +52,7 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const
mPoints[2] = point3;
// Compute the triangle normal
- mNormal = (point3 - point1).cross(point2 - point1);
+ mNormal = (point2 - point1).cross(point3 - point1);
mNormal.normalize();
mVerticesNormals[0] = verticesNormals[0];
From 643c781fa09b050bbe85e71f57b14353bc421470 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 21 Sep 2017 22:44:42 +0200
Subject: [PATCH 081/133] Fix issues with smooth triangle contact
---
.../narrowphase/SAT/SATAlgorithm.cpp | 6 ++--
src/collision/shapes/TriangleShape.cpp | 34 +++++++++----------
src/collision/shapes/TriangleShape.h | 2 +-
3 files changed, 22 insertions(+), 20 deletions(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 2f345738..0d2ae20e 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -857,6 +857,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
contactPointsFound = true;
+ Vector3 outWorldNormal = normalWorld;
+
// Convert the clip incident polyhedron vertex into the incident polyhedron local-space
Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
@@ -868,10 +870,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
- minPenetrationDepth, normalWorld);
+ minPenetrationDepth, outWorldNormal);
// Create a new contact point
- narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
+ narrowPhaseInfo->addContactPoint(outWorldNormal, minPenetrationDepth,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
}
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index 254bc5a1..263595b3 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -85,14 +85,9 @@ void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape
triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2,
isShape1Triangle ? shape1ToWorld : shape2ToWorld,
isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(),
- penetrationDepth,
+ penetrationDepth, isShape1Triangle,
isShape1Triangle ? localContactPointShape2 : localContactPointShape1,
outSmoothVertexNormal);
-
- // Make sure the direction of the contact normal is from shape1 to shape2
- if (!isShape1Triangle) {
- outSmoothVertexNormal = -outSmoothVertexNormal;
- }
}
}
@@ -104,25 +99,30 @@ void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape
// stay aligned with the new contact normal. This method will return the new smooth world contact
// normal of the triangle and the the local contact point on the other shape.
void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform,
- const Transform& worldToOtherShapeTransform, decimal penetrationDepth,
+ const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1,
Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const {
// Get the smooth contact normal of the mesh at the contact point on the triangle
- Vector3 localNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle);
+ Vector3 triangleLocalNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle);
// Convert the local contact normal into world-space
- outSmoothWorldContactTriangleNormal = triangleShapeToWorldTransform.getOrientation() * localNormal;
+ Vector3 triangleWorldNormal = triangleShapeToWorldTransform.getOrientation() * triangleLocalNormal;
- // Convert the contact normal into the local-space of the other shape
- Vector3 normalOtherShape = worldToOtherShapeTransform.getOrientation() * outSmoothWorldContactTriangleNormal;
+ // Penetration axis with direction from triangle to other shape
+ Vector3 triangleToOtherShapePenAxis = isTriangleShape1 ? outSmoothWorldContactTriangleNormal : -outSmoothWorldContactTriangleNormal;
- // Convert the local contact point of the triangle into the local-space of the other shape
- Vector3 trianglePointOtherShape = worldToOtherShapeTransform * triangleShapeToWorldTransform *
- localContactPointTriangle;
+ // The triangle normal should be the one in the direction out of the current colliding face of the triangle
+ if (triangleWorldNormal.dot(triangleToOtherShapePenAxis) < decimal(0.0)) {
+ triangleWorldNormal = -triangleWorldNormal;
+ triangleLocalNormal = -triangleLocalNormal;
+ }
- // Re-align the local contact point on the other shape such that it is aligned along
- // the new contact normal
- Vector3 otherShapePoint = trianglePointOtherShape - normalOtherShape * penetrationDepth;
+ // Compute the final contact normal from shape 1 to shape 2
+ outSmoothWorldContactTriangleNormal = isTriangleShape1 ? triangleWorldNormal : -triangleWorldNormal;
+
+ // Re-align the local contact point on the other shape such that it is aligned along the new contact normal
+ Vector3 otherShapePointTriangleSpace = localContactPointTriangle - triangleLocalNormal * penetrationDepth;
+ Vector3 otherShapePoint = worldToOtherShapeTransform * triangleShapeToWorldTransform * otherShapePointTriangleSpace;
outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z);
}
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index c31a594c..8360c32b 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -99,7 +99,7 @@ class TriangleShape : public ConvexPolyhedronShape {
/// This method implements the technique described in Game Physics Pearl book
void computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform,
- const Transform& worldToOtherShapeTransform, decimal penetrationDepth,
+ const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1,
Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const;
public:
From b33b8e0dc5e59e378839e135e09b4ca3b1e5ab9f Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 25 Sep 2017 23:06:17 +0200
Subject: [PATCH 082/133] Fix issue in SAT algorithm between polyhedron and
capsule
---
src/collision/narrowphase/SAT/SATAlgorithm.cpp | 8 +++++---
1 file changed, 5 insertions(+), 3 deletions(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 0d2ae20e..a663791c 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -391,6 +391,10 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// Project the two clipped points into the polyhedron face
const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);
+ if (isCapsuleShape1) {
+ normalWorld = -normalWorld;
+ }
+
// For each of the two clipped points
for (int i = 0; i<2; i++) {
@@ -405,9 +409,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// Project the clipped point into the capsule bounds
Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius;
- if (isCapsuleShape1) {
- normalWorld = -normalWorld;
- }
+
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
From 310fef1c523c67d34201cef43a9177789d6bac66 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 28 Sep 2017 08:34:45 +0200
Subject: [PATCH 083/133] Fix issue with middle phase collision detection (AABB
not computed in correct space)
---
src/collision/CollisionDetection.cpp | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index b284c034..f27b5ddf 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -218,10 +218,10 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair
concaveShape, allocator);
// Compute the convex shape AABB in the local-space of the convex shape
+ const Transform convexToConcaveTransform = concaveProxyShape->getLocalToWorldTransform().getInverse() *
+ convexProxyShape->getLocalToWorldTransform();
AABB aabb;
- convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
-
- // TODO : Implement smooth concave mesh collision somewhere
+ convexShape->computeAABB(aabb, convexToConcaveTransform);
// Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(middlePhaseCallback, aabb);
From cbfeb608df332cade904b64a744c8291cff42f5d Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 4 Oct 2017 22:38:14 +0200
Subject: [PATCH 084/133] Fix bug and clean the pointers casting in
TriangleVertexArray
---
src/collision/TriangleVertexArray.cpp | 90 ++++++++++++---------------
src/collision/TriangleVertexArray.h | 49 +++++++--------
src/configuration.h | 2 +
3 files changed, 67 insertions(+), 74 deletions(-)
diff --git a/src/collision/TriangleVertexArray.cpp b/src/collision/TriangleVertexArray.cpp
index fa87e684..56f3b422 100644
--- a/src/collision/TriangleVertexArray.cpp
+++ b/src/collision/TriangleVertexArray.cpp
@@ -44,20 +44,20 @@ using namespace reactphysics3d;
* @param verticesStride Number of bytes between the beginning of two consecutive vertices
* @param nbTriangles Number of triangles in the array
* @param indexesStart Pointer to the first triangle index
- * @param indexesStride Number of bytes between the beginning of two consecutive triangle indices
+ * @param indexesStride Number of bytes between the beginning of the three indices of two triangles
* @param vertexDataType Type of data for the vertices (float, double)
* @param indexDataType Type of data for the indices (short, int)
*/
-TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
- uint nbTriangles, void* indexesStart, int indexesStride,
+TriangleVertexArray::TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
+ uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType) {
mNbVertices = nbVertices;
- mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
+ mVerticesStart = static_cast<const uchar*>(verticesStart);
mVerticesStride = verticesStride;
mVerticesNormalsStart = nullptr;
- mVerticesNormalsStride = 0;
+ mVerticesNormalsStride = 3 * sizeof(float);
mNbTriangles = nbTriangles;
- mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
+ mIndicesStart = static_cast<const uchar*>(indexesStart);
mIndicesStride = indexesStride;
mVertexDataType = vertexDataType;
mVertexNormaldDataType = NormalDataType::NORMAL_FLOAT_TYPE;
@@ -85,19 +85,19 @@ TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, i
* @param vertexDataType Type of data for the vertices (float, double)
* @param indexDataType Type of data for the indices (short, int)
*/
-TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
- void* verticesNormalsStart, int verticesNormalsStride,
- uint nbTriangles, void* indexesStart, int indexesStride,
+TriangleVertexArray::TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
+ const void* verticesNormalsStart, uint verticesNormalsStride,
+ uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, NormalDataType normalDataType,
IndexDataType indexDataType) {
mNbVertices = nbVertices;
- mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
+ mVerticesStart = static_cast<const uchar*>(verticesStart);
mVerticesStride = verticesStride;
- mVerticesNormalsStart = reinterpret_cast<unsigned char*>(verticesNormalsStart);
+ mVerticesNormalsStart = static_cast<const uchar*>(verticesNormalsStart);
mVerticesNormalsStride = verticesNormalsStride;
mNbTriangles = nbTriangles;
- mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
+ mIndicesStart = static_cast<const uchar*>(indexesStart);
mIndicesStride = indexesStride;
mVertexDataType = vertexDataType;
mVertexNormaldDataType = normalDataType;
@@ -114,7 +114,8 @@ TriangleVertexArray::~TriangleVertexArray() {
if (!mAreVerticesNormalsProvidedByUser) {
// Release the allocated memory
- float* verticesNormals = reinterpret_cast<float*>(mVerticesNormalsStart);
+ const void* verticesNormalPointer = static_cast<const void*>(mVerticesNormalsStart);
+ const float* verticesNormals = static_cast<const float*>(verticesNormalPointer);
delete[] verticesNormals;
}
}
@@ -160,12 +161,15 @@ void TriangleVertexArray::computeVerticesNormals() {
Vector3 crossProduct = a.cross(b);
decimal sinA = crossProduct.length() / (edgesLengths[previousVertex] * edgesLengths[v]);
- Vector3 normalComponent = std::asin(sinA) * crossProduct;
+ sinA = std::min(std::max(sinA, decimal(0.0)), decimal(1.0));
+ decimal arcSinA = std::asin(sinA);
+ assert(arcSinA >= decimal(0.0));
+ Vector3 normalComponent = arcSinA * crossProduct;
// Add the normal component of this vertex into the normals array
- verticesNormals[verticesIndices[v] * 3] = normalComponent.x;
- verticesNormals[verticesIndices[v] * 3 + 1] = normalComponent.y;
- verticesNormals[verticesIndices[v] * 3 + 2] = normalComponent.z;
+ verticesNormals[verticesIndices[v] * 3] += normalComponent.x;
+ verticesNormals[verticesIndices[v] * 3 + 1] += normalComponent.y;
+ verticesNormals[verticesIndices[v] * 3 + 2] += normalComponent.z;
}
}
@@ -181,7 +185,8 @@ void TriangleVertexArray::computeVerticesNormals() {
verticesNormals[v + 2] = normal.z;
}
- mVerticesNormalsStart = reinterpret_cast<unsigned char*>(verticesNormals);
+ const void* verticesNormalsPointer = static_cast<const void*>(verticesNormals);
+ mVerticesNormalsStart = static_cast<const uchar*>(verticesNormalsPointer);
}
// Return the indices of the three vertices of a given triangle in the array
@@ -189,17 +194,18 @@ void TriangleVertexArray::getTriangleVerticesIndices(uint triangleIndex, uint* o
assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
- void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride);
+ const uchar* triangleIndicesPointer = mIndicesStart + triangleIndex * 3 * mIndicesStride;
+ const void* startTriangleIndices = static_cast<const void*>(triangleIndicesPointer);
// For each vertex of the triangle
for (int i=0; i < 3; i++) {
// Get the index of the current vertex in the triangle
if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
- outVerticesIndices[i] = ((uint*)vertexIndexPointer)[i];
+ outVerticesIndices[i] = static_cast<const uint*>(startTriangleIndices)[i];
}
else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
- outVerticesIndices[i] = ((unsigned short*)vertexIndexPointer)[i];
+ outVerticesIndices[i] = static_cast<const ushort*>(startTriangleIndices)[i];
}
else {
assert(false);
@@ -212,32 +218,25 @@ void TriangleVertexArray::getTriangleVertices(uint triangleIndex, Vector3* outTr
assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
- void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride);
+ // Get the three vertex index of the three vertices of the triangle
+ uint verticesIndices[3];
+ getTriangleVerticesIndices(triangleIndex, verticesIndices);
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
- // Get the index of the current vertex in the triangle
- int vertexIndex = 0;
- if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
- vertexIndex = ((uint*)vertexIndexPointer)[k];
- }
- else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
- vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
- }
- else {
- assert(false);
- }
+ const uchar* vertexPointerChar = mVerticesStart + verticesIndices[k] * mVerticesStride;
+ const void* vertexPointer = static_cast<const void*>(vertexPointerChar);
// Get the vertices components of the triangle
if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
- const float* vertices = (float*)(mVerticesStart + vertexIndex * mVerticesStride);
+ const float* vertices = static_cast<const float*>(vertexPointer);
outTriangleVertices[k][0] = decimal(vertices[0]);
outTriangleVertices[k][1] = decimal(vertices[1]);
outTriangleVertices[k][2] = decimal(vertices[2]);
}
else if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
- const double* vertices = (double*)(mVerticesStart + vertexIndex * mVerticesStride);
+ const double* vertices = static_cast<const double*>(vertexPointer);
outTriangleVertices[k][0] = decimal(vertices[0]);
outTriangleVertices[k][1] = decimal(vertices[1]);
outTriangleVertices[k][2] = decimal(vertices[2]);
@@ -253,32 +252,25 @@ void TriangleVertexArray::getTriangleVerticesNormals(uint triangleIndex, Vector3
assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
- void* vertexIndexPointer = (mIndicesStart + triangleIndex * 3 * mIndicesStride);
+ // Get the three vertex index of the three vertices of the triangle
+ uint verticesIndices[3];
+ getTriangleVerticesIndices(triangleIndex, verticesIndices);
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
- // Get the index of the current vertex in the triangle
- int vertexIndex = 0;
- if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
- vertexIndex = ((uint*)vertexIndexPointer)[k];
- }
- else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
- vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
- }
- else {
- assert(false);
- }
+ const uchar* vertexNormalPointerChar = mVerticesNormalsStart + verticesIndices[k] * mVerticesNormalsStride;
+ const void* vertexNormalPointer = static_cast<const void*>(vertexNormalPointerChar);
// Get the normals from the array
if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE) {
- const float* normal = (float*)(mVerticesNormalsStart + vertexIndex * mVerticesNormalsStride);
+ const float* normal = static_cast<const float*>(vertexNormalPointer);
outTriangleVerticesNormals[k][0] = decimal(normal[0]);
outTriangleVerticesNormals[k][1] = decimal(normal[1]);
outTriangleVerticesNormals[k][2] = decimal(normal[2]);
}
else if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_DOUBLE_TYPE) {
- const double* normal = (double*)(mVerticesNormalsStart + vertexIndex * mVerticesNormalsStride);
+ const double* normal = static_cast<const double*>(vertexNormalPointer);
outTriangleVerticesNormals[k][0] = decimal(normal[0]);
outTriangleVerticesNormals[k][1] = decimal(normal[1]);
outTriangleVerticesNormals[k][2] = decimal(normal[2]);
diff --git a/src/collision/TriangleVertexArray.h b/src/collision/TriangleVertexArray.h
index 20ae9387..3db79c7e 100644
--- a/src/collision/TriangleVertexArray.h
+++ b/src/collision/TriangleVertexArray.h
@@ -64,28 +64,27 @@ class TriangleVertexArray {
uint mNbVertices;
/// Pointer to the first vertex value in the array
- unsigned char* mVerticesStart;
+ const uchar* mVerticesStart;
/// Stride (number of bytes) between the beginning of two vertices
/// values in the array
- int mVerticesStride;
+ uint mVerticesStride;
/// Pointer to the first vertex normal value in the array
- unsigned char* mVerticesNormalsStart;
+ const uchar* mVerticesNormalsStart;
/// Stride (number of bytes) between the beginning of two vertex normals
/// values in the array
- int mVerticesNormalsStride;
+ uint mVerticesNormalsStride;
/// Number of triangles in the array
uint mNbTriangles;
/// Pointer to the first vertex index of the array
- unsigned char* mIndicesStart;
+ const uchar* mIndicesStart;
- /// Stride (number of bytes) between the beginning of two indices in
- /// the array
- int mIndicesStride;
+ /// Stride (number of bytes) between the beginning of the three indices of two triangles
+ uint mIndicesStride;
/// Data type of the vertices in the array
VertexDataType mVertexDataType;
@@ -109,14 +108,14 @@ class TriangleVertexArray {
// -------------------- Methods -------------------- //
/// Constructor without vertices normals
- TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
- uint nbTriangles, void* indexesStart, int indexesStride,
+ TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
+ uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType);
/// Constructor with vertices normals
- TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
- void* verticesNormalsStart, int verticesNormalsStride,
- uint nbTriangles, void* indexesStart, int indexesStride,
+ TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
+ const void* verticesNormalsStart, uint uverticesNormalsStride,
+ uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, NormalDataType normalDataType,
IndexDataType indexDataType);
@@ -139,22 +138,22 @@ class TriangleVertexArray {
uint getNbTriangles() const;
/// Return the vertices stride (number of bytes)
- int getVerticesStride() const;
+ uint getVerticesStride() const;
/// Return the vertex normals stride (number of bytes)
- int getVerticesNormlasStride() const;
+ uint getVerticesNormlasStride() const;
/// Return the indices stride (number of bytes)
- int getIndicesStride() const;
+ uint getIndicesStride() const;
/// Return the pointer to the start of the vertices array
- unsigned char* getVerticesStart() const;
+ const void* getVerticesStart() const;
/// Return the pointer to the start of the vertex normals array
- unsigned char* getVerticesNormalsStart() const;
+ const void* getVerticesNormalsStart() const;
/// Return the pointer to the start of the indices array
- unsigned char* getIndicesStart() const;
+ const void* getIndicesStart() const;
/// Return the vertices coordinates of a triangle
void getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const;
@@ -192,32 +191,32 @@ inline uint TriangleVertexArray::getNbTriangles() const {
}
// Return the vertices stride (number of bytes)
-inline int TriangleVertexArray::getVerticesStride() const {
+inline uint TriangleVertexArray::getVerticesStride() const {
return mVerticesStride;
}
// Return the vertex normals stride (number of bytes)
-inline int TriangleVertexArray::getVerticesNormlasStride() const {
+inline uint TriangleVertexArray::getVerticesNormlasStride() const {
return mVerticesNormalsStride;
}
// Return the indices stride (number of bytes)
-inline int TriangleVertexArray::getIndicesStride() const {
+inline uint TriangleVertexArray::getIndicesStride() const {
return mIndicesStride;
}
// Return the pointer to the start of the vertices array
-inline unsigned char* TriangleVertexArray::getVerticesStart() const {
+inline const void* TriangleVertexArray::getVerticesStart() const {
return mVerticesStart;
}
// Return the pointer to the start of the vertex normals array
-inline unsigned char* TriangleVertexArray::getVerticesNormalsStart() const {
+inline const void* TriangleVertexArray::getVerticesNormalsStart() const {
return mVerticesNormalsStart;
}
// Return the pointer to the start of the indices array
-inline unsigned char* TriangleVertexArray::getIndicesStart() const {
+inline const void* TriangleVertexArray::getIndicesStart() const {
return mIndicesStart;
}
diff --git a/src/configuration.h b/src/configuration.h
index b2c53021..444bd1db 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -48,6 +48,8 @@ namespace reactphysics3d {
// ------------------- Type definitions ------------------- //
using uint = unsigned int;
+using uchar = unsigned char;
+using ushort = unsigned short;
using luint = long unsigned int;
using bodyindex = luint;
using bodyindexpair = std::pair<bodyindex, bodyindex>;
From d62aa419743a68083eb61fa78dcf140b72137d3a Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 4 Oct 2017 22:38:39 +0200
Subject: [PATCH 085/133] Fix issue in ContactManifoldSet
---
src/collision/ContactManifoldSet.cpp | 23 +++++++----------------
1 file changed, 7 insertions(+), 16 deletions(-)
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index 5ad8ce71..2fcea3f3 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -253,14 +253,14 @@ void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
ContactManifold* next = manifold->getNext();
if (previous != nullptr) {
- previous->setNext(manifold->getNext());
+ previous->setNext(next);
}
else {
mManifolds = next;
}
if (next != nullptr) {
- next->setPrevious(manifold->getPrevious());
+ next->setPrevious(previous);
}
// Delete the contact manifold
@@ -286,30 +286,21 @@ void ContactManifoldSet::makeContactsObsolete() {
void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() {
ContactManifold* manifold = mManifolds;
- ContactManifold* previousManifold = nullptr;
while (manifold != nullptr) {
+
+ // Get the next manifold in the linked-list
ContactManifold* nextManifold = manifold->getNext();
+ // If the manifold is obsolete
if (manifold->getIsObsolete()) {
- if (previousManifold != nullptr) {
- previousManifold->setNext(nextManifold);
-
- if (nextManifold != nullptr) {
- nextManifold->setPrevious(previousManifold);
- }
- }
- else {
- mManifolds = nextManifold;
- }
-
// Delete the contact manifold
removeManifold(manifold);
-
}
else {
+
+ // Clear the obsolete contact points of the manifold
manifold->clearObseleteContactPoints();
- previousManifold = manifold;
}
manifold = nextManifold;
From 38eff07d0d6b588dcbe529ba0e6d95ec0725c61d Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 9 Oct 2017 22:36:39 +0200
Subject: [PATCH 086/133] Fix issue and small changes in TriangleVertexArray
---
src/collision/TriangleVertexArray.cpp | 2 +-
src/collision/TriangleVertexArray.h | 10 ++++++++--
2 files changed, 9 insertions(+), 3 deletions(-)
diff --git a/src/collision/TriangleVertexArray.cpp b/src/collision/TriangleVertexArray.cpp
index 56f3b422..7cfb2076 100644
--- a/src/collision/TriangleVertexArray.cpp
+++ b/src/collision/TriangleVertexArray.cpp
@@ -194,7 +194,7 @@ void TriangleVertexArray::getTriangleVerticesIndices(uint triangleIndex, uint* o
assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
- const uchar* triangleIndicesPointer = mIndicesStart + triangleIndex * 3 * mIndicesStride;
+ const uchar* triangleIndicesPointer = mIndicesStart + triangleIndex * mIndicesStride;
const void* startTriangleIndices = static_cast<const void*>(triangleIndicesPointer);
// For each vertex of the triangle
diff --git a/src/collision/TriangleVertexArray.h b/src/collision/TriangleVertexArray.h
index 3db79c7e..3a48c2e3 100644
--- a/src/collision/TriangleVertexArray.h
+++ b/src/collision/TriangleVertexArray.h
@@ -122,6 +122,12 @@ class TriangleVertexArray {
/// Destructor
~TriangleVertexArray();
+ /// Deleted assignment operator
+ TriangleVertexArray& operator=(const TriangleVertexArray& triangleVertexArray) = delete;
+
+ /// Deleted copy-constructor
+ TriangleVertexArray(const TriangleVertexArray& triangleVertexArray) = delete;
+
/// Return the vertex data type
VertexDataType getVertexDataType() const;
@@ -141,7 +147,7 @@ class TriangleVertexArray {
uint getVerticesStride() const;
/// Return the vertex normals stride (number of bytes)
- uint getVerticesNormlasStride() const;
+ uint getVerticesNormalsStride() const;
/// Return the indices stride (number of bytes)
uint getIndicesStride() const;
@@ -196,7 +202,7 @@ inline uint TriangleVertexArray::getVerticesStride() const {
}
// Return the vertex normals stride (number of bytes)
-inline uint TriangleVertexArray::getVerticesNormlasStride() const {
+inline uint TriangleVertexArray::getVerticesNormalsStride() const {
return mVerticesNormalsStride;
}
From 37e2c79cf22b1a32c53e2bad390460b9e3d9dea3 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 9 Oct 2017 22:41:45 +0200
Subject: [PATCH 087/133] Fix issue in ContactManifoldSet
---
src/collision/ContactManifoldSet.cpp | 4 ++++
1 file changed, 4 insertions(+)
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index 2fcea3f3..4e73aa9f 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -239,6 +239,9 @@ void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo)
ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator);
manifold->setPrevious(nullptr);
manifold->setNext(mManifolds);
+ if (mManifolds != nullptr) {
+ mManifolds->setPrevious(manifold);
+ }
mManifolds = manifold;
mNbManifolds++;
@@ -248,6 +251,7 @@ void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo)
void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
assert(mNbManifolds > 0);
+ assert(manifold != nullptr);
ContactManifold* previous = manifold->getPrevious();
ContactManifold* next = manifold->getNext();
From 4a37ba39948ae053445f20a058815f609543c30a Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 9 Oct 2017 23:34:35 +0200
Subject: [PATCH 088/133] Add unit tests for the TriangleVertexArray
---
src/mathematics/mathematics_functions.cpp | 39 ++-
src/mathematics/mathematics_functions.h | 7 +
test/main.cpp | 2 +
test/tests/collision/TestRaycast.h | 5 +-
.../tests/collision/TestTriangleVertexArray.h | 261 ++++++++++++++++++
5 files changed, 298 insertions(+), 16 deletions(-)
create mode 100644 test/tests/collision/TestTriangleVertexArray.h
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index d627ed7b..fd813cdc 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -26,14 +26,27 @@
// Libraries
#include "mathematics_functions.h"
#include "Vector3.h"
+#include "Vector2.h"
#include <cassert>
#include <vector>
using namespace reactphysics3d;
-/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
-/// This method uses the technique described in the book Real-Time collision detection by
-/// Christer Ericson.
+
+// Function to test if two vectors are (almost) equal
+bool reactphysics3d::approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon) {
+ return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon) &&
+ approxEqual(vec1.z, vec2.z, epsilon);
+}
+
+// Function to test if two vectors are (almost) equal
+bool reactphysics3d::approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon) {
+ return approxEqual(vec1.x, vec2.x, epsilon) && approxEqual(vec1.y, vec2.y, epsilon);
+}
+
+// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
+// This method uses the technique described in the book Real-Time collision detection by
+// Christer Ericson.
void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
const Vector3& p, decimal& u, decimal& v, decimal& w) {
const Vector3 v0 = b - a;
@@ -52,7 +65,7 @@ void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, c
u = decimal(1.0) - v - w;
}
-/// Clamp a vector such that it is no longer than a given maximum length
+// Clamp a vector such that it is no longer than a given maximum length
Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) {
if (vector.lengthSquare() > maxLength * maxLength) {
return vector.getUnit() * maxLength;
@@ -60,17 +73,17 @@ Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) {
return vector;
}
-/// Return true if two vectors are parallel
+// Return true if two vectors are parallel
bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& vector2) {
return vector1.cross(vector2).lengthSquare() < decimal(0.00001);
}
-/// Return true if two vectors are orthogonal
+// 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"
+// 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) {
const Vector3 ab = segPointB - segPointA;
@@ -95,9 +108,9 @@ Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, c
return segPointA + t * ab;
}
-/// Compute the closest points between two segments
-/// This method uses the technique described in the book Real-Time
-/// collision detection by Christer Ericson.
+// Compute the closest points between two segments
+// This method uses the technique described in the book Real-Time
+// collision detection by Christer Ericson.
void reactphysics3d::computeClosestPointBetweenTwoSegments(const Vector3& seg1PointA, const Vector3& seg1PointB,
const Vector3& seg2PointA, const Vector3& seg2PointB,
Vector3& closestPointSeg1, Vector3& closestPointSeg2) {
@@ -175,7 +188,7 @@ void reactphysics3d::computeClosestPointBetweenTwoSegments(const Vector3& seg1Po
closestPointSeg2 = seg2PointA + d2 * t;
}
-/// Compute the intersection between a plane and a segment
+// Compute the intersection between a plane and a segment
// Let the plane define by the equation planeNormal.dot(X) = planeD with X a point on the plane and "planeNormal" the plane normal. This method
// computes the intersection P between the plane and the segment (segA, segB). The method returns the value "t" such
// that P = segA + t * (segB - segA). Note that it only returns a value in [0, 1] if there is an intersection. Otherwise,
@@ -282,8 +295,8 @@ std::vector<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA,
return outputVertices;
}
-/// Clip a polygon against multiple planes and return the clipped polygon vertices
-/// This method implements the Sutherland–Hodgman clipping algorithm
+// Clip a polygon against multiple planes and return the clipped polygon vertices
+// This method implements the Sutherland–Hodgman clipping algorithm
std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
const std::vector<Vector3>& planesNormals) {
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index 78b5ba67..9f062826 100755
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -38,6 +38,7 @@
namespace reactphysics3d {
struct Vector3;
+struct Vector2;
// ---------- Mathematics functions ---------- //
@@ -47,6 +48,12 @@ inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON)
return (std::fabs(a - b) < epsilon);
}
+/// Function to test if two vectors are (almost) equal
+bool approxEqual(const Vector3& vec1, const Vector3& vec2, decimal epsilon = MACHINE_EPSILON);
+
+/// Function to test if two vectors are (almost) equal
+bool approxEqual(const Vector2& vec1, const Vector2& vec2, decimal epsilon = MACHINE_EPSILON);
+
/// Function that returns the result of the "value" clamped by
/// two others values "lowerLimit" and "upperLimit"
inline int clamp(int value, int lowerLimit, int upperLimit) {
diff --git a/test/main.cpp b/test/main.cpp
index 54bfbb8f..26d62803 100644
--- a/test/main.cpp
+++ b/test/main.cpp
@@ -38,6 +38,7 @@
#include "tests/collision/TestAABB.h"
#include "tests/collision/TestDynamicAABBTree.h"
#include "tests/collision/TestHalfEdgeStructure.h"
+#include "tests/collision/TestTriangleVertexArray.h"
using namespace reactphysics3d;
@@ -59,6 +60,7 @@ int main() {
testSuite.addTest(new TestAABB("AABB"));
testSuite.addTest(new TestPointInside("IsPointInside"));
+ testSuite.addTest(new TestTriangleVertexArray("TriangleVertexArray"));
testSuite.addTest(new TestRaycast("Raycasting"));
testSuite.addTest(new TestCollisionWorld("CollisionWorld"));
testSuite.addTest(new TestDynamicAABBTree("DynamicAABBTree"));
diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h
index 81013570..f2ae3d6b 100644
--- a/test/tests/collision/TestRaycast.h
+++ b/test/tests/collision/TestRaycast.h
@@ -277,9 +277,8 @@ class TestRaycast : public Test {
TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE;
mConcaveMeshVertexArray =
new TriangleVertexArray(8, &(mConcaveMeshVertices[0]), sizeof(Vector3),
- 12, &(mConcaveMeshIndices[0]), sizeof(uint),
- vertexType,
- TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
+ 12, &(mConcaveMeshIndices[0]), 3 * sizeof(uint),
+ vertexType, TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
// Add the triangle vertex array of the subpart to the triangle mesh
diff --git a/test/tests/collision/TestTriangleVertexArray.h b/test/tests/collision/TestTriangleVertexArray.h
new file mode 100644
index 00000000..282b7d55
--- /dev/null
+++ b/test/tests/collision/TestTriangleVertexArray.h
@@ -0,0 +1,261 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef TEST_TRIANGLE_VERTEX_ARRAY_H
+#define TEST_TRIANGLE_VERTEX_ARRAY_H
+
+// Libraries
+#include "reactphysics3d.h"
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+
+// Class TestTriangleVertexArray
+/**
+ * Unit test for the TestTriangleArray class.
+ */
+class TestTriangleVertexArray : public Test {
+
+ private :
+
+ // ---------- Atributes ---------- //
+
+ float mVertices1[4*3];
+ double mVertices2[4*3];
+ float mNormals2[4*3];
+ uint mIndices1[6];
+ short mIndices2[6];
+ TriangleVertexArray* mTriangleVertexArray1;
+ TriangleVertexArray* mTriangleVertexArray2;
+
+ Vector3 mVertex0;
+ Vector3 mVertex1;
+ Vector3 mVertex2;
+ Vector3 mVertex3;
+ Vector3 mVertex4;
+ Vector3 mVertex5;
+ Vector3 mVertex6;
+ Vector3 mVertex7;
+
+ Vector3 mNormal0;
+ Vector3 mNormal1;
+ Vector3 mNormal2;
+ Vector3 mNormal3;
+
+ public :
+
+ // ---------- Methods ---------- //
+
+ /// Constructor
+ TestTriangleVertexArray(const std::string& name) : Test(name) {
+
+ mVertex0 = Vector3(0, 0, 4);
+ mVertex1 = Vector3(0, 0, -3);
+ mVertex2 = Vector3(-2, 0, 0);
+ mVertex3 = Vector3(0, -5, 0);
+
+ // Initialize data
+ mVertices1[0] = mVertex0.x; mVertices1[1] = mVertex0.y; mVertices1[2] = mVertex0.z;
+ mVertices1[3] = mVertex1.x; mVertices1[4] = mVertex1.y; mVertices1[5] = mVertex1.z;
+ mVertices1[6] = mVertex2.x; mVertices1[7] = mVertex2.y; mVertices1[8] = mVertex2.z;
+ mVertices1[9] = mVertex3.x; mVertices1[10] = mVertex3.y; mVertices1[11] = mVertex3.z;
+
+ mIndices1[0] = 0; mIndices1[1] = 1; mIndices1[2] = 2;
+ mIndices1[3] = 0; mIndices1[4] = 3; mIndices1[5] = 1;
+
+ mVertex4 = Vector3(0, 0, 5);
+ mVertex5 = Vector3(0, 0, -7);
+ mVertex6 = Vector3(-2, 0, 0);
+ mVertex7 = Vector3(0, -5, 0);
+
+ mVertices2[0] = static_cast<double>(mVertex4.x); mVertices2[1] = static_cast<double>(mVertex4.y); mVertices2[2] = static_cast<double>(mVertex4.z);
+ mVertices2[3] = static_cast<double>(mVertex5.x); mVertices2[4] = static_cast<double>(mVertex5.y); mVertices2[5] = static_cast<double>(mVertex5.z);
+ mVertices2[6] = static_cast<double>(mVertex6.x); mVertices2[7] = static_cast<double>(mVertex6.y); mVertices2[8] = static_cast<double>(mVertex6.z);
+ mVertices2[9] = static_cast<double>(mVertex7.x); mVertices2[10] = static_cast<double>(mVertex7.y); mVertices2[11] = static_cast<double>(mVertex7.z);
+
+ mIndices2[0] = 0; mIndices2[1] = 1; mIndices2[2] = 2;
+ mIndices2[3] = 0; mIndices2[4] = 3; mIndices2[5] = 1;
+
+ mNormal0 = Vector3(2, 4, 6);
+ mNormal1 = Vector3(1, 6, -3);
+ mNormal2 = Vector3(-2, 4, 7);
+ mNormal3 = Vector3(-5, 2, 9);
+ mNormal0.normalize();
+ mNormal1.normalize();
+ mNormal2.normalize();
+ mNormal3.normalize();
+ mNormals2[0] = mNormal0.x; mNormals2[1] = mNormal0.y; mNormals2[2] = mNormal0.z;
+ mNormals2[3] = mNormal1.x; mNormals2[4] = mNormal1.y; mNormals2[5] = mNormal1.z;
+ mNormals2[6] = mNormal2.x; mNormals2[7] = mNormal2.y; mNormals2[8] = mNormal2.z;
+ mNormals2[9] = mNormal3.x; mNormals2[10] = mNormal3.y; mNormals2[11] = mNormal3.z;
+
+ // Create triangle vertex array with automatic normals computation
+ mTriangleVertexArray1 = new TriangleVertexArray(4, static_cast<const void*>(mVertices1), 3 * sizeof(float),
+ 2, static_cast<const void*>(mIndices1), 3 * sizeof(uint),
+ TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
+ TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
+
+ // Create triangle vertex array with normals defined by the user
+ mTriangleVertexArray2 = new TriangleVertexArray(4, static_cast<const void*>(mVertices2), 3 * sizeof(double),
+ static_cast<const void*>(mNormals2), 3 * sizeof(float),
+ 2, static_cast<const void*>(mIndices2), 3 * sizeof(short),
+ TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE,
+ TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE,
+ TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE);
+
+ }
+
+ /// Destructor
+ virtual ~TestTriangleVertexArray() {
+ delete mTriangleVertexArray1;
+ delete mTriangleVertexArray2;
+ }
+
+ /// Run the tests
+ void run() {
+
+ // ----- First triangle vertex array ----- //
+
+ test(mTriangleVertexArray1->getVertexDataType() == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE);
+ test(mTriangleVertexArray1->getIndexDataType() == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
+ test(mTriangleVertexArray1->getVertexNormalDataType() == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE);
+ test(mTriangleVertexArray1->getNbTriangles() == 2);
+ test(mTriangleVertexArray1->getNbVertices() == 4);
+ test(mTriangleVertexArray1->getIndicesStart() == static_cast<const void*>(mIndices1));
+ test(mTriangleVertexArray1->getVerticesStart() == static_cast<const void*>(mVertices1));
+ test(mTriangleVertexArray1->getIndicesStride() == (3 * sizeof(uint)));
+ test(mTriangleVertexArray1->getVerticesStride() == (3 * sizeof(float)));
+
+ // Get triangle indices
+
+ uint triangle0Indices[3];
+ mTriangleVertexArray1->getTriangleVerticesIndices(0, triangle0Indices);
+
+ test(triangle0Indices[0] == mIndices1[0]);
+ test(triangle0Indices[1] == mIndices1[1]);
+ test(triangle0Indices[2] == mIndices1[2]);
+
+ uint triangle1Indices[3];
+ mTriangleVertexArray1->getTriangleVerticesIndices(1, triangle1Indices);
+
+ test(triangle1Indices[0] == mIndices1[3]);
+ test(triangle1Indices[1] == mIndices1[4]);
+ test(triangle1Indices[2] == mIndices1[5]);
+
+ // Get triangle vertices
+
+ Vector3 triangle0Vertices[3];
+ mTriangleVertexArray1->getTriangleVertices(0, triangle0Vertices);
+
+ test(approxEqual(triangle0Vertices[0], mVertex0, decimal(0.0000001)));
+ test(approxEqual(triangle0Vertices[1], mVertex1, decimal(0.0000001)));
+ test(approxEqual(triangle0Vertices[2], mVertex2, decimal(0.0000001)));
+
+ Vector3 triangle1Vertices[3];
+ mTriangleVertexArray1->getTriangleVertices(1, triangle1Vertices);
+
+ test(approxEqual(triangle1Vertices[0], mVertex0, decimal(0.0000001)));
+ test(approxEqual(triangle1Vertices[1], mVertex3, decimal(0.0000001)));
+ test(approxEqual(triangle1Vertices[2], mVertex1, decimal(0.0000001)));
+
+ // Get triangle normals
+
+ Vector3 triangle0Normals[3];
+ mTriangleVertexArray1->getTriangleVerticesNormals(0, triangle0Normals);
+
+ Vector3 triangle1Normals[3];
+ mTriangleVertexArray1->getTriangleVerticesNormals(1, triangle1Normals);
+
+ const Vector3 normal0Test(decimal(0.9792), decimal(0.20268), 0);
+ const Vector3 normal2Test(0, 1, 0);
+ const Vector3 normal3Test(1, 0, 0);
+
+ test(approxEqual(triangle0Normals[0], normal0Test, decimal(0.0001)));
+ test(approxEqual(triangle0Normals[2], normal2Test, decimal(0.0001)));
+ test(approxEqual(triangle1Normals[1], normal3Test, decimal(0.0001)));
+
+ // ----- Second triangle vertex array ----- //
+
+ test(mTriangleVertexArray2->getVertexDataType() == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE);
+ test(mTriangleVertexArray2->getIndexDataType() == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE);
+ test(mTriangleVertexArray2->getVertexNormalDataType() == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE);
+ test(mTriangleVertexArray2->getNbTriangles() == 2);
+ test(mTriangleVertexArray2->getNbVertices() == 4);
+ test(mTriangleVertexArray2->getIndicesStart() == static_cast<const void*>(mIndices2));
+ test(mTriangleVertexArray2->getVerticesStart() == static_cast<const void*>(mVertices2));
+ test(mTriangleVertexArray2->getVerticesNormalsStart() == static_cast<const void*>(mNormals2));
+ test(mTriangleVertexArray2->getIndicesStride() == (3 * sizeof(short)));
+ test(mTriangleVertexArray2->getVerticesStride() == (3 * sizeof(double)));
+ test(mTriangleVertexArray2->getVerticesNormalsStride() == (3 * sizeof(float)));
+
+ // Get triangle indices
+
+ mTriangleVertexArray2->getTriangleVerticesIndices(0, triangle0Indices);
+
+ test(triangle0Indices[0] == mIndices2[0]);
+ test(triangle0Indices[1] == mIndices2[1]);
+ test(triangle0Indices[2] == mIndices2[2]);
+
+ mTriangleVertexArray2->getTriangleVerticesIndices(1, triangle1Indices);
+
+ test(triangle1Indices[0] == mIndices2[3]);
+ test(triangle1Indices[1] == mIndices2[4]);
+ test(triangle1Indices[2] == mIndices2[5]);
+
+ // Get triangle vertices
+
+ mTriangleVertexArray2->getTriangleVertices(0, triangle0Vertices);
+
+ test(approxEqual(triangle0Vertices[0], mVertex4, decimal(0.0000001)));
+ test(approxEqual(triangle0Vertices[1], mVertex5, decimal(0.0000001)));
+ test(approxEqual(triangle0Vertices[2], mVertex6, decimal(0.0000001)));
+
+ mTriangleVertexArray2->getTriangleVertices(1, triangle1Vertices);
+
+ test(approxEqual(triangle1Vertices[0], mVertex4, decimal(0.0000001)));
+ test(approxEqual(triangle1Vertices[1], mVertex7, decimal(0.0000001)));
+ test(approxEqual(triangle1Vertices[2], mVertex5, decimal(0.0000001)));
+
+ // Get triangle normals
+
+ mTriangleVertexArray2->getTriangleVerticesNormals(0, triangle0Normals);
+ mTriangleVertexArray2->getTriangleVerticesNormals(1, triangle1Normals);
+
+ test(approxEqual(triangle0Normals[0], mNormal0, decimal(0.000001)));
+ test(approxEqual(triangle0Normals[1], mNormal1, decimal(0.000001)));
+ test(approxEqual(triangle0Normals[2], mNormal2, decimal(0.000001)));
+
+ test(approxEqual(triangle1Normals[0], mNormal0, decimal(0.000001)));
+ test(approxEqual(triangle1Normals[1], mNormal3, decimal(0.000001)));
+ test(approxEqual(triangle1Normals[2], mNormal1, decimal(0.000001)));
+ }
+
+};
+
+}
+
+#endif
+
From 17d1ee36813334749bc044cd44326e1c2d1618d4 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 9 Oct 2017 23:35:30 +0200
Subject: [PATCH 089/133] Change in the ConcaveMesh class of the testbed
application
---
testbed/common/ConcaveMesh.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp
index d6a6bbe3..c7e162a3 100644
--- a/testbed/common/ConcaveMesh.cpp
+++ b/testbed/common/ConcaveMesh.cpp
@@ -46,7 +46,7 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position,
// Vertex and Indices array for the triangle mesh (data in shared and not copied)
rp3d::TriangleVertexArray* vertexArray =
new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3),
- getNbFaces(i), &(mIndices[i][0]), sizeof(int),
+ getNbFaces(i), &(mIndices[i][0]), 3 * sizeof(int),
rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
@@ -97,7 +97,7 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass,
// Vertex and Indices array for the triangle mesh (data in shared and not copied)
rp3d::TriangleVertexArray* vertexArray =
new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3),
- getNbFaces(i), &(mIndices[i][0]), sizeof(int),
+ getNbFaces(i), &(mIndices[i][0]), 3 * sizeof(int),
rp3d::TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE,
rp3d::TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE);
From d5617526ff91baa77ca2b759994f981713a5e4a1 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 12 Oct 2017 20:07:39 +0200
Subject: [PATCH 090/133] Modify the policy to drop contact manifolds. First
drop the old ones before the new ones
---
src/collision/CollisionDetection.cpp | 5 ++-
src/collision/ContactManifold.cpp | 3 +-
src/collision/ContactManifold.h | 21 ++++++++++++
src/collision/ContactManifoldSet.cpp | 51 ++++++++++++++++++++++------
src/collision/ContactManifoldSet.h | 18 ++++++++--
src/engine/OverlappingPair.h | 9 +++++
6 files changed, 92 insertions(+), 15 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index f27b5ddf..6e0e6ed6 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -71,7 +71,7 @@ void CollisionDetection::computeCollisionDetection() {
computeMiddlePhase();
// Compute the narrow-phase collision detection
- computeNarrowPhase();
+ computeNarrowPhase();
// Reset the linked list of narrow-phase info
mNarrowPhaseInfoList = nullptr;
@@ -410,6 +410,9 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) {
potentialManifold = potentialManifold->mNext;
}
+ // Reset the isNew status of the manifolds
+ pair->resetIsNewManifoldStatus();
+
// Clear the obsolete contact manifolds and contact points
pair->clearObsoleteManifoldsAndContactPoints();
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index 62ca7a0c..37ad3ad8 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -34,7 +34,7 @@ ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyS
: mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
- mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) {
+ mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false), mIsNew(true) {
// For each contact point info in the manifold
const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
@@ -85,6 +85,7 @@ void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo)
mContactPoints = contactPoint;
mNbContactPoints++;
+ assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
}
// Clear the obsolete contact points
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index e34c4543..b24ed378 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -143,6 +143,9 @@ class ContactManifold {
/// True if the contact manifold is obsolete
bool mIsObsolete;
+ /// True if the contact manifold is new (has just been added from potential contacts)
+ bool mIsNew;
+
// -------------------- Methods -------------------- //
/// Return true if the contact manifold has already been added into an island
@@ -204,6 +207,12 @@ class ContactManifold {
/// Return the friction twist accumulated impulse
decimal getFrictionTwistImpulse() const;
+
+ /// Return true if the manifold has just been created
+ bool getIsNew() const;
+
+ /// Set the isNew attribute
+ void setIsNew(bool isNew);
public:
@@ -408,6 +417,18 @@ inline short ContactManifold::getContactNormalId() const {
return mContactNormalId;
}
+
+// Return true if the manifold has just been created
+inline bool ContactManifold::getIsNew() const {
+ return mIsNew;
+}
+
+// Set the isNew attribute
+inline void ContactManifold::setIsNew(bool isNew) {
+ mIsNew = isNew;
+}
+
+
}
#endif
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index 4e73aa9f..06ae4963 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -66,8 +66,9 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa
if (mNbManifolds >= mNbMaxManifolds) {
// We need to remove a manifold from the set.
- // We seach for the one with the smallest maximum penetration depth among its contact points
- ContactManifold* minDepthManifold = getManifoldWithSmallestContactPenetrationDepth(contactManifoldInfo->getLargestPenetrationDepth());
+ // We search for an old manifold with the smallest maximum penetration depth among its contact points.
+ // If we do not find an old manifold, we select a new one that has the smallest contact penetration depth.
+ ContactManifold* minDepthManifold = selectManifoldToRemove(contactManifoldInfo->getLargestPenetrationDepth());
// If the manifold with the minimum penetration depth is an existing one
if (minDepthManifold != nullptr) {
@@ -135,25 +136,53 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold,
oldManifold->setIsObsolete(false, false);
}
-// Return the manifold with the smallest contact penetration depth among its points
-ContactManifold* ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const {
+// Return the manifold to remove (because it is too old or has not the largest penetration depth)
+ContactManifold* ContactManifoldSet::selectManifoldToRemove(decimal penDepthNewManifold) const {
assert(mNbManifolds == mNbMaxManifolds);
+ assert(mManifolds != nullptr);
- ContactManifold* minDepthManifold = nullptr;
- decimal minDepth = initDepth;
+ // Look for a manifold that is not new and with the smallest contact penetration depth.
+ // At the same time, we also look for a new manifold with the smallest contact penetration depth
+ // in case no old manifold exists.
+ ContactManifold* minDepthOldManifold = nullptr;
+ ContactManifold* minDepthNewManifold = nullptr;
+ decimal minDepthOld = DECIMAL_LARGEST;
+ decimal minDepthNew = penDepthNewManifold;
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
- decimal depth = manifold->getLargestContactDepth();
- if (depth < minDepth) {
- minDepth = depth;
- minDepthManifold = manifold;
+
+ // Get the largest contact point penetration depth of the manifold
+ const decimal depth = manifold->getLargestContactDepth();
+
+ // If it is a new manifold
+ if (manifold->getIsNew()) {
+
+ if (depth < minDepthNew) {
+ minDepthNew = depth;
+ minDepthNewManifold = manifold;
+ }
+ }
+ else {
+
+ if (depth < minDepthOld) {
+ minDepthOld = depth;
+ minDepthOldManifold = manifold;
+ }
}
manifold = manifold->getNext();
}
- return minDepthManifold;
+ // If there was a contact manifold that was not new
+ if (minDepthOldManifold != nullptr) {
+
+ // Return the old manifold with the smallest penetration depth
+ return minDepthOldManifold;
+ }
+
+ // Otherwise, we return the new manifold with the smallest penetration depth
+ return minDepthNewManifold;
}
// Return the contact manifold with a similar average normal.
diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h
index 1676f5a2..ea832cdb 100644
--- a/src/collision/ContactManifoldSet.h
+++ b/src/collision/ContactManifoldSet.h
@@ -74,8 +74,8 @@ class ContactManifoldSet {
// Return the contact manifold with a similar average normal.
ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const;
- /// Return the manifold with the smallest contact penetration depth
- ContactManifold* getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const;
+ /// Return the manifold to remove (because it is too old or has not the largest penetration depth)
+ ContactManifold* selectManifoldToRemove(decimal penDepthNewManifold) const;
/// Update a previous similar manifold with a new one
void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold);
@@ -124,6 +124,9 @@ class ContactManifoldSet {
/// Clear the obsolete contact manifolds and contact points
void clearObsoleteManifoldsAndContactPoints();
+ /// Reset the isNew status of all the manifolds
+ void resetIsNewManifoldStatus();
+
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket
@@ -177,6 +180,17 @@ inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape
}
}
+
+// Reset the isNew status of all the manifolds
+inline void ContactManifoldSet::resetIsNewManifoldStatus() {
+
+ ContactManifold* manifold = mManifolds;
+ while (manifold != nullptr) {
+ manifold->setIsNew(false);
+ manifold = manifold->getNext();
+ }
+}
+
}
#endif
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index bb7c1048..2725a8d5 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -172,6 +172,9 @@ class OverlappingPair {
/// Clear the obsolete contact manifold and contact points
void clearObsoleteManifoldsAndContactPoints();
+ /// Reset the isNew status of all the manifolds
+ void resetIsNewManifoldStatus();
+
/// Return the pair of bodies index
static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
@@ -269,6 +272,12 @@ inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() {
mContactManifoldSet.clearObsoleteManifoldsAndContactPoints();
}
+
+// Reset the isNew status of all the manifolds
+inline void OverlappingPair::resetIsNewManifoldStatus() {
+ mContactManifoldSet.resetIsNewManifoldStatus();
+}
+
}
#endif
From de494bb0fd74bf5f09bde09f7eac8ea64ce4ea35 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 18 Oct 2017 00:41:32 +0200
Subject: [PATCH 091/133] Changes and bug fixes in ContactManifold and
ContactManifoldSet
---
src/collision/CollisionDetection.cpp | 8 +-
src/collision/ContactManifold.cpp | 126 ++++++++++++++++++++-------
src/collision/ContactManifold.h | 32 +++----
src/collision/ContactManifoldSet.cpp | 98 ++++++++-------------
src/collision/ContactManifoldSet.h | 19 +---
src/collision/ContactPointInfo.h | 2 +
src/constraint/ContactPoint.cpp | 16 +---
src/constraint/ContactPoint.h | 49 +++++------
src/engine/ContactSolver.cpp | 9 +-
src/engine/OverlappingPair.h | 23 ++---
10 files changed, 189 insertions(+), 193 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 6e0e6ed6..f650351a 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -104,7 +104,7 @@ void CollisionDetection::computeMiddlePhase() {
OverlappingPair* pair = it->second;
// Make all the contact manifolds and contact points of the pair obsolete
- pair->makeContactsObselete();
+ pair->makeContactsObsolete();
ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2();
@@ -410,12 +410,12 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) {
potentialManifold = potentialManifold->mNext;
}
- // Reset the isNew status of the manifolds
- pair->resetIsNewManifoldStatus();
-
// Clear the obsolete contact manifolds and contact points
pair->clearObsoleteManifoldsAndContactPoints();
+ // Reduce the contact manifolds and contact points if there are too many of them
+ pair->reduceContactManifolds();
+
// Reset the potential contacts of the pair
pair->clearPotentialContactManifolds();
}
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index 37ad3ad8..55ce8ba3 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -34,20 +34,14 @@ ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyS
: mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
- mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false), mIsNew(true) {
+ mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) {
// For each contact point info in the manifold
const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
while(pointInfo != nullptr) {
- // Create the new contact point
- ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
- ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
-
- // Add the new contact point into the manifold
- contact->setNext(mContactPoints);
- mContactPoints = contact;
- mNbContactPoints++;
+ // Add the new contact point
+ addContactPoint(pointInfo);
pointInfo = pointInfo->next;
}
@@ -73,52 +67,122 @@ ContactManifold::~ContactManifold() {
}
}
+// Remove a contact point
+void ContactManifold::removeContactPoint(ContactPoint* contactPoint) {
+
+ assert(mNbContactPoints > 0);
+ assert(mContactPoints != nullptr);
+ assert(contactPoint != nullptr);
+
+ ContactPoint* previous = contactPoint->getPrevious();
+ ContactPoint* next = contactPoint->getNext();
+
+ if (previous != nullptr) {
+ previous->setNext(next);
+ }
+ else {
+ mContactPoints = next;
+ }
+
+ if (next != nullptr) {
+ next->setPrevious(previous);
+ }
+
+ // Delete the contact point
+ contactPoint->~ContactPoint();
+ mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
+
+ mNbContactPoints--;
+ assert(mNbContactPoints >= 0);
+}
+
// Add a contact point
void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
+ assert(contactPointInfo != nullptr);
+
// Create the new contact point
- ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
- ContactPoint(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
+ ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo);
// Add the new contact point into the manifold
contactPoint->setNext(mContactPoints);
+ contactPoint->setPrevious(nullptr);
+ if (mContactPoints != nullptr) {
+ mContactPoints->setPrevious(contactPoint);
+ }
mContactPoints = contactPoint;
mNbContactPoints++;
- assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
}
// Clear the obsolete contact points
-void ContactManifold::clearObseleteContactPoints() {
+void ContactManifold::clearObsoleteContactPoints() {
+ assert(mContactPoints != nullptr);
+
+ // For each contact point of the manifold
ContactPoint* contactPoint = mContactPoints;
- ContactPoint* previousContactPoint = nullptr;
while (contactPoint != nullptr) {
ContactPoint* nextContactPoint = contactPoint->getNext();
+ // If the contact point is obsolete
if (contactPoint->getIsObsolete()) {
- // Delete the contact point
- contactPoint->~ContactPoint();
- mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
-
- if (previousContactPoint != nullptr) {
- previousContactPoint->setNext(nextContactPoint);
- }
- else {
- mContactPoints = nextContactPoint;
- }
-
- mNbContactPoints--;
- }
- else {
- previousContactPoint = contactPoint;
+ // Remove the contact point
+ removeContactPoint(contactPoint);
}
contactPoint = nextContactPoint;
}
- assert(mNbContactPoints >= 0);
- assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
+ assert(mNbContactPoints > 0);
+ assert(mContactPoints != nullptr);
+}
+
+// Make sure we do not have too much contact points by keeping only the best
+// contact points of the manifold (with largest penetration depth)
+void ContactManifold::reduce() {
+
+ assert(mContactPoints != nullptr);
+
+ // Remove contact points while there is too much contact points
+ while (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
+ removeNonOptimalContactPoint();
+ }
+
+ assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD && mNbContactPoints > 0);
+ assert(mContactPoints != nullptr);
+}
+
+// Remove a contact point that is not optimal (with a small penetration depth)
+void ContactManifold::removeNonOptimalContactPoint() {
+
+ assert(mContactPoints != nullptr);
+ assert(mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD);
+
+ // Get the contact point with the minimum penetration depth among all points
+ ContactPoint* contactPoint = mContactPoints;
+ ContactPoint* minContactPoint = nullptr;
+ decimal minPenetrationDepth = DECIMAL_LARGEST;
+ while (contactPoint != nullptr) {
+
+ ContactPoint* nextContactPoint = contactPoint->getNext();
+
+ if (contactPoint->getPenetrationDepth() < minPenetrationDepth) {
+
+ minContactPoint = contactPoint;
+ minPenetrationDepth = contactPoint->getPenetrationDepth();
+ }
+
+ contactPoint = nextContactPoint;
+ }
+
+ assert(minContactPoint != nullptr);
+
+ // Remove the non optimal contact point
+ removeContactPoint(minContactPoint);
+
+ assert(mNbContactPoints > 0);
+ assert(mContactPoints != nullptr);
}
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index b24ed378..b5c25cb9 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -143,9 +143,6 @@ class ContactManifold {
/// True if the contact manifold is obsolete
bool mIsObsolete;
- /// True if the contact manifold is new (has just been added from potential contacts)
- bool mIsNew;
-
// -------------------- Methods -------------------- //
/// Return true if the contact manifold has already been added into an island
@@ -161,7 +158,7 @@ class ContactManifold {
void setIsObsolete(bool isObselete, bool setContactPoints);
/// Clear the obsolete contact points
- void clearObseleteContactPoints();
+ void clearObsoleteContactPoints();
/// Return the contact normal direction Id of the manifold
short getContactNormalId() const;
@@ -184,6 +181,15 @@ class ContactManifold {
/// Add a contact point
void addContactPoint(const ContactPointInfo* contactPointInfo);
+ /// Make sure we do not have too much contact points by keeping only the best ones
+ void reduce();
+
+ /// Remove a contact point that is not optimal (with a small penetration depth)
+ void removeNonOptimalContactPoint();
+
+ /// Remove a contact point
+ void removeContactPoint(ContactPoint* contactPoint);
+
/// Set the friction twist accumulated impulse
void setFrictionTwistImpulse(decimal frictionTwistImpulse);
@@ -208,12 +214,6 @@ class ContactManifold {
/// Return the friction twist accumulated impulse
decimal getFrictionTwistImpulse() const;
- /// Return true if the manifold has just been created
- bool getIsNew() const;
-
- /// Set the isNew attribute
- void setIsNew(bool isNew);
-
public:
// -------------------- Methods -------------------- //
@@ -417,18 +417,6 @@ inline short ContactManifold::getContactNormalId() const {
return mContactNormalId;
}
-
-// Return true if the manifold has just been created
-inline bool ContactManifold::getIsNew() const {
- return mIsNew;
-}
-
-// Set the isNew attribute
-inline void ContactManifold::setIsNew(bool isNew) {
- mIsNew = isNew;
-}
-
-
}
#endif
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index 06ae4963..9b8bf58b 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -60,43 +60,16 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa
}
else {
- bool addNewManifold = true;
-
- // If there are too much contact manifolds in the set
- if (mNbManifolds >= mNbMaxManifolds) {
-
- // We need to remove a manifold from the set.
- // We search for an old manifold with the smallest maximum penetration depth among its contact points.
- // If we do not find an old manifold, we select a new one that has the smallest contact penetration depth.
- ContactManifold* minDepthManifold = selectManifoldToRemove(contactManifoldInfo->getLargestPenetrationDepth());
-
- // If the manifold with the minimum penetration depth is an existing one
- if (minDepthManifold != nullptr) {
-
- // Remove the manifold
- removeManifold(minDepthManifold);
- }
- else {
-
- // The manifold we do not want to get is the new one. Therefore, we do not add it to the set
- addNewManifold = false;
- }
- }
-
- // If we need to add the new contact manifold
- if (addNewManifold) {
-
- // Create a new contact manifold
- createManifold(contactManifoldInfo);
- }
+ // Create a new contact manifold
+ createManifold(contactManifoldInfo);
}
}
// Update a previous similar manifold with a new one
void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) {
- assert(oldManifold != nullptr);
- assert(newManifold != nullptr);
+ assert(oldManifold != nullptr);
+ assert(newManifold != nullptr);
// For each contact point of the new manifold
ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo();
@@ -114,7 +87,7 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold,
if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) {
// Replace (update) the old contact point with the new one
- oldContactPoint->update(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
+ oldContactPoint->update(contactPointInfo);
isSimilarPointFound = true;
break;
}
@@ -136,53 +109,34 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold,
oldManifold->setIsObsolete(false, false);
}
-// Return the manifold to remove (because it is too old or has not the largest penetration depth)
-ContactManifold* ContactManifoldSet::selectManifoldToRemove(decimal penDepthNewManifold) const {
+// Remove a contact manifold that is the least optimal (smaller penetration depth)
+void ContactManifoldSet::removeNonOptimalManifold() {
- assert(mNbManifolds == mNbMaxManifolds);
+ assert(mNbManifolds > mNbMaxManifolds);
assert(mManifolds != nullptr);
// Look for a manifold that is not new and with the smallest contact penetration depth.
// At the same time, we also look for a new manifold with the smallest contact penetration depth
// in case no old manifold exists.
- ContactManifold* minDepthOldManifold = nullptr;
- ContactManifold* minDepthNewManifold = nullptr;
- decimal minDepthOld = DECIMAL_LARGEST;
- decimal minDepthNew = penDepthNewManifold;
+ ContactManifold* minDepthManifold = nullptr;
+ decimal minDepth = DECIMAL_LARGEST;
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
// Get the largest contact point penetration depth of the manifold
const decimal depth = manifold->getLargestContactDepth();
- // If it is a new manifold
- if (manifold->getIsNew()) {
-
- if (depth < minDepthNew) {
- minDepthNew = depth;
- minDepthNewManifold = manifold;
- }
- }
- else {
-
- if (depth < minDepthOld) {
- minDepthOld = depth;
- minDepthOldManifold = manifold;
- }
+ if (depth < minDepth) {
+ minDepth = depth;
+ minDepthManifold = manifold;
}
manifold = manifold->getNext();
}
- // If there was a contact manifold that was not new
- if (minDepthOldManifold != nullptr) {
-
- // Return the old manifold with the smallest penetration depth
- return minDepthOldManifold;
- }
-
- // Otherwise, we return the new manifold with the smallest penetration depth
- return minDepthNewManifold;
+ // Remove the non optimal manifold
+ assert(minDepthManifold != nullptr);
+ removeManifold(minDepthManifold);
}
// Return the contact manifold with a similar average normal.
@@ -262,7 +216,6 @@ void ContactManifoldSet::clear() {
// Create a new contact manifold and add it to the set
void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) {
- assert(mNbManifolds < mNbMaxManifolds);
ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator);
@@ -333,9 +286,26 @@ void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() {
else {
// Clear the obsolete contact points of the manifold
- manifold->clearObseleteContactPoints();
+ manifold->clearObsoleteContactPoints();
}
manifold = nextManifold;
}
}
+
+
+// Remove some contact manifolds and contact points if there are too many of them
+void ContactManifoldSet::reduce() {
+
+ // Remove non optimal contact manifold while there are too many manifolds in the set
+ while (mNbManifolds > mNbMaxManifolds) {
+ removeNonOptimalManifold();
+ }
+
+ // Reduce all the contact manifolds in case they have too many contact points
+ ContactManifold* manifold = mManifolds;
+ while (manifold != nullptr) {
+ manifold->reduce();
+ manifold = manifold->getNext();
+ }
+}
diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h
index ea832cdb..4b33eb8c 100644
--- a/src/collision/ContactManifoldSet.h
+++ b/src/collision/ContactManifoldSet.h
@@ -74,8 +74,8 @@ class ContactManifoldSet {
// Return the contact manifold with a similar average normal.
ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const;
- /// Return the manifold to remove (because it is too old or has not the largest penetration depth)
- ContactManifold* selectManifoldToRemove(decimal penDepthNewManifold) const;
+ /// Remove a contact manifold that is the least optimal (smaller penetration depth)
+ void removeNonOptimalManifold();
/// Update a previous similar manifold with a new one
void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold);
@@ -124,8 +124,8 @@ class ContactManifoldSet {
/// Clear the obsolete contact manifolds and contact points
void clearObsoleteManifoldsAndContactPoints();
- /// Reset the isNew status of all the manifolds
- void resetIsNewManifoldStatus();
+ // Remove some contact manifolds and contact points if there are too many of them
+ void reduce();
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
@@ -180,17 +180,6 @@ inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape
}
}
-
-// Reset the isNew status of all the manifolds
-inline void ContactManifoldSet::resetIsNewManifoldStatus() {
-
- ContactManifold* manifold = mManifolds;
- while (manifold != nullptr) {
- manifold->setIsNew(false);
- manifold = manifold->getNext();
- }
-}
-
}
#endif
diff --git a/src/collision/ContactPointInfo.h b/src/collision/ContactPointInfo.h
index 1ff8bfdd..6fbf9834 100644
--- a/src/collision/ContactPointInfo.h
+++ b/src/collision/ContactPointInfo.h
@@ -77,6 +77,8 @@ struct ContactPointInfo {
: normal(contactNormal), penetrationDepth(penDepth),
localPoint1(localPt1), localPoint2(localPt2), next(nullptr), isUsed(false) {
+ assert(contactNormal.lengthSquare() > decimal(0.8));
+ assert(penDepth > decimal(0.0));
}
/// Destructor
diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp
index 2cd951b4..b99d52f1 100644
--- a/src/constraint/ContactPoint.cpp
+++ b/src/constraint/ContactPoint.cpp
@@ -31,26 +31,22 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
-ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform,
- const Transform& body2Transform)
+ContactPoint::ContactPoint(const ContactPointInfo* contactInfo)
: mNormal(contactInfo->normal),
mPenetrationDepth(contactInfo->penetrationDepth),
mLocalPointOnBody1(contactInfo->localPoint1),
mLocalPointOnBody2(contactInfo->localPoint2),
- mIsRestingContact(false), mIsObsolete(false), mNext(nullptr) {
+ mIsRestingContact(false), mIsObsolete(false), mNext(nullptr), mPrevious(nullptr) {
assert(mPenetrationDepth > decimal(0.0));
-
- // Compute the world position of the contact points
- mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
- mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
+ assert(mNormal.lengthSquare() > decimal(0.8));
mIsObsolete = false;
}
// Update the contact point with a new one that is similar (very close)
/// The idea is to keep the cache impulse (for warm starting the contact solver)
-void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, const Transform& body2Transform) {
+void ContactPoint::update(const ContactPointInfo* contactInfo) {
assert(isSimilarWithContactPoint(contactInfo));
assert(contactInfo->penetrationDepth > decimal(0.0));
@@ -60,9 +56,5 @@ void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform&
mLocalPointOnBody1 = contactInfo->localPoint1;
mLocalPointOnBody2 = contactInfo->localPoint2;
- // Compute the world position of the contact points
- mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
- mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
-
mIsObsolete = false;
}
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index 288abdcc..e2a9a5e5 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -59,12 +59,6 @@ class ContactPoint {
/// Contact point on body 2 in local space of body 2
Vector3 mLocalPointOnBody2;
- /// Contact point on body 1 in world space
- Vector3 mWorldPointOnBody1;
-
- /// Contact point on body 2 in world space
- Vector3 mWorldPointOnBody2;
-
/// True if the contact is a resting contact (exists for more than one time step)
bool mIsRestingContact;
@@ -74,14 +68,16 @@ class ContactPoint {
/// True if the contact point is obsolete
bool mIsObsolete;
- /// Pointer to the next contact point in the linked-list
+ /// Pointer to the next contact point in the double linked-list
ContactPoint* mNext;
+ /// Pointer to the previous contact point in the double linked-list
+ ContactPoint* mPrevious;
+
// -------------------- Methods -------------------- //
/// Update the contact point with a new one that is similar (very close)
- void update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
- const Transform& body2Transform);
+ void update(const ContactPointInfo* contactInfo);
/// Return true if the contact point is similar (close enougth) to another given contact point
bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
@@ -99,6 +95,9 @@ class ContactPoint {
/// Set the next contact point in the linked list
void setNext(ContactPoint* next);
+ /// Set the previous contact point in the linked list
+ void setPrevious(ContactPoint* previous);
+
/// Return true if the contact point is obsolete
bool getIsObsolete() const;
@@ -107,8 +106,7 @@ class ContactPoint {
// -------------------- Methods -------------------- //
/// Constructor
- ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform,
- const Transform& body2Transform);
+ ContactPoint(const ContactPointInfo* contactInfo);
/// Destructor
~ContactPoint() = default;
@@ -128,18 +126,15 @@ class ContactPoint {
/// Return the contact local point on body 2
Vector3 getLocalPointOnBody2() const;
- /// Return the contact world point on body 1
- Vector3 getWorldPointOnBody1() const;
-
- /// Return the contact world point on body 2
- Vector3 getWorldPointOnBody2() const;
-
/// Return the cached penetration impulse
decimal getPenetrationImpulse() const;
/// Return true if the contact is a resting contact
bool getIsRestingContact() const;
+ /// Return the previous contact point in the linked list
+ inline ContactPoint* getPrevious() const;
+
/// Return the next contact point in the linked list
ContactPoint* getNext() const;
@@ -170,16 +165,6 @@ inline Vector3 ContactPoint::getLocalPointOnBody2() const {
return mLocalPointOnBody2;
}
-// Return the contact world point on body 1
-inline Vector3 ContactPoint::getWorldPointOnBody1() const {
- return mWorldPointOnBody1;
-}
-
-// Return the contact world point on body 2
-inline Vector3 ContactPoint::getWorldPointOnBody2() const {
- return mWorldPointOnBody2;
-}
-
// Return the cached penetration impulse
inline decimal ContactPoint::getPenetrationImpulse() const {
return mPenetrationImpulse;
@@ -226,6 +211,16 @@ inline void ContactPoint::setNext(ContactPoint* next) {
mNext = next;
}
+// Return the previous contact point in the linked list
+inline ContactPoint* ContactPoint::getPrevious() const {
+ return mPrevious;
+}
+
+// Set the previous contact point in the linked list
+inline void ContactPoint::setPrevious(ContactPoint* previous) {
+ mPrevious = previous;
+}
+
// Return the penetration depth of the contact
inline decimal ContactPoint::getPenetrationDepth() const {
return mPenetrationDepth;
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 94a82b14..819a1b76 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -120,6 +120,10 @@ void ContactSolver::initializeForIsland(Island* island) {
assert(body1 != nullptr);
assert(body2 != nullptr);
+ // Get the two contact shapes
+ const ProxyShape* shape1 = externalManifold->getShape1();
+ const ProxyShape* shape2 = externalManifold->getShape2();
+
// Get the position of the two bodies
const Vector3& x1 = body1->mCenterOfMassWorld;
const Vector3& x2 = body2->mCenterOfMassWorld;
@@ -149,11 +153,12 @@ void ContactSolver::initializeForIsland(Island* island) {
// For each contact point of the contact manifold
ContactPoint* externalContact = externalManifold->getContactPoints();
+ assert(externalContact != nullptr);
while (externalContact != nullptr) {
// Get the contact point on the two bodies
- Vector3 p1 = externalContact->getWorldPointOnBody1();
- Vector3 p2 = externalContact->getWorldPointOnBody2();
+ Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnBody1();
+ Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnBody2();
new (mContactPoints + mNbContactPoints) ContactPointSolver();
mContactPoints[mNbContactPoints].externalContact = externalContact;
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index 2725a8d5..47f13bfb 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -136,9 +136,6 @@ class OverlappingPair {
/// Return the last frame collision info
LastFrameCollisionInfo& getLastFrameCollisionInfo();
- /// Return the number of contacts in the cache
- uint getNbContactPoints() const;
-
/// Return the a reference to the contact manifold set
const ContactManifoldSet& getContactManifoldSet();
@@ -167,13 +164,13 @@ class OverlappingPair {
void reducePotentialContactManifolds();
/// Make the contact manifolds and contact points obsolete
- void makeContactsObselete();
+ void makeContactsObsolete();
/// Clear the obsolete contact manifold and contact points
void clearObsoleteManifoldsAndContactPoints();
- /// Reset the isNew status of all the manifolds
- void resetIsNewManifoldStatus();
+ /// Reduce the contact manifolds that have too many contact points
+ void reduceContactManifolds();
/// Return the pair of bodies index
static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
@@ -206,18 +203,13 @@ inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() {
return mLastFrameCollisionInfo;
}
-// Return the number of contact points in the contact manifold
-inline uint OverlappingPair::getNbContactPoints() const {
- return mContactManifoldSet.getTotalNbContactPoints();
-}
-
// Return the contact manifold
inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() {
return mContactManifoldSet;
}
// Make the contact manifolds and contact points obsolete
-inline void OverlappingPair::makeContactsObselete() {
+inline void OverlappingPair::makeContactsObsolete() {
mContactManifoldSet.makeContactsObsolete();
}
@@ -272,10 +264,9 @@ inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() {
mContactManifoldSet.clearObsoleteManifoldsAndContactPoints();
}
-
-// Reset the isNew status of all the manifolds
-inline void OverlappingPair::resetIsNewManifoldStatus() {
- mContactManifoldSet.resetIsNewManifoldStatus();
+// Reduce the contact manifolds that have too many contact points
+inline void OverlappingPair::reduceContactManifolds() {
+ mContactManifoldSet.reduce();
}
}
From 0250d8c4bdaca0c7eaf581e5f85839058d72055c Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 18 Oct 2017 19:35:20 +0200
Subject: [PATCH 092/133] Fix issue in SAT algorithm
---
src/collision/narrowphase/SAT/SATAlgorithm.cpp | 13 ++++++++++++-
1 file changed, 12 insertions(+), 1 deletion(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index a663791c..4411187b 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -731,7 +731,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1,
polyhedron2, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1,
minFaceIndex, narrowPhaseInfo, minPenetrationDepth);
- assert(contactsFound);
+
+ // There should be clipping points here. If it is not the case, it might be
+ // because of a numerical issue
+ if (!contactsFound) {
+
+ lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
+
+ // Return no collision
+ return false;
+ }
}
lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
From cdec7413c516ffe2671d0cbde45f5b60ef1d68e5 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 18 Oct 2017 21:26:53 +0200
Subject: [PATCH 093/133] Improve capsule resting on another shape stability
---
.../narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp | 5 +++--
src/mathematics/mathematics_functions.cpp | 2 +-
2 files changed, 4 insertions(+), 3 deletions(-)
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index 72ba2e06..d6de739a 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -86,14 +86,15 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
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);
+ Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA);
+ capsuleInnerSegmentDirection.normalize();
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)
+ if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection)
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
// Remove the previous contact point computed by GJK
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index fd813cdc..37335abe 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -80,7 +80,7 @@ bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& v
// 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);
+ return std::abs(vector1.dot(vector2)) < decimal(0.001);
}
// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC"
From c1295f1d7aa71f092dce03a5be2207d6f67bbe1f Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 19 Oct 2017 07:26:11 +0200
Subject: [PATCH 094/133] Remove contactNormaldId attribute and fix typo
---
src/collision/ContactManifold.cpp | 2 +-
src/collision/ContactManifold.h | 8 ----
src/collision/ContactManifoldInfo.cpp | 11 ++----
src/collision/ContactManifoldInfo.h | 13 +-----
src/collision/ContactManifoldSet.cpp | 57 +++++++--------------------
src/collision/ContactManifoldSet.h | 9 +----
src/configuration.h | 9 ++++-
src/engine/DynamicsWorld.cpp | 2 +-
src/engine/OverlappingPair.cpp | 13 +++---
9 files changed, 38 insertions(+), 86 deletions(-)
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index 55ce8ba3..c2d60422 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -31,7 +31,7 @@ using namespace reactphysics3d;
// Constructor
ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, Allocator& memoryAllocator)
- : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()),
+ : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) {
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index b5c25cb9..80a9f456 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -104,9 +104,6 @@ class ContactManifold {
/// Contact points in the manifold
ContactPoint* mContactPoints;
- /// Normal direction Id (Unique Id representing the normal direction)
- short int mContactNormalId;
-
/// Number of contacts in the cache
int8 mNbContactPoints;
@@ -412,11 +409,6 @@ inline void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoint
}
}
-// Return the contact normal direction Id of the manifold
-inline short ContactManifold::getContactNormalId() const {
- return mContactNormalId;
-}
-
}
#endif
diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp
index a6b8c6df..f22593ac 100644
--- a/src/collision/ContactManifoldInfo.cpp
+++ b/src/collision/ContactManifoldInfo.cpp
@@ -30,8 +30,9 @@ using namespace reactphysics3d;
// Constructor
ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator)
- : mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator),
- mContactNormalId(-1) {}
+ : mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator) {
+
+}
// Destructor
ContactManifoldInfo::~ContactManifoldInfo() {
@@ -41,13 +42,9 @@ ContactManifoldInfo::~ContactManifoldInfo() {
}
// Add a new contact point into the manifold
-void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId) {
+void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo) {
assert(contactPointInfo->penetrationDepth > decimal(0.0));
- assert(contactNormalId >= 0);
- assert(mContactNormalId == -1 || contactNormalId == mContactNormalId);
-
- mContactNormalId = contactNormalId;
// Add it into the linked list of contact points
contactPointInfo->next = mContactPointsList;
diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h
index 5851c375..4126f4c8 100644
--- a/src/collision/ContactManifoldInfo.h
+++ b/src/collision/ContactManifoldInfo.h
@@ -59,9 +59,6 @@ class ContactManifoldInfo {
/// Reference the the memory allocator where the contact point infos have been allocated
Allocator& mAllocator;
- /// Contact normal direction Id (Identify the contact normal direction of points in manifold)
- short mContactNormalId;
-
public:
// -------------------- Methods -------------------- //
@@ -79,7 +76,7 @@ class ContactManifoldInfo {
ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete;
/// Add a new contact point into the manifold
- void addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId);
+ void addContactPoint(ContactPointInfo* contactPointInfo);
/// Remove all the contact points
void reset();
@@ -93,9 +90,6 @@ class ContactManifoldInfo {
/// Return the pointer to the next manifold info in the linked-list
ContactManifoldInfo* getNext();
- /// Return the contact normal Id
- short getContactNormalId() const;
-
/// Reduce the number of contact points of the currently computed manifold
void reduce(const Transform& shape1ToWorldTransform);
@@ -114,11 +108,6 @@ inline ContactManifoldInfo* ContactManifoldInfo::getNext() {
return mNext;
}
-// Return the contact normal Id
-inline short ContactManifoldInfo::getContactNormalId() const {
- return mContactNormalId;
-}
-
}
#endif
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index 9b8bf58b..ce3e4492 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -50,7 +50,7 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa
assert(contactManifoldInfo->getFirstContactPointInfo() != nullptr);
// Try to find an existing contact manifold with similar contact normal
- ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo->getContactNormalId());
+ ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo);
// If a similar manifold has been found
if (similarManifold != nullptr) {
@@ -139,15 +139,25 @@ void ContactManifoldSet::removeNonOptimalManifold() {
removeManifold(minDepthManifold);
}
-// Return the contact manifold with a similar average normal.
-// If no manifold has close enough average normal, it returns nullptr
-ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
+// Return the contact manifold with a similar contact normal.
+// If no manifold has close enough contact normal, it returns nullptr
+ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const {
+
+ // Get the contact normal of the first point of the manifold
+ const ContactPointInfo* contactPoint = contactManifold->getFirstContactPointInfo();
+ assert(contactPoint != nullptr);
ContactManifold* manifold = mManifolds;
// Return the Id of the manifold with the same normal direction id (if exists)
while (manifold != nullptr) {
- if (normalDirectionId == manifold->getContactNormalId()) {
+
+ // Get the first contact point of the current manifold
+ const ContactPoint* point = manifold->getContactPoints();
+ assert(point != nullptr);
+
+ // If the contact normal of the two manifolds are close enough
+ if (contactPoint->normal.dot(point->getNormal()) >= COS_ANGLE_SIMILAR_CONTACT_MANIFOLD) {
return manifold;
}
@@ -157,43 +167,6 @@ ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(short int n
return nullptr;
}
-// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
-// Each face of the cube is divided into 4x4 buckets. This method maps the
-// normal vector into of the of the bucket and returns a unique Id for the bucket
-short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) {
-
- assert(normal.lengthSquare() > MACHINE_EPSILON);
-
- int faceNo;
- decimal u, v;
- decimal max = max3(std::fabs(normal.x), std::fabs(normal.y), std::fabs(normal.z));
- Vector3 normalScaled = normal / max;
-
- if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
- faceNo = normalScaled.x > 0 ? 0 : 1;
- u = normalScaled.y;
- v = normalScaled.z;
- }
- else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) {
- faceNo = normalScaled.y > 0 ? 2 : 3;
- u = normalScaled.x;
- v = normalScaled.z;
- }
- else {
- faceNo = normalScaled.z > 0 ? 4 : 5;
- u = normalScaled.x;
- v = normalScaled.y;
- }
-
- int indexU = std::floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
- int indexV = std::floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
- if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--;
- if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--;
-
- const int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS;
- return faceNo * 200 + indexU * nbSubDivInFace + indexV;
-}
-
// Clear the contact manifold set
void ContactManifoldSet::clear() {
diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h
index 4b33eb8c..13d27b70 100644
--- a/src/collision/ContactManifoldSet.h
+++ b/src/collision/ContactManifoldSet.h
@@ -71,8 +71,8 @@ class ContactManifoldSet {
/// Create a new contact manifold and add it to the set
void createManifold(const ContactManifoldInfo* manifoldInfo);
- // Return the contact manifold with a similar average normal.
- ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const;
+ // Return the contact manifold with a similar contact normal.
+ ContactManifold* selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const;
/// Remove a contact manifold that is the least optimal (smaller penetration depth)
void removeNonOptimalManifold();
@@ -126,11 +126,6 @@ class ContactManifoldSet {
// Remove some contact manifolds and contact points if there are too many of them
void reduce();
-
- // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
- // Each face of the cube is divided into 4x4 buckets. This method maps the
- // normal vector into of the of the bucket and returns a unique Id for the bucket
- static short int computeCubemapNormalId(const Vector3& normal);
};
// Return the first proxy shape
diff --git a/src/configuration.h b/src/configuration.h
index 444bd1db..779b45e2 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -99,8 +99,8 @@ constexpr decimal DEFAULT_BOUNCINESS = decimal(0.5);
/// Default rolling resistance
constexpr decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0);
-/// True if the spleeping technique is enabled
-constexpr bool SPLEEPING_ENABLED = true;
+/// True if the sleeping technique is enabled
+constexpr bool SLEEPING_ENABLED = true;
/// Distance threshold for two contact points for a valid persistent contact (in meters)
constexpr decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
@@ -143,6 +143,11 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
/// least one concave collision shape.
constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
+/// This is used to test if two contact manifold are similar (same contact normal) in order to
+/// merge them. If the cosine of the angle between the normals of the two manifold are larger
+/// than the value bellow, the manifold are considered to be similar.
+constexpr decimal COS_ANGLE_SIMILAR_CONTACT_MANIFOLD = decimal(0.95);
+
/// Size (in bytes) of the single frame allocator
constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 15728640; // 15 Mb
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 136d34f6..09712c3c 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -44,7 +44,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
- mIsSleepingEnabled(SPLEEPING_ENABLED), mGravity(gravity),
+ mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity),
mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr),
mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr),
mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr),
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index f3f85fa1..0055e683 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -54,21 +54,22 @@ void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo
ContactPointInfo* nextContactPoint = contactPoint->next;
- // Compute the contact normal id
- short contactNormalId = ContactManifoldSet::computeCubemapNormalId(contactPoint->normal);
-
// Look if the contact point correspond to an existing potential manifold
// (if the contact point normal is similar to the normal of an existing manifold)
ContactManifoldInfo* manifold = mPotentialContactManifolds;
bool similarManifoldFound = false;
while(manifold != nullptr) {
+ // Get the first contact point
+ const ContactPointInfo* point = manifold->getFirstContactPointInfo();
+ assert(point != nullptr);
+
// If we have found a corresponding manifold for the new contact point
// (a manifold with a similar contact normal direction)
- if (manifold->getContactNormalId() == contactNormalId) {
+ if (point->normal.dot(contactPoint->normal) >= COS_ANGLE_SIMILAR_CONTACT_MANIFOLD) {
// Add the contact point to the manifold
- manifold->addContactPoint(contactPoint, contactNormalId);
+ manifold->addContactPoint(contactPoint);
similarManifoldFound = true;
@@ -90,7 +91,7 @@ void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo
mPotentialContactManifolds = manifoldInfo;
// Add the contact point to the manifold
- manifoldInfo->addContactPoint(contactPoint, contactNormalId);
+ manifoldInfo->addContactPoint(contactPoint);
}
contactPoint = nextContactPoint;
From cdaa297a7800a10ecd14dc0ddf7fbc4561502887 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 19 Oct 2017 17:19:33 +0200
Subject: [PATCH 095/133] Remove unused variable
---
src/engine/OverlappingPair.h | 4 ----
1 file changed, 4 deletions(-)
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index 47f13bfb..ce22bb7e 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -56,10 +56,6 @@ struct LastFrameCollisionInfo {
/// True if we were using SAT algorithm to check for collision in the previous frame
bool wasUsingSAT;
- /// True if there was a narrow-phase collision
- /// in the previous frame
- bool wasCollidingLastFrame;
-
// ----- GJK Algorithm -----
/// Previous separating axis
From b1aad2b7c493e6b46cf28e138b65d522ae624333 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 19 Oct 2017 17:42:21 +0200
Subject: [PATCH 096/133] Refactor the testbed application
---
testbed/common/Box.h | 2 +-
testbed/common/Capsule.h | 3 +-
testbed/common/ConvexMesh.h | 3 +-
testbed/common/Dumbbell.h | 4 +-
testbed/common/PhysicsObject.h | 3 +
testbed/common/Sphere.h | 3 +-
.../CollisionDetectionScene.cpp | 60 ++----
.../CollisionDetectionScene.h | 10 +-
.../collisionshapes/CollisionShapesScene.cpp | 154 +-------------
.../collisionshapes/CollisionShapesScene.h | 9 +-
testbed/scenes/cubes/CubesScene.cpp | 35 +---
testbed/scenes/cubes/CubesScene.h | 7 -
.../scenes/heightfield/HeightFieldScene.cpp | 191 +++++++++++++-----
testbed/scenes/heightfield/HeightFieldScene.h | 47 ++++-
testbed/scenes/joints/JointsScene.cpp | 27 +--
testbed/scenes/joints/JointsScene.h | 3 -
testbed/scenes/raycast/RaycastScene.cpp | 33 +--
testbed/scenes/raycast/RaycastScene.h | 6 +-
testbed/src/SceneDemo.cpp | 32 ++-
testbed/src/SceneDemo.h | 10 +-
testbed/src/TestbedApplication.cpp | 2 +-
21 files changed, 281 insertions(+), 363 deletions(-)
mode change 100644 => 100755 testbed/scenes/cubes/CubesScene.cpp
mode change 100644 => 100755 testbed/scenes/cubes/CubesScene.h
diff --git a/testbed/common/Box.h b/testbed/common/Box.h
index a1f05d5e..bd7a6024 100644
--- a/testbed/common/Box.h
+++ b/testbed/common/Box.h
@@ -86,7 +86,7 @@ class Box : public PhysicsObject {
~Box();
/// Render the cube at the correct position and with the correct orientation
- void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
+ virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/Capsule.h b/testbed/common/Capsule.h
index f117984f..5c99120b 100644
--- a/testbed/common/Capsule.h
+++ b/testbed/common/Capsule.h
@@ -94,8 +94,7 @@ class Capsule : public PhysicsObject {
~Capsule();
/// Render the sphere at the correct position and with the correct orientation
- void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
+ virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h
index e2ba5836..1b55611b 100644
--- a/testbed/common/ConvexMesh.h
+++ b/testbed/common/ConvexMesh.h
@@ -90,8 +90,7 @@ class ConvexMesh : public PhysicsObject {
~ConvexMesh();
/// Render the mesh at the correct position and with the correct orientation
- void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
+ virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h
index ea2d3d88..aa6bded1 100644
--- a/testbed/common/Dumbbell.h
+++ b/testbed/common/Dumbbell.h
@@ -89,13 +89,11 @@ class Dumbbell : public PhysicsObject {
Dumbbell(const openglframework::Vector3& position, rp3d::CollisionWorld* world,
const std::string& meshFolderPath);
-
/// Destructor
~Dumbbell();
/// Render the sphere at the correct position and with the correct orientation
- void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
+ virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/common/PhysicsObject.h b/testbed/common/PhysicsObject.h
index 0da224c3..7fcc612d 100644
--- a/testbed/common/PhysicsObject.h
+++ b/testbed/common/PhysicsObject.h
@@ -62,6 +62,9 @@ class PhysicsObject : public openglframework::Mesh {
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor)=0;
+ /// Render the sphere at the correct position and with the correct orientation
+ virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe)=0;
+
/// Set the color of the box
void setColor(const openglframework::Color& color);
diff --git a/testbed/common/Sphere.h b/testbed/common/Sphere.h
index 62f1809e..000cc776 100644
--- a/testbed/common/Sphere.h
+++ b/testbed/common/Sphere.h
@@ -90,8 +90,7 @@ class Sphere : public PhysicsObject {
~Sphere();
/// Render the sphere at the correct position and with the correct orientation
- void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
+ void virtual render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index 7606a61e..3b186943 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -59,6 +59,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Set the color
mSphere1->setColor(mGreyColorDemo);
mSphere1->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mSphere1);
// ---------- Sphere 2 ---------- //
openglframework::Vector3 position2(12, 8, 0);
@@ -70,6 +71,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Set the color
mSphere2->setColor(mGreyColorDemo);
mSphere2->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mSphere2);
// ---------- Capsule 1 ---------- //
openglframework::Vector3 position3(-6, 7, 0);
@@ -81,6 +83,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Set the color
mCapsule1->setColor(mGreyColorDemo);
mCapsule1->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mCapsule1);
// ---------- Capsule 2 ---------- //
openglframework::Vector3 position4(11, -8, 0);
@@ -92,6 +95,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Set the color
mCapsule2->setColor(mGreyColorDemo);
mCapsule2->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mCapsule2);
// ---------- Box 1 ---------- //
openglframework::Vector3 position5(-4, -7, 0);
@@ -103,6 +107,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Set the color
mBox1->setColor(mGreyColorDemo);
mBox1->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mBox1);
// ---------- Box 2 ---------- //
openglframework::Vector3 position6(0, 8, 0);
@@ -114,6 +119,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Set the color
mBox2->setColor(mGreyColorDemo);
mBox2->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mBox2);
// ---------- Convex Mesh ---------- //
openglframework::Vector3 position7(-5, 0, 0);
@@ -125,9 +131,10 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Set the color
mConvexMesh->setColor(mGreyColorDemo);
mConvexMesh->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mConvexMesh);
// ---------- Concave Mesh ---------- //
- openglframework::Vector3 position8(0, 0, 0);
+ openglframework::Vector3 position8(0, 100, 0);
// Create a convex mesh and a corresponding collision body in the dynamics world
mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj");
@@ -136,16 +143,18 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Set the color
mConcaveMesh->setColor(mGreyColorDemo);
mConcaveMesh->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mConcaveMesh);
// ---------- Heightfield ---------- //
- //openglframework::Vector3 position9(0, 0, 0);
+ openglframework::Vector3 position9(0, -12, 0);
// Create a convex mesh and a corresponding collision body in the dynamics world
- //mHeightField = new HeightField(position9, mCollisionWorld);
+ mHeightField = new HeightField(position9, mCollisionWorld);
// Set the color
- //mHeightField->setColor(mGreyColorDemo);
- //mHeightField->setSleepingColor(mRedColorDemo);
+ mHeightField->setColor(mGreyColorDemo);
+ mHeightField->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mHeightField);
mAllShapes[mSelectedShapeIndex]->setColor(mBlueColorDemo);
}
@@ -187,6 +196,9 @@ CollisionDetectionScene::~CollisionDetectionScene() {
mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody());
delete mConcaveMesh;
+ mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody());
+ delete mHeightField;
+
/*
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody());
@@ -210,11 +222,7 @@ CollisionDetectionScene::~CollisionDetectionScene() {
// Destroy the dumbbell
delete mDumbbell;
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody());
-
- // Destroy the convex mesh
- delete mHeightField;
+
*/
mContactManager.resetPoints();
@@ -226,12 +234,6 @@ CollisionDetectionScene::~CollisionDetectionScene() {
delete mCollisionWorld;
}
-// Update the physics world (take a simulation step)
-void CollisionDetectionScene::updatePhysics() {
-
-
-}
-
// Take a step for the simulation
void CollisionDetectionScene::update() {
@@ -242,32 +244,6 @@ void CollisionDetectionScene::update() {
SceneDemo::update();
}
-// Render the scene
-void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
-
- // Render the shapes
- if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mBox1->getCollisionBody()->isActive()) mBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mBox2->getCollisionBody()->isActive()) mBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
-
- /*
- if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix);
- if (mCone->getCollisionBody()->isActive()) mCone->render(shader, worldToCameraMatrix);
- if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(shader, worldToCameraMatrix);
- if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix);
- if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix);
- if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix);
- */
-
- shader.unbind();
-}
-
void CollisionDetectionScene::selectNextShape() {
int previousIndex = mSelectedShapeIndex;
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index 8e94eaf5..8473ae73 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -148,7 +148,7 @@ class CollisionDetectionScene : public SceneDemo {
ConvexMesh* mConvexMesh;
//Dumbbell* mDumbbell;
ConcaveMesh* mConcaveMesh;
- //HeightField* mHeightField;
+ HeightField* mHeightField;
std::vector<PhysicsObject*> mAllShapes;
@@ -170,17 +170,9 @@ class CollisionDetectionScene : public SceneDemo {
/// Destructor
virtual ~CollisionDetectionScene() override;
- /// Update the physics world (take a simulation step)
- /// Can be called several times per frame
- virtual void updatePhysics() override;
-
/// Take a step for the simulation
virtual void update() override;
- /// Render the scene in a single pass
- virtual void renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) override;
-
/// Reset the scene
virtual void reset() override;
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
index 746942ff..e63f39d4 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
@@ -71,6 +71,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// Add the mesh the list of dumbbells in the scene
mDumbbells.push_back(dumbbell);
+ mPhysicsObjects.push_back(dumbbell);
}
// Create all the boxes of the scene
@@ -95,6 +96,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// Add the sphere the list of sphere in the scene
mBoxes.push_back(box);
+ mPhysicsObjects.push_back(box);
}
// Create all the spheres of the scene
@@ -123,6 +125,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// Add the sphere the list of sphere in the scene
mSpheres.push_back(sphere);
+ mPhysicsObjects.push_back(sphere);
}
// Create all the capsules of the scene
@@ -150,6 +153,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// Add the cylinder the list of sphere in the scene
mCapsules.push_back(capsule);
+ mPhysicsObjects.push_back(capsule);
}
// Create all the convex meshes of the scene
@@ -174,12 +178,14 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// Add the mesh the list of sphere in the scene
mConvexMeshes.push_back(mesh);
+ mPhysicsObjects.push_back(mesh);
}
// ---------- Create the floor ---------
openglframework::Vector3 floorPosition(0, 0, 0);
mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath);
+ mPhysicsObjects.push_back(mFloor);
// Set the box color
mFloor->setColor(mGreyColorDemo);
@@ -230,68 +236,16 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// Destructor
CollisionShapesScene::~CollisionShapesScene() {
- // Destroy all the boxes of the scene
- for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
+ // Destroy all the physics objects of the scene
+ for (std::vector<PhysicsObject*>::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
// Destroy the corresponding rigid body from the dynamics world
mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
- // Destroy the box
+ // Destroy the object
delete (*it);
}
- // Destroy all the sphere of the scene
- for (std::vector<Sphere*>::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) {
-
- // Destroy the corresponding rigid body from the dynamics world
- mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
-
- // Destroy the sphere
- delete (*it);
- }
-
- // Destroy all the capsules of the scene
- for (std::vector<Capsule*>::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) {
-
- // Destroy the corresponding rigid body from the dynamics world
- mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
-
- // Destroy the sphere
- delete (*it);
- }
-
- // Destroy all the convex meshes of the scene
- for (std::vector<ConvexMesh*>::iterator it = mConvexMeshes.begin();
- it != mConvexMeshes.end(); ++it) {
-
- // Destroy the corresponding rigid body from the dynamics world
- mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
-
- // Destroy the convex mesh
- delete (*it);
- }
-
- // Destroy all the dumbbell of the scene
- for (std::vector<Dumbbell*>::iterator it = mDumbbells.begin();
- it != mDumbbells.end(); ++it) {
-
- // Destroy the corresponding rigid body from the dynamics world
- mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
-
- // Destroy the convex mesh
- delete (*it);
- }
-
- // Destroy the rigid body of the floor
- mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody());
- //mDynamicsWorld->destroyRigidBody(mConcaveMesh->getRigidBody());
-
- // Destroy the floor
- delete mFloor;
-
- // Destroy the convex mesh
- //delete mConcaveMesh;
-
// Destroy the dynamics world
delete mDynamicsWorld;
}
@@ -315,96 +269,6 @@ void CollisionShapesScene::updatePhysics() {
mDynamicsWorld->update(mEngineSettings.timeStep);
}
-// Take a step for the simulation
-void CollisionShapesScene::update() {
-
- SceneDemo::update();
-
- // Update the position and orientation of the boxes
- for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
-
- // Update the transform used for the rendering
- (*it)->updateTransform(mInterpolationFactor);
- }
-
- // Update the position and orientation of the sphere
- for (std::vector<Sphere*>::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) {
-
- // Update the transform used for the rendering
- (*it)->updateTransform(mInterpolationFactor);
- }
-
- // Update the position and orientation of the capsules
- for (std::vector<Capsule*>::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) {
-
- // Update the transform used for the rendering
- (*it)->updateTransform(mInterpolationFactor);
- }
-
- // Update the position and orientation of the convex meshes
- for (std::vector<ConvexMesh*>::iterator it = mConvexMeshes.begin();
- it != mConvexMeshes.end(); ++it) {
-
- // Update the transform used for the rendering
- (*it)->updateTransform(mInterpolationFactor);
- }
-
- // Update the position and orientation of the dumbbells
- for (std::vector<Dumbbell*>::iterator it = mDumbbells.begin();
- it != mDumbbells.end(); ++it) {
-
- // Update the transform used for the rendering
- (*it)->updateTransform(mInterpolationFactor);
- }
-
- //mConcaveMesh->updateTransform(mInterpolationFactor);
-
- mFloor->updateTransform(mInterpolationFactor);
-}
-
-// Render the scene
-void CollisionShapesScene::renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
-
- // Bind the shader
- shader.bind();
-
- // Render all the boxes of the scene
- for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- }
-
- // Render all the sphere of the scene
- for (std::vector<Sphere*>::iterator it = mSpheres.begin(); it != mSpheres.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- }
-
- // Render all the capsules of the scene
- for (std::vector<Capsule*>::iterator it = mCapsules.begin(); it != mCapsules.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- }
-
- // Render all the convex meshes of the scene
- for (std::vector<ConvexMesh*>::iterator it = mConvexMeshes.begin();
- it != mConvexMeshes.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- }
-
- // Render all the dumbbells of the scene
- for (std::vector<Dumbbell*>::iterator it = mDumbbells.begin();
- it != mDumbbells.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- }
-
- // Render the floor
- mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
-
- //mConcaveMesh->render(shader, worldToCameraMatrix);
-
- // Unbind the shader
- shader.unbind();
-}
-
/// Reset the scene
void CollisionShapesScene::reset() {
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.h b/testbed/scenes/collisionshapes/CollisionShapesScene.h
index b253acef..70e28755 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.h
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.h
@@ -71,7 +71,7 @@ class CollisionShapesScene : public SceneDemo {
// -------------------- Attributes -------------------- //
- /// All the spheres of the scene
+ /// All the boxes of the scene
std::vector<Box*> mBoxes;
std::vector<Sphere*> mSpheres;
@@ -104,13 +104,6 @@ class CollisionShapesScene : public SceneDemo {
/// Can be called several times per frame
virtual void updatePhysics() override;
- /// Take a step for the simulation
- virtual void update() override;
-
- /// Render the scene in a single pass
- virtual void renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) override;
-
/// Reset the scene
virtual void reset() override;
diff --git a/testbed/scenes/cubes/CubesScene.cpp b/testbed/scenes/cubes/CubesScene.cpp
old mode 100644
new mode 100755
index 81929e58..3ddb1d61
--- a/testbed/scenes/cubes/CubesScene.cpp
+++ b/testbed/scenes/cubes/CubesScene.cpp
@@ -82,6 +82,7 @@ CubesScene::CubesScene(const std::string& name)
// The floor must be a static rigid body
mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC);
+ mPhysicsObjects.push_back(mFloor);
// Change the material properties of the floor rigid body
//rp3d::Material& material = mFloor->getRigidBody()->getMaterial();
@@ -102,6 +103,7 @@ CubesScene::CubesScene(const std::string& name)
// Add the box the list of box in the scene
mBoxes.push_back(cube);
+ mPhysicsObjects.push_back(cube);
// Get the physics engine parameters
mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled();
@@ -157,39 +159,6 @@ void CubesScene::updatePhysics() {
mDynamicsWorld->update(mEngineSettings.timeStep);
}
-// Update the scene
-void CubesScene::update() {
-
- SceneDemo::update();
-
- // Update the position and orientation of the boxes
- for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
-
- // Update the transform used for the rendering
- (*it)->updateTransform(mInterpolationFactor);
- }
-
- mFloor->updateTransform(mInterpolationFactor);
-}
-
-// Render the scene in a single pass
-void CubesScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
-
- // Bind the shader
- shader.bind();
-
- // Render all the cubes of the scene
- for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- }
-
- // Render the floor
- mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
-
- // Unbind the shader
- shader.unbind();
-}
-
// Reset the scene
void CubesScene::reset() {
diff --git a/testbed/scenes/cubes/CubesScene.h b/testbed/scenes/cubes/CubesScene.h
old mode 100644
new mode 100755
index 4ad57f77..aa4b7073
--- a/testbed/scenes/cubes/CubesScene.h
+++ b/testbed/scenes/cubes/CubesScene.h
@@ -72,13 +72,6 @@ class CubesScene : public SceneDemo {
/// Can be called several times per frame
virtual void updatePhysics() override;
- /// Update the scene (take a simulation step)
- virtual void update() override;
-
- /// Render the scene in a single pass
- virtual void renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) override;
-
/// Reset the scene
virtual void reset() override;
diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp
index 3e4061c9..329712e9 100644
--- a/testbed/scenes/heightfield/HeightFieldScene.cpp
+++ b/testbed/scenes/heightfield/HeightFieldScene.cpp
@@ -33,6 +33,8 @@ using namespace heightfieldscene;
// Constructor
HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SCENE_RADIUS) {
+ std::string meshFolderPath("meshes/");
+
// Compute the radius and the center of the scene
openglframework::Vector3 center(0, 5, 0);
@@ -45,25 +47,138 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
// Create the dynamics world for the physics simulation
mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
- // ---------- Create the boxes ----------- //
+ float radius = 3.0f;
- // For each box
- for (int i=0; i<NB_BOXES; i++) {
+ for (int i = 0; i<NB_COMPOUND_SHAPES; i++) {
- // Position
- openglframework::Vector3 position(15, 10 + 6 * i, 0);
+ // Position
+ float angle = i * 30.0f;
+ openglframework::Vector3 position(radius * cos(angle),
+ 100 + i * (DUMBBELL_HEIGHT + 0.3f),
+ radius * sin(angle));
- // Create a box and a corresponding rigid in the dynamics world
- mBoxes[i] = new Box(Vector3(3, 3, 3), position, 80.1, mDynamicsWorld, mMeshFolderPath);
+ // Create a convex mesh and a corresponding rigid in the dynamics world
+ Dumbbell* dumbbell = new Dumbbell(position, mDynamicsWorld, meshFolderPath);
- // Set the box color
- mBoxes[i]->setColor(mDemoColors[2]);
- mBoxes[i]->setSleepingColor(mRedColorDemo);
+ // Set the box color
+ dumbbell->setColor(mDemoColors[i % mNbDemoColors]);
+ dumbbell->setSleepingColor(mRedColorDemo);
- // Change the material properties of the rigid body
- rp3d::Material& boxMaterial = mBoxes[i]->getRigidBody()->getMaterial();
- boxMaterial.setBounciness(rp3d::decimal(0.2));
- }
+ // Change the material properties of the rigid body
+ rp3d::Material& material = dumbbell->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.2));
+
+ // Add the mesh the list of dumbbells in the scene
+ mDumbbells.push_back(dumbbell);
+ mPhysicsObjects.push_back(dumbbell);
+ }
+
+ // Create all the boxes of the scene
+ for (int i = 0; i<NB_BOXES; i++) {
+
+ // Position
+ float angle = i * 30.0f;
+ openglframework::Vector3 position(radius * cos(angle),
+ 60 + i * (BOX_SIZE.y + 0.8f),
+ radius * sin(angle));
+
+ // Create a sphere and a corresponding rigid in the dynamics world
+ Box* box = new Box(BOX_SIZE, position, BOX_MASS, mDynamicsWorld, mMeshFolderPath);
+
+ // Set the box color
+ box->setColor(mDemoColors[i % mNbDemoColors]);
+ box->setSleepingColor(mRedColorDemo);
+
+ // Change the material properties of the rigid body
+ rp3d::Material& material = box->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.2));
+
+ // Add the sphere the list of sphere in the scene
+ mBoxes.push_back(box);
+ mPhysicsObjects.push_back(box);
+ }
+
+ // Create all the spheres of the scene
+ for (int i = 0; i<NB_SPHERES; i++) {
+
+ // Position
+ float angle = i * 35.0f;
+ openglframework::Vector3 position(radius * cos(angle),
+ 50 + i * (SPHERE_RADIUS + 0.8f),
+ radius * sin(angle));
+
+ // Create a sphere and a corresponding rigid in the dynamics world
+ Sphere* sphere = new Sphere(SPHERE_RADIUS, position, BOX_MASS, mDynamicsWorld,
+ meshFolderPath);
+
+ // Add some rolling resistance
+ sphere->getRigidBody()->getMaterial().setRollingResistance(0.08);
+
+ // Set the box color
+ sphere->setColor(mDemoColors[i % mNbDemoColors]);
+ sphere->setSleepingColor(mRedColorDemo);
+
+ // Change the material properties of the rigid body
+ rp3d::Material& material = sphere->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.2));
+
+ // Add the sphere the list of sphere in the scene
+ mSpheres.push_back(sphere);
+ mPhysicsObjects.push_back(sphere);
+ }
+
+ // Create all the capsules of the scene
+ for (int i = 0; i<NB_CAPSULES; i++) {
+
+ // Position
+ float angle = i * 45.0f;
+ openglframework::Vector3 position(radius * cos(angle),
+ 15 + i * (CAPSULE_HEIGHT + 0.3f),
+ radius * sin(angle));
+
+ // Create a cylinder and a corresponding rigid in the dynamics world
+ Capsule* capsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position,
+ CAPSULE_MASS, mDynamicsWorld, meshFolderPath);
+
+ capsule->getRigidBody()->getMaterial().setRollingResistance(0.08);
+
+ // Set the box color
+ capsule->setColor(mDemoColors[i % mNbDemoColors]);
+ capsule->setSleepingColor(mRedColorDemo);
+
+ // Change the material properties of the rigid body
+ rp3d::Material& material = capsule->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.2));
+
+ // Add the cylinder the list of sphere in the scene
+ mCapsules.push_back(capsule);
+ mPhysicsObjects.push_back(capsule);
+ }
+
+ // Create all the convex meshes of the scene
+ for (int i = 0; i<NB_MESHES; i++) {
+
+ // Position
+ float angle = i * 30.0f;
+ openglframework::Vector3 position(radius * cos(angle),
+ 5 + i * (CAPSULE_HEIGHT + 0.3f),
+ radius * sin(angle));
+
+ // Create a convex mesh and a corresponding rigid in the dynamics world
+ ConvexMesh* mesh = new ConvexMesh(position, MESH_MASS, mDynamicsWorld, meshFolderPath + "convexmesh.obj");
+
+ // Set the box color
+ mesh->setColor(mDemoColors[i % mNbDemoColors]);
+ mesh->setSleepingColor(mRedColorDemo);
+
+ // Change the material properties of the rigid body
+ rp3d::Material& material = mesh->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.2));
+
+ // Add the mesh the list of sphere in the scene
+ mConvexMeshes.push_back(mesh);
+ mPhysicsObjects.push_back(mesh);
+ }
// ---------- Create the height field ---------- //
@@ -77,6 +192,8 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
// Set the mesh as beeing static
mHeightField->getRigidBody()->setType(rp3d::BodyType::STATIC);
+ mPhysicsObjects.push_back(mHeightField);
+
// Set the color
mHeightField->setColor(mGreyColorDemo);
mHeightField->setSleepingColor(mGreyColorDemo);
@@ -101,18 +218,15 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
// Destructor
HeightFieldScene::~HeightFieldScene() {
- // Destroy the corresponding rigid body from the dynamics world
- for (int i=0; i<NB_BOXES; i++) {
- mDynamicsWorld->destroyRigidBody(mBoxes[i]->getRigidBody());
- }
- mDynamicsWorld->destroyRigidBody(mHeightField->getRigidBody());
+ // Destroy all the physics objects of the scene
+ for (std::vector<PhysicsObject*>::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
- for (int i=0; i<NB_BOXES; i++) {
- delete mBoxes[i];
- }
+ // Destroy the corresponding rigid body from the dynamics world
+ mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
- // Destroy the convex mesh
- delete mHeightField;
+ // Destroy the object
+ delete (*it);
+ }
// Destroy the dynamics world
delete mDynamicsWorld;
@@ -137,35 +251,6 @@ void HeightFieldScene::updatePhysics() {
mDynamicsWorld->update(mEngineSettings.timeStep);
}
-// Update the scene
-void HeightFieldScene::update() {
-
- SceneDemo::update();
-
- // Update the transform used for the rendering
- mHeightField->updateTransform(mInterpolationFactor);
-
- for (int i=0; i<NB_BOXES; i++) {
- mBoxes[i]->updateTransform(mInterpolationFactor);
- }
-}
-
-// Render the scene in a single pass
-void HeightFieldScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
-
- // Bind the shader
- shader.bind();
-
- mHeightField->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
-
- for (int i=0; i<NB_BOXES; i++) {
- mBoxes[i]->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- }
-
- // Unbind the shader
- shader.unbind();
-}
-
// Reset the scene
void HeightFieldScene::reset() {
diff --git a/testbed/scenes/heightfield/HeightFieldScene.h b/testbed/scenes/heightfield/HeightFieldScene.h
index 7eb9527d..7306ae2c 100644
--- a/testbed/scenes/heightfield/HeightFieldScene.h
+++ b/testbed/scenes/heightfield/HeightFieldScene.h
@@ -30,6 +30,10 @@
#include "openglframework.h"
#include "reactphysics3d.h"
#include "Box.h"
+#include "Sphere.h"
+#include "ConvexMesh.h"
+#include "Capsule.h"
+#include "Dumbbell.h"
#include "SceneDemo.h"
#include "HeightField.h"
@@ -37,17 +41,47 @@ namespace heightfieldscene {
// Constants
const float SCENE_RADIUS = 50.0f;
+static const int NB_BOXES = 10;
+static const int NB_SPHERES = 5;
+static const int NB_CAPSULES = 5;
+static const int NB_MESHES = 3;
+static const int NB_COMPOUND_SHAPES = 3;
+const openglframework::Vector3 BOX_SIZE(2, 2, 2);
+const float SPHERE_RADIUS = 1.5f;
+const float CONE_RADIUS = 2.0f;
+const float CONE_HEIGHT = 3.0f;
+const float CYLINDER_RADIUS = 1.0f;
+const float CYLINDER_HEIGHT = 5.0f;
+const float CAPSULE_RADIUS = 1.0f;
+const float CAPSULE_HEIGHT = 1.0f;
+const float DUMBBELL_HEIGHT = 1.0f;
+const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters
+const float BOX_MASS = 1.0f;
+const float CONE_MASS = 1.0f;
+const float CYLINDER_MASS = 1.0f;
+const float CAPSULE_MASS = 1.0f;
+const float MESH_MASS = 1.0f;
+const float FLOOR_MASS = 100.0f;
// Class HeightFieldScene
class HeightFieldScene : public SceneDemo {
- static const int NB_BOXES = 10;
-
protected :
// -------------------- Attributes -------------------- //
- Box* mBoxes[NB_BOXES];
+ /// All the boxes of the scene
+ std::vector<Box*> mBoxes;
+
+ std::vector<Sphere*> mSpheres;
+
+ std::vector<Capsule*> mCapsules;
+
+ /// All the convex meshes of the scene
+ std::vector<ConvexMesh*> mConvexMeshes;
+
+ /// All the dumbbell of the scene
+ std::vector<Dumbbell*> mDumbbells;
/// Height field
HeightField* mHeightField;
@@ -69,13 +103,6 @@ class HeightFieldScene : public SceneDemo {
/// Can be called several times per frame
virtual void updatePhysics() override;
- /// Update the scene (take a simulation step)
- virtual void update() override;
-
- /// Render the scene in a single pass
- virtual void renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) override ;
-
/// Reset the scene
virtual void reset() override ;
diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp
index 23bcdb93..282f7e51 100644
--- a/testbed/scenes/joints/JointsScene.cpp
+++ b/testbed/scenes/joints/JointsScene.cpp
@@ -136,25 +136,6 @@ void JointsScene::updatePhysics() {
mDynamicsWorld->update(mEngineSettings.timeStep);
}
-// Take a step for the simulation
-void JointsScene::update() {
-
- SceneDemo::update();
-
- // Update the position and orientation of the boxes
- mSliderJointBottomBox->updateTransform(mInterpolationFactor);
- mSliderJointTopBox->updateTransform(mInterpolationFactor);
- mPropellerBox->updateTransform(mInterpolationFactor);
- mFixedJointBox1->updateTransform(mInterpolationFactor);
- mFixedJointBox2->updateTransform(mInterpolationFactor);
- for (int i=0; i<NB_BALLSOCKETJOINT_BOXES; i++) {
- mBallAndSocketJointChainBoxes[i]->updateTransform(mInterpolationFactor);
- }
-
- // Update the position and orientation of the floor
- mFloor->updateTransform(mInterpolationFactor);
-}
-
// Render the scene
void JointsScene::renderSinglePass(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix) {
@@ -281,6 +262,8 @@ void JointsScene::createBallAndSocketJoints() {
rp3d::Material& material = mBallAndSocketJointChainBoxes[i]->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.4));
+ mPhysicsObjects.push_back(mBallAndSocketJointChainBoxes[i]);
+
positionBox.y -= boxDimension.y + 0.5f;
}
@@ -324,6 +307,7 @@ void JointsScene::createSliderJoint() {
// Change the material properties of the rigid body
rp3d::Material& material1 = mSliderJointBottomBox->getRigidBody()->getMaterial();
material1.setBounciness(0.4f);
+ mPhysicsObjects.push_back(mSliderJointBottomBox);
// --------------- Create the second box --------------- //
@@ -341,6 +325,7 @@ void JointsScene::createSliderJoint() {
// Change the material properties of the rigid body
rp3d::Material& material2 = mSliderJointTopBox->getRigidBody()->getMaterial();
material2.setBounciness(0.4f);
+ mPhysicsObjects.push_back(mSliderJointTopBox);
// --------------- Create the joint --------------- //
@@ -381,6 +366,7 @@ void JointsScene::createPropellerHingeJoint() {
// Change the material properties of the rigid body
rp3d::Material& material = mPropellerBox->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.4));
+ mPhysicsObjects.push_back(mPropellerBox);
// --------------- Create the Hinge joint --------------- //
@@ -420,6 +406,7 @@ void JointsScene::createFixedJoints() {
// Change the material properties of the rigid body
rp3d::Material& material1 = mFixedJointBox1->getRigidBody()->getMaterial();
material1.setBounciness(rp3d::decimal(0.4));
+ mPhysicsObjects.push_back(mFixedJointBox1);
// --------------- Create the second box --------------- //
@@ -436,6 +423,7 @@ void JointsScene::createFixedJoints() {
// Change the material properties of the rigid body
rp3d::Material& material2 = mFixedJointBox2->getRigidBody()->getMaterial();
material2.setBounciness(rp3d::decimal(0.4));
+ mPhysicsObjects.push_back(mFixedJointBox2);
// --------------- Create the first fixed joint --------------- //
@@ -478,4 +466,5 @@ void JointsScene::createFloor() {
// Change the material properties of the rigid body
rp3d::Material& material = mFloor->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.3));
+ mPhysicsObjects.push_back(mFloor);
}
diff --git a/testbed/scenes/joints/JointsScene.h b/testbed/scenes/joints/JointsScene.h
index a9474393..16ccfe8c 100644
--- a/testbed/scenes/joints/JointsScene.h
+++ b/testbed/scenes/joints/JointsScene.h
@@ -126,9 +126,6 @@ class JointsScene : public SceneDemo {
/// Can be called several times per frame
virtual void updatePhysics() override;
- /// Take a step for the simulation
- virtual void update() override;
-
/// Render the scene in a single pass
virtual void renderSinglePass(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix) override;
diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp
index d420073f..2ded75d6 100644
--- a/testbed/scenes/raycast/RaycastScene.cpp
+++ b/testbed/scenes/raycast/RaycastScene.cpp
@@ -56,6 +56,7 @@ RaycastScene::RaycastScene(const std::string& name)
// Set the box color
mDumbbell->setColor(mGreyColorDemo);
mDumbbell->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mDumbbell);
// ---------- Box ---------- //
openglframework::Vector3 position2(0, 0, 0);
@@ -67,6 +68,7 @@ RaycastScene::RaycastScene(const std::string& name)
// Set the box color
mBox->setColor(mGreyColorDemo);
mBox->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mBox);
// ---------- Sphere ---------- //
openglframework::Vector3 position3(0, 0, 0);
@@ -78,6 +80,7 @@ RaycastScene::RaycastScene(const std::string& name)
// Set the color
mSphere->setColor(mGreyColorDemo);
mSphere->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mSphere);
// ---------- Capsule ---------- //
openglframework::Vector3 position6(0, 0, 0);
@@ -89,6 +92,7 @@ RaycastScene::RaycastScene(const std::string& name)
// Set the color
mCapsule->setColor(mGreyColorDemo);
mCapsule->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mCapsule);
// ---------- Convex Mesh ---------- //
openglframework::Vector3 position7(0, 0, 0);
@@ -99,6 +103,7 @@ RaycastScene::RaycastScene(const std::string& name)
// Set the color
mConvexMesh->setColor(mGreyColorDemo);
mConvexMesh->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mConvexMesh);
// ---------- Concave Mesh ---------- //
openglframework::Vector3 position8(0, 0, 0);
@@ -109,6 +114,7 @@ RaycastScene::RaycastScene(const std::string& name)
// Set the color
mConcaveMesh->setColor(mGreyColorDemo);
mConcaveMesh->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mConcaveMesh);
// ---------- Heightfield ---------- //
openglframework::Vector3 position9(0, 0, 0);
@@ -119,6 +125,7 @@ RaycastScene::RaycastScene(const std::string& name)
// Set the color
mHeightField->setColor(mGreyColorDemo);
mHeightField->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mHeightField);
// Create the lines that will be used for raycasting
createLines();
@@ -256,12 +263,6 @@ RaycastScene::~RaycastScene() {
mVAO.destroy();
}
-// Update the physics world (take a simulation step)
-void RaycastScene::updatePhysics() {
-
-
-}
-
// Take a step for the simulation
void RaycastScene::update() {
@@ -327,14 +328,18 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, const openg
mColorShader.unbind();
- // Render the shapes
- if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mSphere->getCollisionBody()->isActive()) mSphere->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ // Bind the shader
+ shader.bind();
+
+ // Render all the physics objects of the scene
+ for (std::vector<PhysicsObject*>::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
+ if ((*it)->getCollisionBody()->isActive()) {
+ (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ }
+ }
+
+ // Unbind the shader
+ shader.unbind();
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h
index 39b73799..1400fcf7 100644
--- a/testbed/scenes/raycast/RaycastScene.h
+++ b/testbed/scenes/raycast/RaycastScene.h
@@ -57,7 +57,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 = 9;
+const int NB_BODIES = 7;
// Raycast manager
class RaycastManager : public rp3d::RaycastCallback {
@@ -175,10 +175,6 @@ class RaycastScene : public SceneDemo {
/// Destructor
virtual ~RaycastScene() override;
- /// Update the physics world (take a simulation step)
- /// Can be called several times per frame
- virtual void updatePhysics() override;
-
/// Take a step for the simulation
virtual void update() override;
diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp
index 85e8a66f..d406bcea 100644
--- a/testbed/src/SceneDemo.cpp
+++ b/testbed/src/SceneDemo.cpp
@@ -80,7 +80,7 @@ SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMa
// Destructor
SceneDemo::~SceneDemo() {
-
+
mShadowMapTexture.destroy();
mFBOShadowMap.destroy();
mVBOQuad.destroy();
@@ -101,6 +101,19 @@ void SceneDemo::update() {
// Update the contact points
updateContactPoints();
+
+ // Update the position and orientation of the physics objects
+ for (std::vector<PhysicsObject*>::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
+
+ // Update the transform used for the rendering
+ (*it)->updateTransform(mInterpolationFactor);
+ }
+}
+
+// Update the physics world (take a simulation step)
+// Can be called several times per frame
+void SceneDemo::updatePhysics() {
+
}
// Render the scene (in multiple passes for shadow mapping)
@@ -202,6 +215,21 @@ void SceneDemo::render() {
//drawTextureQuad();
}
+// Render the scene in a single pass
+void SceneDemo::renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
+
+ // Bind the shader
+ shader.bind();
+
+ // Render all the physics objects of the scene
+ for (std::vector<PhysicsObject*>::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
+ (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ }
+
+ // Unbind the shader
+ shader.unbind();
+}
+
// Create the Shadow map FBO and texture
void SceneDemo::createShadowMapFBOAndTexture() {
@@ -345,7 +373,7 @@ std::vector<ContactPoint> SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn
rp3d::ContactPoint* contactPoint = manifold->getContactPoints();
while (contactPoint != nullptr) {
- rp3d::Vector3 point = contactPoint->getWorldPointOnBody1();
+ rp3d::Vector3 point = manifold->getShape1()->getLocalToWorldTransform() * contactPoint->getLocalPointOnBody1();
rp3d::Vector3 normalWorld = contactPoint->getNormal();
openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z);
ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red());
diff --git a/testbed/src/SceneDemo.h b/testbed/src/SceneDemo.h
index f7b5f7ce..a8b1941f 100644
--- a/testbed/src/SceneDemo.h
+++ b/testbed/src/SceneDemo.h
@@ -30,6 +30,7 @@
#include "Scene.h"
#include "VisualContactPoint.h"
#include "reactphysics3d.h"
+#include "PhysicsObject.h"
// Constants
const int SHADOWMAP_WIDTH = 2048;
@@ -95,6 +96,8 @@ class SceneDemo : public Scene {
std::string mMeshFolderPath;
+ std::vector<PhysicsObject*> mPhysicsObjects;
+
// -------------------- Methods -------------------- //
// Create the Shadow map FBO and texture
@@ -128,12 +131,15 @@ class SceneDemo : public Scene {
/// Update the scene
virtual void update() override;
+ /// Update the physics world (take a simulation step)
+ /// Can be called several times per frame
+ virtual void updatePhysics() override;
+
/// Render the scene (possibly in multiple passes for shadow mapping)
virtual void render() override;
/// Render the scene in a single pass
- virtual void renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix)=0 ;
+ virtual void renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix);
/// Enabled/Disable the shadow mapping
virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override;
diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp
index 09f464f3..2b9195e5 100644
--- a/testbed/src/TestbedApplication.cpp
+++ b/testbed/src/TestbedApplication.cpp
@@ -125,7 +125,7 @@ void TestbedApplication::createScenes() {
mScenes.push_back(concaveMeshScene);
assert(mScenes.size() > 0);
- mCurrentScene = mScenes[0];
+ mCurrentScene = mScenes[5];
// Get the engine settings from the scene
mEngineSettings = mCurrentScene->getEngineSettings();
From 5da57a96c80be764d77e38478c33b64de2d9bc34 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 24 Oct 2017 22:47:35 +0200
Subject: [PATCH 097/133] Fix issue with sphere and capsule SAT collision
detection
---
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 4 ++++
src/collision/narrowphase/SAT/SATAlgorithm.cpp | 15 ++++++++-------
2 files changed, 12 insertions(+), 7 deletions(-)
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index d6de739a..89336c4e 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -114,6 +114,10 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
+ if (isCapsuleShape1) {
+ faceNormalWorld = -faceNormalWorld;
+ }
+
// Compute and create two contact points
satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 4411187b..1ba5143b 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -97,10 +97,12 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
if (reportContacts) {
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
- Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal);
- Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius();
+ Vector3 minFaceNormalWorld = polyhedronToWorldTransform.getOrientation() * minFaceNormal;
+ Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * (-minFaceNormalWorld * sphere->getRadius());
Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
+ Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld;
+
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
@@ -243,6 +245,9 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace;
+ if (isCapsuleShape1) {
+ normalWorld = -normalWorld;
+ }
const decimal capsuleRadius = capsuleShape->getRadius();
// If the separating axis is a face normal
@@ -391,12 +396,8 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// Project the two clipped points into the polyhedron face
const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);
- if (isCapsuleShape1) {
- normalWorld = -normalWorld;
- }
-
// For each of the two clipped points
- for (int i = 0; i<2; i++) {
+ for (uint i = 0; i<2; i++) {
// Compute the penetration depth of the two clipped points (to filter out the points that does not correspond to the penetration depth)
const decimal clipPointPenDepth = (planesPoints[0] - clipSegment[i]).dot(faceNormal);
From 6a69ef76c5cd6467f90c4b3b20ba30704d960696 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 1 Nov 2017 23:07:56 +0100
Subject: [PATCH 098/133] Fix issue with ProxyShape::mBroadPhaseId not set when
body was sleeping or inactive
---
src/body/CollisionBody.cpp | 26 +-
src/collision/CollisionDetection.cpp | 245 ++++++++++--------
src/collision/CollisionDetection.h | 8 +-
.../broadphase/BroadPhaseAlgorithm.cpp | 6 +
.../broadphase/BroadPhaseAlgorithm.h | 3 +
src/engine/CollisionWorld.cpp | 11 +
src/engine/CollisionWorld.h | 3 +
7 files changed, 178 insertions(+), 124 deletions(-)
diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index 39d47de4..64ce6467 100644
--- a/src/body/CollisionBody.cpp
+++ b/src/body/CollisionBody.cpp
@@ -111,7 +111,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
if (current == proxyShape) {
mProxyCollisionShapes = current->mNext;
- if (mIsActive) {
+ if (mIsActive && proxyShape->mBroadPhaseID != -1) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
}
@@ -131,7 +131,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
ProxyShape* elementToRemove = current->mNext;
current->mNext = elementToRemove->mNext;
- if (mIsActive) {
+ if (mIsActive && proxyShape->mBroadPhaseID != -1) {
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
}
@@ -157,7 +157,7 @@ void CollisionBody::removeAllCollisionShapes() {
// Remove the proxy collision shape
ProxyShape* nextElement = current->mNext;
- if (mIsActive) {
+ if (mIsActive && current->mBroadPhaseID != -1) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
}
@@ -202,12 +202,15 @@ void CollisionBody::updateBroadPhaseState() const {
// Update the broad-phase state of a proxy collision shape of the body
void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert) const {
- // Recompute the world-space AABB of the collision shape
- AABB aabb;
- proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform());
+ if (proxyShape->mBroadPhaseID != -1) {
- // Update the broad-phase state for the proxy collision shape
- mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert);
+ // Recompute the world-space AABB of the collision shape
+ AABB aabb;
+ proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform());
+
+ // Update the broad-phase state for the proxy collision shape
+ mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert) ;
+ }
}
// Set whether or not the body is active
@@ -240,8 +243,11 @@ void CollisionBody::setIsActive(bool isActive) {
// For each proxy shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
- // Remove the proxy shape from the collision detection
- mWorld.mCollisionDetection.removeProxyCollisionShape(shape);
+ if (shape->mBroadPhaseID != -1) {
+
+ // Remove the proxy shape from the collision detection
+ mWorld.mCollisionDetection.removeProxyCollisionShape(shape);
+ }
}
// Reset the contact manifold list of the body
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index f650351a..baaf0db5 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -109,6 +109,8 @@ void CollisionDetection::computeMiddlePhase() {
ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2();
+ assert(shape1->mBroadPhaseID != -1);
+ assert(shape2->mBroadPhaseID != -1);
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
// Check if the two shapes are still overlapping. Otherwise, we destroy the
@@ -282,6 +284,8 @@ void CollisionDetection::computeNarrowPhase() {
/// This method is called by the broad-phase collision detection algorithm
void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
+ assert(shape1->mBroadPhaseID != -1);
+ assert(shape2->mBroadPhaseID != -1);
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
// Check if the collision filtering allows collision between the two shapes
@@ -313,6 +317,8 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
// Remove a body from the collision detection
void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
+ assert(proxyShape->mBroadPhaseID != -1);
+
// Remove all the overlapping pairs involving this proxy shape
std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
@@ -683,87 +689,90 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
ProxyShape* bodyProxyShape = body->getProxyShapesList();
while (bodyProxyShape != nullptr) {
- // Get the AABB of the shape
- const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
+ if (bodyProxyShape->mBroadPhaseID != -1) {
- // Ask the broad-phase to get all the overlapping shapes
- LinkedList<int> overlappingNodes(mPoolAllocator);
- mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
+ // Get the AABB of the shape
+ const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
- const bodyindex bodyId = body->getID();
+ // Ask the broad-phase to get all the overlapping shapes
+ LinkedList<int> overlappingNodes(mPoolAllocator);
+ mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
- // For each overlaping proxy shape
- LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
- while (element != nullptr) {
+ const bodyindex bodyId = body->getID();
- // Get the overlapping proxy shape
- int broadPhaseId = element->data;
- ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId);
+ // For each overlaping proxy shape
+ LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
+ while (element != nullptr) {
- // If the proxy shape is from a body that we have not already reported collision and the
- // two proxy collision shapes are not from the same body
- if (reportedBodies.find(proxyShape->getBody()->getID()) == reportedBodies.end() &&
- proxyShape->getBody()->getID() != bodyId) {
+ // Get the overlapping proxy shape
+ int broadPhaseId = element->data;
+ ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId);
- // Check if the collision filtering allows collision between the two shapes
- if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
+ // If the proxy shape is from a body that we have not already reported collision and the
+ // two proxy collision shapes are not from the same body
+ if (reportedBodies.find(proxyShape->getBody()->getID()) == reportedBodies.end() &&
+ proxyShape->getBody()->getID() != bodyId) {
- // Create a temporary overlapping pair
- OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
+ // Check if the collision filtering allows collision between the two shapes
+ if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
- // Compute the middle-phase collision detection between the two shapes
- NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
+ // Create a temporary overlapping pair
+ OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
- bool isColliding = false;
+ // Compute the middle-phase collision detection between the two shapes
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
- // For each narrow-phase info object
- while (narrowPhaseInfo != nullptr) {
+ bool isColliding = false;
- // If we have not found a collision yet
- if (!isColliding) {
+ // For each narrow-phase info object
+ while (narrowPhaseInfo != nullptr) {
- const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
- const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
+ // If we have not found a collision yet
+ if (!isColliding) {
- // Select the narrow phase algorithm to use according to the two collision shapes
- NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
+ const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
+ const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
- // If there is a collision algorithm for those two kinds of shapes
- if (narrowPhaseAlgorithm != nullptr) {
+ // Select the narrow phase algorithm to use according to the two collision shapes
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
- // Use the narrow-phase collision detection algorithm to check
- // if there really is a collision. If a collision occurs, the
- // notifyContact() callback method will be called.
- isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
+ // If there is a collision algorithm for those two kinds of shapes
+ if (narrowPhaseAlgorithm != nullptr) {
+
+ // Use the narrow-phase collision detection algorithm to check
+ // if there really is a collision. If a collision occurs, the
+ // notifyContact() callback method will be called.
+ isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
+ }
}
+
+ NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+ narrowPhaseInfo = narrowPhaseInfo->next;
+
+ // Call the destructor
+ currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
+ // Release the allocated memory
+ mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
- NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
- narrowPhaseInfo = narrowPhaseInfo->next;
+ // Return if we have found a narrow-phase collision
+ if (isColliding) {
- // Call the destructor
- currentNarrowPhaseInfo->~NarrowPhaseInfo();
+ CollisionBody* overlapBody = proxyShape->getBody();
- // Release the allocated memory
- mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
- }
+ // Add the body into the set of reported bodies
+ reportedBodies.insert(overlapBody->getID());
- // Return if we have found a narrow-phase collision
- if (isColliding) {
-
- CollisionBody* overlapBody = proxyShape->getBody();
-
- // Add the body into the set of reported bodies
- reportedBodies.insert(overlapBody->getID());
-
- // Notify the overlap to the user
- overlapCallback->notifyOverlap(overlapBody);
+ // Notify the overlap to the user
+ overlapCallback->notifyOverlap(overlapBody);
+ }
}
}
- }
- // Go to the next overlapping proxy shape
- element = element->next;
+ // Go to the next overlapping proxy shape
+ element = element->next;
+ }
}
// Go to the next proxy shape
@@ -858,85 +867,88 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
ProxyShape* bodyProxyShape = body->getProxyShapesList();
while (bodyProxyShape != nullptr) {
- // Get the AABB of the shape
- const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
+ if (bodyProxyShape->mBroadPhaseID != -1) {
- // Ask the broad-phase to get all the overlapping shapes
- LinkedList<int> overlappingNodes(mPoolAllocator);
- mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
+ // Get the AABB of the shape
+ const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
- const bodyindex bodyId = body->getID();
+ // Ask the broad-phase to get all the overlapping shapes
+ LinkedList<int> overlappingNodes(mPoolAllocator);
+ mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
- // For each overlaping proxy shape
- LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
- while (element != nullptr) {
+ const bodyindex bodyId = body->getID();
- // Get the overlapping proxy shape
- int broadPhaseId = element->data;
- ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId);
+ // For each overlaping proxy shape
+ LinkedList<int>::ListElement* element = overlappingNodes.getListHead();
+ while (element != nullptr) {
- // If the two proxy collision shapes are not from the same body
- if (proxyShape->getBody()->getID() != bodyId) {
+ // Get the overlapping proxy shape
+ int broadPhaseId = element->data;
+ ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId);
- // Check if the collision filtering allows collision between the two shapes
- if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
+ // If the two proxy collision shapes are not from the same body
+ if (proxyShape->getBody()->getID() != bodyId) {
- // Create a temporary overlapping pair
- OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
+ // Check if the collision filtering allows collision between the two shapes
+ if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
- // Compute the middle-phase collision detection between the two shapes
- NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
+ // Create a temporary overlapping pair
+ OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
- // For each narrow-phase info object
- while (narrowPhaseInfo != nullptr) {
+ // Compute the middle-phase collision detection between the two shapes
+ NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
- const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
- const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
+ // For each narrow-phase info object
+ while (narrowPhaseInfo != nullptr) {
- // Select the narrow phase algorithm to use according to the two collision shapes
- NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
+ const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType();
+ const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType();
- // If there is a collision algorithm for those two kinds of shapes
- if (narrowPhaseAlgorithm != nullptr) {
+ // Select the narrow phase algorithm to use according to the two collision shapes
+ NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
- // Use the narrow-phase collision detection algorithm to check
- // if there really is a collision. If a collision occurs, the
- // notifyContact() callback method will be called.
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
+ // If there is a collision algorithm for those two kinds of shapes
+ if (narrowPhaseAlgorithm != nullptr) {
- // Add the contact points as a potential contact manifold into the pair
- narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
+ // Use the narrow-phase collision detection algorithm to check
+ // if there really is a collision. If a collision occurs, the
+ // notifyContact() callback method will be called.
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
+
+ // Add the contact points as a potential contact manifold into the pair
+ narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
+ }
}
+
+ NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
+ narrowPhaseInfo = narrowPhaseInfo->next;
+
+ // Call the destructor
+ currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
+ // Release the allocated memory
+ mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
- NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
- narrowPhaseInfo = narrowPhaseInfo->next;
+ // Process the potential contacts
+ processPotentialContacts(&pair);
- // Call the destructor
- currentNarrowPhaseInfo->~NarrowPhaseInfo();
+ if (pair.hasContacts()) {
- // Release the allocated memory
- mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ // Report the contacts to the user
+ CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
+ callback->notifyContact(collisionInfo);
+ }
}
-
- // Process the potential contacts
- processPotentialContacts(&pair);
-
- if (pair.hasContacts()) {
-
- // Report the contacts to the user
- CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
- callback->notifyContact(collisionInfo);
- }
}
+
+ // Go to the next overlapping proxy shape
+ element = element->next;
}
- // Go to the next overlapping proxy shape
- element = element->next;
+ // Go to the next proxy shape
+ bodyProxyShape = bodyProxyShape->getNext();
}
-
- // Go to the next proxy shape
- bodyProxyShape = bodyProxyShape->getNext();
}
}
@@ -1031,7 +1043,14 @@ EventListener* CollisionDetection::getWorldEventListener() {
return mWorld->mEventListener;
}
-/// Return a reference to the world memory allocator
+// Return a reference to the world memory allocator
PoolAllocator& CollisionDetection::getWorldMemoryAllocator() {
return mWorld->mPoolAllocator;
}
+
+
+// Return the world-space AABB of a given proxy shape
+const AABB CollisionDetection::getWorldAABB(const ProxyShape* proxyShape) const {
+ assert(proxyShape->mBroadPhaseID > -1);
+ return mBroadPhaseAlgorithm.getFatAABB(proxyShape->mBroadPhaseID);
+}
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 848e7d09..6f28f788 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -219,6 +219,9 @@ class CollisionDetection {
/// Return a reference to the world memory allocator
PoolAllocator& getWorldMemoryAllocator();
+ /// Return the world-space AABB of a given proxy shape
+ const AABB getWorldAABB(const ProxyShape* proxyShape) const;
+
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
@@ -259,7 +262,10 @@ inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
/// We simply put the shape in the list of collision shape that have moved in the
/// previous frame so that it is tested for collision again in the broad-phase.
inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
- mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID);
+
+ if (shape->mBroadPhaseID != -1) {
+ mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID);
+ }
}
// Update a proxy collision shape (that has moved for instance)
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp
index bc03f44d..c86cd0bc 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp
@@ -117,6 +117,8 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
// Add a proxy collision shape into the broad-phase collision detection
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
+ assert(proxyShape->mBroadPhaseID == -1);
+
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID
int nodeId = mDynamicAABBTree.addObject(aabb, proxyShape);
@@ -131,8 +133,12 @@ void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const A
// Remove a proxy collision shape from the broad-phase collision detection
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
+ assert(proxyShape->mBroadPhaseID != -1);
+
int broadPhaseID = proxyShape->mBroadPhaseID;
+ proxyShape->mBroadPhaseID = -1;
+
// Remove the collision shape from the dynamic AABB tree
mDynamicAABBTree.removeObject(broadPhaseID);
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h
index 8d2cda35..94499d44 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.h
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.h
@@ -232,6 +232,9 @@ inline bool BroadPhasePair::smallerThan(const BroadPhasePair& pair1, const Broad
// Return true if the two broad-phase collision shapes are overlapping
inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
const ProxyShape* shape2) const {
+
+ if (shape1->mBroadPhaseID == -1 || shape2->mBroadPhaseID == -1) return false;
+
// Get the two AABBs of the collision shapes
const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1->mBroadPhaseID);
const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2->mBroadPhaseID);
diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp
index d160021c..e8f4155a 100644
--- a/src/engine/CollisionWorld.cpp
+++ b/src/engine/CollisionWorld.cpp
@@ -164,3 +164,14 @@ bool CollisionWorld::testOverlap(CollisionBody* body1, CollisionBody* body2) {
return mCollisionDetection.testOverlap(body1, body2);
}
+
+// Return the current world-space AABB of given proxy shape
+AABB CollisionWorld::getWorldAABB(const ProxyShape* proxyShape) const {
+
+ if (proxyShape->mBroadPhaseID == -1) {
+ return AABB();
+ }
+
+ return mCollisionDetection.getWorldAABB(proxyShape);
+}
+
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index 82d2601f..d27b8018 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -147,6 +147,9 @@ class CollisionWorld {
/// Test and report collisions between all shapes of the world
void testCollision(CollisionCallback* callback);
+ /// Return the current world-space AABB of given proxy shape
+ AABB getWorldAABB(const ProxyShape* proxyShape) const;
+
// -------------------- Friendship -------------------- //
friend class CollisionDetection;
From ad0f805f53be29b0f3c7e17b5fdc1986c51a759c Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 1 Nov 2017 23:09:02 +0100
Subject: [PATCH 099/133] Fix robustness issue in SAT Algorithm (convex
polyhedron vs capsule)
---
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 5 ++++-
src/collision/narrowphase/SAT/SATAlgorithm.cpp | 13 +++++++++----
src/collision/narrowphase/SAT/SATAlgorithm.h | 2 +-
3 files changed, 14 insertions(+), 6 deletions(-)
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index 89336c4e..e895eef7 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -119,10 +119,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
}
// Compute and create two contact points
- satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
+ bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1);
+ if (!contactsFound) {
+ return false;
+ }
break;
}
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 1ba5143b..e6d09dd1 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -256,7 +256,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
if (reportContacts) {
- computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
+ return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1);
@@ -349,7 +349,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe
// Compute the two contact points between a polyhedron and a capsule when the separating
// axis is a face normal of the polyhedron
-void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
+bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
@@ -391,13 +391,14 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// First we clip the inner segment of the capsule with the four planes of the adjacent faces
std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals);
- assert(clipSegment.size() == 2);
// Project the two clipped points into the polyhedron face
const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);
+ bool contactFound = false;
+
// For each of the two clipped points
- for (uint i = 0; i<2; i++) {
+ for (uint i = 0; i<clipSegment.size(); i++) {
// Compute the penetration depth of the two clipped points (to filter out the points that does not correspond to the penetration depth)
const decimal clipPointPenDepth = (planesPoints[0] - clipSegment[i]).dot(faceNormal);
@@ -405,6 +406,8 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// If the clipped point is one that produce this penetration depth, we keep it
if (clipPointPenDepth > penetrationDepth - capsuleRadius - decimal(0.001)) {
+ contactFound = true;
+
Vector3 contactPointPolyhedron = clipSegment[i] + delta;
// Project the clipped point into the capsule bounds
@@ -426,6 +429,8 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
}
}
+
+ return contactFound;
}
// This method returns true if an edge of a polyhedron and a capsule forms a
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 37e4f7d2..5c721a00 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -126,7 +126,7 @@ class SATAlgorithm {
bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
/// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
- void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
+ bool computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
From 7495ff6598d218ae8d8549dd7320047791f4d5a3 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 2 Nov 2017 22:58:41 +0100
Subject: [PATCH 100/133] Refactor the testbed application and display objects
AABBs
---
testbed/CMakeLists.txt | 2 +
testbed/common/AABB.cpp | 202 +++++++++++++
testbed/common/AABB.h | 79 +++++
testbed/common/Box.cpp | 44 +--
testbed/common/Box.h | 10 +-
testbed/common/Capsule.cpp | 40 +--
testbed/common/Capsule.h | 10 +-
testbed/common/ConcaveMesh.cpp | 40 +--
testbed/common/ConcaveMesh.h | 10 +-
testbed/common/ConvexMesh.cpp | 40 +--
testbed/common/ConvexMesh.h | 10 +-
testbed/common/Dumbbell.cpp | 42 +--
testbed/common/Dumbbell.h | 10 +-
testbed/common/HeightField.cpp | 38 +--
testbed/common/HeightField.h | 10 +-
testbed/common/PhysicsObject.h | 2 +-
testbed/common/Sphere.cpp | 39 +--
testbed/common/Sphere.h | 10 +-
.../CollisionDetectionScene.cpp | 125 ++++----
.../CollisionDetectionScene.h | 7 +-
.../collisionshapes/CollisionShapesScene.cpp | 208 ++++----------
.../collisionshapes/CollisionShapesScene.h | 13 +-
.../scenes/concavemesh/ConcaveMeshScene.cpp | 270 +++++++++++-------
testbed/scenes/concavemesh/ConcaveMeshScene.h | 57 ++--
testbed/scenes/cubes/CubesScene.cpp | 132 +++------
testbed/scenes/cubes/CubesScene.h | 13 +-
.../scenes/heightfield/HeightFieldScene.cpp | 169 +++++------
testbed/scenes/heightfield/HeightFieldScene.h | 13 +-
testbed/scenes/joints/JointsScene.cpp | 135 ++++-----
testbed/scenes/joints/JointsScene.h | 11 +-
testbed/scenes/raycast/RaycastScene.cpp | 56 ++--
testbed/scenes/raycast/RaycastScene.h | 5 +-
testbed/src/Gui.cpp | 8 +
testbed/src/Scene.cpp | 16 +-
testbed/src/Scene.h | 53 ++--
testbed/src/SceneDemo.cpp | 86 +++++-
testbed/src/SceneDemo.h | 25 +-
testbed/src/TestbedApplication.cpp | 34 ++-
testbed/src/TestbedApplication.h | 6 +-
39 files changed, 1066 insertions(+), 1014 deletions(-)
create mode 100644 testbed/common/AABB.cpp
create mode 100644 testbed/common/AABB.h
diff --git a/testbed/CMakeLists.txt b/testbed/CMakeLists.txt
index a5620028..e5e3b312 100644
--- a/testbed/CMakeLists.txt
+++ b/testbed/CMakeLists.txt
@@ -94,6 +94,8 @@ SET(COMMON_SOURCES
common/VisualContactPoint.cpp
common/PerlinNoise.h
common/PerlinNoise.cpp
+ common/AABB.h
+ common/AABB.cpp
)
# Examples scenes source files
diff --git a/testbed/common/AABB.cpp b/testbed/common/AABB.cpp
new file mode 100644
index 00000000..8c43700c
--- /dev/null
+++ b/testbed/common/AABB.cpp
@@ -0,0 +1,202 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "AABB.h"
+
+// Macros
+#define MEMBER_OFFSET(s,m) ((char *)nullptr + (offsetof(s,m)))
+
+// Initialize static variables
+openglframework::VertexBufferObject AABB::mVBOVertices(GL_ARRAY_BUFFER);
+openglframework::VertexBufferObject AABB::mVBONormals(GL_ARRAY_BUFFER);
+openglframework::VertexBufferObject AABB::mVBOIndices(GL_ELEMENT_ARRAY_BUFFER);
+openglframework::VertexArrayObject AABB::mVAO;
+
+
+// Initialize the data to render AABBs
+void AABB::init() {
+ createVBOAndVAO();
+}
+
+// Destroy the data used to render AABBs
+void AABB::destroy() {
+
+ // Destroy the VBOs and VAO
+ mVBOVertices.destroy();
+ mVBONormals.destroy();
+ mVAO.destroy();
+}
+
+// Render the AABB
+void AABB::render(const openglframework::Vector3& position, const openglframework::Vector3& dimension,
+ openglframework::Color color, openglframework::Shader& shader,
+ const openglframework::Matrix4& worldToCameraMatrix) {
+
+ // Render in wireframe mode
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+
+ // Bind the shader
+ shader.bind();
+
+ // Transform matrix
+ openglframework::Matrix4 transformMatrix = openglframework::Matrix4::identity();
+ transformMatrix = openglframework::Matrix4::translationMatrix(position) * transformMatrix;
+
+ // Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
+ // model-view matrix)
+ const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * transformMatrix;
+ const openglframework::Matrix3 normalMatrix =
+ localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
+ shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
+
+
+ // Compute the scaling matrix
+ openglframework::Matrix4 scalingMatrix = openglframework::Matrix4(dimension.x, 0, 0, 0,
+ 0, dimension.y, 0, 0,
+ 0, 0, dimension.z, 0,
+ 0, 0, 0, 1);
+
+ transformMatrix = transformMatrix * scalingMatrix;
+
+ // Set the model to camera matrix
+ shader.setMatrix4x4Uniform("localToWorldMatrix", transformMatrix);
+ shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
+
+ // Set the vertex color
+ openglframework::Vector4 colorVec(color.r, color.g, color.b, color.a);
+ shader.setVector4Uniform("vertexColor", colorVec, false);
+
+ // Bind the VAO
+ mVAO.bind();
+
+ mVBOVertices.bind();
+
+ // Get the location of shader attribute variables
+ GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
+ GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false);
+
+ glEnableVertexAttribArray(vertexPositionLoc);
+ glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr);
+
+ mVBONormals.bind();
+
+ if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)nullptr);
+ if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc);
+
+ // Draw the geometry
+ glDrawElements(GL_LINES, 12 * 2, GL_UNSIGNED_INT, (char*)nullptr);
+
+ glDisableVertexAttribArray(vertexPositionLoc);
+ if (vertexNormalLoc != -1) glDisableVertexAttribArray(vertexNormalLoc);
+
+ mVBONormals.unbind();
+ mVBOVertices.unbind();
+
+ // Unbind the VAO
+ mVAO.unbind();
+
+ // Unbind the shader
+ shader.unbind();
+
+ // Disable wireframe mode
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+}
+
+// Create the Vertex Buffer Objects used to render to box with OpenGL.
+/// We create two VBOs (one for vertices and one for indices)
+void AABB::createVBOAndVAO() {
+
+ std::vector<openglframework::Vector3> vertices;
+ std::vector<openglframework::Vector3> normals;
+ std::vector<unsigned int> indices;
+
+ // Vertices
+ vertices.push_back(openglframework::Vector3(-0.5, -0.5, 0.5));
+ vertices.push_back(openglframework::Vector3(0.5, -0.5, 0.5));
+ vertices.push_back(openglframework::Vector3(0.5, 0.5, 0.5));
+ vertices.push_back(openglframework::Vector3(-0.5, 0.5, 0.5));
+ vertices.push_back(openglframework::Vector3(0.5, -0.5, -0.5));
+ vertices.push_back(openglframework::Vector3(0.5, 0.5, -0.5));
+ vertices.push_back(openglframework::Vector3(-0.5, 0.5, -0.5));
+ vertices.push_back(openglframework::Vector3(-0.5, -0.5, -0.5));
+
+ // Normals
+ normals.push_back(openglframework::Vector3(1, -1, 1));
+ normals.push_back(openglframework::Vector3(1, 1, 1));
+ normals.push_back(openglframework::Vector3(-1, 1, 1));
+ normals.push_back(openglframework::Vector3(-1, -1, 1));
+ normals.push_back(openglframework::Vector3(1, -1, -1));
+ normals.push_back(openglframework::Vector3(1, 1, -1));
+ normals.push_back(openglframework::Vector3(-1, 1, -1));
+ normals.push_back(openglframework::Vector3(-1, -1, -1));
+
+ // Indices
+ indices.push_back(0); indices.push_back(1); indices.push_back(1); indices.push_back(2);
+ indices.push_back(2); indices.push_back(3); indices.push_back(3); indices.push_back(0);
+
+ indices.push_back(4); indices.push_back(5); indices.push_back(5); indices.push_back(6);
+ indices.push_back(6); indices.push_back(7); indices.push_back(7); indices.push_back(4);
+
+ indices.push_back(1); indices.push_back(4); indices.push_back(2); indices.push_back(5);
+ indices.push_back(0); indices.push_back(7); indices.push_back(3); indices.push_back(6);
+
+ // Create the VBO for the vertices data
+ mVBOVertices.create();
+ mVBOVertices.bind();
+ GLsizei sizeVertices = static_cast<GLsizei>(vertices.size() * sizeof(openglframework::Vector3));
+ mVBOVertices.copyDataIntoVBO(sizeVertices, &(vertices[0]), GL_STATIC_DRAW);
+ mVBOVertices.unbind();
+
+ // Create the VBO for the normals data
+ mVBONormals.create();
+ mVBONormals.bind();
+ GLsizei sizeNormals = static_cast<GLsizei>(normals.size() * sizeof(openglframework::Vector3));
+ mVBONormals.copyDataIntoVBO(sizeNormals, &(normals[0]), GL_STATIC_DRAW);
+ mVBONormals.unbind();
+
+ // Create th VBO for the indices data
+ mVBOIndices.create();
+ mVBOIndices.bind();
+ GLsizei sizeIndices = static_cast<GLsizei>(indices.size() * sizeof(unsigned int));
+ mVBOIndices.copyDataIntoVBO(sizeIndices, &(indices[0]), GL_STATIC_DRAW);
+ mVBOIndices.unbind();
+
+ // Create the VAO for both VBOs
+ mVAO.create();
+ mVAO.bind();
+
+ // Bind the VBO of vertices
+ mVBOVertices.bind();
+
+ // Bind the VBO of normals
+ mVBONormals.bind();
+
+ // Bind the VBO of indices
+ mVBOIndices.bind();
+
+ // Unbind the VAO
+ mVAO.unbind();
+}
diff --git a/testbed/common/AABB.h b/testbed/common/AABB.h
new file mode 100644
index 00000000..e73326b5
--- /dev/null
+++ b/testbed/common/AABB.h
@@ -0,0 +1,79 @@
+/********************************************************************************
+ * ReactPhysics3D physics library, http://www.reactphysics3d.com *
+ * Copyright (c) 2010-2016 Daniel Chappuis *
+ *********************************************************************************
+ * *
+ * This software is provided 'as-is', without any express or implied warranty. *
+ * In no event will the authors be held liable for any damages arising from the *
+ * use of this software. *
+ * *
+ * Permission is granted to anyone to use this software for any purpose, *
+ * including commercial applications, and to alter it and redistribute it *
+ * freely, subject to the following restrictions: *
+ * *
+ * 1. The origin of this software must not be misrepresented; you must not claim *
+ * that you wrote the original software. If you use this software in a *
+ * product, an acknowledgment in the product documentation would be *
+ * appreciated but is not required. *
+ * *
+ * 2. Altered source versions must be plainly marked as such, and must not be *
+ * misrepresented as being the original software. *
+ * *
+ * 3. This notice may not be removed or altered from any source distribution. *
+ * *
+ ********************************************************************************/
+
+#ifndef AABB_H
+#define AABB_H
+
+// Libraries
+#include "openglframework.h"
+#include "reactphysics3d.h"
+
+// Class AABB
+class AABB {
+
+ private :
+
+ // -------------------- Attributes -------------------- //
+
+ /// Size of each side of the box
+ float mSize[3];
+
+ /// Scaling matrix (applied to a cube to obtain the correct box dimensions)
+ openglframework::Matrix4 mScalingMatrix;
+
+ /// Vertex Buffer Object for the vertices data
+ static openglframework::VertexBufferObject mVBOVertices;
+
+ /// Vertex Buffer Object for the normals data
+ static openglframework::VertexBufferObject mVBONormals;
+
+ /// Vertex Buffer Object for the indices
+ static openglframework::VertexBufferObject mVBOIndices;
+
+ /// Vertex Array Object for the vertex data
+ static openglframework::VertexArrayObject mVAO;
+
+ // -------------------- Methods -------------------- //
+
+ /// Create a the VAO and VBOs to render to box with OpenGL
+ static void createVBOAndVAO();
+
+ public :
+
+ // -------------------- Methods -------------------- //
+
+ /// Initialize the data to render AABBs
+ static void init();
+
+ /// Destroy the data used to render AABBs
+ static void destroy();
+
+ /// Render the cube at the correct position and with the correct orientation
+ static void render(const openglframework::Vector3& position, const openglframework::Vector3& dimension,
+ openglframework::Color color, openglframework::Shader& shader,
+ const openglframework::Matrix4& worldToCameraMatrix);
+};
+
+#endif
diff --git a/testbed/common/Box.cpp b/testbed/common/Box.cpp
index 09dc388e..508fa516 100644
--- a/testbed/common/Box.cpp
+++ b/testbed/common/Box.cpp
@@ -38,8 +38,8 @@ openglframework::VertexArrayObject Box::mVAO;
int Box::totalNbBoxes = 0;
// Constructor
-Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath)
+Box::Box(const openglframework::Vector3& size, reactphysics3d::CollisionWorld* world,
+ const std::string& meshFolderPath)
: PhysicsObject(meshFolderPath + "cube.obj") {
// Initialize the size of the box
@@ -53,23 +53,16 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
0, 0, mSize[2], 0,
0, 0, 0, 1);
- // Initialize the position where the cube will be rendered
- translateWorld(position);
-
// Create the collision shape for the rigid body (box shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
+ //mBoxShape->setLocalScaling(rp3d::Vector3(2, 2, 2));
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- mPreviousTransform = transform;
+ mPreviousTransform = rp3d::Transform::identity();
// Create a rigid body in the dynamics world
- mBody = world->createCollisionBody(transform);
+ mBody = world->createCollisionBody(mPreviousTransform);
// Add the collision shape to the body
mProxyShape = mBody->addCollisionShape(mBoxShape, rp3d::Transform::identity());
@@ -87,8 +80,8 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
}
// Constructor
-Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& position,
- float mass, reactphysics3d::DynamicsWorld* world, const std::string& meshFolderPath)
+Box::Box(const openglframework::Vector3& size, float mass, reactphysics3d::DynamicsWorld* world,
+ const std::string& meshFolderPath)
: PhysicsObject(meshFolderPath + "cube.obj") {
// Load the mesh from a file
@@ -108,23 +101,15 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& p
0, 0, mSize[2], 0,
0, 0, 0, 1);
- // Initialize the position where the cube will be rendered
- translateWorld(position);
-
// Create the collision shape for the rigid body (box shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- mPreviousTransform = transform;
+ mPreviousTransform = rp3d::Transform::identity();
// Create a rigid body in the dynamics world
- rp3d::RigidBody* body = world->createRigidBody(transform);
+ rp3d::RigidBody* body = world->createRigidBody(mPreviousTransform);
// Add the collision shape to the body
mProxyShape = body->addCollisionShape(mBoxShape, rp3d::Transform::identity(), mass);
@@ -158,12 +143,7 @@ Box::~Box() {
}
// Render the cube at the correct position and with the correct orientation
-void Box::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
- }
+void Box::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the shader
shader.bind();
@@ -217,10 +197,6 @@ void Box::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
- }
}
// Create the Vertex Buffer Objects used to render to box with OpenGL.
diff --git a/testbed/common/Box.h b/testbed/common/Box.h
index bd7a6024..42d234f9 100644
--- a/testbed/common/Box.h
+++ b/testbed/common/Box.h
@@ -75,24 +75,22 @@ class Box : public PhysicsObject {
// -------------------- Methods -------------------- //
/// Constructor
- Box(const openglframework::Vector3& size, const openglframework::Vector3& position,
- reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath);
+ Box(const openglframework::Vector3& size, reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath);
/// Constructor
- Box(const openglframework::Vector3& size, const openglframework::Vector3& position,
- float mass, reactphysics3d::DynamicsWorld *world, const std::string& meshFolderPath);
+ Box(const openglframework::Vector3& size, float mass, reactphysics3d::DynamicsWorld *world, const std::string& meshFolderPath);
/// Destructor
~Box();
/// Render the cube at the correct position and with the correct orientation
- virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override;
+ virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
/// Set the scaling of the object
- void setScaling(const openglframework::Vector3& scaling);
+ void setScaling(const openglframework::Vector3& scaling) override;
};
// Update the transform matrix of the object
diff --git a/testbed/common/Capsule.cpp b/testbed/common/Capsule.cpp
index 413d6eb2..b48baf69 100644
--- a/testbed/common/Capsule.cpp
+++ b/testbed/common/Capsule.cpp
@@ -34,9 +34,7 @@ openglframework::VertexArrayObject Capsule::mVAO;
int Capsule::totalNbCapsules = 0;
// Constructor
-Capsule::Capsule(float radius, float height, const openglframework::Vector3& position,
- reactphysics3d::CollisionWorld* world,
- const std::string& meshFolderPath)
+Capsule::Capsule(float radius, float height, rp3d::CollisionWorld* world, const std::string& meshFolderPath)
: PhysicsObject(meshFolderPath + "capsule.obj"), mRadius(radius), mHeight(height) {
// Compute the scaling matrix
@@ -45,23 +43,16 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
0, 0, mRadius, 0,
0, 0, 0, 1.0f);
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
-
// Create the collision shape for the rigid body (sphere shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight);
+ //mCapsuleShape->setLocalScaling(rp3d::Vector3(2, 2, 2));
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- mPreviousTransform = transform;
+ mPreviousTransform = rp3d::Transform::identity();
// Create a rigid body corresponding in the dynamics world
- mBody = world->createCollisionBody(transform);
+ mBody = world->createCollisionBody(mPreviousTransform);
// Add a collision shape to the body and specify the mass of the shape
mProxyShape = mBody->addCollisionShape(mCapsuleShape, rp3d::Transform::identity());
@@ -77,8 +68,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
}
// Constructor
-Capsule::Capsule(float radius, float height, const openglframework::Vector3& position,
- float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
+Capsule::Capsule(float radius, float height, float mass, rp3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath)
: PhysicsObject(meshFolderPath + "capsule.obj"), mRadius(radius), mHeight(height) {
@@ -88,21 +78,15 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
0, 0, mRadius, 0,
0, 0, 0, 1.0f);
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
+ mPreviousTransform = rp3d::Transform::identity();
// Create the collision shape for the rigid body (sphere shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight);
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
// Create a rigid body corresponding in the dynamics world
- rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
+ rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform);
// Add a collision shape to the body and specify the mass of the shape
mProxyShape = body->addCollisionShape(mCapsuleShape, rp3d::Transform::identity(), mass);
@@ -140,11 +124,7 @@ Capsule::~Capsule() {
// Render the sphere at the correct position and with the correct orientation
void Capsule::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
- }
+ const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the shader
shader.bind();
@@ -198,10 +178,6 @@ void Capsule::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
- }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/Capsule.h b/testbed/common/Capsule.h
index 5c99120b..d0e74436 100644
--- a/testbed/common/Capsule.h
+++ b/testbed/common/Capsule.h
@@ -82,25 +82,23 @@ class Capsule : public PhysicsObject {
// -------------------- Methods -------------------- //
/// Constructor
- Capsule(float radius, float height, const openglframework::Vector3& position,
- reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath);
+ Capsule(float radius, float height, rp3d::CollisionWorld* world, const std::string& meshFolderPath);
/// Constructor
- Capsule(float radius, float height, const openglframework::Vector3& position,
- float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
+ Capsule(float radius, float height, float mass, rp3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath);
/// Destructor
~Capsule();
/// Render the sphere at the correct position and with the correct orientation
- virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override;
+ virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
/// Set the scaling of the object
- void setScaling(const openglframework::Vector3& scaling);
+ void setScaling(const openglframework::Vector3& scaling) override;
};
// Update the transform matrix of the object
diff --git a/testbed/common/ConcaveMesh.cpp b/testbed/common/ConcaveMesh.cpp
index c7e162a3..f6b04b9c 100644
--- a/testbed/common/ConcaveMesh.cpp
+++ b/testbed/common/ConcaveMesh.cpp
@@ -27,16 +27,11 @@
#include "ConcaveMesh.h"
// Constructor
-ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world,
- const std::string& meshPath)
+ConcaveMesh::ConcaveMesh(rp3d::CollisionWorld* world, const std::string& meshPath)
: PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
-
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
@@ -58,15 +53,10 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position,
// do not forget to delete it at the end
mConcaveShape = new rp3d::ConcaveMeshShape(&mPhysicsTriangleMesh);
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- mPreviousTransform = transform;
+ mPreviousTransform = rp3d::Transform::identity();
// Create a rigid body corresponding to the sphere in the dynamics world
- mBody = world->createCollisionBody(transform);
+ mBody = world->createCollisionBody(mPreviousTransform);
// Add a collision shape to the body and specify the mass of the collision shape
mProxyShape = mBody->addCollisionShape(mConcaveShape, rp3d::Transform::identity());
@@ -78,16 +68,11 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position,
}
// Constructor
-ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass,
- reactphysics3d::DynamicsWorld* dynamicsWorld,
- const std::string& meshPath)
+ConcaveMesh::ConcaveMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath)
: PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
-
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
@@ -109,13 +94,10 @@ ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass,
// do not forget to delete it at the end
mConcaveShape = new rp3d::ConcaveMeshShape(&mPhysicsTriangleMesh);
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
+ mPreviousTransform = rp3d::Transform::identity();
// Create a rigid body corresponding to the sphere in the dynamics world
- rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
+ rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform);
// Add a collision shape to the body and specify the mass of the collision shape
mProxyShape = body->addCollisionShape(mConcaveShape, rp3d::Transform::identity(), mass);
@@ -151,11 +133,7 @@ ConcaveMesh::~ConcaveMesh() {
// Render the sphere at the correct position and with the correct orientation
void ConcaveMesh::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
- }
+ const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the shader
shader.bind();
@@ -209,10 +187,6 @@ void ConcaveMesh::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
- }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/ConcaveMesh.h b/testbed/common/ConcaveMesh.h
index afda3d45..17755352 100644
--- a/testbed/common/ConcaveMesh.h
+++ b/testbed/common/ConcaveMesh.h
@@ -76,25 +76,23 @@ class ConcaveMesh : public PhysicsObject {
// -------------------- Methods -------------------- //
/// Constructor
- ConcaveMesh(const openglframework::Vector3& position,
- rp3d::CollisionWorld* world, const std::string& meshPath);
+ ConcaveMesh(rp3d::CollisionWorld* world, const std::string& meshPath);
/// Constructor
- ConcaveMesh(const openglframework::Vector3& position, float mass,
- rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath);
+ ConcaveMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath);
/// Destructor
~ConcaveMesh();
/// Render the mesh at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
+ const openglframework::Matrix4& worldToCameraMatrix) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
/// Set the scaling of the object
- void setScaling(const openglframework::Vector3& scaling);
+ void setScaling(const openglframework::Vector3& scaling) override;
};
// Update the transform matrix of the object
diff --git a/testbed/common/ConvexMesh.cpp b/testbed/common/ConvexMesh.cpp
index b3cb3b95..a59c2f3d 100644
--- a/testbed/common/ConvexMesh.cpp
+++ b/testbed/common/ConvexMesh.cpp
@@ -27,16 +27,11 @@
#include "ConvexMesh.h"
// Constructor
-ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world,
- const std::string& meshPath)
+ConvexMesh::ConvexMesh(rp3d::CollisionWorld* world, const std::string& meshPath)
: PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
-
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
@@ -64,15 +59,10 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
// do not forget to delete it at the end
mConvexShape = new rp3d::ConvexMeshShape(mPolyhedronMesh);
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- mPreviousTransform = transform;
+ mPreviousTransform = rp3d::Transform::identity();
// Create a rigid body corresponding to the sphere in the dynamics world
- mBody = world->createCollisionBody(transform);
+ mBody = world->createCollisionBody(mPreviousTransform);
// Add a collision shape to the body and specify the mass of the collision shape
mProxyShape = mBody->addCollisionShape(mConvexShape, rp3d::Transform::identity());
@@ -84,16 +74,11 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
}
// Constructor
-ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
- reactphysics3d::DynamicsWorld* dynamicsWorld,
- const std::string& meshPath)
+ConvexMesh::ConvexMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath)
: PhysicsObject(meshPath), mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
-
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
@@ -121,13 +106,10 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
// not forget to delete it at the end
mConvexShape = new rp3d::ConvexMeshShape(mPolyhedronMesh);
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
+ mPreviousTransform = rp3d::Transform::identity();
// Create a rigid body corresponding to the sphere in the dynamics world
- rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
+ rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform);
// Add a collision shape to the body and specify the mass of the collision shape
mProxyShape = body->addCollisionShape(mConvexShape, rp3d::Transform::identity(), mass);
@@ -161,11 +143,7 @@ ConvexMesh::~ConvexMesh() {
// Render the sphere at the correct position and with the correct orientation
void ConvexMesh::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
- }
+ const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the shader
shader.bind();
@@ -219,10 +197,6 @@ void ConvexMesh::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
- }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/ConvexMesh.h b/testbed/common/ConvexMesh.h
index 1b55611b..865921ca 100644
--- a/testbed/common/ConvexMesh.h
+++ b/testbed/common/ConvexMesh.h
@@ -79,24 +79,22 @@ class ConvexMesh : public PhysicsObject {
// -------------------- Methods -------------------- //
/// Constructor
- ConvexMesh(const openglframework::Vector3& position,
- rp3d::CollisionWorld* world, const std::string& meshPath);
+ ConvexMesh(rp3d::CollisionWorld* world, const std::string& meshPath);
/// Constructor
- ConvexMesh(const openglframework::Vector3& position, float mass,
- rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath);
+ ConvexMesh(float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshPath);
/// Destructor
~ConvexMesh();
/// Render the mesh at the correct position and with the correct orientation
- virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override;
+ virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
/// Set the scaling of the object
- void setScaling(const openglframework::Vector3& scaling);
+ void setScaling(const openglframework::Vector3& scaling) override;
};
// Update the transform matrix of the object
diff --git a/testbed/common/Dumbbell.cpp b/testbed/common/Dumbbell.cpp
index 02afe091..f90f0cc9 100644
--- a/testbed/common/Dumbbell.cpp
+++ b/testbed/common/Dumbbell.cpp
@@ -34,8 +34,7 @@ openglframework::VertexArrayObject Dumbbell::mVAO;
int Dumbbell::totalNbDumbbells = 0;
// Constructor
-Dumbbell::Dumbbell(const openglframework::Vector3 &position,
- reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath)
+Dumbbell::Dumbbell(rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath)
: PhysicsObject(meshFolderPath + "dumbbell.obj") {
// Identity scaling matrix
@@ -43,9 +42,6 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
mDistanceBetweenSphere = 8.0f;
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
-
// Create a sphere collision shape for the two ends of the dumbbell
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
@@ -61,13 +57,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
const rp3d::decimal massCylinder = rp3d::decimal(1.0);
mCapsuleShape = new rp3d::CapsuleShape(radiusCapsule, heightCapsule);
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::decimal angleAroundX = 0;//rp3d::PI / 2;
- rp3d::Quaternion initOrientation(angleAroundX, 0, 0);
- rp3d::Transform transformBody(initPosition, initOrientation);
-
- mPreviousTransform = transformBody;
+ mPreviousTransform = rp3d::Transform::identity();
// Initial transform of the first sphere collision shape of the dumbbell (in local-space)
rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity());
@@ -79,7 +69,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity());
// Create a rigid body corresponding to the dumbbell in the dynamics world
- rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transformBody);
+ rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform);
// Add the three collision shapes to the body and specify the mass and transform of the shapes
mProxyShapeSphere1 = body->addCollisionShape(mSphereShape, transformSphereShape1, massSphere);
@@ -99,8 +89,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
}
// Constructor
-Dumbbell::Dumbbell(const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath)
+Dumbbell::Dumbbell(rp3d::CollisionWorld* world, const std::string& meshFolderPath)
: PhysicsObject(meshFolderPath + "dumbbell.obj"){
// Identity scaling matrix
@@ -108,9 +97,6 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
mDistanceBetweenSphere = 8.0f;
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
-
// Create a sphere collision shape for the two ends of the dumbbell
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
@@ -124,12 +110,6 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
const rp3d::decimal heightCapsule = rp3d::decimal(7.0);
mCapsuleShape = new rp3d::CapsuleShape(radiusCapsule, heightCapsule);
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::decimal angleAroundX = 0;//rp3d::PI / 2;
- rp3d::Quaternion initOrientation(angleAroundX, 0, 0);
- rp3d::Transform transformBody(initPosition, initOrientation);
-
// Initial transform of the first sphere collision shape of the dumbbell (in local-space)
rp3d::Transform transformSphereShape1(rp3d::Vector3(0, mDistanceBetweenSphere / 2.0f, 0), rp3d::Quaternion::identity());
@@ -139,8 +119,10 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
// Initial transform of the cylinder collision shape of the dumbell (in local-space)
rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity());
+ mPreviousTransform = rp3d::Transform::identity();
+
// Create a rigid body corresponding to the dumbbell in the dynamics world
- mBody = world->createCollisionBody(transformBody);
+ mBody = world->createCollisionBody(mPreviousTransform);
// Add the three collision shapes to the body and specify the mass and transform of the shapes
mProxyShapeSphere1 = mBody->addCollisionShape(mSphereShape, transformSphereShape1);
@@ -179,11 +161,7 @@ Dumbbell::~Dumbbell() {
// Render the sphere at the correct position and with the correct orientation
void Dumbbell::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
- }
+ const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the shader
shader.bind();
@@ -237,10 +215,6 @@ void Dumbbell::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
- }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/Dumbbell.h b/testbed/common/Dumbbell.h
index aa6bded1..135beabc 100644
--- a/testbed/common/Dumbbell.h
+++ b/testbed/common/Dumbbell.h
@@ -82,24 +82,22 @@ class Dumbbell : public PhysicsObject {
// -------------------- Methods -------------------- //
/// Constructor
- Dumbbell(const openglframework::Vector3& position, rp3d::DynamicsWorld* dynamicsWorld,
- const std::string& meshFolderPath);
+ Dumbbell(rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath);
/// Constructor
- Dumbbell(const openglframework::Vector3& position, rp3d::CollisionWorld* world,
- const std::string& meshFolderPath);
+ Dumbbell(rp3d::CollisionWorld* world, const std::string& meshFolderPath);
/// Destructor
~Dumbbell();
/// Render the sphere at the correct position and with the correct orientation
- virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override;
+ virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
/// Set the scaling of the object
- void setScaling(const openglframework::Vector3& scaling);
+ void setScaling(const openglframework::Vector3& scaling) override;
};
// Update the transform matrix of the object
diff --git a/testbed/common/HeightField.cpp b/testbed/common/HeightField.cpp
index 377cab07..e979936d 100644
--- a/testbed/common/HeightField.cpp
+++ b/testbed/common/HeightField.cpp
@@ -28,15 +28,11 @@
#include "PerlinNoise.h"
// Constructor
-HeightField::HeightField(const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world)
+HeightField::HeightField(rp3d::CollisionWorld* world)
: mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
-
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
@@ -51,15 +47,10 @@ HeightField::HeightField(const openglframework::Vector3 &position,
mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight,
mHeightData, rp3d::HeightFieldShape::HeightDataType::HEIGHT_FLOAT_TYPE);
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- mPreviousTransform = transform;
+ mPreviousTransform = rp3d::Transform::identity();
// Create a rigid body corresponding to the sphere in the dynamics world
- mBody = world->createCollisionBody(transform);
+ mBody = world->createCollisionBody(mPreviousTransform);
// Add a collision shape to the body and specify the mass of the collision shape
mProxyShape = mBody->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity());
@@ -71,15 +62,11 @@ HeightField::HeightField(const openglframework::Vector3 &position,
}
// Constructor
-HeightField::HeightField(const openglframework::Vector3 &position, float mass,
- reactphysics3d::DynamicsWorld* dynamicsWorld)
+HeightField::HeightField(float mass, rp3d::DynamicsWorld* dynamicsWorld)
: mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
-
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
@@ -94,13 +81,10 @@ HeightField::HeightField(const openglframework::Vector3 &position, float mass,
mHeightFieldShape = new rp3d::HeightFieldShape(NB_POINTS_WIDTH, NB_POINTS_LENGTH, mMinHeight, mMaxHeight,
mHeightData, rp3d::HeightFieldShape::HeightDataType::HEIGHT_FLOAT_TYPE);
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
+ mPreviousTransform = rp3d::Transform::identity();
// Create a rigid body corresponding to the sphere in the dynamics world
- rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
+ rp3d::RigidBody* body = dynamicsWorld->createRigidBody(mPreviousTransform);
// Add a collision shape to the body and specify the mass of the collision shape
mProxyShape = body->addCollisionShape(mHeightFieldShape, rp3d::Transform::identity(), mass);
@@ -131,11 +115,7 @@ HeightField::~HeightField() {
// Render the sphere at the correct position and with the correct orientation
void HeightField::render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) {
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
- }
+ const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the shader
shader.bind();
@@ -189,10 +169,6 @@ void HeightField::render(openglframework::Shader& shader,
// Unbind the shader
shader.unbind();
-
- if (wireframe) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
- }
}
// Compute the heights of the height field
diff --git a/testbed/common/HeightField.h b/testbed/common/HeightField.h
index acb39163..1ff390ee 100644
--- a/testbed/common/HeightField.h
+++ b/testbed/common/HeightField.h
@@ -89,25 +89,23 @@ class HeightField : public PhysicsObject {
// -------------------- Methods -------------------- //
/// Constructor
- HeightField(const openglframework::Vector3& position,
- rp3d::CollisionWorld* world);
+ HeightField(rp3d::CollisionWorld* world);
/// Constructor
- HeightField(const openglframework::Vector3& position, float mass,
- rp3d::DynamicsWorld* dynamicsWorld);
+ HeightField(float mass, rp3d::DynamicsWorld* dynamicsWorld);
/// Destructor
~HeightField();
/// Render the mesh at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix, bool wireframe);
+ const openglframework::Matrix4& worldToCameraMatrix) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
/// Set the scaling of the object
- void setScaling(const openglframework::Vector3& scaling);
+ void setScaling(const openglframework::Vector3& scaling) override;
};
// Update the transform matrix of the object
diff --git a/testbed/common/PhysicsObject.h b/testbed/common/PhysicsObject.h
index 7fcc612d..e1714048 100644
--- a/testbed/common/PhysicsObject.h
+++ b/testbed/common/PhysicsObject.h
@@ -63,7 +63,7 @@ class PhysicsObject : public openglframework::Mesh {
virtual void updateTransform(float interpolationFactor)=0;
/// Render the sphere at the correct position and with the correct orientation
- virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe)=0;
+ virtual void render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix)=0;
/// Set the color of the box
void setColor(const openglframework::Color& color);
diff --git a/testbed/common/Sphere.cpp b/testbed/common/Sphere.cpp
index 9eaf62c2..f7c53579 100644
--- a/testbed/common/Sphere.cpp
+++ b/testbed/common/Sphere.cpp
@@ -34,8 +34,7 @@ openglframework::VertexArrayObject Sphere::mVAO;
int Sphere::totalNbSpheres = 0;
// Constructor
-Sphere::Sphere(float radius, const openglframework::Vector3 &position,
- reactphysics3d::CollisionWorld* world,
+Sphere::Sphere(float radius, rp3d::CollisionWorld* world,
const std::string& meshFolderPath)
: PhysicsObject(meshFolderPath + "sphere.obj"), mRadius(radius) {
@@ -45,23 +44,16 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
0, 0, mRadius, 0,
0, 0, 0, 1);
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
-
// Create the collision shape for the rigid body (sphere shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
mCollisionShape = new rp3d::SphereShape(mRadius);
+ //mCollisionShape->setLocalScaling(rp3d::Vector3(2, 2, 2));
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- mPreviousTransform = transform;
+ mPreviousTransform = rp3d::Transform::identity();
// Create a rigid body corresponding to the sphere in the dynamics world
- mBody = world->createCollisionBody(transform);
+ mBody = world->createCollisionBody(mPreviousTransform);
// Add a collision shape to the body and specify the mass of the shape
mProxyShape = mBody->addCollisionShape(mCollisionShape, rp3d::Transform::identity());
@@ -77,8 +69,7 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
}
// Constructor
-Sphere::Sphere(float radius, const openglframework::Vector3 &position,
- float mass, reactphysics3d::DynamicsWorld* world,
+Sphere::Sphere(float radius, float mass, reactphysics3d::DynamicsWorld* world,
const std::string& meshFolderPath)
: PhysicsObject(meshFolderPath + "sphere.obj"), mRadius(radius) {
@@ -88,21 +79,15 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
0, 0, mRadius, 0,
0, 0, 0, 1);
- // Initialize the position where the sphere will be rendered
- translateWorld(position);
-
// Create the collision shape for the rigid body (sphere shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
mCollisionShape = new rp3d::SphereShape(mRadius);
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
+ mPreviousTransform = rp3d::Transform::identity();
// Create a rigid body corresponding to the sphere in the dynamics world
- rp3d::RigidBody* body = world->createRigidBody(transform);
+ rp3d::RigidBody* body = world->createRigidBody(mPreviousTransform);
// Add a collision shape to the body and specify the mass of the shape
mProxyShape = body->addCollisionShape(mCollisionShape, rp3d::Transform::identity(), mass);
@@ -138,11 +123,7 @@ Sphere::~Sphere() {
}
// Render the sphere at the correct position and with the correct orientation
-void Sphere::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireFrame) {
-
- if (wireFrame) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
- }
+void Sphere::render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the shader
shader.bind();
@@ -196,10 +177,6 @@ void Sphere::render(openglframework::Shader& shader, const openglframework::Matr
// Unbind the shader
shader.unbind();
-
- if (wireFrame) {
- glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
- }
}
// Create the Vertex Buffer Objects used to render with OpenGL.
diff --git a/testbed/common/Sphere.h b/testbed/common/Sphere.h
index 000cc776..fe2b3f83 100644
--- a/testbed/common/Sphere.h
+++ b/testbed/common/Sphere.h
@@ -79,24 +79,22 @@ class Sphere : public PhysicsObject {
// -------------------- Methods -------------------- //
/// Constructor
- Sphere(float radius, const openglframework::Vector3& position,
- rp3d::CollisionWorld* world, const std::string& meshFolderPath);
+ Sphere(float radius, rp3d::CollisionWorld* world, const std::string& meshFolderPath);
/// Constructor
- Sphere(float radius, const openglframework::Vector3& position,
- float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath);
+ Sphere(float radius, float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath);
/// Destructor
~Sphere();
/// Render the sphere at the correct position and with the correct orientation
- void virtual render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix, bool wireframe) override;
+ void virtual render(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) override;
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor) override;
/// Set the scaling of the object
- void setScaling(const openglframework::Vector3& scaling);
+ void setScaling(const openglframework::Vector3& scaling) override;
};
// Update the transform matrix of the object
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index 3b186943..f00dbdba 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -31,8 +31,8 @@ using namespace openglframework;
using namespace collisiondetectionscene;
// Constructor
-CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
- : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
+CollisionDetectionScene::CollisionDetectionScene(const std::string& name, EngineSettings& settings)
+ : SceneDemo(name, settings, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
mContactManager(mPhongShader, mMeshFolderPath),
mAreNormalsDisplayed(false) {
@@ -47,61 +47,57 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
setScenePosition(center, SCENE_RADIUS);
// Create the dynamics world for the physics simulation
- mCollisionWorld = new rp3d::CollisionWorld();
+ mPhysicsWorld = new rp3d::CollisionWorld();
// ---------- Sphere 1 ---------- //
- openglframework::Vector3 position1(12, 0, 0);
// Create a sphere and a corresponding collision body in the dynamics world
- mSphere1 = new Sphere(4, position1, mCollisionWorld, mMeshFolderPath);
+ mSphere1 = new Sphere(4, mPhysicsWorld, mMeshFolderPath);
mAllShapes.push_back(mSphere1);
// Set the color
mSphere1->setColor(mGreyColorDemo);
mSphere1->setSleepingColor(mRedColorDemo);
- mPhysicsObjects.push_back(mSphere1);
+ //mSphere1->setScaling(0.5f);
+ mPhysicsObjects.push_back(mSphere1);
// ---------- Sphere 2 ---------- //
- 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);
+ mSphere2 = new Sphere(2, mPhysicsWorld, mMeshFolderPath);
mAllShapes.push_back(mSphere2);
// Set the color
mSphere2->setColor(mGreyColorDemo);
mSphere2->setSleepingColor(mRedColorDemo);
- mPhysicsObjects.push_back(mSphere2);
+ mPhysicsObjects.push_back(mSphere2);
- // ---------- Capsule 1 ---------- //
- openglframework::Vector3 position3(-6, 7, 0);
-
- // Create a cylinder and a corresponding collision body in the dynamics world
- mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position3, mCollisionWorld, mMeshFolderPath);
- mAllShapes.push_back(mCapsule1);
-
- // Set the color
- mCapsule1->setColor(mGreyColorDemo);
- mCapsule1->setSleepingColor(mRedColorDemo);
- mPhysicsObjects.push_back(mCapsule1);
-
- // ---------- Capsule 2 ---------- //
- openglframework::Vector3 position4(11, -8, 0);
+ // ---------- Capsule 1 ---------- //
// Create a cylinder and a corresponding collision body in the dynamics world
- mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position4, mCollisionWorld, mMeshFolderPath);
+ mCapsule1 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsWorld, mMeshFolderPath);
+ mAllShapes.push_back(mCapsule1);
+
+ // Set the color
+ mCapsule1->setColor(mGreyColorDemo);
+ mCapsule1->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mCapsule1);
+
+ // ---------- Capsule 2 ---------- //
+
+ // Create a cylinder and a corresponding collision body in the dynamics world
+ mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsWorld, mMeshFolderPath);
mAllShapes.push_back(mCapsule2);
// Set the color
mCapsule2->setColor(mGreyColorDemo);
mCapsule2->setSleepingColor(mRedColorDemo);
- mPhysicsObjects.push_back(mCapsule2);
+ mPhysicsObjects.push_back(mCapsule2);
- // ---------- Box 1 ---------- //
- openglframework::Vector3 position5(-4, -7, 0);
+ // ---------- Box 1 ---------- //
// Create a cylinder and a corresponding collision body in the dynamics world
- mBox1 = new Box(BOX_SIZE, position5, mCollisionWorld, mMeshFolderPath);
+ mBox1 = new Box(BOX_SIZE, mPhysicsWorld, mMeshFolderPath);
mAllShapes.push_back(mBox1);
// Set the color
@@ -110,10 +106,9 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mPhysicsObjects.push_back(mBox1);
// ---------- Box 2 ---------- //
- 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);
+ mBox2 = new Box(openglframework::Vector3(3, 2, 5), mPhysicsWorld, mMeshFolderPath);
mAllShapes.push_back(mBox2);
// Set the color
@@ -122,10 +117,9 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mPhysicsObjects.push_back(mBox2);
// ---------- Convex Mesh ---------- //
- openglframework::Vector3 position7(-5, 0, 0);
// Create a convex mesh and a corresponding collision body in the dynamics world
- mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj");
+ mConvexMesh = new ConvexMesh(mPhysicsWorld, mMeshFolderPath + "convexmesh.obj");
mAllShapes.push_back(mConvexMesh);
// Set the color
@@ -134,10 +128,9 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mPhysicsObjects.push_back(mConvexMesh);
// ---------- Concave Mesh ---------- //
- openglframework::Vector3 position8(0, 100, 0);
// Create a convex mesh and a corresponding collision body in the dynamics world
- mConcaveMesh = new ConcaveMesh(position8, mCollisionWorld, mMeshFolderPath + "city.obj");
+ mConcaveMesh = new ConcaveMesh(mPhysicsWorld, mMeshFolderPath + "city.obj");
mAllShapes.push_back(mConcaveMesh);
// Set the color
@@ -146,10 +139,9 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
mPhysicsObjects.push_back(mConcaveMesh);
// ---------- Heightfield ---------- //
- openglframework::Vector3 position9(0, -12, 0);
// Create a convex mesh and a corresponding collision body in the dynamics world
- mHeightField = new HeightField(position9, mCollisionWorld);
+ mHeightField = new HeightField(mPhysicsWorld);
// Set the color
mHeightField->setColor(mGreyColorDemo);
@@ -162,76 +154,59 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name)
// Reset the scene
void CollisionDetectionScene::reset() {
+ mSphere1->setTransform(rp3d::Transform(rp3d::Vector3(12, 0, 0), rp3d::Quaternion::identity()));
+ mSphere2->setTransform(rp3d::Transform(rp3d::Vector3(12, 8, 0), rp3d::Quaternion::identity()));
+ mCapsule1->setTransform(rp3d::Transform(rp3d::Vector3(-6, 7, 0), rp3d::Quaternion::identity()));
+ mCapsule2->setTransform(rp3d::Transform(rp3d::Vector3(11, -8, 0), rp3d::Quaternion::identity()));
+ mBox1->setTransform(rp3d::Transform(rp3d::Vector3(-4, -7, 0), rp3d::Quaternion::identity()));
+ mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 8, 0), rp3d::Quaternion::identity()));
+ mConvexMesh->setTransform(rp3d::Transform(rp3d::Vector3(-5, 0, 0), rp3d::Quaternion::identity()));
+ mConcaveMesh->setTransform(rp3d::Transform(rp3d::Vector3(0, 100, 0), rp3d::Quaternion::identity()));
+ mHeightField->setTransform(rp3d::Transform(rp3d::Vector3(0, -12, 0), rp3d::Quaternion::identity()));
}
// Destructor
CollisionDetectionScene::~CollisionDetectionScene() {
// Destroy the box rigid body from the dynamics world
- //mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody());
+ //mPhysicsWorld->destroyCollisionBody(mBox->getCollisionBody());
//delete mBox;
// Destroy the spheres
- mCollisionWorld->destroyCollisionBody(mSphere1->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mSphere1->getCollisionBody());
delete mSphere1;
- mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mSphere2->getCollisionBody());
delete mSphere2;
- mCollisionWorld->destroyCollisionBody(mCapsule1->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mCapsule1->getCollisionBody());
delete mCapsule1;
- mCollisionWorld->destroyCollisionBody(mCapsule2->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mCapsule2->getCollisionBody());
delete mCapsule2;
- mCollisionWorld->destroyCollisionBody(mBox1->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mBox1->getCollisionBody());
delete mBox1;
- mCollisionWorld->destroyCollisionBody(mBox2->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mBox2->getCollisionBody());
delete mBox2;
- mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mConvexMesh->getCollisionBody());
delete mConvexMesh;
- mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody());
delete mConcaveMesh;
- mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mHeightField->getCollisionBody());
delete mHeightField;
- /*
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody());
- delete mCone;
-
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody());
-
- // Destroy the sphere
- delete mCylinder;
-
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody());
-
- // Destroy the sphere
- delete mCapsule;
-
- // Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody());
-
- // Destroy the dumbbell
- delete mDumbbell;
-
-
- */
-
mContactManager.resetPoints();
// Destroy the static data for the visual contact points
VisualContactPoint::destroyStaticData();
// Destroy the collision world
- delete mCollisionWorld;
+ delete mPhysicsWorld;
}
// Take a step for the simulation
@@ -239,14 +214,14 @@ void CollisionDetectionScene::update() {
mContactManager.resetPoints();
- mCollisionWorld->testCollision(&mContactManager);
+ mPhysicsWorld->testCollision(&mContactManager);
SceneDemo::update();
}
void CollisionDetectionScene::selectNextShape() {
- int previousIndex = mSelectedShapeIndex;
+ uint previousIndex = mSelectedShapeIndex;
mSelectedShapeIndex++;
if (mSelectedShapeIndex >= mAllShapes.size()) {
mSelectedShapeIndex = 0;
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index 8473ae73..4bea0c0b 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -152,10 +152,7 @@ class CollisionDetectionScene : public SceneDemo {
std::vector<PhysicsObject*> mAllShapes;
- int mSelectedShapeIndex;
-
- /// Collision world used for the physics simulation
- rp3d::CollisionWorld* mCollisionWorld;
+ uint mSelectedShapeIndex;
/// Select the next shape
void selectNextShape();
@@ -165,7 +162,7 @@ class CollisionDetectionScene : public SceneDemo {
// -------------------- Methods -------------------- //
/// Constructor
- CollisionDetectionScene(const std::string& name);
+ CollisionDetectionScene(const std::string& name, EngineSettings& settings);
/// Destructor
virtual ~CollisionDetectionScene() override;
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
index e63f39d4..a6fd6662 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
@@ -31,8 +31,8 @@ using namespace openglframework;
using namespace collisionshapesscene;
// Constructor
-CollisionShapesScene::CollisionShapesScene(const std::string& name)
- : SceneDemo(name, SCENE_RADIUS) {
+CollisionShapesScene::CollisionShapesScene(const std::string& name, EngineSettings& settings)
+ : SceneDemo(name, settings, SCENE_RADIUS) {
std::string meshFolderPath("meshes/");
@@ -43,23 +43,15 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
setScenePosition(center, SCENE_RADIUS);
// Gravity vector in the dynamics world
- rp3d::Vector3 gravity(0, -9.81, 0);
+ rp3d::Vector3 gravity(0, -9.81f, 0);
// Create the dynamics world for the physics simulation
- mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
-
- float radius = 3.0f;
+ mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
for (int i=0; i<NB_COMPOUND_SHAPES; i++) {
- // Position
- float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 100 + i * (DUMBBELL_HEIGHT + 0.3f),
- radius * sin(angle));
-
// Create a convex mesh and a corresponding rigid in the dynamics world
- Dumbbell* dumbbell = new Dumbbell(position, mDynamicsWorld, meshFolderPath);
+ Dumbbell* dumbbell = new Dumbbell(getDynamicsWorld(), meshFolderPath);
// Set the box color
dumbbell->setColor(mDemoColors[i % mNbDemoColors]);
@@ -77,14 +69,8 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// Create all the boxes of the scene
for (int i=0; i<NB_BOXES; i++) {
- // Position
- float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 60 + i * (BOX_SIZE.y + 0.8f),
- radius * sin(angle));
-
// Create a sphere and a corresponding rigid in the dynamics world
- Box* box = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld, mMeshFolderPath);
+ Box* box = new Box(BOX_SIZE, BOX_MASS, getDynamicsWorld(), mMeshFolderPath);
// Set the box color
box->setColor(mDemoColors[i % mNbDemoColors]);
@@ -102,18 +88,11 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// Create all the spheres of the scene
for (int i=0; i<NB_SPHERES; i++) {
- // Position
- float angle = i * 35.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 50 + i * (SPHERE_RADIUS + 0.8f),
- radius * sin(angle));
-
// Create a sphere and a corresponding rigid in the dynamics world
- Sphere* sphere = new Sphere(SPHERE_RADIUS, position , BOX_MASS, mDynamicsWorld,
- meshFolderPath);
+ Sphere* sphere = new Sphere(SPHERE_RADIUS, BOX_MASS, getDynamicsWorld(), meshFolderPath);
// Add some rolling resistance
- sphere->getRigidBody()->getMaterial().setRollingResistance(0.08);
+ sphere->getRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08));
// Set the box color
sphere->setColor(mDemoColors[i % mNbDemoColors]);
@@ -131,17 +110,11 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// Create all the capsules of the scene
for (int i=0; i<NB_CAPSULES; i++) {
- // Position
- float angle = i * 45.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 15 + i * (CAPSULE_HEIGHT + 0.3f),
- radius * sin(angle));
-
// Create a cylinder and a corresponding rigid in the dynamics world
- Capsule* capsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position ,
- CAPSULE_MASS, mDynamicsWorld, meshFolderPath);
+ Capsule* capsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, CAPSULE_MASS,
+ getDynamicsWorld(), meshFolderPath);
- capsule->getRigidBody()->getMaterial().setRollingResistance(0.08);
+ capsule->getRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08f));
// Set the box color
capsule->setColor(mDemoColors[i % mNbDemoColors]);
@@ -159,14 +132,8 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// Create all the convex meshes of the scene
for (int i=0; i<NB_MESHES; i++) {
- // Position
- float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 5 + i * (CAPSULE_HEIGHT + 0.3f),
- radius * sin(angle));
-
// Create a convex mesh and a corresponding rigid in the dynamics world
- ConvexMesh* mesh = new ConvexMesh(position, MESH_MASS, mDynamicsWorld, meshFolderPath + "convexmesh.obj");
+ ConvexMesh* mesh = new ConvexMesh(MESH_MASS, getDynamicsWorld(), meshFolderPath + "convexmesh.obj");
// Set the box color
mesh->setColor(mDemoColors[i % mNbDemoColors]);
@@ -183,8 +150,7 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// ---------- Create the floor ---------
- openglframework::Vector3 floorPosition(0, 0, 0);
- mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath);
+ mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath);
mPhysicsObjects.push_back(mFloor);
// Set the box color
@@ -198,39 +164,16 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
rp3d::Material& material = mFloor->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.2));
- // ---------- Create the triangular mesh ---------- //
-
- /*
- // Position
- openglframework::Vector3 position(0, 0, 0);
- rp3d::decimal mass = 1.0;
-
- // Create a convex mesh and a corresponding rigid in the dynamics world
- mConcaveMesh = new ConcaveMesh(position, mass, mDynamicsWorld, meshFolderPath);
-
- // Set the mesh as beeing static
- mConcaveMesh->getRigidBody()->setType(rp3d::STATIC);
-
- // Set the box color
- mConcaveMesh->setColor(mDemoColors[0]);
- mConcaveMesh->setSleepingColor(mRedColorDemo);
-
- // Change the material properties of the rigid body
- rp3d::Material& material = mConcaveMesh->getRigidBody()->getMaterial();
- material.setBounciness(rp3d::decimal(0.2));
- material.setFrictionCoefficient(0.1);
- */
-
// Get the physics engine parameters
- mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled();
- rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity();
+ mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled();
+ rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity();
mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z);
- mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled();
- mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity();
- mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity();
- mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver();
- mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver();
- mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep();
+ mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled();
+ mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity();
+ mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity();
+ mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver();
+ mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver();
+ mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep();
}
// Destructor
@@ -240,126 +183,81 @@ CollisionShapesScene::~CollisionShapesScene() {
for (std::vector<PhysicsObject*>::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
// Destroy the corresponding rigid body from the dynamics world
- mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
+ getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody());
// Destroy the object
delete (*it);
}
// Destroy the dynamics world
- delete mDynamicsWorld;
-}
-
-// Update the physics world (take a simulation step)
-void CollisionShapesScene::updatePhysics() {
-
- // Update the physics engine parameters
- mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled);
- rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y,
- mEngineSettings.gravity.z);
- mDynamicsWorld->setGravity(gravity);
- mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled);
- mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity);
- mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity);
- mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations);
- mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations);
- mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep);
-
- // Take a simulation step
- mDynamicsWorld->update(mEngineSettings.timeStep);
+ delete mPhysicsWorld;
}
/// Reset the scene
void CollisionShapesScene::reset() {
- float radius = 3.0f;
+ const float radius = 3.0f;
- for (int i=0; i<NB_COMPOUND_SHAPES; i++) {
+ for (uint i = 0; i<NB_COMPOUND_SHAPES; i++) {
// Position
float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 100 + i * (DUMBBELL_HEIGHT + 0.3f),
- radius * sin(angle));
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 125 + i * (DUMBBELL_HEIGHT + 0.3f),
+ radius * std::sin(angle));
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- // Reset the transform
- mDumbbells[i]->setTransform(transform);
+ mDumbbells[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
}
// Create all the boxes of the scene
- for (int i=0; i<NB_BOXES; i++) {
+ for (uint i = 0; i<NB_BOXES; i++) {
// Position
float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 60 + i * (BOX_SIZE.y + 0.8f),
- radius * sin(angle));
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 85 + i * (BOX_SIZE.y + 0.8f),
+ radius * std::sin(angle));
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- // Reset the transform
- mBoxes[i]->setTransform(transform);
+ mBoxes[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
}
// Create all the spheres of the scene
- for (int i=0; i<NB_SPHERES; i++) {
+ for (uint i = 0; i<NB_SPHERES; i++) {
// Position
float angle = i * 35.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 50 + i * (SPHERE_RADIUS + 0.8f),
- radius * sin(angle));
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 75 + i * (SPHERE_RADIUS + 0.8f),
+ radius * std::sin(angle));
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- // Reset the transform
- mSpheres[i]->setTransform(transform);
+ mSpheres[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
}
// Create all the capsules of the scene
- for (int i=0; i<NB_CAPSULES; i++) {
+ for (uint i = 0; i<NB_CAPSULES; i++) {
// Position
float angle = i * 45.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 15 + i * (CAPSULE_HEIGHT + 0.3f),
- radius * sin(angle));
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 40 + i * (CAPSULE_HEIGHT + 0.3f),
+ radius * std::sin(angle));
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- // Reset the transform
- mCapsules[i]->setTransform(transform);
+ mCapsules[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
}
// Create all the convex meshes of the scene
- for (int i=0; i<NB_MESHES; i++) {
+ for (uint i = 0; i<NB_MESHES; i++) {
// Position
float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 5 + i * (CAPSULE_HEIGHT + 0.3f),
- radius * sin(angle));
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 30 + i * (CAPSULE_HEIGHT + 0.3f),
+ radius * std::sin(angle));
- // Initial position and orientation of the rigid body
- rp3d::Vector3 initPosition(position.x, position.y, position.z);
- rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
- rp3d::Transform transform(initPosition, initOrientation);
-
- // Reset the transform
- mConvexMeshes[i]->setTransform(transform);
+ mConvexMeshes[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
}
+
+ // ---------- Create the triangular mesh ---------- //
+
+ mFloor->setTransform(rp3d::Transform::identity());
}
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.h b/testbed/scenes/collisionshapes/CollisionShapesScene.h
index 70e28755..b22d8429 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.h
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.h
@@ -45,7 +45,7 @@ const float SCENE_RADIUS = 30.0f;
const int NB_BOXES = 5;
const int NB_SPHERES = 5;
const int NB_CAPSULES = 5;
-const int NB_MESHES = 3;
+const int NB_MESHES = 4;
const int NB_COMPOUND_SHAPES = 3;
const openglframework::Vector3 BOX_SIZE(2, 2, 2);
const float SPHERE_RADIUS = 1.5f;
@@ -87,23 +87,16 @@ class CollisionShapesScene : public SceneDemo {
/// Box for the floor
Box* mFloor;
- /// Dynamics world used for the physics simulation
- rp3d::DynamicsWorld* mDynamicsWorld;
-
public:
// -------------------- Methods -------------------- //
/// Constructor
- CollisionShapesScene(const std::string& name);
+ CollisionShapesScene(const std::string& name, EngineSettings& settings);
/// Destructor
virtual ~CollisionShapesScene() override;
- /// Update the physics world (take a simulation step)
- /// Can be called several times per frame
- virtual void updatePhysics() override;
-
/// Reset the scene
virtual void reset() override;
@@ -113,7 +106,7 @@ class CollisionShapesScene : public SceneDemo {
// Return all the contact points of the scene
inline std::vector<ContactPoint> CollisionShapesScene::getContactPoints() const {
- return computeContactPointsOfWorld(mDynamicsWorld);
+ return computeContactPointsOfWorld(getDynamicsWorld());
}
}
diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
index 11474819..d70a02af 100644
--- a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
+++ b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
@@ -31,8 +31,8 @@ using namespace openglframework;
using namespace trianglemeshscene;
// Constructor
-ConcaveMeshScene::ConcaveMeshScene(const std::string& name)
- : SceneDemo(name, SCENE_RADIUS) {
+ConcaveMeshScene::ConcaveMeshScene(const std::string& name, EngineSettings& settings)
+ : SceneDemo(name, settings, SCENE_RADIUS) {
std::string meshFolderPath("meshes/");
@@ -46,38 +46,115 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name)
rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0);
// Create the dynamics world for the physics simulation
- mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
+ mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
- // ---------- Create the boxes ----------- //
+ for (int i = 0; i<NB_COMPOUND_SHAPES; i++) {
- for (int i=0; i<NB_BOXES_X; i++) {
+ // Create a convex mesh and a corresponding rigid in the dynamics world
+ Dumbbell* dumbbell = new Dumbbell(getDynamicsWorld(), meshFolderPath);
- for (int j=0; j<NB_BOXES_Z; j++) {
+ // Set the box color
+ dumbbell->setColor(mDemoColors[i % mNbDemoColors]);
+ dumbbell->setSleepingColor(mRedColorDemo);
- // Position
- openglframework::Vector3 boxPosition(-NB_BOXES_X * BOX_SIZE * BOXES_SPACE / 2 + i * BOX_SIZE * BOXES_SPACE, 30, -NB_BOXES_Z * BOX_SIZE * BOXES_SPACE / 2 + j * BOX_SIZE * BOXES_SPACE);
+ // Change the material properties of the rigid body
+ rp3d::Material& material = dumbbell->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.2));
- // Create a sphere and a corresponding rigid in the dynamics world
- mBoxes[i * NB_BOXES_Z + j] = new Box(Vector3(BOX_SIZE, BOX_SIZE, BOX_SIZE) * 0.5f, boxPosition, 80.1, mDynamicsWorld, mMeshFolderPath);
+ // Add the mesh the list of dumbbells in the scene
+ mDumbbells.push_back(dumbbell);
+ mPhysicsObjects.push_back(dumbbell);
+ }
- // Set the sphere color
- mBoxes[i * NB_BOXES_Z + j]->setColor(mDemoColors[0]);
- mBoxes[i * NB_BOXES_Z + j]->setSleepingColor(mRedColorDemo);
+ // Create all the boxes of the scene
+ for (int i = 0; i<NB_BOXES; i++) {
- // Change the material properties of the rigid body
- rp3d::Material& boxMaterial = mBoxes[i * NB_BOXES_Z + j]->getRigidBody()->getMaterial();
- boxMaterial.setBounciness(rp3d::decimal(0.2));
- }
+ // Create a sphere and a corresponding rigid in the dynamics world
+ Box* box = new Box(BOX_SIZE, BOX_MASS, getDynamicsWorld(), mMeshFolderPath);
+
+ // Set the box color
+ box->setColor(mDemoColors[i % mNbDemoColors]);
+ box->setSleepingColor(mRedColorDemo);
+
+ // Change the material properties of the rigid body
+ rp3d::Material& material = box->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.2));
+
+ // Add the sphere the list of sphere in the scene
+ mBoxes.push_back(box);
+ mPhysicsObjects.push_back(box);
+ }
+
+ // Create all the spheres of the scene
+ for (int i = 0; i<NB_SPHERES; i++) {
+
+ // Create a sphere and a corresponding rigid in the dynamics world
+ Sphere* sphere = new Sphere(SPHERE_RADIUS, BOX_MASS, getDynamicsWorld(), meshFolderPath);
+
+ // Add some rolling resistance
+ sphere->getRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08));
+
+ // Set the box color
+ sphere->setColor(mDemoColors[i % mNbDemoColors]);
+ sphere->setSleepingColor(mRedColorDemo);
+
+ // Change the material properties of the rigid body
+ rp3d::Material& material = sphere->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.2));
+
+ // Add the sphere the list of sphere in the scene
+ mSpheres.push_back(sphere);
+ mPhysicsObjects.push_back(sphere);
+ }
+
+ // Create all the capsules of the scene
+ for (int i = 0; i<NB_CAPSULES; i++) {
+
+ // Create a cylinder and a corresponding rigid in the dynamics world
+ Capsule* capsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, CAPSULE_MASS,
+ getDynamicsWorld(), meshFolderPath);
+
+ capsule->getRigidBody()->getMaterial().setRollingResistance(rp3d::decimal(0.08));
+
+ // Set the box color
+ capsule->setColor(mDemoColors[i % mNbDemoColors]);
+ capsule->setSleepingColor(mRedColorDemo);
+
+ // Change the material properties of the rigid body
+ rp3d::Material& material = capsule->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.2));
+
+ // Add the cylinder the list of sphere in the scene
+ mCapsules.push_back(capsule);
+ mPhysicsObjects.push_back(capsule);
+ }
+
+ // Create all the convex meshes of the scene
+ for (int i = 0; i<NB_MESHES; i++) {
+
+ // Create a convex mesh and a corresponding rigid in the dynamics world
+ ConvexMesh* mesh = new ConvexMesh(MESH_MASS, getDynamicsWorld(), meshFolderPath + "convexmesh.obj");
+
+ // Set the box color
+ mesh->setColor(mDemoColors[i % mNbDemoColors]);
+ mesh->setSleepingColor(mRedColorDemo);
+
+ // Change the material properties of the rigid body
+ rp3d::Material& material = mesh->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.2));
+
+ // Add the mesh the list of sphere in the scene
+ mConvexMeshes.push_back(mesh);
+ mPhysicsObjects.push_back(mesh);
}
// ---------- Create the triangular mesh ---------- //
// Position
- openglframework::Vector3 position(0, 0, 0);
rp3d::decimal mass = 1.0;
// Create a convex mesh and a corresponding rigid in the dynamics world
- mConcaveMesh = new ConcaveMesh(position, mass, mDynamicsWorld, meshFolderPath + "city.obj");
+ mConcaveMesh = new ConcaveMesh(mass, getDynamicsWorld(), meshFolderPath + "city.obj");
// Set the mesh as beeing static
mConcaveMesh->getRigidBody()->setType(rp3d::BodyType::STATIC);
@@ -86,106 +163,107 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name)
mConcaveMesh->setColor(mGreyColorDemo);
mConcaveMesh->setSleepingColor(mGreyColorDemo);
+ mPhysicsObjects.push_back(mConcaveMesh);
+
// Change the material properties of the rigid body
rp3d::Material& material = mConcaveMesh->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.2));
- material.setFrictionCoefficient(0.1);
+ material.setFrictionCoefficient(rp3d::decimal(0.1));
// Get the physics engine parameters
- mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled();
- rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity();
+ mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled();
+ rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity();
mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z);
- mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled();
- mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity();
- mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity();
- mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver();
- mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver();
- mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep();
+ mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled();
+ mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity();
+ mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity();
+ mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver();
+ mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver();
+ mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep();
}
// Destructor
ConcaveMeshScene::~ConcaveMeshScene() {
- // Destroy the corresponding rigid body from the dynamics world
- mDynamicsWorld->destroyRigidBody(mConcaveMesh->getRigidBody());
+ // Destroy all the physics objects of the scene
+ for (std::vector<PhysicsObject*>::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
- // Destroy the boxes
- for (int i=0; i<NB_BOXES_X * NB_BOXES_Z; i++) {
- mDynamicsWorld->destroyRigidBody(mBoxes[i]->getRigidBody());
- delete mBoxes[i];
+ // Destroy the corresponding rigid body from the dynamics world
+ getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody());
+
+ // Destroy the object
+ delete (*it);
}
- // Destroy the convex mesh
- delete mConcaveMesh;
-
// Destroy the dynamics world
- delete mDynamicsWorld;
-}
-
-// Update the physics world (take a simulation step)
-void ConcaveMeshScene::updatePhysics() {
-
- // Update the physics engine parameters
- mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled);
- rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y,
- mEngineSettings.gravity.z);
- mDynamicsWorld->setGravity(gravity);
- mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled);
- mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity);
- mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity);
- mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations);
- mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations);
- mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep);
-
- // Take a simulation step
- mDynamicsWorld->update(mEngineSettings.timeStep);
-}
-
-// Update the scene
-void ConcaveMeshScene::update() {
-
- SceneDemo::update();
-
- // Update the transform used for the rendering
- mConcaveMesh->updateTransform(mInterpolationFactor);
-
- for (int i=0; i<NB_BOXES_X * NB_BOXES_Z; i++) {
- mBoxes[i]->updateTransform(mInterpolationFactor);
- }
-}
-
-// Render the scene in a single pass
-void ConcaveMeshScene::renderSinglePass(Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
-
- // Bind the shader
- shader.bind();
-
- mConcaveMesh->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
-
- for (int i=0; i<NB_BOXES_X * NB_BOXES_Z; i++) {
- mBoxes[i]->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- }
-
- // Unbind the shader
- shader.unbind();
+ delete getDynamicsWorld();
}
// Reset the scene
void ConcaveMeshScene::reset() {
- // Reset the transform
- rp3d::Transform transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity());
- mConcaveMesh->setTransform(transform);
+ const float radius = 15.0f;
- for (int i=0; i<NB_BOXES_X; i++) {
- for (int j=0; j<NB_BOXES_Z; j++) {
+ for (uint i = 0; i<NB_COMPOUND_SHAPES; i++) {
- // Position
- rp3d::Vector3 boxPosition(-NB_BOXES_X * BOX_SIZE * BOXES_SPACE / 2 + i * BOX_SIZE * BOXES_SPACE, 30, -NB_BOXES_Z * BOX_SIZE * BOXES_SPACE / 2 + j * BOX_SIZE * BOXES_SPACE);
+ // Position
+ float angle = i * 30.0f;
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 125 + i * (DUMBBELL_HEIGHT + 0.3f),
+ radius * std::sin(angle));
- rp3d::Transform boxTransform(boxPosition, rp3d::Quaternion::identity());
- mBoxes[i * NB_BOXES_Z + j]->setTransform(boxTransform);
- }
+ mDumbbells[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
}
+ // Create all the boxes of the scene
+ for (uint i = 0; i<NB_BOXES; i++) {
+
+ // Position
+ float angle = i * 30.0f;
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 85 + i * (BOX_SIZE.y + 0.8f),
+ radius * std::sin(angle));
+
+ mBoxes[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
+ }
+
+ // Create all the spheres of the scene
+ for (uint i = 0; i<NB_SPHERES; i++) {
+
+ // Position
+ float angle = i * 35.0f;
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 75 + i * (SPHERE_RADIUS + 0.8f),
+ radius * std::sin(angle));
+
+ mSpheres[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
+ }
+
+ // Create all the capsules of the scene
+ for (uint i = 0; i<NB_CAPSULES; i++) {
+
+ // Position
+ float angle = i * 45.0f;
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 40 + i * (CAPSULE_HEIGHT + 0.3f),
+ radius * std::sin(angle));
+
+ mCapsules[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
+ }
+
+ // Create all the convex meshes of the scene
+ for (uint i = 0; i<NB_MESHES; i++) {
+
+ // Position
+ float angle = i * 30.0f;
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 30 + i * (CAPSULE_HEIGHT + 0.3f),
+ radius * std::sin(angle));
+
+ mConvexMeshes[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
+ }
+
+ // ---------- Create the triangular mesh ---------- //
+
+ mConcaveMesh->setTransform(rp3d::Transform::identity());
}
diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.h b/testbed/scenes/concavemesh/ConcaveMeshScene.h
index 5943fe6c..a8d566a1 100644
--- a/testbed/scenes/concavemesh/ConcaveMeshScene.h
+++ b/testbed/scenes/concavemesh/ConcaveMeshScene.h
@@ -33,15 +33,34 @@
#include "SceneDemo.h"
#include "ConcaveMesh.h"
#include "Box.h"
+#include "Capsule.h"
+#include "Dumbbell.h"
+#include "Sphere.h"
+#include "ConvexMesh.h"
namespace trianglemeshscene {
// Constants
const float SCENE_RADIUS = 70.0f; // Radius of the scene in meters
-const int NB_BOXES_X = 8;
-const int NB_BOXES_Z = 8;
-const float BOX_SIZE = 3.0f;
-const float BOXES_SPACE = 2.0f;
+static const int NB_BOXES = 10;
+static const int NB_SPHERES = 5;
+static const int NB_CAPSULES = 5;
+static const int NB_MESHES = 3;
+static const int NB_COMPOUND_SHAPES = 3;
+const openglframework::Vector3 BOX_SIZE(2, 2, 2);
+const float SPHERE_RADIUS = 1.5f;
+const float CONE_RADIUS = 2.0f;
+const float CONE_HEIGHT = 3.0f;
+const float CYLINDER_RADIUS = 1.0f;
+const float CYLINDER_HEIGHT = 5.0f;
+const float CAPSULE_RADIUS = 1.0f;
+const float CAPSULE_HEIGHT = 1.0f;
+const float DUMBBELL_HEIGHT = 1.0f;
+const float BOX_MASS = 1.0f;
+const float CONE_MASS = 1.0f;
+const float CYLINDER_MASS = 1.0f;
+const float CAPSULE_MASS = 1.0f;
+const float MESH_MASS = 1.0f;
// Class TriangleMeshScene
class ConcaveMeshScene : public SceneDemo {
@@ -50,35 +69,31 @@ class ConcaveMeshScene : public SceneDemo {
// -------------------- Attributes -------------------- //
- Box* mBoxes[NB_BOXES_X * NB_BOXES_Z];
+ std::vector<Box*> mBoxes;
+
+ std::vector<Sphere*> mSpheres;
+
+ std::vector<Capsule*> mCapsules;
+
+ /// All the convex meshes of the scene
+ std::vector<ConvexMesh*> mConvexMeshes;
+
+ /// All the dumbbell of the scene
+ std::vector<Dumbbell*> mDumbbells;
/// Concave triangles mesh
ConcaveMesh* mConcaveMesh;
- /// Dynamics world used for the physics simulation
- rp3d::DynamicsWorld* mDynamicsWorld;
-
public:
// -------------------- Methods -------------------- //
/// Constructor
- ConcaveMeshScene(const std::string& name);
+ ConcaveMeshScene(const std::string& name, EngineSettings& settings);
/// Destructor
virtual ~ConcaveMeshScene() override;
- /// Update the physics world (take a simulation step)
- /// Can be called several times per frame
- virtual void updatePhysics() override;
-
- /// Update the scene (take a simulation step)
- virtual void update() override;
-
- /// Render the scene in a single pass
- virtual void renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) override;
-
/// Reset the scene
virtual void reset() override;
@@ -88,7 +103,7 @@ class ConcaveMeshScene : public SceneDemo {
// Return all the contact points of the scene
inline std::vector<ContactPoint> ConcaveMeshScene::getContactPoints() const {
- return computeContactPointsOfWorld(mDynamicsWorld);
+ return computeContactPointsOfWorld(getDynamicsWorld());
}
}
diff --git a/testbed/scenes/cubes/CubesScene.cpp b/testbed/scenes/cubes/CubesScene.cpp
index 3ddb1d61..fa7f4ba5 100755
--- a/testbed/scenes/cubes/CubesScene.cpp
+++ b/testbed/scenes/cubes/CubesScene.cpp
@@ -31,8 +31,8 @@ using namespace openglframework;
using namespace cubesscene;
// Constructor
-CubesScene::CubesScene(const std::string& name)
- : SceneDemo(name, SCENE_RADIUS) {
+CubesScene::CubesScene(const std::string& name, EngineSettings& settings)
+ : SceneDemo(name, settings, SCENE_RADIUS) {
// Compute the radius and the center of the scene
openglframework::Vector3 center(0, 5, 0);
@@ -44,39 +44,31 @@ CubesScene::CubesScene(const std::string& name)
rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0);
// Create the dynamics world for the physics simulation
- mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
+ mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
- float radius = 2.0f;
+ // Create all the cubes of the scene
+ for (int i=0; i<NB_CUBES; i++) {
- //// Create all the cubes of the scene
- //for (int i=0; i<NB_CUBES; i++) {
+ // Create a cube and a corresponding rigid in the dynamics world
+ Box* cube = new Box(BOX_SIZE, BOX_MASS, getDynamicsWorld(), mMeshFolderPath);
- // // Position of the cubes
- // float angle = i * 30.0f;
- // openglframework::Vector3 position(radius * cos(angle),
- // 30 + i * (BOX_SIZE.y + 0.3f),
- // 0);
+ // Set the box color
+ cube->setColor(mDemoColors[i % mNbDemoColors]);
+ cube->setSleepingColor(mRedColorDemo);
- // // Create a cube and a corresponding rigid in the dynamics world
- // Box* cube = new Box(BOX_SIZE, position , BOX_MASS, mDynamicsWorld);
+ // Change the material properties of the rigid body
+ rp3d::Material& material = cube->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.4));
- // // Set the box color
- // cube->setColor(mDemoColors[i % mNbDemoColors]);
- // cube->setSleepingColor(mRedColorDemo);
-
- // // Change the material properties of the rigid body
- // rp3d::Material& material = cube->getRigidBody()->getMaterial();
- // material.setBounciness(rp3d::decimal(0.4));
-
- // // Add the box the list of box in the scene
- // mBoxes.push_back(cube);
- //}
+ // Add the box the list of box in the scene
+ mBoxes.push_back(cube);
+ mPhysicsObjects.push_back(cube);
+ }
// ------------------------- FLOOR ----------------------- //
// Create the floor
- openglframework::Vector3 floorPosition(0, 0, 0);
- mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath);
+ mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath);
mFloor->setColor(mGreyColorDemo);
mFloor->setSleepingColor(mGreyColorDemo);
@@ -84,37 +76,16 @@ CubesScene::CubesScene(const std::string& name)
mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC);
mPhysicsObjects.push_back(mFloor);
- // Change the material properties of the floor rigid body
- //rp3d::Material& material = mFloor->getRigidBody()->getMaterial();
- //material.setBounciness(rp3d::decimal(0.3));
-
- // ------------------------- BOX ----------------------- //
-
- // Create a cube and a corresponding rigid in the dynamics world
- Box* cube = new Box(BOX_SIZE, Vector3(0, 10, 0), BOX_MASS, mDynamicsWorld, mMeshFolderPath);
-
- // Set the box color
- cube->setColor(mDemoColors[0]);
- cube->setSleepingColor(mRedColorDemo);
-
- // Change the material properties of the rigid body
- //rp3d::Material& material = cube->getRigidBody()->getMaterial();
- //material.setBounciness(rp3d::decimal(0.4));
-
- // Add the box the list of box in the scene
- mBoxes.push_back(cube);
- mPhysicsObjects.push_back(cube);
-
// Get the physics engine parameters
- mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled();
- rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity();
+ mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled();
+ rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity();
mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z);
- mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled();
- mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity();
- mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity();
- mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver();
- mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver();
- mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep();
+ mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled();
+ mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity();
+ mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity();
+ mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver();
+ mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver();
+ mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep();
}
// Destructor
@@ -124,39 +95,20 @@ CubesScene::~CubesScene() {
for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
// Destroy the corresponding rigid body from the dynamics world
- mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
+ getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody());
// Destroy the cube
delete (*it);
}
// Destroy the rigid body of the floor
- mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody());
+ getDynamicsWorld()->destroyRigidBody(mFloor->getRigidBody());
// Destroy the floor
delete mFloor;
// Destroy the dynamics world
- delete mDynamicsWorld;
-}
-
-// Update the physics world (take a simulation step)
-void CubesScene::updatePhysics() {
-
- // Update the physics engine parameters
- mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled);
- rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y,
- mEngineSettings.gravity.z);
- mDynamicsWorld->setGravity(gravity);
- mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled);
- mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity);
- mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity);
- mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations);
- mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations);
- mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep);
-
- // Take a simulation step
- mDynamicsWorld->update(mEngineSettings.timeStep);
+ delete getDynamicsWorld();
}
// Reset the scene
@@ -164,19 +116,21 @@ void CubesScene::reset() {
float radius = 2.0f;
-// for (int i=0; i<NB_CUBES; i++) {
+ // Create all the cubes of the scene
+ std::vector<Box*>::iterator it;
+ int i = 0;
+ for (it = mBoxes.begin(); it != mBoxes.end(); ++it) {
-// // Position of the cubes
-// float angle = i * 30.0f;
-// openglframework::Vector3 position(radius * cos(angle),
-// 10 + i * (BOX_SIZE.y + 0.3f),
-// 0);
+ // Position of the cubes
+ float angle = i * 30.0f;
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 10 + i * (BOX_SIZE.y + 0.3f),
+ 0);
-// // Initial position and orientation of the rigid body
-// rp3d::Vector3 initPosition(position.x, position.y, position.z);
-// rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
-// rp3d::Transform transform(initPosition, initOrientation);
+ (*it)->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
-// mBoxes[i]->resetTransform(transform);
-// }
+ i++;
+ }
+
+ mFloor->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()));
}
diff --git a/testbed/scenes/cubes/CubesScene.h b/testbed/scenes/cubes/CubesScene.h
index aa4b7073..6d553835 100755
--- a/testbed/scenes/cubes/CubesScene.h
+++ b/testbed/scenes/cubes/CubesScene.h
@@ -38,7 +38,7 @@ namespace cubesscene {
const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters
const int NB_CUBES = 30; // Number of boxes in the scene
const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters
-const openglframework::Vector3 FLOOR_SIZE(5, 5.0, 5); // Floor dimensions in meters
+const openglframework::Vector3 FLOOR_SIZE(50, 1, 50); // Floor dimensions in meters
const float BOX_MASS = 1.0f; // Box mass in kilograms
const float FLOOR_MASS = 100.0f; // Floor mass in kilograms
@@ -55,23 +55,16 @@ class CubesScene : public SceneDemo {
/// Box for the floor
Box* mFloor;
- /// Dynamics world used for the physics simulation
- rp3d::DynamicsWorld* mDynamicsWorld;
-
public:
// -------------------- Methods -------------------- //
/// Constructor
- CubesScene(const std::string& name);
+ CubesScene(const std::string& name, EngineSettings& settings);
/// Destructor
virtual ~CubesScene() override;
- /// Update the physics world (take a simulation step)
- /// Can be called several times per frame
- virtual void updatePhysics() override;
-
/// Reset the scene
virtual void reset() override;
@@ -81,7 +74,7 @@ class CubesScene : public SceneDemo {
// Return all the contact points of the scene
inline std::vector<ContactPoint> CubesScene::getContactPoints() const {
- return computeContactPointsOfWorld(mDynamicsWorld);
+ return computeContactPointsOfWorld(getDynamicsWorld());
}
}
diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp
index 329712e9..238018dd 100644
--- a/testbed/scenes/heightfield/HeightFieldScene.cpp
+++ b/testbed/scenes/heightfield/HeightFieldScene.cpp
@@ -31,7 +31,8 @@ using namespace openglframework;
using namespace heightfieldscene;
// Constructor
-HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SCENE_RADIUS) {
+HeightFieldScene::HeightFieldScene(const std::string& name, EngineSettings& settings)
+ : SceneDemo(name, settings, SCENE_RADIUS) {
std::string meshFolderPath("meshes/");
@@ -45,20 +46,12 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0);
// Create the dynamics world for the physics simulation
- mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
-
- float radius = 3.0f;
+ mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
for (int i = 0; i<NB_COMPOUND_SHAPES; i++) {
- // Position
- float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 100 + i * (DUMBBELL_HEIGHT + 0.3f),
- radius * sin(angle));
-
// Create a convex mesh and a corresponding rigid in the dynamics world
- Dumbbell* dumbbell = new Dumbbell(position, mDynamicsWorld, meshFolderPath);
+ Dumbbell* dumbbell = new Dumbbell(getDynamicsWorld(), meshFolderPath);
// Set the box color
dumbbell->setColor(mDemoColors[i % mNbDemoColors]);
@@ -76,14 +69,8 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
// Create all the boxes of the scene
for (int i = 0; i<NB_BOXES; i++) {
- // Position
- float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 60 + i * (BOX_SIZE.y + 0.8f),
- radius * sin(angle));
-
// Create a sphere and a corresponding rigid in the dynamics world
- Box* box = new Box(BOX_SIZE, position, BOX_MASS, mDynamicsWorld, mMeshFolderPath);
+ Box* box = new Box(BOX_SIZE, BOX_MASS, getDynamicsWorld(), mMeshFolderPath);
// Set the box color
box->setColor(mDemoColors[i % mNbDemoColors]);
@@ -101,18 +88,11 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
// Create all the spheres of the scene
for (int i = 0; i<NB_SPHERES; i++) {
- // Position
- float angle = i * 35.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 50 + i * (SPHERE_RADIUS + 0.8f),
- radius * sin(angle));
-
// Create a sphere and a corresponding rigid in the dynamics world
- Sphere* sphere = new Sphere(SPHERE_RADIUS, position, BOX_MASS, mDynamicsWorld,
- meshFolderPath);
+ Sphere* sphere = new Sphere(SPHERE_RADIUS, BOX_MASS, getDynamicsWorld(), meshFolderPath);
// Add some rolling resistance
- sphere->getRigidBody()->getMaterial().setRollingResistance(0.08);
+ sphere->getRigidBody()->getMaterial().setRollingResistance(0.08f);
// Set the box color
sphere->setColor(mDemoColors[i % mNbDemoColors]);
@@ -130,17 +110,11 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
// Create all the capsules of the scene
for (int i = 0; i<NB_CAPSULES; i++) {
- // Position
- float angle = i * 45.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 15 + i * (CAPSULE_HEIGHT + 0.3f),
- radius * sin(angle));
-
// Create a cylinder and a corresponding rigid in the dynamics world
- Capsule* capsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position,
- CAPSULE_MASS, mDynamicsWorld, meshFolderPath);
+ Capsule* capsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, CAPSULE_MASS,
+ getDynamicsWorld(), meshFolderPath);
- capsule->getRigidBody()->getMaterial().setRollingResistance(0.08);
+ capsule->getRigidBody()->getMaterial().setRollingResistance(0.08f);
// Set the box color
capsule->setColor(mDemoColors[i % mNbDemoColors]);
@@ -158,14 +132,8 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
// Create all the convex meshes of the scene
for (int i = 0; i<NB_MESHES; i++) {
- // Position
- float angle = i * 30.0f;
- openglframework::Vector3 position(radius * cos(angle),
- 5 + i * (CAPSULE_HEIGHT + 0.3f),
- radius * sin(angle));
-
// Create a convex mesh and a corresponding rigid in the dynamics world
- ConvexMesh* mesh = new ConvexMesh(position, MESH_MASS, mDynamicsWorld, meshFolderPath + "convexmesh.obj");
+ ConvexMesh* mesh = new ConvexMesh(MESH_MASS, getDynamicsWorld(), meshFolderPath + "convexmesh.obj");
// Set the box color
mesh->setColor(mDemoColors[i % mNbDemoColors]);
@@ -183,11 +151,10 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
// ---------- Create the height field ---------- //
// Position
- openglframework::Vector3 position(0, 0, 0);
rp3d::decimal mass = 1.0;
// Create a convex mesh and a corresponding rigid in the dynamics world
- mHeightField = new HeightField(position, mass, mDynamicsWorld);
+ mHeightField = new HeightField(mass, getDynamicsWorld());
// Set the mesh as beeing static
mHeightField->getRigidBody()->setType(rp3d::BodyType::STATIC);
@@ -201,18 +168,18 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
// Change the material properties of the rigid body
rp3d::Material& material = mHeightField->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.2));
- material.setFrictionCoefficient(0.1);
+ material.setFrictionCoefficient(0.1f);
// Get the physics engine parameters
- mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled();
- rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity();
+ mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled();
+ rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity();
mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z);
- mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled();
- mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity();
- mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity();
- mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver();
- mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver();
- mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep();
+ mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled();
+ mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity();
+ mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity();
+ mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver();
+ mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver();
+ mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep();
}
// Destructor
@@ -222,47 +189,81 @@ HeightFieldScene::~HeightFieldScene() {
for (std::vector<PhysicsObject*>::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
// Destroy the corresponding rigid body from the dynamics world
- mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
+ getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody());
// Destroy the object
delete (*it);
}
// Destroy the dynamics world
- delete mDynamicsWorld;
-}
-
-// Update the physics world (take a simulation step)
-void HeightFieldScene::updatePhysics() {
-
- // Update the physics engine parameters
- mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled);
- rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y,
- mEngineSettings.gravity.z);
- mDynamicsWorld->setGravity(gravity);
- mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled);
- mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity);
- mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity);
- mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations);
- mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations);
- mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep);
-
- // Take a simulation step
- mDynamicsWorld->update(mEngineSettings.timeStep);
+ delete getDynamicsWorld();
}
// Reset the scene
void HeightFieldScene::reset() {
- // Reset the transform
- rp3d::Transform transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity());
- mHeightField->setTransform(transform);
+ const float radius = 3.0f;
- float heightFieldWidth = 10.0f;
- float stepDist = heightFieldWidth / (NB_BOXES + 1);
- for (int i=0; i<NB_BOXES; i++) {
- rp3d::Vector3 boxPosition(-heightFieldWidth * 0.5f + i * stepDist , 14 + 6.0f * i, -heightFieldWidth * 0.5f + i * stepDist);
- rp3d::Transform boxTransform(boxPosition, rp3d::Quaternion::identity());
- mBoxes[i]->setTransform(boxTransform);
+ for (uint i = 0; i<NB_COMPOUND_SHAPES; i++) {
+
+ // Position
+ float angle = i * 30.0f;
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 125 + i * (DUMBBELL_HEIGHT + 0.3f),
+ radius * std::sin(angle));
+
+ mDumbbells[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
}
+
+ // Create all the boxes of the scene
+ for (uint i = 0; i<NB_BOXES; i++) {
+
+ // Position
+ float angle = i * 30.0f;
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 85 + i * (BOX_SIZE.y + 0.8f),
+ radius * std::sin(angle));
+
+ mBoxes[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
+ }
+
+ // Create all the spheres of the scene
+ for (uint i = 0; i<NB_SPHERES; i++) {
+
+ // Position
+ float angle = i * 35.0f;
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 75 + i * (SPHERE_RADIUS + 0.8f),
+ radius * std::sin(angle));
+
+ mSpheres[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
+ }
+
+ // Create all the capsules of the scene
+ for (uint i = 0; i<NB_CAPSULES; i++) {
+
+ // Position
+ float angle = i * 45.0f;
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 40 + i * (CAPSULE_HEIGHT + 0.3f),
+ radius * std::sin(angle));
+
+ mCapsules[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
+ }
+
+ // Create all the convex meshes of the scene
+ for (uint i = 0; i<NB_MESHES; i++) {
+
+ // Position
+ float angle = i * 30.0f;
+ rp3d::Vector3 position(radius * std::cos(angle),
+ 30 + i * (CAPSULE_HEIGHT + 0.3f),
+ radius * std::sin(angle));
+
+ mConvexMeshes[i]->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
+ }
+
+ // ---------- Create the triangular mesh ---------- //
+
+ mHeightField->setTransform(rp3d::Transform::identity());
}
diff --git a/testbed/scenes/heightfield/HeightFieldScene.h b/testbed/scenes/heightfield/HeightFieldScene.h
index 7306ae2c..2a3e68f5 100644
--- a/testbed/scenes/heightfield/HeightFieldScene.h
+++ b/testbed/scenes/heightfield/HeightFieldScene.h
@@ -44,7 +44,7 @@ const float SCENE_RADIUS = 50.0f;
static const int NB_BOXES = 10;
static const int NB_SPHERES = 5;
static const int NB_CAPSULES = 5;
-static const int NB_MESHES = 3;
+static const int NB_MESHES = 4;
static const int NB_COMPOUND_SHAPES = 3;
const openglframework::Vector3 BOX_SIZE(2, 2, 2);
const float SPHERE_RADIUS = 1.5f;
@@ -86,23 +86,16 @@ class HeightFieldScene : public SceneDemo {
/// Height field
HeightField* mHeightField;
- /// Dynamics world used for the physics simulation
- rp3d::DynamicsWorld* mDynamicsWorld;
-
public:
// -------------------- Methods -------------------- //
/// Constructor
- HeightFieldScene(const std::string& name);
+ HeightFieldScene(const std::string& name, EngineSettings& settings);
/// Destructor
virtual ~HeightFieldScene() override;
- /// Update the physics world (take a simulation step)
- /// Can be called several times per frame
- virtual void updatePhysics() override;
-
/// Reset the scene
virtual void reset() override ;
@@ -112,7 +105,7 @@ class HeightFieldScene : public SceneDemo {
// Return all the contact points of the scene
inline std::vector<ContactPoint> HeightFieldScene::getContactPoints() const {
- return computeContactPointsOfWorld(mDynamicsWorld);
+ return computeContactPointsOfWorld(getDynamicsWorld());
}
}
diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp
index 282f7e51..3252e90e 100644
--- a/testbed/scenes/joints/JointsScene.cpp
+++ b/testbed/scenes/joints/JointsScene.cpp
@@ -32,8 +32,8 @@ using namespace openglframework;
using namespace jointsscene;
// Constructor
-JointsScene::JointsScene(const std::string& name)
- : SceneDemo(name, SCENE_RADIUS) {
+JointsScene::JointsScene(const std::string& name, EngineSettings& settings)
+ : SceneDemo(name, settings, SCENE_RADIUS) {
// Compute the radius and the center of the scene
openglframework::Vector3 center(0, 5, 0);
@@ -45,7 +45,7 @@ JointsScene::JointsScene(const std::string& name)
rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0);
// Create the dynamics world for the physics simulation
- mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
+ mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
// Create the Ball-and-Socket joint
createBallAndSocketJoints();
@@ -63,37 +63,37 @@ JointsScene::JointsScene(const std::string& name)
createFloor();
// Get the physics engine parameters
- mEngineSettings.isGravityEnabled = mDynamicsWorld->isGravityEnabled();
- rp3d::Vector3 gravityVector = mDynamicsWorld->getGravity();
+ mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled();
+ rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity();
mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z);
- mEngineSettings.isSleepingEnabled = mDynamicsWorld->isSleepingEnabled();
- mEngineSettings.sleepLinearVelocity = mDynamicsWorld->getSleepLinearVelocity();
- mEngineSettings.sleepAngularVelocity = mDynamicsWorld->getSleepAngularVelocity();
- mEngineSettings.nbPositionSolverIterations = mDynamicsWorld->getNbIterationsPositionSolver();
- mEngineSettings.nbVelocitySolverIterations = mDynamicsWorld->getNbIterationsVelocitySolver();
- mEngineSettings.timeBeforeSleep = mDynamicsWorld->getTimeBeforeSleep();
+ mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled();
+ mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity();
+ mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity();
+ mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver();
+ mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver();
+ mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep();
}
// Destructor
JointsScene::~JointsScene() {
// Destroy the joints
- mDynamicsWorld->destroyJoint(mSliderJoint);
- mDynamicsWorld->destroyJoint(mPropellerHingeJoint);
- mDynamicsWorld->destroyJoint(mFixedJoint1);
- mDynamicsWorld->destroyJoint(mFixedJoint2);
+ getDynamicsWorld()->destroyJoint(mSliderJoint);
+ getDynamicsWorld()->destroyJoint(mPropellerHingeJoint);
+ getDynamicsWorld()->destroyJoint(mFixedJoint1);
+ getDynamicsWorld()->destroyJoint(mFixedJoint2);
for (int i=0; i<NB_BALLSOCKETJOINT_BOXES-1; i++) {
- mDynamicsWorld->destroyJoint(mBallAndSocketJoints[i]);
+ getDynamicsWorld()->destroyJoint(mBallAndSocketJoints[i]);
}
// Destroy all the rigid bodies of the scene
- mDynamicsWorld->destroyRigidBody(mSliderJointBottomBox->getRigidBody());
- mDynamicsWorld->destroyRigidBody(mSliderJointTopBox->getRigidBody());
- mDynamicsWorld->destroyRigidBody(mPropellerBox->getRigidBody());
- mDynamicsWorld->destroyRigidBody(mFixedJointBox1->getRigidBody());
- mDynamicsWorld->destroyRigidBody(mFixedJointBox2->getRigidBody());
+ getDynamicsWorld()->destroyRigidBody(mSliderJointBottomBox->getRigidBody());
+ getDynamicsWorld()->destroyRigidBody(mSliderJointTopBox->getRigidBody());
+ getDynamicsWorld()->destroyRigidBody(mPropellerBox->getRigidBody());
+ getDynamicsWorld()->destroyRigidBody(mFixedJointBox1->getRigidBody());
+ getDynamicsWorld()->destroyRigidBody(mFixedJointBox2->getRigidBody());
for (int i=0; i<NB_BALLSOCKETJOINT_BOXES; i++) {
- mDynamicsWorld->destroyRigidBody(mBallAndSocketJointChainBoxes[i]->getRigidBody());
+ getDynamicsWorld()->destroyRigidBody(mBallAndSocketJointChainBoxes[i]->getRigidBody());
}
delete mSliderJointBottomBox;
@@ -106,58 +106,21 @@ JointsScene::~JointsScene() {
}
// Destroy the floor
- mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody());
+ getDynamicsWorld()->destroyRigidBody(mFloor->getRigidBody());
delete mFloor;
// Destroy the dynamics world
- delete mDynamicsWorld;
+ delete getDynamicsWorld();
}
// Update the physics world (take a simulation step)
void JointsScene::updatePhysics() {
- // Update the physics engine parameters
- mDynamicsWorld->setIsGratityEnabled(mEngineSettings.isGravityEnabled);
- rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y,
- mEngineSettings.gravity.z);
- mDynamicsWorld->setGravity(gravity);
- mDynamicsWorld->enableSleeping(mEngineSettings.isSleepingEnabled);
- mDynamicsWorld->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity);
- mDynamicsWorld->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity);
- mDynamicsWorld->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations);
- mDynamicsWorld->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations);
- mDynamicsWorld->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep);
-
// Update the motor speed of the Slider Joint (to move up and down)
- long double motorSpeed = 2 * cos(mEngineSettings.elapsedTime * 1.5);
+ double motorSpeed = 2.0 * std::cos(static_cast<double>(mEngineSettings.elapsedTime) * 1.5);
mSliderJoint->setMotorSpeed(rp3d::decimal(motorSpeed));
- // Take a simulation step
- mDynamicsWorld->update(mEngineSettings.timeStep);
-}
-
-// Render the scene
-void JointsScene::renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) {
-
- // Bind the shader
- shader.bind();
-
- // Render all the boxes
- mSliderJointBottomBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- mSliderJointTopBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- mPropellerBox->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- mFixedJointBox1->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- mFixedJointBox2->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- for (int i=0; i<NB_BALLSOCKETJOINT_BOXES; i++) {
- mBallAndSocketJointChainBoxes[i]->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
- }
-
- // Render the floor
- mFloor->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
-
- // Unbind the shader
- shader.unbind();
+ SceneDemo::updatePhysics();
}
// Reset the scene
@@ -236,15 +199,16 @@ void JointsScene::createBallAndSocketJoints() {
// --------------- Create the boxes --------------- //
- openglframework::Vector3 positionBox(0, 15, 5);
+ rp3d::Vector3 positionBox(0, 15, 5);
openglframework::Vector3 boxDimension(1, 1, 1);
const float boxMass = 0.5f;
for (int i=0; i<NB_BALLSOCKETJOINT_BOXES; i++) {
// Create a box and a corresponding rigid in the dynamics world
- mBallAndSocketJointChainBoxes[i] = new Box(boxDimension, positionBox , boxMass,
- mDynamicsWorld, mMeshFolderPath);
+ mBallAndSocketJointChainBoxes[i] = new Box(boxDimension, boxMass,
+ getDynamicsWorld(), mMeshFolderPath);
+ mBallAndSocketJointChainBoxes[i]->setTransform(rp3d::Transform(positionBox, rp3d::Quaternion::identity()));
// Set the box color
mBallAndSocketJointChainBoxes[i]->setColor(mDemoColors[i % mNbDemoColors]);
@@ -281,7 +245,7 @@ void JointsScene::createBallAndSocketJoints() {
// Create the joint in the dynamics world
mBallAndSocketJoints[i] = dynamic_cast<rp3d::BallAndSocketJoint*>(
- mDynamicsWorld->createJoint(jointInfo));
+ getDynamicsWorld()->createJoint(jointInfo));
}
}
@@ -291,11 +255,12 @@ void JointsScene::createSliderJoint() {
// --------------- Create the first box --------------- //
// Position of the box
- openglframework::Vector3 positionBox1(0, 2.1f, 0);
+ rp3d::Vector3 positionBox1(0, 2.1f, 0);
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 box1Dimension(2, 4, 2);
- mSliderJointBottomBox = new Box(box1Dimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath);
+ mSliderJointBottomBox = new Box(box1Dimension , BOX_MASS, getDynamicsWorld(), mMeshFolderPath);
+ mSliderJointBottomBox->setTransform(rp3d::Transform(positionBox1, rp3d::Quaternion::identity()));
// Set the box color
mSliderJointBottomBox->setColor(mBlueColorDemo);
@@ -312,11 +277,12 @@ void JointsScene::createSliderJoint() {
// --------------- Create the second box --------------- //
// Position of the box
- openglframework::Vector3 positionBox2(0, 4.2f, 0);
+ rp3d::Vector3 positionBox2(0, 4.2f, 0);
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 box2Dimension(1.5f, 4, 1.5f);
- mSliderJointTopBox = new Box(box2Dimension, positionBox2, BOX_MASS, mDynamicsWorld, mMeshFolderPath);
+ mSliderJointTopBox = new Box(box2Dimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath);
+ mSliderJointTopBox->setTransform(rp3d::Transform(positionBox2, rp3d::Quaternion::identity()));
// Set the box color
mSliderJointTopBox->setColor(mOrangeColorDemo);
@@ -344,7 +310,7 @@ void JointsScene::createSliderJoint() {
jointInfo.isCollisionEnabled = false;
// Create the joint in the dynamics world
- mSliderJoint = dynamic_cast<rp3d::SliderJoint*>(mDynamicsWorld->createJoint(jointInfo));
+ mSliderJoint = dynamic_cast<rp3d::SliderJoint*>(getDynamicsWorld()->createJoint(jointInfo));
}
/// Create the boxes and joint for the Hinge joint example
@@ -353,11 +319,12 @@ void JointsScene::createPropellerHingeJoint() {
// --------------- Create the propeller box --------------- //
// Position of the box
- openglframework::Vector3 positionBox1(0, 7, 0);
+ rp3d::Vector3 positionBox1(0, 7, 0);
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 boxDimension(10, 1, 1);
- mPropellerBox = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath);
+ mPropellerBox = new Box(boxDimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath);
+ mPropellerBox->setTransform(rp3d::Transform(positionBox1, rp3d::Quaternion::identity()));
// Set the box color
mPropellerBox->setColor(mYellowColorDemo);
@@ -384,7 +351,7 @@ void JointsScene::createPropellerHingeJoint() {
jointInfo.isCollisionEnabled = false;
// Create the joint in the dynamics world
- mPropellerHingeJoint = dynamic_cast<rp3d::HingeJoint*>(mDynamicsWorld->createJoint(jointInfo));
+ mPropellerHingeJoint = dynamic_cast<rp3d::HingeJoint*>(getDynamicsWorld()->createJoint(jointInfo));
}
/// Create the boxes and joints for the fixed joints
@@ -393,11 +360,12 @@ void JointsScene::createFixedJoints() {
// --------------- Create the first box --------------- //
// Position of the box
- openglframework::Vector3 positionBox1(5, 7, 0);
+ rp3d::Vector3 positionBox1(5, 7, 0);
// Create a box and a corresponding rigid in the dynamics world
openglframework::Vector3 boxDimension(1.5, 1.5, 1.5);
- mFixedJointBox1 = new Box(boxDimension, positionBox1 , BOX_MASS, mDynamicsWorld, mMeshFolderPath);
+ mFixedJointBox1 = new Box(boxDimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath);
+ mFixedJointBox1->setTransform(rp3d::Transform(positionBox1, rp3d::Quaternion::identity()));
// Set the box color
mFixedJointBox1->setColor(mPinkColorDemo);
@@ -411,10 +379,11 @@ void JointsScene::createFixedJoints() {
// --------------- Create the second box --------------- //
// Position of the box
- openglframework::Vector3 positionBox2(-5, 7, 0);
+ rp3d::Vector3 positionBox2(-5, 7, 0);
// Create a box and a corresponding rigid in the dynamics world
- mFixedJointBox2 = new Box(boxDimension, positionBox2 , BOX_MASS, mDynamicsWorld, mMeshFolderPath);
+ mFixedJointBox2 = new Box(boxDimension, BOX_MASS, getDynamicsWorld(), mMeshFolderPath);
+ mFixedJointBox2->setTransform(rp3d::Transform(positionBox2, rp3d::Quaternion::identity()));
// Set the box color
mFixedJointBox2->setColor(mBlueColorDemo);
@@ -435,7 +404,7 @@ void JointsScene::createFixedJoints() {
jointInfo1.isCollisionEnabled = false;
// Create the joint in the dynamics world
- mFixedJoint1 = dynamic_cast<rp3d::FixedJoint*>(mDynamicsWorld->createJoint(jointInfo1));
+ mFixedJoint1 = dynamic_cast<rp3d::FixedJoint*>(getDynamicsWorld()->createJoint(jointInfo1));
// --------------- Create the second fixed joint --------------- //
@@ -446,15 +415,15 @@ void JointsScene::createFixedJoints() {
jointInfo2.isCollisionEnabled = false;
// Create the joint in the dynamics world
- mFixedJoint2 = dynamic_cast<rp3d::FixedJoint*>(mDynamicsWorld->createJoint(jointInfo2));
+ mFixedJoint2 = dynamic_cast<rp3d::FixedJoint*>(getDynamicsWorld()->createJoint(jointInfo2));
}
// Create the floor
void JointsScene::createFloor() {
// Create the floor
- openglframework::Vector3 floorPosition(0, 0, 0);
- mFloor = new Box(FLOOR_SIZE, floorPosition, FLOOR_MASS, mDynamicsWorld, mMeshFolderPath);
+ rp3d::Vector3 floorPosition(0, 0, 0);
+ mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath);
// Set the box color
mFloor->setColor(mGreyColorDemo);
diff --git a/testbed/scenes/joints/JointsScene.h b/testbed/scenes/joints/JointsScene.h
index 16ccfe8c..dad9af38 100644
--- a/testbed/scenes/joints/JointsScene.h
+++ b/testbed/scenes/joints/JointsScene.h
@@ -92,9 +92,6 @@ class JointsScene : public SceneDemo {
/// Box for the floor
Box* mFloor;
- /// Dynamics world used for the physics simulation
- rp3d::DynamicsWorld* mDynamicsWorld;
-
// -------------------- Methods -------------------- //
/// Create the boxes and joints for the Ball-and-Socket joint example
@@ -117,7 +114,7 @@ class JointsScene : public SceneDemo {
// -------------------- Methods -------------------- //
/// Constructor
- JointsScene(const std::string& name);
+ JointsScene(const std::string& name, EngineSettings& settings);
/// Destructor
virtual ~JointsScene() override ;
@@ -126,10 +123,6 @@ class JointsScene : public SceneDemo {
/// Can be called several times per frame
virtual void updatePhysics() override;
- /// Render the scene in a single pass
- virtual void renderSinglePass(openglframework::Shader& shader,
- const openglframework::Matrix4& worldToCameraMatrix) override;
-
/// Reset the scene
virtual void reset() override;
@@ -139,7 +132,7 @@ class JointsScene : public SceneDemo {
// Return all the contact points of the scene
inline std::vector<ContactPoint> JointsScene::getContactPoints() const {
- return computeContactPointsOfWorld(mDynamicsWorld);
+ return computeContactPointsOfWorld(getDynamicsWorld());
}
}
diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp
index 2ded75d6..bf806183 100644
--- a/testbed/scenes/raycast/RaycastScene.cpp
+++ b/testbed/scenes/raycast/RaycastScene.cpp
@@ -31,8 +31,8 @@ using namespace openglframework;
using namespace raycastscene;
// Constructor
-RaycastScene::RaycastScene(const std::string& name)
- : SceneDemo(name, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
+RaycastScene::RaycastScene(const std::string& name, EngineSettings& settings)
+ : SceneDemo(name, settings, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
mRaycastManager(mPhongShader, mMeshFolderPath), mCurrentBodyIndex(-1),
mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) {
@@ -45,13 +45,12 @@ RaycastScene::RaycastScene(const std::string& name)
setScenePosition(center, SCENE_RADIUS);
// Create the dynamics world for the physics simulation
- mCollisionWorld = new rp3d::CollisionWorld();
+ mPhysicsWorld = new rp3d::CollisionWorld();
// ---------- Dumbbell ---------- //
- openglframework::Vector3 position1(0, 0, 0);
// Create a convex mesh and a corresponding collision body in the dynamics world
- mDumbbell = new Dumbbell(position1, mCollisionWorld, mMeshFolderPath);
+ mDumbbell = new Dumbbell(mPhysicsWorld, mMeshFolderPath);
// Set the box color
mDumbbell->setColor(mGreyColorDemo);
@@ -59,10 +58,9 @@ RaycastScene::RaycastScene(const std::string& name)
mPhysicsObjects.push_back(mDumbbell);
// ---------- Box ---------- //
- openglframework::Vector3 position2(0, 0, 0);
// Create a box and a corresponding collision body in the dynamics world
- mBox = new Box(BOX_SIZE, position2, mCollisionWorld, mMeshFolderPath);
+ mBox = new Box(BOX_SIZE, mPhysicsWorld, mMeshFolderPath);
mBox->getCollisionBody()->setIsActive(false);
// Set the box color
@@ -71,11 +69,9 @@ RaycastScene::RaycastScene(const std::string& name)
mPhysicsObjects.push_back(mBox);
// ---------- Sphere ---------- //
- openglframework::Vector3 position3(0, 0, 0);
// Create a sphere and a corresponding collision body in the dynamics world
- mSphere = new Sphere(SPHERE_RADIUS, position3, mCollisionWorld,
- mMeshFolderPath);
+ mSphere = new Sphere(SPHERE_RADIUS, mPhysicsWorld, mMeshFolderPath);
// Set the color
mSphere->setColor(mGreyColorDemo);
@@ -86,8 +82,7 @@ RaycastScene::RaycastScene(const std::string& name)
openglframework::Vector3 position6(0, 0, 0);
// Create a cylinder and a corresponding collision body in the dynamics world
- mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 ,
- mCollisionWorld, mMeshFolderPath);
+ mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, mPhysicsWorld, mMeshFolderPath);
// Set the color
mCapsule->setColor(mGreyColorDemo);
@@ -95,10 +90,9 @@ RaycastScene::RaycastScene(const std::string& name)
mPhysicsObjects.push_back(mCapsule);
// ---------- Convex Mesh ---------- //
- openglframework::Vector3 position7(0, 0, 0);
// Create a convex mesh and a corresponding collision body in the dynamics world
- mConvexMesh = new ConvexMesh(position7, mCollisionWorld, mMeshFolderPath + "convexmesh.obj");
+ mConvexMesh = new ConvexMesh(mPhysicsWorld, mMeshFolderPath + "convexmesh.obj");
// Set the color
mConvexMesh->setColor(mGreyColorDemo);
@@ -106,10 +100,9 @@ RaycastScene::RaycastScene(const std::string& name)
mPhysicsObjects.push_back(mConvexMesh);
// ---------- 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 + "city.obj");
+ mConcaveMesh = new ConcaveMesh(mPhysicsWorld, mMeshFolderPath + "city.obj");
// Set the color
mConcaveMesh->setColor(mGreyColorDemo);
@@ -117,10 +110,9 @@ RaycastScene::RaycastScene(const std::string& name)
mPhysicsObjects.push_back(mConcaveMesh);
// ---------- Heightfield ---------- //
- openglframework::Vector3 position9(0, 0, 0);
// Create a convex mesh and a corresponding collision body in the dynamics world
- mHeightField = new HeightField(position9, mCollisionWorld);
+ mHeightField = new HeightField(mPhysicsWorld);
// Set the color
mHeightField->setColor(mGreyColorDemo);
@@ -139,7 +131,7 @@ RaycastScene::RaycastScene(const std::string& name)
// Create the raycast lines
void RaycastScene::createLines() {
- int nbRaysOneDimension = std::sqrt(float(NB_RAYS));
+ int nbRaysOneDimension = static_cast<int>(std::sqrt(float(NB_RAYS)));
for (int i=0; i<nbRaysOneDimension; i++) {
for (int j=0; j<nbRaysOneDimension; j++) {
@@ -201,45 +193,49 @@ void RaycastScene::changeBody() {
// Reset the scene
void RaycastScene::reset() {
+ std::vector<PhysicsObject*>::iterator it;
+ for (it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
+ (*it)->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()));
+ }
}
// Destructor
RaycastScene::~RaycastScene() {
// Destroy the box rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mBox->getCollisionBody());
delete mBox;
// Destroy the sphere
- mCollisionWorld->destroyCollisionBody(mSphere->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mSphere->getCollisionBody());
delete mSphere;
// Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mCapsule->getCollisionBody());
// Destroy the sphere
delete mCapsule;
// Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mConvexMesh->getCollisionBody());
// Destroy the convex mesh
delete mConvexMesh;
// Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mDumbbell->getCollisionBody());
// Destroy the dumbbell
delete mDumbbell;
// Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mConcaveMesh->getCollisionBody());
// Destroy the convex mesh
delete mConcaveMesh;
// Destroy the corresponding rigid body from the dynamics world
- mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody());
+ mPhysicsWorld->destroyCollisionBody(mHeightField->getCollisionBody());
// Destroy the convex mesh
delete mHeightField;
@@ -250,7 +246,7 @@ RaycastScene::~RaycastScene() {
VisualContactPoint::destroyStaticData();
// Destroy the collision world
- delete mCollisionWorld;
+ delete mPhysicsWorld;
// Destroy the lines
for (std::vector<Line*>::iterator it = mLines.begin(); it != mLines.end();
@@ -284,7 +280,7 @@ void RaycastScene::update() {
// Perform a raycast query on the physics world by passing a raycast
// callback class in argument.
- mCollisionWorld->raycast(ray, &mRaycastManager);
+ mPhysicsWorld->raycast(ray, &mRaycastManager);
}
SceneDemo::update();
@@ -307,7 +303,7 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, const openg
mColorShader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
// Set the vertex color
- openglframework::Vector4 color(1, 0.55, 0, 1);
+ openglframework::Vector4 color(1, 0.55f, 0, 1);
mColorShader.setVector4Uniform("vertexColor", color, false);
// Get the location of shader attribute variables
@@ -334,7 +330,7 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader, const openg
// Render all the physics objects of the scene
for (std::vector<PhysicsObject*>::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
if ((*it)->getCollisionBody()->isActive()) {
- (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ (*it)->render(shader, worldToCameraMatrix);
}
}
diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h
index 1400fcf7..8024ff1a 100644
--- a/testbed/scenes/raycast/RaycastScene.h
+++ b/testbed/scenes/raycast/RaycastScene.h
@@ -146,9 +146,6 @@ class RaycastScene : public SceneDemo {
ConcaveMesh* mConcaveMesh;
HeightField* mHeightField;
- /// Collision world used for the physics simulation
- rp3d::CollisionWorld* mCollisionWorld;
-
/// All the points to render the lines
std::vector<openglframework::Vector3> mLinePoints;
@@ -170,7 +167,7 @@ class RaycastScene : public SceneDemo {
// -------------------- Methods -------------------- //
/// Constructor
- RaycastScene(const std::string& name);
+ RaycastScene(const std::string& name, EngineSettings& settings);
/// Destructor
virtual ~RaycastScene() override;
diff --git a/testbed/src/Gui.cpp b/testbed/src/Gui.cpp
index cd9f8f4e..6ad24335 100644
--- a/testbed/src/Gui.cpp
+++ b/testbed/src/Gui.cpp
@@ -388,6 +388,14 @@ void Gui::createSettingsPanel() {
mApp->mIsContactPointsDisplayed = value;
});
+
+ // Display/Hide the AABBs
+ CheckBox* checkboxAABBs = new CheckBox(mRenderingPanel, "AABBs");
+ checkboxAABBs->setChecked(mApp->mIsAABBsDisplayed);
+ checkboxAABBs->setCallback([&](bool value) {
+ mApp->mIsAABBsDisplayed = value;
+ });
+
// Enabled/Disable VSync
CheckBox* checkboxVSync = new CheckBox(mRenderingPanel, "V-Sync");
checkboxVSync->setChecked(mApp->mIsVSyncEnabled);
diff --git a/testbed/src/Scene.cpp b/testbed/src/Scene.cpp
index e872b3bc..b6668649 100644
--- a/testbed/src/Scene.cpp
+++ b/testbed/src/Scene.cpp
@@ -30,10 +30,10 @@
using namespace openglframework;
// Constructor
-Scene::Scene(const std::string& name, bool isShadowMappingEnabled)
- : mName(name), mInterpolationFactor(0.0f), mViewportX(0), mViewportY(0),
+Scene::Scene(const std::string& name, EngineSettings& engineSettings, bool isShadowMappingEnabled)
+ : mName(name), mEngineSettings(engineSettings), mInterpolationFactor(0.0f), mViewportX(0), mViewportY(0),
mViewportWidth(0), mViewportHeight(0), mIsShadowMappingEnabled(isShadowMappingEnabled),
- mIsContactPointsDisplayed(true), mIsWireframeEnabled(false) {
+ mIsContactPointsDisplayed(true), mIsAABBsDisplayed(false), mIsWireframeEnabled(false) {
}
@@ -74,14 +74,14 @@ bool Scene::mapMouseCoordinatesToSphere(double xMouse, double yMouse,
if ((xMouse >= 0) && (xMouse <= mWindowWidth) && (yMouse >= 0) && (yMouse <= mWindowHeight)) {
float x = float(xMouse - 0.5f * mWindowWidth) / float(mWindowWidth);
float y = float(0.5f * mWindowHeight - yMouse) / float(mWindowHeight);
- float sinx = sin(PI * x * 0.5f);
- float siny = sin(PI * y * 0.5f);
+ float sinx = std::sin(PI * x * 0.5f);
+ float siny = std::sin(PI * y * 0.5f);
float sinx2siny2 = sinx * sinx + siny * siny;
// Compute the point on the sphere
spherePoint.x = sinx;
spherePoint.y = siny;
- spherePoint.z = (sinx2siny2 < 1.0) ? sqrt(1.0f - sinx2siny2) : 0.0f;
+ spherePoint.z = (sinx2siny2 < 1.0f) ? std::sqrt(1.0f - sinx2siny2) : 0.0f;
return true;
}
@@ -175,9 +175,9 @@ void Scene::rotate(int xMouse, int yMouse) {
float cosAngle = mLastPointOnSphere.dot(newPoint3D);
float epsilon = std::numeric_limits<float>::epsilon();
- if (fabs(cosAngle) < 1.0f && axis.length() > epsilon) {
+ if (std::abs(cosAngle) < 1.0f && axis.length() > epsilon) {
axis.normalize();
- float angle = 2.0f * acos(cosAngle);
+ float angle = 2.0f * std::acos(cosAngle);
// Rotate the camera around the center of the scene
mCamera.rotateAroundLocalPoint(axis, -angle, mCenterScene);
diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h
index aa29b9e5..e33e9da9 100644
--- a/testbed/src/Scene.h
+++ b/testbed/src/Scene.h
@@ -28,6 +28,7 @@
// Libraries
#include "openglframework.h"
+#include "reactphysics3d.h"
// Structure ContactPoint
struct ContactPoint {
@@ -52,8 +53,8 @@ struct EngineSettings {
long double elapsedTime; // Elapsed time (in seconds)
float timeStep; // Current time step (in seconds)
- int nbVelocitySolverIterations; // Nb of velocity solver iterations
- int nbPositionSolverIterations; // Nb of position solver iterations
+ uint nbVelocitySolverIterations; // Nb of velocity solver iterations
+ uint nbPositionSolverIterations; // Nb of position solver iterations
bool isSleepingEnabled; // True if sleeping technique is enabled
float timeBeforeSleep; // Time of inactivity before a body sleep
float sleepLinearVelocity; // Sleep linear velocity
@@ -65,6 +66,23 @@ struct EngineSettings {
EngineSettings() : elapsedTime(0.0f), timeStep(0.0f) {
}
+
+ /// Return default engine settings
+ static EngineSettings defaultSettings() {
+
+ EngineSettings defaultSettings;
+
+ defaultSettings.timeStep = 1.0f / 60.0f;
+ defaultSettings.nbVelocitySolverIterations = rp3d::DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS;
+ defaultSettings.nbPositionSolverIterations = rp3d::DEFAULT_POSITION_SOLVER_NB_ITERATIONS;
+ defaultSettings.isSleepingEnabled = rp3d::SLEEPING_ENABLED;
+ defaultSettings.timeBeforeSleep = rp3d::DEFAULT_TIME_BEFORE_SLEEP;
+ defaultSettings.sleepLinearVelocity = rp3d::DEFAULT_SLEEP_LINEAR_VELOCITY;
+ defaultSettings.sleepAngularVelocity = rp3d::DEFAULT_SLEEP_ANGULAR_VELOCITY;
+ defaultSettings.isGravityEnabled = true;
+
+ return defaultSettings;
+ }
};
// Class Scene
@@ -79,7 +97,7 @@ class Scene {
std::string mName;
/// Physics engine settings
- EngineSettings mEngineSettings;
+ EngineSettings& mEngineSettings;
/// Camera
openglframework::Camera mCamera;
@@ -111,6 +129,9 @@ class Scene {
/// True if contact points are displayed
bool mIsContactPointsDisplayed;
+ /// True if the AABBs of the phycis objects are displayed
+ bool mIsAABBsDisplayed;
+
/// True if we render shapes in wireframe mode
bool mIsWireframeEnabled;
@@ -140,7 +161,7 @@ class Scene {
// -------------------- Methods -------------------- //
/// Constructor
- Scene(const std::string& name, bool isShadowMappingEnabled = false);
+ Scene(const std::string& name, EngineSettings& engineSettings, bool isShadowMappingEnabled = false);
/// Destructor
virtual ~Scene();
@@ -184,12 +205,6 @@ class Scene {
/// Return a reference to the camera
const openglframework::Camera& getCamera() const;
- /// Get the engine settings
- EngineSettings getEngineSettings() const;
-
- /// Set the engine settings
- void setEngineSettings(const EngineSettings& settings);
-
/// Set the interpolation factor
void setInterpolationFactor(float interpolationFactor);
@@ -205,6 +220,9 @@ class Scene {
/// Display/Hide the contact points
void virtual setIsContactPointsDisplayed(bool display);
+ /// Display/Hide the AABBs
+ void setIsAABBsDisplayed(bool display);
+
/// Return true if wireframe rendering is enabled
bool getIsWireframeEnabled() const;
@@ -244,16 +262,6 @@ inline void Scene::setViewport(int x, int y, int width, int height) {
mViewportHeight = height;
}
-// Get the engine settings
-inline EngineSettings Scene::getEngineSettings() const {
- return mEngineSettings;
-}
-
-// Set the engine settings
-inline void Scene::setEngineSettings(const EngineSettings& settings) {
- mEngineSettings = settings;
-}
-
// Set the interpolation factor
inline void Scene::setInterpolationFactor(float interpolationFactor) {
mInterpolationFactor = interpolationFactor;
@@ -279,6 +287,11 @@ inline void Scene::setIsContactPointsDisplayed(bool display) {
mIsContactPointsDisplayed = display;
}
+// Display/Hide the AABBs
+inline void Scene::setIsAABBsDisplayed(bool display) {
+ mIsAABBsDisplayed = display;
+}
+
// Return true if wireframe rendering is enabled
inline bool Scene::getIsWireframeEnabled() const {
return mIsWireframeEnabled;
diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp
index d406bcea..db5d030e 100644
--- a/testbed/src/SceneDemo.cpp
+++ b/testbed/src/SceneDemo.cpp
@@ -26,27 +26,28 @@
// Libraries
#include "SceneDemo.h"
#include <GLFW/glfw3.h>
+#include "AABB.h"
using namespace openglframework;
int SceneDemo::shadowMapTextureLevel = 0;
-openglframework::Color SceneDemo::mGreyColorDemo = Color(0.70f, 0.70f, 0.7f, 1.0);
-openglframework::Color SceneDemo::mYellowColorDemo = Color(0.9, 0.88, 0.145, 1.0);
-openglframework::Color SceneDemo::mBlueColorDemo = Color(0, 0.66, 0.95, 1.0);
-openglframework::Color SceneDemo::mOrangeColorDemo = Color(0.9, 0.35, 0, 1.0);
-openglframework::Color SceneDemo::mPinkColorDemo = Color(0.83, 0.48, 0.64, 1.0);
-openglframework::Color SceneDemo::mRedColorDemo = Color(0.95, 0, 0, 1.0);
+openglframework::Color SceneDemo::mGreyColorDemo = Color(0.70f, 0.70f, 0.7f, 1.0f);
+openglframework::Color SceneDemo::mYellowColorDemo = Color(0.9f, 0.88f, 0.145f, 1.0f);
+openglframework::Color SceneDemo::mBlueColorDemo = Color(0, 0.66f, 0.95f, 1.0f);
+openglframework::Color SceneDemo::mOrangeColorDemo = Color(0.9f, 0.35f, 0, 1.0f);
+openglframework::Color SceneDemo::mPinkColorDemo = Color(0.83f, 0.48f, 0.64f, 1.0f);
+openglframework::Color SceneDemo::mRedColorDemo = Color(0.95f, 0, 0, 1.0f);
int SceneDemo::mNbDemoColors = 4;
openglframework::Color SceneDemo::mDemoColors[] = {SceneDemo::mYellowColorDemo, SceneDemo::mBlueColorDemo,
SceneDemo::mOrangeColorDemo, SceneDemo::mPinkColorDemo};
// Constructor
-SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMappingEnabled)
- : Scene(name, isShadowMappingEnabled), mIsShadowMappingInitialized(false),
+SceneDemo::SceneDemo(const std::string& name, EngineSettings& settings, float sceneRadius, bool isShadowMappingEnabled)
+ : Scene(name, settings, isShadowMappingEnabled), mIsShadowMappingInitialized(false),
mDepthShader("shaders/depth.vert", "shaders/depth.frag"),
mPhongShader("shaders/phong.vert", "shaders/phong.frag"),
- mQuadShader("shaders/quad.vert", "shaders/quad.frag"),
mColorShader("shaders/color.vert", "shaders/color.frag"),
+ mQuadShader("shaders/quad.vert", "shaders/quad.frag"),
mVBOQuad(GL_ARRAY_BUFFER), mMeshFolderPath("meshes/") {
shadowMapTextureLevel++;
@@ -75,6 +76,9 @@ SceneDemo::SceneDemo(const std::string& name, float sceneRadius, bool isShadowMa
createQuadVBO();
+ // Init rendering for the AABBs
+ AABB::init();
+
VisualContactPoint::createStaticData(mMeshFolderPath);
}
@@ -93,6 +97,9 @@ SceneDemo::~SceneDemo() {
// Destroy the contact points
removeAllContactPoints();
+ // Destroy rendering data for the AABB
+ AABB::destroy();
+
VisualContactPoint::destroyStaticData();
}
@@ -114,6 +121,23 @@ void SceneDemo::update() {
// Can be called several times per frame
void SceneDemo::updatePhysics() {
+ if (getDynamicsWorld() != nullptr) {
+
+ // Update the physics engine parameters
+ getDynamicsWorld()->setIsGratityEnabled(mEngineSettings.isGravityEnabled);
+ rp3d::Vector3 gravity(mEngineSettings.gravity.x, mEngineSettings.gravity.y,
+ mEngineSettings.gravity.z);
+ getDynamicsWorld()->setGravity(gravity);
+ getDynamicsWorld()->enableSleeping(mEngineSettings.isSleepingEnabled);
+ getDynamicsWorld()->setSleepLinearVelocity(mEngineSettings.sleepLinearVelocity);
+ getDynamicsWorld()->setSleepAngularVelocity(mEngineSettings.sleepAngularVelocity);
+ getDynamicsWorld()->setNbIterationsPositionSolver(mEngineSettings.nbPositionSolverIterations);
+ getDynamicsWorld()->setNbIterationsVelocitySolver(mEngineSettings.nbVelocitySolverIterations);
+ getDynamicsWorld()->setTimeBeforeSleep(mEngineSettings.timeBeforeSleep);
+
+ // Take a simulation step
+ getDynamicsWorld()->update(mEngineSettings.timeStep);
+ }
}
// Render the scene (in multiple passes for shadow mapping)
@@ -209,6 +233,11 @@ void SceneDemo::render() {
renderContactPoints(mPhongShader, worldToCameraMatrix);
}
+ // Render the AABBs
+ if (mIsAABBsDisplayed) {
+ renderAABBs(worldToCameraMatrix);
+ }
+
if (mIsShadowMappingEnabled) mShadowMapTexture.unbind();
mPhongShader.unbind();
@@ -218,16 +247,24 @@ void SceneDemo::render() {
// Render the scene in a single pass
void SceneDemo::renderSinglePass(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
- // Bind the shader
+ if (mIsWireframeEnabled) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
+ }
+
+ // Bind the shader
shader.bind();
// Render all the physics objects of the scene
for (std::vector<PhysicsObject*>::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
- (*it)->render(shader, worldToCameraMatrix, mIsWireframeEnabled);
+ (*it)->render(mIsWireframeEnabled ? mColorShader : shader, worldToCameraMatrix);
}
// Unbind the shader
shader.unbind();
+
+ if (mIsWireframeEnabled) {
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
+ }
}
// Create the Shadow map FBO and texture
@@ -338,13 +375,38 @@ void SceneDemo::updateContactPoints() {
// Render the contact points
void SceneDemo::renderContactPoints(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
- // Render all the raycast hit points
+ // Render all the contact points
for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
it != mContactPoints.end(); ++it) {
(*it)->render(mColorShader, worldToCameraMatrix);
}
}
+// Render the AABBs
+void SceneDemo::renderAABBs(const openglframework::Matrix4& worldToCameraMatrix) {
+
+ // For each physics object of the scene
+ for (std::vector<PhysicsObject*>::iterator it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
+
+ // For each proxy shape of the object
+ rp3d::ProxyShape* proxyShape = (*it)->getCollisionBody()->getProxyShapesList();
+ while (proxyShape != nullptr) {
+
+ // Get the broad-phase AABB corresponding to the proxy shape
+ rp3d::AABB aabb = mPhysicsWorld->getWorldAABB(proxyShape);
+
+ openglframework::Vector3 aabbCenter(aabb.getCenter().x, aabb.getCenter().y, aabb.getCenter().z);
+ openglframework::Vector3 aabbMin(aabb.getMin().x, aabb.getMin().y, aabb.getMin().z);
+ openglframework::Vector3 aabbMax(aabb.getMax().x, aabb.getMax().y, aabb.getMax().z);
+
+ // Render the AABB
+ AABB::render(aabbCenter, aabbMax - aabbMin, Color::green(), mColorShader, worldToCameraMatrix);
+
+ proxyShape = proxyShape->getNext();
+ }
+ }
+}
+
void SceneDemo::removeAllContactPoints() {
// Destroy all the visual contact points
diff --git a/testbed/src/SceneDemo.h b/testbed/src/SceneDemo.h
index a8b1941f..359c2374 100644
--- a/testbed/src/SceneDemo.h
+++ b/testbed/src/SceneDemo.h
@@ -98,6 +98,8 @@ class SceneDemo : public Scene {
std::vector<PhysicsObject*> mPhysicsObjects;
+ rp3d::CollisionWorld* mPhysicsWorld;
+
// -------------------- Methods -------------------- //
// Create the Shadow map FBO and texture
@@ -116,14 +118,25 @@ class SceneDemo : public Scene {
void renderContactPoints(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix);
+
+ /// Render the AABBs
+ void renderAABBs(const openglframework::Matrix4& worldToCameraMatrix);
+
+ /// Remove all contact points
void removeAllContactPoints();
+ /// Return a reference to the dynamics world
+ rp3d::DynamicsWorld* getDynamicsWorld();
+
+ /// Return a reference to the dynamics world
+ const rp3d::DynamicsWorld* getDynamicsWorld() const;
+
public:
// -------------------- Methods -------------------- //
/// Constructor
- SceneDemo(const std::string& name, float sceneRadius, bool isShadowMappingEnabled = true);
+ SceneDemo(const std::string& name, EngineSettings& settings, float sceneRadius, bool isShadowMappingEnabled = true);
/// Destructor
virtual ~SceneDemo() override;
@@ -158,6 +171,16 @@ inline void SceneDemo::setIsShadowMappingEnabled(bool isShadowMappingEnabled) {
}
}
+// Return a reference to the dynamics world
+inline rp3d::DynamicsWorld* SceneDemo::getDynamicsWorld() {
+ return dynamic_cast<rp3d::DynamicsWorld*>(mPhysicsWorld);
+}
+
+// Return a reference to the dynamics world
+inline const rp3d::DynamicsWorld* SceneDemo::getDynamicsWorld() const {
+ return dynamic_cast<rp3d::DynamicsWorld*>(mPhysicsWorld);
+}
+
#endif
diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp
index 2b9195e5..d94103ef 100644
--- a/testbed/src/TestbedApplication.cpp
+++ b/testbed/src/TestbedApplication.cpp
@@ -59,12 +59,14 @@ TestbedApplication::TestbedApplication(bool isFullscreen)
mIsMultisamplingActive = true;
mWidth = 1280;
mHeight = 720;
+ mEngineSettings = EngineSettings::defaultSettings();
mSinglePhysicsStepEnabled = false;
mSinglePhysicsStepDone = false;
mWindowToFramebufferRatio = Vector2(1, 1);
mIsShadowMappingEnabled = true;
mIsVSyncEnabled = false;
mIsContactPointsDisplayed = false;
+ mIsAABBsDisplayed = false;
mIsWireframeEnabled = false;
init();
@@ -97,39 +99,38 @@ void TestbedApplication::init() {
void TestbedApplication::createScenes() {
// Cubes scene
- CubesScene* cubeScene = new CubesScene("Cubes");
+ CubesScene* cubeScene = new CubesScene("Cubes", mEngineSettings);
mScenes.push_back(cubeScene);
// Joints scene
- JointsScene* jointsScene = new JointsScene("Joints");
+ JointsScene* jointsScene = new JointsScene("Joints", mEngineSettings);
mScenes.push_back(jointsScene);
// Collision shapes scene
- CollisionShapesScene* collisionShapesScene = new CollisionShapesScene("Collision Shapes");
+ CollisionShapesScene* collisionShapesScene = new CollisionShapesScene("Collision Shapes", mEngineSettings);
mScenes.push_back(collisionShapesScene);
// Heightfield shape scene
- HeightFieldScene* heightFieldScene = new HeightFieldScene("Heightfield");
+ HeightFieldScene* heightFieldScene = new HeightFieldScene("Heightfield", mEngineSettings);
mScenes.push_back(heightFieldScene);
// Raycast scene
- RaycastScene* raycastScene = new RaycastScene("Raycast");
+ RaycastScene* raycastScene = new RaycastScene("Raycast", mEngineSettings);
mScenes.push_back(raycastScene);
// Collision Detection scene
- CollisionDetectionScene* collisionDetectionScene = new CollisionDetectionScene("Collision Detection");
+ CollisionDetectionScene* collisionDetectionScene = new CollisionDetectionScene("Collision Detection", mEngineSettings);
mScenes.push_back(collisionDetectionScene);
// Concave Mesh scene
- ConcaveMeshScene* concaveMeshScene = new ConcaveMeshScene("Concave Mesh");
+ ConcaveMeshScene* concaveMeshScene = new ConcaveMeshScene("Concave Mesh", mEngineSettings);
mScenes.push_back(concaveMeshScene);
assert(mScenes.size() > 0);
- mCurrentScene = mScenes[5];
- // Get the engine settings from the scene
- mEngineSettings = mCurrentScene->getEngineSettings();
- mEngineSettings.timeStep = DEFAULT_TIMESTEP;
+ const int firstSceneIndex = 0;
+
+ switchScene(mScenes[firstSceneIndex]);
}
// Remove all the scenes
@@ -152,9 +153,8 @@ void TestbedApplication::updateSinglePhysicsStep() {
// Update the physics of the current scene
void TestbedApplication::updatePhysics() {
- // Set the engine settings
+ // Update the elapsed time
mEngineSettings.elapsedTime = mTimer.getPhysicsTime();
- mCurrentScene->setEngineSettings(mEngineSettings);
if (mTimer.isRunning()) {
@@ -202,6 +202,9 @@ void TestbedApplication::update() {
// Display/Hide contact points
mCurrentScene->setIsContactPointsDisplayed(mIsContactPointsDisplayed);
+ // Display/Hide the AABBs
+ mCurrentScene->setIsAABBsDisplayed(mIsAABBsDisplayed);
+
// Enable/Disable wireframe mode
mCurrentScene->setIsWireframeEnabled(mIsWireframeEnabled);
@@ -259,11 +262,6 @@ void TestbedApplication::switchScene(Scene* newScene) {
mCurrentScene = newScene;
- // Get the engine settings of the scene
- float currentTimeStep = mEngineSettings.timeStep;
- mEngineSettings = mCurrentScene->getEngineSettings();
- mEngineSettings.timeStep = currentTimeStep;
-
// Reset the scene
mCurrentScene->reset();
diff --git a/testbed/src/TestbedApplication.h b/testbed/src/TestbedApplication.h
index 8790a1f7..3a9fe071 100644
--- a/testbed/src/TestbedApplication.h
+++ b/testbed/src/TestbedApplication.h
@@ -38,9 +38,6 @@ using namespace nanogui;
// Macro for OpenGL errors
#define checkOpenGLErrors() checkOpenGLErrorsInternal(__FILE__,__LINE__)
-// Constants
-const float DEFAULT_TIMESTEP = 1.0f / 60.0f;
-
/// Class TestbedApplication
class TestbedApplication : public Screen {
@@ -109,6 +106,9 @@ class TestbedApplication : public Screen {
/// True if contact points are displayed
bool mIsContactPointsDisplayed;
+ /// True if the AABBs of physics objects are displayed
+ bool mIsAABBsDisplayed;
+
/// True if the wireframe rendering is enabled
bool mIsWireframeEnabled;
From 002264a5a14dbcaad67ce5364b4aabd32d29d184 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 2 Nov 2017 23:01:32 +0100
Subject: [PATCH 101/133] Remove unused files
---
.../narrowphase/ConcaveVsConvexAlgorithm.cpp | 302 ------------------
.../narrowphase/ConcaveVsConvexAlgorithm.h | 196 ------------
2 files changed, 498 deletions(-)
delete mode 100644 src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
delete mode 100644 src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
deleted file mode 100644
index 8a2475a2..00000000
--- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
+++ /dev/null
@@ -1,302 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-/*
-// Libraries
-#include "collision/shapes/ConcaveShape.h"
-#include "collision/shapes/TriangleShape.h"
-#include "ConcaveVsConvexAlgorithm.h"
-#include "collision/CollisionDetection.h"
-#include "engine/CollisionWorld.h"
-#include <algorithm>
-
-using namespace reactphysics3d;
-
-// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase)
-void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
- const Vector3* verticesNormals) {
-
- // Create a triangle collision shape
- decimal margin = mConcaveShape->getTriangleMargin();
- TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
- TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
- verticesNormals, meshSubPart, triangleIndex, margin);
-
- // Create a narrow phase info for the narrow-phase collision detection
- NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList;
- narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo)))
- NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(),
- triangleShape, mConvexProxyShape->getLocalToWorldTransform(),
- mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(),
- mConcaveProxyShape->getCachedCollisionData());
- narrowPhaseInfoList->next = firstNarrowPhaseInfo;
-}
-
-// Return true and compute a contact info if the two bounding volumes collide
-void ConcaveVsConvexAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- NarrowPhaseCallback* narrowPhaseCallback) {
-
-// ProxyShape* convexProxyShape;
-// ProxyShape* concaveProxyShape;
-// const ConvexShape* convexShape;
-// const ConcaveShape* concaveShape;
-
-// // Collision shape 1 is convex, collision shape 2 is concave
-// if (shape1Info.collisionShape->isConvex()) {
-// convexProxyShape = shape1Info.proxyShape;
-// convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
-// concaveProxyShape = shape2Info.proxyShape;
-// concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
-// }
-// else { // Collision shape 2 is convex, collision shape 1 is concave
-// convexProxyShape = shape2Info.proxyShape;
-// convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
-// concaveProxyShape = shape1Info.proxyShape;
-// concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
-// }
-
-// // Set the parameters of the callback object
-// ConvexVsTriangleCallback convexVsTriangleCallback;
-// convexVsTriangleCallback.setCollisionDetection(mCollisionDetection);
-// convexVsTriangleCallback.setConvexShape(convexShape);
-// convexVsTriangleCallback.setConcaveShape(concaveShape);
-// convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
-// convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
-
-// // Compute the convex shape AABB in the local-space of the convex shape
-// AABB aabb;
-// convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
-
-// // If smooth mesh collision is enabled for the concave mesh
-// if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
-
-// std::vector<SmoothMeshContactInfo> contactPoints;
-
-// SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
-
-// convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback);
-
-// // Call the convex vs triangle callback for each triangle of the concave shape
-// concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
-
-// // Run the smooth mesh collision algorithm
-// processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
-// }
-// else {
-
-// convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
-
-// // Call the convex vs triangle callback for each triangle of the concave shape
-// concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
-// }
-}
-
-//// Test collision between a triangle and the convex mesh shape
-//void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
-
-// // Create a triangle collision shape
-// decimal margin = mConcaveShape->getTriangleMargin();
-// TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
-
-// // Select the collision algorithm to use between the triangle and the convex shape
-// NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(),
-// mConvexShape->getType());
-
-// // If there is no collision algorithm between those two kinds of shapes
-// if (algo == nullptr) return;
-
-// // Notify the narrow-phase algorithm about the overlapping pair we are going to test
-// algo->setCurrentOverlappingPair(mOverlappingPair);
-
-// // Create the CollisionShapeInfo objects
-// CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(),
-// mOverlappingPair, mConvexProxyShape->getCachedCollisionData());
-// CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape,
-// mConcaveProxyShape->getLocalToWorldTransform(),
-// mOverlappingPair, mConcaveProxyShape->getCachedCollisionData());
-
-// // Use the collision algorithm to test collision between the triangle and the other convex shape
-// algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback);
-//}
-
-// Process the concave triangle mesh collision using the smooth mesh collision algorithm described
-// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
-// issue with some internal edges.
-//void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
-// std::vector<SmoothMeshContactInfo> contactPoints,
-// NarrowPhaseCallback* narrowPhaseCallback) {
-
-// // Set with the triangle vertices already processed to void further contacts with same triangle
-// std::unordered_multimap<int, Vector3> processTriangleVertices;
-
-// // Sort the list of narrow-phase contacts according to their penetration depth
-// std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
-
-// // For each contact point (from smaller penetration depth to larger)
-// std::vector<SmoothMeshContactInfo>::const_iterator it;
-// for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
-
-// const SmoothMeshContactInfo info = *it;
-// const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
-
-// // Compute the barycentric coordinates of the point in the triangle
-// decimal u, v, w;
-// computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
-// info.triangleVertices[1],
-// info.triangleVertices[2],
-// contactPoint, u, v, w);
-// int nbZeros = 0;
-// bool isUZero = approxEqual(u, 0, 0.0001);
-// bool isVZero = approxEqual(v, 0, 0.0001);
-// bool isWZero = approxEqual(w, 0, 0.0001);
-// if (isUZero) nbZeros++;
-// if (isVZero) nbZeros++;
-// if (isWZero) nbZeros++;
-
-// // If it is a vertex contact
-// if (nbZeros == 2) {
-
-// Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
-
-// // Check that this triangle vertex has not been processed yet
-// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
-
-// // Keep the contact as it is and report it
-// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
-// }
-// }
-// else if (nbZeros == 1) { // If it is an edge contact
-
-// Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
-// Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
-
-// // Check that this triangle edge has not been processed yet
-// if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
-// !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
-
-// // Keep the contact as it is and report it
-// narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
-// }
-
-// }
-// else { // If it is a face contact
-
-// ContactPointInfo newContactInfo(info.contactInfo);
-
-// ProxyShape* firstShape;
-// ProxyShape* secondShape;
-// if (info.isFirstShapeTriangle) {
-// firstShape = overlappingPair->getShape1();
-// secondShape = overlappingPair->getShape2();
-// }
-// else {
-// firstShape = overlappingPair->getShape2();
-// secondShape = overlappingPair->getShape1();
-// }
-
-// // We use the triangle normal as the contact normal
-// Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
-// Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
-// Vector3 localNormal = a.cross(b);
-// newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
-// Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
-// Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
-// newContactInfo.normal.normalize();
-// if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
-// newContactInfo.normal = -newContactInfo.normal;
-// }
-
-// // We recompute the contact point on the second body with the new normal as described in
-// // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
-// // Dirk Gregorius) to avoid adding torque
-// Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
-// if (info.isFirstShapeTriangle) {
-// Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
-// newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
-// }
-// else {
-// Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
-// newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
-// }
-
-// // Report the contact
-// narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
-// }
-
-// // Add the three vertices of the triangle to the set of processed
-// // triangle vertices
-// addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
-// addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
-// addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
-// }
-//}
-
-// Return true if the vertex is in the set of already processed vertices
-bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) const {
-
- int key = int(vertex.x * vertex.y * vertex.z);
-
- auto range = processTriangleVertices.equal_range(key);
- for (auto it = range.first; it != range.second; ++it) {
- if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true;
- }
-
- return false;
-}
-
-
-//// Called by a narrow-phase collision algorithm when a new contact has been found
-//void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
-// const ContactPointInfo& contactInfo) {
-// Vector3 triangleVertices[3];
-// bool isFirstShapeTriangle;
-
-// // If the collision shape 1 is the triangle
-// if (contactInfo.collisionShape1->getType() == CollisionShapeType::TRIANGLE) {
-// assert(contactInfo.collisionShape2->getType() != CollisionShapeType::TRIANGLE);
-
-// const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1);
-// triangleVertices[0] = triangleShape->getVertex(0);
-// triangleVertices[1] = triangleShape->getVertex(1);
-// triangleVertices[2] = triangleShape->getVertex(2);
-
-// isFirstShapeTriangle = true;
-// }
-// else { // If the collision shape 2 is the triangle
-// assert(contactInfo.collisionShape2->getType() == CollisionShapeType::TRIANGLE);
-
-// const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2);
-// triangleVertices[0] = triangleShape->getVertex(0);
-// triangleVertices[1] = triangleShape->getVertex(1);
-// triangleVertices[2] = triangleShape->getVertex(2);
-
-// isFirstShapeTriangle = false;
-// }
-// SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
-
-// // Add the narrow-phase contact into the list of contact to process for
-// // smooth mesh collision
-// mContactPoints.push_back(smoothContactInfo);
-//}
-*/
diff --git a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h b/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
deleted file mode 100644
index 70cd3dbc..00000000
--- a/src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
+++ /dev/null
@@ -1,196 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2016 Daniel Chappuis *
-*********************************************************************************
-* *
-* This software is provided 'as-is', without any express or implied warranty. *
-* In no event will the authors be held liable for any damages arising from the *
-* use of this software. *
-* *
-* Permission is granted to anyone to use this software for any purpose, *
-* including commercial applications, and to alter it and redistribute it *
-* freely, subject to the following restrictions: *
-* *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-* that you wrote the original software. If you use this software in a *
-* product, an acknowledgment in the product documentation would be *
-* appreciated but is not required. *
-* *
-* 2. Altered source versions must be plainly marked as such, and must not be *
-* misrepresented as being the original software. *
-* *
-* 3. This notice may not be removed or altered from any source distribution. *
-* *
-********************************************************************************/
-/*
-#ifndef REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
-#define REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
-
-// Libraries
-#include "NarrowPhaseAlgorithm.h"
-#include "collision/shapes/ConvexShape.h"
-#include "collision/shapes/ConcaveShape.h"
-#include "memory/SingleFrameAllocator.h"
-#include <unordered_map>
-
-/// Namespace ReactPhysics3D
-namespace reactphysics3d {
-
-// Class ConvexVsTriangleCallback
-class MiddlePhaseTriangleCallback : public TriangleCallback {
-
- protected:
-
- /// Broadphase overlapping pair
- OverlappingPair* mOverlappingPair;
-
- /// Pointer to the concave proxy shape
- ProxyShape* mConcaveProxyShape;
-
- /// Pointer to the convex proxy shape
- ProxyShape* mConvexProxyShape;
-
- /// Pointer to the concave collision shape
- const ConcaveShape* mConcaveShape;
-
- /// Reference to the single-frame memory allocator
- Allocator& mAllocator;
-
- public:
-
- /// Pointer to the first element of the linked-list of narrow-phase info
- NarrowPhaseInfo* narrowPhaseInfoList;
-
- /// Constructor
- MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair,
- ProxyShape* concaveProxyShape,
- ProxyShape* convexProxyShape, const ConcaveShape* concaveShape,
- Allocator& allocator)
- :mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape),
- mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape),
- mAllocator(allocator), narrowPhaseInfoList(nullptr) {
-
- }
-
- /// Test collision between a triangle and the convex mesh shape
- virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints,
- const Vector3* verticesNormals) override;
-};
-
-// Class SmoothMeshContactInfo
-struct SmoothMeshContactInfo {
-
- public:
-
- ContactManifoldInfo* contactManifoldInfo;
- ContactPointInfo* contactInfo;
- bool isFirstShapeTriangle;
- Vector3 triangleVertices[3];
- bool isUVWZero[3];
-
- /// Constructor
- SmoothMeshContactInfo(ContactManifoldInfo* manifoldInfo, ContactPointInfo* contactPointInfo,
- bool firstShapeTriangle,
- const Vector3& trianglePoint1, const Vector3& trianglePoint2,
- const Vector3& trianglePoint3, bool isUZero, bool isVZero, bool isWZero)
- : contactManifoldInfo(manifoldInfo), contactInfo(contactPointInfo) {
-
- isFirstShapeTriangle = firstShapeTriangle;
-
- triangleVertices[0] = trianglePoint1;
- triangleVertices[1] = trianglePoint2;
- triangleVertices[2] = trianglePoint3;
-
- isUVWZero[0] = isUZero;
- isUVWZero[1] = isVZero;
- isUVWZero[2] = isWZero;
- }
-
-};
-
-struct ContactsDepthCompare {
- bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2)
- {
- return contact1.contactInfo->penetrationDepth < contact2.contactInfo->penetrationDepth;
- }
-};
-
-/// Method used to compare two smooth mesh contact info to sort them
-//inline static bool contactsDepthCompare(const SmoothMeshContactInfo& contact1,
-// const SmoothMeshContactInfo& contact2) {
-// return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
-//}
-
-// TODO : Delete this
-// Class SmoothCollisionNarrowPhaseCallback
-class SmoothCollisionNarrowPhaseCallback {
-
- private:
-
- std::vector<SmoothMeshContactInfo>& mContactPoints;
-
-
- public:
-
- // Constructor
- SmoothCollisionNarrowPhaseCallback(std::vector<SmoothMeshContactInfo>& contactPoints)
- : mContactPoints(contactPoints) {
-
- }
-
-};
-
-// TODO : Delete this
-// Class ConcaveVsConvexAlgorithm
-class ConcaveVsConvexAlgorithm {
-
- protected :
-
- // -------------------- Attributes -------------------- //
-
- // -------------------- Methods -------------------- //
-
- /// Process the concave triangle mesh collision using the smooth mesh collision algorithm
- void processSmoothMeshCollision(OverlappingPair* overlappingPair,
- std::vector<SmoothMeshContactInfo> contactPoints,
- NarrowPhaseCallback* narrowPhaseCallback);
-
- /// Add a triangle vertex into the set of processed triangles
- void addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices,
- const Vector3& vertex);
-
- /// Return true if the vertex is in the set of already processed vertices
- bool hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices,
- const Vector3& vertex) const;
-
- public :
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- ConcaveVsConvexAlgorithm() = default;
-
- /// Destructor
- ~ConcaveVsConvexAlgorithm() = default;
-
- /// Private copy-constructor
- ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm) = delete;
-
- /// Private assignment operator
- ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm) = delete;
-
- /// Compute a contact info if the two bounding volume collide
- void testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
- NarrowPhaseCallback* narrowPhaseCallback);
-};
-
-// Add a triangle vertex into the set of processed triangles
-inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) {
- processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex));
-}
-
-}
-
-#endif
-
-*/
From fd427c0337b58422d1a4bd5e575ba10fc29f0968 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 3 Nov 2017 07:11:19 +0100
Subject: [PATCH 102/133] Fix compilation errors because of removed files
---
CMakeLists.txt | 2 --
src/collision/narrowphase/DefaultCollisionDispatch.h | 1 -
2 files changed, 3 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index dcc2e4bf..c4cfde6e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -83,8 +83,6 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp"
"src/collision/narrowphase/SphereVsCapsuleAlgorithm.h"
"src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp"
- "src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
- "src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
"src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h"
"src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp"
"src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h"
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h
index 7b3945d3..779f7048 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.h
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.h
@@ -28,7 +28,6 @@
// Libraries
#include "CollisionDispatch.h"
-#include "ConcaveVsConvexAlgorithm.h"
#include "SphereVsSphereAlgorithm.h"
#include "SphereVsConvexPolyhedronAlgorithm.h"
#include "SphereVsCapsuleAlgorithm.h"
From 6e322882eb21e66a44a1f055beb75f4b52f9408e Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 5 Nov 2017 23:10:55 +0100
Subject: [PATCH 103/133] Fix wrong world AABB computation that caused
broad-phase collision misses
---
src/collision/shapes/CollisionShape.cpp | 50 +++++++++++++++++--------
1 file changed, 34 insertions(+), 16 deletions(-)
diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp
index d03b4a1c..9622e795 100644
--- a/src/collision/shapes/CollisionShape.cpp
+++ b/src/collision/shapes/CollisionShape.cpp
@@ -32,15 +32,19 @@
using namespace reactphysics3d;
// Constructor
-CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type) : mName(name), mType(type), mScaling(1.0, 1.0, 1.0) {
+CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type)
+ : mType(type), mName(name), mScaling(1.0, 1.0, 1.0) {
}
-// Compute the world-space AABB of the collision shape given a transform
+// Compute the world-space AABB of the collision shape given a transform from shape
+// local-space to world-space. The technique is described in the book Real-Time Collision
+// Detection by Christer Ericson.
/**
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
* computed in world-space coordinates
- * @param transform Transform used to compute the AABB of the collision shape
+ * @param transform Transform from shape local-space to world-space used to compute
+ * the AABB of the collision shape
*/
void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
@@ -51,20 +55,34 @@ void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
Vector3 maxBounds;
getLocalBounds(minBounds, maxBounds);
- // Rotate the local bounds according to the orientation of the body
- Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix();
- Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
- worldAxis.getColumn(1).dot(minBounds),
- worldAxis.getColumn(2).dot(minBounds));
- Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
- worldAxis.getColumn(1).dot(maxBounds),
- worldAxis.getColumn(2).dot(maxBounds));
+ const Vector3 translation = transform.getPosition();
+ Matrix3x3 matrix = transform.getOrientation().getMatrix();
+ Vector3 resultMin;
+ Vector3 resultMax;
- // Compute the minimum and maximum coordinates of the rotated extents
- Vector3 minCoordinates = transform.getPosition() + worldMinBounds;
- Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds;
+ // For each of the three axis
+ for (int i=0; i<3; i++) {
+
+ // Add translation component
+ resultMin[i] = translation[i];
+ resultMax[i] = translation[i];
+
+ for (int j=0; j<3; j++) {
+ decimal e = matrix[i][j] * minBounds[j];
+ decimal f = matrix[i][j] * maxBounds[j];
+
+ if (e < f) {
+ resultMin[i] += e;
+ resultMax[i] += f;
+ }
+ else {
+ resultMin[i] += f;
+ resultMax[i] += e;
+ }
+ }
+ }
// Update the AABB with the new minimum and maximum coordinates
- aabb.setMin(minCoordinates);
- aabb.setMax(maxCoordinates);
+ aabb.setMin(resultMin);
+ aabb.setMax(resultMax);
}
From e91cded83161106314db4d5f9680595d66a6b2c0 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 5 Nov 2017 23:15:47 +0100
Subject: [PATCH 104/133] Update code documentation and fix warnings
---
src/collision/shapes/BoxShape.cpp | 1 -
src/collision/shapes/BoxShape.h | 15 ++++-----------
src/collision/shapes/ConcaveMeshShape.h | 2 +-
src/mathematics/Matrix3x3.h | 6 +++---
4 files changed, 8 insertions(+), 16 deletions(-)
diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp
index 01c4d965..6b8c8d6b 100644
--- a/src/collision/shapes/BoxShape.cpp
+++ b/src/collision/shapes/BoxShape.cpp
@@ -35,7 +35,6 @@ using namespace reactphysics3d;
// Constructor
/**
* @param extent The vector with the three extents of the box (in meters)
- * @param margin The collision margin (in meters) around the collision shape
*/
BoxShape::BoxShape(const Vector3& extent)
: ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent) {
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index 56c14797..5643a5c8 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -41,14 +41,7 @@ namespace reactphysics3d {
* This class represents a 3D box shape. Those axis are unit length.
* The three extents are half-widths of the box along the three
* axis x, y, z local axis. The "transform" of the corresponding
- * rigid body will give an orientation and a position to the box. This
- * collision shape uses an extra margin distance around it for collision
- * detection purpose. The default margin is 4cm (if your units are meters,
- * which is recommended). In case, you want to simulate small objects
- * (smaller than the margin distance), you might want to reduce the margin by
- * specifying your own margin distance using the "margin" parameter in the
- * constructor of the box shape. Otherwise, it is recommended to use the
- * default margin distance by not using the "margin" parameter in the constructor.
+ * body will give an orientation and a position to the box.
*/
class BoxShape : public ConvexPolyhedronShape {
@@ -171,9 +164,9 @@ inline size_t BoxShape::getSizeInBytes() const {
// Return a local support point in a given direction without the objec margin
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
- return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x,
- direction.y < 0.0 ? -mExtent.y : mExtent.y,
- direction.z < 0.0 ? -mExtent.z : mExtent.z);
+ return Vector3(direction.x < decimal(0.0) ? -mExtent.x : mExtent.x,
+ direction.y < decimal(0.0) ? -mExtent.y : mExtent.y,
+ direction.z < decimal(0.0) ? -mExtent.z : mExtent.z);
}
// Return true if a point is inside the collision shape
diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h
index b99f2b4a..82c08794 100644
--- a/src/collision/shapes/ConcaveMeshShape.h
+++ b/src/collision/shapes/ConcaveMeshShape.h
@@ -103,7 +103,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
// Class ConcaveMeshShape
/**
* This class represents a static concave mesh shape. Note that collision detection
- * with a concave mesh shape can be very expensive. You should use only use
+ * with a concave mesh shape can be very expensive. You should only use
* this shape for a static mesh.
*/
class ConcaveMeshShape : public ConcaveShape {
diff --git a/src/mathematics/Matrix3x3.h b/src/mathematics/Matrix3x3.h
index dfa23085..ebb8792d 100644
--- a/src/mathematics/Matrix3x3.h
+++ b/src/mathematics/Matrix3x3.h
@@ -233,9 +233,9 @@ inline Matrix3x3 Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(const Vect
// Return the matrix with absolute values
inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const {
- return Matrix3x3(fabs(mRows[0][0]), fabs(mRows[0][1]), fabs(mRows[0][2]),
- fabs(mRows[1][0]), fabs(mRows[1][1]), fabs(mRows[1][2]),
- fabs(mRows[2][0]), fabs(mRows[2][1]), fabs(mRows[2][2]));
+ return Matrix3x3(std::fabs(mRows[0][0]), std::fabs(mRows[0][1]), std::fabs(mRows[0][2]),
+ std::fabs(mRows[1][0]), std::fabs(mRows[1][1]), std::fabs(mRows[1][2]),
+ std::fabs(mRows[2][0]), std::fabs(mRows[2][1]), std::fabs(mRows[2][2]));
}
// Overloaded operator for addition
From c6f93789975013df309f735516515ff6c2b91636 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 8 Nov 2017 21:13:02 +0100
Subject: [PATCH 105/133] Enable vsync in the testbed application
---
testbed/src/TestbedApplication.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp
index d94103ef..f2a43bc1 100644
--- a/testbed/src/TestbedApplication.cpp
+++ b/testbed/src/TestbedApplication.cpp
@@ -64,7 +64,7 @@ TestbedApplication::TestbedApplication(bool isFullscreen)
mSinglePhysicsStepDone = false;
mWindowToFramebufferRatio = Vector2(1, 1);
mIsShadowMappingEnabled = true;
- mIsVSyncEnabled = false;
+ mIsVSyncEnabled = true;
mIsContactPointsDisplayed = false;
mIsAABBsDisplayed = false;
mIsWireframeEnabled = false;
From b39bb3ba37f4ae068d31afeb8ce169fa2b1f5bb6 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 8 Nov 2017 21:14:21 +0100
Subject: [PATCH 106/133] Initialize the timestep in the DynamicsWorld
constructor
---
src/engine/DynamicsWorld.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 09712c3c..26835166 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -44,7 +44,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
- mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity),
+ mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr),
mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr),
mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr),
From 013431487eb633f4df09ed6c4292091ae0ef6e4e Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 8 Nov 2017 21:17:16 +0100
Subject: [PATCH 107/133] Rename setScaling() to setLocalScaling()
---
src/collision/ProxyShape.h | 2 +-
src/collision/shapes/CollisionShape.h | 4 ++--
src/collision/shapes/ConvexMeshShape.h | 21 ++++++---------------
3 files changed, 9 insertions(+), 18 deletions(-)
diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h
index 36bec740..8282af01 100644
--- a/src/collision/ProxyShape.h
+++ b/src/collision/ProxyShape.h
@@ -332,7 +332,7 @@ inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBit
* @return The local scaling vector
*/
inline Vector3 ProxyShape::getLocalScaling() const {
- return mCollisionShape->getScaling();
+ return mCollisionShape->getLocalScaling();
}
// Set the local scaling vector of the collision shape
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index 77f96dfb..f6690c58 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -113,7 +113,7 @@ class CollisionShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;
/// Return the scaling vector of the collision shape
- Vector3 getScaling() const;
+ Vector3 getLocalScaling() const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
@@ -147,7 +147,7 @@ inline CollisionShapeType CollisionShape::getType() const {
}
// Return the scaling vector of the collision shape
-inline Vector3 CollisionShape::getScaling() const {
+inline Vector3 CollisionShape::getLocalScaling() const {
return mScaling;
}
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index 4ceabe11..71077af2 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -46,18 +46,9 @@ class CollisionWorld;
// Class ConvexMeshShape
/**
* This class represents a convex mesh shape. In order to create a convex mesh shape, you
- * need to indicate the local-space position of the mesh vertices. You do it either by
- * passing a vertices array to the constructor or using the addVertex() method. Make sure
- * that the set of vertices that you use to create the shape are indeed part of a convex
- * mesh. The center of mass of the shape will be at the origin of the local-space geometry
- * that you use to create the mesh. The method used for collision detection with a convex
- * mesh shape has an O(n) running time with "n" beeing the number of vertices in the mesh.
- * Therefore, you should try not to use too many vertices. However, it is possible to speed
- * up the collision detection by using the edges information of your mesh. The running time
- * of the collision detection that uses the edges is almost O(1) constant time at the cost
- * of additional memory used to store the vertices. You can indicate edges information
- * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
- * in order to use the edges information for collision detection.
+ * need to indicate the local-space position of the mesh vertices. The center of mass
+ * of the shape will be at the origin of the local-space geometry that you use to create
+ * the mesh.
*/
class ConvexMeshShape : public ConvexPolyhedronShape {
@@ -79,9 +70,6 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
/// Recompute the bounds of the mesh
void recalculateBounds();
- /// Set the scaling vector of the collision shape
- virtual void setLocalScaling(const Vector3& scaling) override;
-
/// Return a local support point in a given direction without the object margin.
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
@@ -110,6 +98,9 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
/// Deleted assignment operator
ConvexMeshShape& operator=(const ConvexMeshShape& shape) = delete;
+ /// Set the scaling vector of the collision shape
+ virtual void setLocalScaling(const Vector3& scaling) override;
+
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
From 8bfa6dd137d49e448cc5a5565e5deffac6b8388c Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 8 Nov 2017 21:26:15 +0100
Subject: [PATCH 108/133] Fix issue : the scaling factor was not used when
recomputing AABB tree for concave mesh shape
---
src/collision/shapes/ConcaveMeshShape.cpp | 5 +++++
1 file changed, 5 insertions(+)
diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp
index 84e95e2c..566b7bc4 100644
--- a/src/collision/shapes/ConcaveMeshShape.cpp
+++ b/src/collision/shapes/ConcaveMeshShape.cpp
@@ -57,6 +57,11 @@ void ConcaveMeshShape::initBVHTree() {
// Get the triangle vertices
triangleVertexArray->getTriangleVertices(triangleIndex, trianglePoints);
+ // Apply the scaling factor to the vertices
+ trianglePoints[0] *= mScaling.x;
+ trianglePoints[1] *= mScaling.y;
+ trianglePoints[2] *= mScaling.z;
+
// Create the AABB for the triangle
AABB aabb = AABB::createAABBForTriangle(trianglePoints);
From de95e15147e8edf8d72a2f1177e7f8de06ebb0c5 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 8 Nov 2017 21:28:00 +0100
Subject: [PATCH 109/133] Fix issue with the shape order in concave vs convex
middle-phase collision detection
---
src/collision/MiddlePhaseTriangleCallback.cpp | 15 +++++++++++----
1 file changed, 11 insertions(+), 4 deletions(-)
diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp
index de6c22ea..9d163799 100644
--- a/src/collision/MiddlePhaseTriangleCallback.cpp
+++ b/src/collision/MiddlePhaseTriangleCallback.cpp
@@ -38,12 +38,19 @@ void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIn
TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
verticesNormals, meshSubPart, triangleIndex);
+ bool isShape1Convex = mOverlappingPair->getShape1()->getCollisionShape()->isConvex();
+ ProxyShape* shape1 = isShape1Convex ? mConvexProxyShape : mConcaveProxyShape;
+ ProxyShape* shape2 = isShape1Convex ? mConcaveProxyShape : mConvexProxyShape;
+
// Create a narrow phase info for the narrow-phase collision detection
NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList;
narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo)))
- NarrowPhaseInfo(mOverlappingPair, mConvexProxyShape->getCollisionShape(),
- triangleShape, mConvexProxyShape->getLocalToWorldTransform(),
- mConcaveProxyShape->getLocalToWorldTransform(), mConvexProxyShape->getCachedCollisionData(),
- mConcaveProxyShape->getCachedCollisionData(), mAllocator);
+ NarrowPhaseInfo(mOverlappingPair,
+ isShape1Convex ? mConvexProxyShape->getCollisionShape() : triangleShape,
+ isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(),
+ shape1->getLocalToWorldTransform(),
+ shape2->getLocalToWorldTransform(),
+ shape1->getCachedCollisionData(),
+ shape2->getCachedCollisionData(), mAllocator);
narrowPhaseInfoList->next = firstNarrowPhaseInfo;
}
From 222636391e89ad3718cb00985120de8ba1eb18c9 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 10 Nov 2017 17:51:02 +0100
Subject: [PATCH 110/133] Use the true triangle face normal if contact is not
on an edge in smooth triangle contact
---
src/collision/shapes/TriangleShape.cpp | 11 ++++++++++-
1 file changed, 10 insertions(+), 1 deletion(-)
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index 263595b3..142ba181 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -133,13 +133,22 @@ void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle,
/// normal of the mesh at this point. The idea to solve this problem is to use the real (smooth) surface
/// normal of the mesh at this point as the contact normal. This technique is described in the chapter 5
/// of the Game Physics Pearl book by Gino van der Bergen and Dirk Gregorius. The vertices normals of the
-/// mesh are either provided by the user or precomputed if the user did not provide them.
+/// mesh are either provided by the user or precomputed if the user did not provide them. Note that we only
+/// use the interpolated normal if the contact point is on an edge of the triangle. If the contact is in the
+/// middle of the triangle, we return the true triangle normal.
Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const {
// Compute the barycentric coordinates of the point in the triangle
decimal u, v, w;
computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w);
+ // If the contact is in the middle of the triangle face (not on the edges)
+ if (u > MACHINE_EPSILON && v > MACHINE_EPSILON && w > MACHINE_EPSILON) {
+
+ // We return the true triangle face normal (not the interpolated one)
+ return mNormal;
+ }
+
// We compute the contact normal as the barycentric interpolation of the three vertices normals
return (u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]).getUnit();
}
From e9709c3db50add07cb4b9de05aedb4d8dbba983f Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Fri, 10 Nov 2017 19:57:50 +0100
Subject: [PATCH 111/133] Refactor the Profiler. Now there is one profiler
instance per CollisionWorld/DynamicsWorld instance instead of a static one
---
src/body/CollisionBody.cpp | 7 ++
src/body/CollisionBody.h | 24 ++++
src/body/RigidBody.cpp | 28 ++++-
src/body/RigidBody.h | 7 ++
src/collision/CollisionDetection.cpp | 106 +++---------------
src/collision/CollisionDetection.h | 36 +++++-
src/collision/MiddlePhaseTriangleCallback.cpp | 7 ++
src/collision/MiddlePhaseTriangleCallback.h | 17 +++
src/collision/ProxyShape.h | 26 +++++
.../broadphase/BroadPhaseAlgorithm.cpp | 7 ++
.../broadphase/BroadPhaseAlgorithm.h | 30 ++++-
src/collision/broadphase/DynamicAABBTree.cpp | 4 +-
src/collision/broadphase/DynamicAABBTree.h | 24 ++++
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 10 +-
src/collision/narrowphase/CollisionDispatch.h | 29 ++++-
...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 7 ++
.../narrowphase/DefaultCollisionDispatch.h | 25 +++++
.../narrowphase/GJK/GJKAlgorithm.cpp | 2 +-
src/collision/narrowphase/GJK/GJKAlgorithm.h | 24 ++++
.../narrowphase/NarrowPhaseAlgorithm.h | 24 ++++
.../narrowphase/SAT/SATAlgorithm.cpp | 6 +-
src/collision/narrowphase/SAT/SATAlgorithm.h | 25 +++++
.../SphereVsConvexPolyhedronAlgorithm.cpp | 14 +++
src/collision/shapes/CollisionShape.cpp | 2 +-
src/collision/shapes/CollisionShape.h | 25 +++++
src/collision/shapes/ConcaveMeshShape.cpp | 16 ++-
src/collision/shapes/ConcaveMeshShape.h | 35 ++++++
src/collision/shapes/HeightFieldShape.cpp | 16 ++-
src/collision/shapes/HeightFieldShape.h | 17 +++
src/collision/shapes/TriangleShape.cpp | 3 +-
src/engine/CollisionWorld.cpp | 13 +++
src/engine/CollisionWorld.h | 21 ++++
src/engine/ConstraintSolver.cpp | 12 +-
src/engine/ConstraintSolver.h | 23 ++++
src/engine/ContactSolver.cpp | 12 +-
src/engine/ContactSolver.h | 24 ++++
src/engine/DynamicsWorld.cpp | 43 ++++---
src/engine/DynamicsWorld.h | 14 ---
src/engine/Profiler.cpp | 35 +++++-
src/engine/Profiler.h | 82 +++++++++-----
.../CollisionDetectionScene.cpp | 69 +++++++-----
.../CollisionDetectionScene.h | 2 +-
.../collisionshapes/CollisionShapesScene.cpp | 9 +-
.../scenes/concavemesh/ConcaveMeshScene.cpp | 7 ++
testbed/scenes/cubes/CubesScene.cpp | 6 +
.../scenes/heightfield/HeightFieldScene.cpp | 9 +-
testbed/scenes/joints/JointsScene.cpp | 6 +
testbed/scenes/raycast/RaycastScene.cpp | 6 +
testbed/src/Scene.h | 4 +-
49 files changed, 790 insertions(+), 210 deletions(-)
mode change 100755 => 100644 testbed/scenes/collisiondetection/CollisionDetectionScene.h
mode change 100755 => 100644 testbed/scenes/cubes/CubesScene.cpp
diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index 64ce6467..e588f93c 100644
--- a/src/body/CollisionBody.cpp
+++ b/src/body/CollisionBody.cpp
@@ -74,6 +74,13 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, decimal(1));
+#ifdef IS_PROFILING_ACTIVE
+
+ // Set the profiler
+ proxyShape->setProfiler(mProfiler);
+
+#endif
+
// Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == nullptr) {
mProxyCollisionShapes = proxyShape;
diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h
index 2d5101b9..9943dc86 100644
--- a/src/body/CollisionBody.h
+++ b/src/body/CollisionBody.h
@@ -36,6 +36,7 @@
#include "collision/RaycastInfo.h"
#include "memory/PoolAllocator.h"
#include "configuration.h"
+#include "engine/Profiler.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
@@ -85,6 +86,13 @@ class CollisionBody : public Body {
/// Reference to the world the body belongs to
CollisionWorld& mWorld;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
// -------------------- Methods -------------------- //
/// Reset the contact manifold lists
@@ -177,6 +185,13 @@ class CollisionBody : public Body {
/// Return the body local-space coordinates of a vector given in the world-space coordinates
Vector3 getLocalVector(const Vector3& worldVector) const;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ virtual void setProfiler(Profiler* profiler);
+
+#endif
+
// -------------------- Friendship -------------------- //
friend class CollisionWorld;
@@ -313,6 +328,15 @@ inline bool CollisionBody::testAABBOverlap(const AABB& worldAABB) const {
return worldAABB.testCollision(getAABB());
}
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void CollisionBody::setProfiler(Profiler* profiler) {
+ mProfiler = profiler;
+}
+
+#endif
+
}
#endif
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index 6e6a55e6..d0f0c782 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -228,6 +228,13 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, mass);
+#ifdef IS_PROFILING_ACTIVE
+
+ // Set the profiler
+ proxyShape->setProfiler(mProfiler);
+
+#endif
+
// Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == nullptr) {
mProxyCollisionShapes = proxyShape;
@@ -410,7 +417,7 @@ void RigidBody::recomputeMassInformation() {
// Update the broad-phase state for this body (because it has moved for instance)
void RigidBody::updateBroadPhaseState() const {
- PROFILE("RigidBody::updateBroadPhaseState()");
+ PROFILE("RigidBody::updateBroadPhaseState()", mProfiler);
DynamicsWorld& world = static_cast<DynamicsWorld&>(mWorld);
const Vector3 displacement = world.mTimeStep * mLinearVelocity;
@@ -427,3 +434,22 @@ void RigidBody::updateBroadPhaseState() const {
}
}
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+void RigidBody::setProfiler(Profiler* profiler) {
+
+ CollisionBody::setProfiler(profiler);
+
+ // Set the profiler for each proxy shape
+ ProxyShape* proxyShape = getProxyShapesList();
+ while (proxyShape != nullptr) {
+
+ proxyShape->setProfiler(profiler);
+
+ proxyShape = proxyShape->getNext();
+ }
+}
+
+#endif
+
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index a38b53d5..6ba7a596 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -227,6 +227,13 @@ class RigidBody : public CollisionBody {
/// the collision shapes attached to the body.
void recomputeMassInformation();
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler) override;
+
+#endif
+
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index baaf0db5..7cb865f9 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -57,12 +57,19 @@ CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& mem
// Fill-in the collision detection matrix with algorithms
fillInCollisionMatrix();
+
+#ifdef IS_PROFILING_ACTIVE
+
+ mProfiler = nullptr;
+
+#endif
+
}
// Compute the collision detection
void CollisionDetection::computeCollisionDetection() {
- PROFILE("CollisionDetection::computeCollisionDetection()");
+ PROFILE("CollisionDetection::computeCollisionDetection()", mProfiler);
// Compute the broad-phase collision detection
computeBroadPhase();
@@ -80,7 +87,7 @@ void CollisionDetection::computeCollisionDetection() {
// Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() {
- PROFILE("CollisionDetection::computeBroadPhase()");
+ PROFILE("CollisionDetection::computeBroadPhase()", mProfiler);
// If new collision shapes have been added to bodies
if (mIsCollisionShapesAdded) {
@@ -95,7 +102,7 @@ void CollisionDetection::computeBroadPhase() {
// Compute the middle-phase collision detection
void CollisionDetection::computeMiddlePhase() {
- PROFILE("CollisionDetection::computeMiddlePhase()");
+ PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler);
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
@@ -219,6 +226,13 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair
MiddlePhaseTriangleCallback middlePhaseCallback(pair, concaveProxyShape, convexProxyShape,
concaveShape, allocator);
+#ifdef IS_PROFILING_ACTIVE
+
+ // Set the profiler
+ middlePhaseCallback.setProfiler(mProfiler);
+
+#endif
+
// Compute the convex shape AABB in the local-space of the convex shape
const Transform convexToConcaveTransform = concaveProxyShape->getLocalToWorldTransform().getInverse() *
convexProxyShape->getLocalToWorldTransform();
@@ -236,7 +250,7 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair
// Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhase() {
- PROFILE("CollisionDetection::computeNarrowPhase()");
+ PROFILE("CollisionDetection::computeNarrowPhase()", mProfiler);
NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList;
while (currentNarrowPhaseInfo != nullptr) {
@@ -444,90 +458,6 @@ void CollisionDetection::reportAllContacts() {
}
}
-// Process the potential contacts where one collion is a concave shape.
-// This method processes the concave triangle mesh collision using the smooth mesh collision algorithm described
-// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
-// issue with some internal edges.
-void CollisionDetection::processSmoothMeshContacts(OverlappingPair* pair) {
-
-// // Set with the triangle vertices already processed to void further contacts with same triangle
-// std::unordered_multimap<int, Vector3> processTriangleVertices;
-
-// std::vector<SmoothMeshContactInfo*> smoothContactPoints;
-
-// // If the collision shape 1 is the triangle
-// bool isFirstShapeTriangle = pair->getShape1()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE;
-// assert(isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE);
-// assert(!isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() != CollisionShapeType::TRIANGLE);
-
-// const TriangleShape* triangleShape = nullptr;
-// if (isFirstShapeTriangle) {
-// triangleShape = static_cast<const TriangleShape*>(pair->getShape1()->getCollisionShape());
-// }
-// else {
-// triangleShape = static_cast<const TriangleShape*>(pair->getShape2()->getCollisionShape());
-// }
-// assert(triangleShape != nullptr);
-
-// // Get the temporary memory allocator
-// Allocator& allocator = pair->getTemporaryAllocator();
-
-// // For each potential contact manifold of the pair
-// ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds();
-// while (potentialManifold != nullptr) {
-
-// // For each contact point of the potential manifold
-// ContactPointInfo* contactPointInfo = potentialManifold->getFirstContactPointInfo();
-// while (contactPointInfo != nullptr) {
-
-// // Compute the barycentric coordinates of the point in the triangle
-// decimal u, v, w;
-// computeBarycentricCoordinatesInTriangle(triangleShape->getVertexPosition(0),
-// triangleShape->getVertexPosition(1),
-// triangleShape->getVertexPosition(2),
-// isFirstShapeTriangle ? contactPointInfo->localPoint1 : contactPointInfo->localPoint2,
-// u, v, w);
-// int nbZeros = 0;
-// bool isUZero = approxEqual(u, 0, 0.0001);
-// bool isVZero = approxEqual(v, 0, 0.0001);
-// bool isWZero = approxEqual(w, 0, 0.0001);
-// if (isUZero) nbZeros++;
-// if (isVZero) nbZeros++;
-// if (isWZero) nbZeros++;
-
-// // If the triangle contact point is on a triangle vertex of a triangle edge
-// if (nbZeros == 1 || nbZeros == 2) {
-
-
-// // Create a smooth mesh contact info
-// SmoothMeshContactInfo* smoothContactInfo = new (allocator.allocate(sizeof(SmoothMeshContactInfo)))
-// SmoothMeshContactInfo(potentialManifold, contactInfo, isFirstShapeTriangle,
-// triangleShape->getVertexPosition(0),
-// triangleShape->getVertexPosition(1),
-// triangleShape->getVertexPosition(2));
-
-// smoothContactPoints.push_back(smoothContactInfo);
-
-// // Remove the contact point info from the manifold. If the contact point will be kept in the future, we
-// // will put the contact point back in the manifold.
-// ...
-// }
-
-// // Note that we do not remove the contact points that are not on the vertices or edges of the triangle
-// // from the contact manifold because we know we will keep to contact points. We only remove the vertices
-// // and edges contact points for the moment. If those points will be kept in the future, we will have to
-// // put them back again in the contact manifold
-// }
-
-// potentialManifold = potentialManifold->mNext;
-// }
-
-// // Sort the list of narrow-phase contacts according to their penetration depth
-// std::sort(smoothContactPoints.begin(), smoothContactPoints.end(), ContactsDepthCompare());
-
-// ...
-}
-
// Compute the middle-phase collision detection between two proxy shapes
NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) {
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 6f28f788..fc33c00f 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -100,6 +100,13 @@ class CollisionDetection {
/// True if some collision shapes have been added previously
bool mIsCollisionShapesAdded;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
// -------------------- Methods -------------------- //
/// Compute the broad-phase collision detection
@@ -143,6 +150,7 @@ class CollisionDetection {
/// Process the potential contacts where one collion is a concave shape
void processSmoothMeshContacts(OverlappingPair* pair);
+
public :
@@ -219,6 +227,13 @@ class CollisionDetection {
/// Return a reference to the world memory allocator
PoolAllocator& getWorldMemoryAllocator();
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler);
+
+#endif
+
/// Return the world-space AABB of a given proxy shape
const AABB getWorldAABB(const ProxyShape* proxyShape) const;
@@ -234,6 +249,14 @@ inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisio
// Fill-in the collision matrix with the new algorithms to use
fillInCollisionMatrix();
+
+#ifdef IS_PROFILING_ACTIVE
+
+ // Set the profiler
+ mCollisionDispatch->setProfiler(mProfiler);
+
+#endif
+
}
// Add a body to the collision detection
@@ -298,7 +321,7 @@ inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const {
- PROFILE("CollisionDetection::raycast()");
+ PROFILE("CollisionDetection::raycast()", mProfiler);
RaycastTest rayCastTest(raycastCallback);
@@ -312,6 +335,17 @@ inline CollisionWorld* CollisionDetection::getWorld() {
return mWorld;
}
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void CollisionDetection::setProfiler(Profiler* profiler) {
+ mProfiler = profiler;
+ mBroadPhaseAlgorithm.setProfiler(profiler);
+ mCollisionDispatch->setProfiler(profiler);
+}
+
+#endif
+
}
#endif
diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp
index 9d163799..0bda49d4 100644
--- a/src/collision/MiddlePhaseTriangleCallback.cpp
+++ b/src/collision/MiddlePhaseTriangleCallback.cpp
@@ -38,6 +38,13 @@ void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIn
TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
verticesNormals, meshSubPart, triangleIndex);
+#ifdef IS_PROFILING_ACTIVE
+
+ // Set the profiler to the triangle shape
+ triangleShape->setProfiler(mProfiler);
+
+#endif
+
bool isShape1Convex = mOverlappingPair->getShape1()->getCollisionShape()->isConvex();
ProxyShape* shape1 = isShape1Convex ? mConvexProxyShape : mConcaveProxyShape;
ProxyShape* shape2 = isShape1Convex ? mConcaveProxyShape : mConvexProxyShape;
diff --git a/src/collision/MiddlePhaseTriangleCallback.h b/src/collision/MiddlePhaseTriangleCallback.h
index 36bfac24..4927c6e0 100644
--- a/src/collision/MiddlePhaseTriangleCallback.h
+++ b/src/collision/MiddlePhaseTriangleCallback.h
@@ -58,6 +58,13 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
/// Reference to the single-frame memory allocator
Allocator& mAllocator;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
public:
/// Pointer to the first element of the linked-list of narrow-phase info
@@ -77,6 +84,16 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
/// Test collision between a triangle and the convex mesh shape
virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints,
const Vector3* verticesNormals) override;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler) {
+ mProfiler = profiler;
+ }
+
+#endif
+
};
}
diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h
index 8282af01..574758d4 100644
--- a/src/collision/ProxyShape.h
+++ b/src/collision/ProxyShape.h
@@ -85,6 +85,13 @@ class ProxyShape {
/// proxy shape will collide with every collision categories by default.
unsigned short mCollideWithMaskBits;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
// -------------------- Methods -------------------- //
/// Return the collision shape
@@ -170,6 +177,13 @@ class ProxyShape {
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler);
+
+#endif
+
// -------------------- Friendship -------------------- //
friend class OverlappingPair;
@@ -359,6 +373,18 @@ inline bool ProxyShape::testAABBOverlap(const AABB& worldAABB) const {
return worldAABB.testCollision(getWorldAABB());
}
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void ProxyShape::setProfiler(Profiler* profiler) {
+
+ mProfiler = profiler;
+
+ mCollisionShape->setProfiler(profiler);
+}
+
+#endif
+
}
#endif
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp
index c86cd0bc..c1484434 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp
@@ -44,6 +44,13 @@ BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
// Allocate memory for the array of potential overlapping pairs
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs != nullptr);
+
+#ifdef IS_PROFILING_ACTIVE
+
+ mProfiler = nullptr;
+
+#endif
+
}
// Destructor
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h
index 94499d44..7e1ff262 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.h
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.h
@@ -162,6 +162,13 @@ class BroadPhaseAlgorithm {
/// Reference to the collision detection object
CollisionDetection& mCollisionDetection;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
public :
// -------------------- Methods -------------------- //
@@ -215,8 +222,15 @@ class BroadPhaseAlgorithm {
const AABB& getFatAABB(int broadPhaseId) const;
/// Ray casting method
- void raycast(const Ray& ray, RaycastTest& raycastTest,
- unsigned short raycastWithCategoryMaskBits) const;
+ void raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler);
+
+#endif
+
};
// Method used to compare two pairs for sorting algorithm
@@ -252,7 +266,7 @@ inline const AABB& BroadPhaseAlgorithm::getFatAABB(int broadPhaseId) const {
inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const {
- PROFILE("BroadPhaseAlgorithm::raycast()");
+ PROFILE("BroadPhaseAlgorithm::raycast()", mProfiler);
BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
@@ -264,6 +278,16 @@ inline ProxyShape* BroadPhaseAlgorithm::getProxyShapeForBroadPhaseId(int broadPh
return static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(broadPhaseId));
}
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void BroadPhaseAlgorithm::setProfiler(Profiler* profiler) {
+ mProfiler = profiler;
+ mDynamicAABBTree.setProfiler(profiler);
+}
+
+#endif
+
}
#endif
diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp
index f829c000..5fa33d9d 100644
--- a/src/collision/broadphase/DynamicAABBTree.cpp
+++ b/src/collision/broadphase/DynamicAABBTree.cpp
@@ -171,7 +171,7 @@ void DynamicAABBTree::removeObject(int nodeID) {
/// (this can be useful if the shape AABB has become much smaller than the previous one for instance).
bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) {
- PROFILE("DynamicAABBTree::updateObject()");
+ PROFILE("DynamicAABBTree::updateObject()", mProfiler);
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
@@ -633,7 +633,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
// Ray casting method
void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const {
- PROFILE("DynamicAABBTree::raycast()");
+ PROFILE("DynamicAABBTree::raycast()", mProfiler);
decimal maxFraction = ray.maxFraction;
diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h
index 8025e4f9..54751138 100644
--- a/src/collision/broadphase/DynamicAABBTree.h
+++ b/src/collision/broadphase/DynamicAABBTree.h
@@ -155,6 +155,13 @@ class DynamicAABBTree {
/// without triggering a large modification of the tree which can be costly
decimal mExtraAABBGap;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
// -------------------- Methods -------------------- //
/// Allocate and return a node to use in the tree
@@ -237,6 +244,14 @@ class DynamicAABBTree {
/// Clear all the nodes and reset the tree
void reset();
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler);
+
+#endif
+
};
// Return true if the node is a leaf of the tree
@@ -292,6 +307,15 @@ inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) {
return nodeId;
}
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void DynamicAABBTree::setProfiler(Profiler* profiler) {
+ mProfiler = profiler;
+}
+
+#endif
+
}
#endif
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index e895eef7..315dcf7a 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -41,7 +41,15 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
- SATAlgorithm satAlgorithm;
+ SATAlgorithm satAlgorithm;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ gjkAlgorithm.setProfiler(mProfiler);
+ satAlgorithm.setProfiler(mProfiler);
+
+#endif
+
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
diff --git a/src/collision/narrowphase/CollisionDispatch.h b/src/collision/narrowphase/CollisionDispatch.h
index 230ebbb3..d8351d6a 100644
--- a/src/collision/narrowphase/CollisionDispatch.h
+++ b/src/collision/narrowphase/CollisionDispatch.h
@@ -41,6 +41,13 @@ class CollisionDispatch {
protected:
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
public:
/// Constructor
@@ -49,13 +56,29 @@ class CollisionDispatch {
/// Destructor
virtual ~CollisionDispatch() = default;
-
/// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes.
- virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type,
- int shape2Type)=0;
+ virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, int shape2Type)=0;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ virtual void setProfiler(Profiler* profiler);
+
+#endif
+
};
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void CollisionDispatch::setProfiler(Profiler* profiler) {
+
+ mProfiler = profiler;
+}
+
+#endif
+
}
#endif
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
index c6297fb2..9f2bb47e 100644
--- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
@@ -38,6 +38,13 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo*
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ satAlgorithm.setProfiler(mProfiler);
+
+#endif
+
bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h
index 779f7048..fe22b4a9 100644
--- a/src/collision/narrowphase/DefaultCollisionDispatch.h
+++ b/src/collision/narrowphase/DefaultCollisionDispatch.h
@@ -77,8 +77,33 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes.
virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) override;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ virtual void setProfiler(Profiler* profiler) override;
+
+#endif
+
};
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void DefaultCollisionDispatch::setProfiler(Profiler* profiler) {
+
+ CollisionDispatch::setProfiler(profiler);
+
+ mSphereVsSphereAlgorithm.setProfiler(profiler);
+ mCapsuleVsCapsuleAlgorithm.setProfiler(profiler);
+ mSphereVsCapsuleAlgorithm.setProfiler(profiler);
+ mSphereVsConvexPolyhedronAlgorithm.setProfiler(profiler);
+ mCapsuleVsConvexPolyhedronAlgorithm.setProfiler(profiler);
+ mConvexPolyhedronVsConvexPolyhedronAlgorithm.setProfiler(profiler);
+}
+
+#endif
+
}
#endif
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index 629811eb..c0f436a6 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -49,7 +49,7 @@ using namespace reactphysics3d;
/// the correct penetration depth and contact points between the enlarged objects.
GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
- PROFILE("GJKAlgorithm::testCollision()");
+ PROFILE("GJKAlgorithm::testCollision()", mProfiler);
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h
index b8c7319c..a9daa897 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.h
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h
@@ -62,6 +62,13 @@ class GJKAlgorithm {
// -------------------- Attributes -------------------- //
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
// -------------------- Methods -------------------- //
public :
@@ -94,8 +101,25 @@ class GJKAlgorithm {
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler);
+
+#endif
+
};
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void GJKAlgorithm::setProfiler(Profiler* profiler) {
+ mProfiler = profiler;
+}
+
+#endif
+
}
#endif
diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
index 6a8746c6..34fa5962 100644
--- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h
+++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
@@ -67,6 +67,13 @@ class NarrowPhaseAlgorithm {
// -------------------- Attributes -------------------- //
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
public :
// -------------------- Methods -------------------- //
@@ -85,8 +92,25 @@ class NarrowPhaseAlgorithm {
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler);
+
+#endif
+
};
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void NarrowPhaseAlgorithm::setProfiler(Profiler* profiler) {
+ mProfiler = profiler;
+}
+
+#endif
+
}
#endif
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index e6d09dd1..47953f1a 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -47,7 +47,7 @@ const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
// Test collision between a sphere and a convex mesh
bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
- PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()");
+ PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()", mProfiler);
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
@@ -139,7 +139,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd
// Test collision between a capsule and a convex mesh
bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
- PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()");
+ PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()", mProfiler);
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
@@ -448,7 +448,7 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
// Test collision between two convex polyhedrons
bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
- PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()");
+ PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()", mProfiler);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 5c721a00..60f7a091 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -50,6 +50,13 @@ class SATAlgorithm {
/// make sure the contact manifold does not change too much between frames.
static const decimal SAME_SEPARATING_AXIS_BIAS;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
// -------------------- Methods -------------------- //
/// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis)
@@ -138,8 +145,26 @@ class SATAlgorithm {
/// Test collision between two convex meshes
bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler);
+
+#endif
+
};
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void SATAlgorithm::setProfiler(Profiler* profiler) {
+
+ mProfiler = profiler;
+}
+
+#endif
+
}
#endif
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index 7ecda325..ae3764ab 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -43,6 +43,13 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ gjkAlgorithm.setProfiler(mProfiler);
+
+#endif
+
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
@@ -60,6 +67,13 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ satAlgorithm.setProfiler(mProfiler);
+
+#endif
+
bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp
index 9622e795..a89747c9 100644
--- a/src/collision/shapes/CollisionShape.cpp
+++ b/src/collision/shapes/CollisionShape.cpp
@@ -48,7 +48,7 @@ CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type)
*/
void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
- PROFILE("CollisionShape::computeAABB()");
+ PROFILE("CollisionShape::computeAABB()", mProfiler);
// Get the local bounds in x,y and z direction
Vector3 minBounds;
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index f6690c58..feb4c502 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -35,6 +35,7 @@
#include "AABB.h"
#include "collision/RaycastInfo.h"
#include "memory/PoolAllocator.h"
+#include "engine/Profiler.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -69,6 +70,13 @@ class CollisionShape {
/// Scaling vector of the collision shape
Vector3 mScaling;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
// -------------------- Methods -------------------- //
@@ -124,6 +132,13 @@ class CollisionShape {
/// Compute the world-space AABB of the collision shape given a transform
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ virtual void setProfiler(Profiler* profiler);
+
+#endif
+
// -------------------- Friendship -------------------- //
friend class ProxyShape;
@@ -156,6 +171,16 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
mScaling = scaling;
}
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void CollisionShape::setProfiler(Profiler* profiler) {
+
+ mProfiler = profiler;
+}
+
+#endif
+
}
#endif
diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp
index 566b7bc4..646638dd 100644
--- a/src/collision/shapes/ConcaveMeshShape.cpp
+++ b/src/collision/shapes/ConcaveMeshShape.cpp
@@ -113,11 +113,18 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
/// the ray hits many triangles.
bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
- PROFILE("ConcaveMeshShape::raycast()");
+ PROFILE("ConcaveMeshShape::raycast()", mProfiler);
// Create the callback object that will compute ray casting against triangles
ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray);
+#ifdef IS_PROFILING_ACTIVE
+
+ // Set the profiler
+ raycastCallback.setProfiler(mProfiler);
+
+#endif
+
// 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.
@@ -159,6 +166,13 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
verticesNormals, data[0], data[1]);
triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
+
+#ifdef IS_PROFILING_ACTIVE
+
+ // Set the profiler to the triangle shape
+ triangleShape.setProfiler(mProfiler);
+
+#endif
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h
index 82c08794..678d5325 100644
--- a/src/collision/shapes/ConcaveMeshShape.h
+++ b/src/collision/shapes/ConcaveMeshShape.h
@@ -78,6 +78,13 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
const Ray& mRay;
bool mIsHit;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
public:
// Constructor
@@ -98,6 +105,15 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
bool getIsHit() const {
return mIsHit;
}
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler) {
+ mProfiler = profiler;
+ }
+
+#endif
};
// Class ConcaveMeshShape
@@ -165,6 +181,13 @@ class ConcaveMeshShape : public ConcaveShape {
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const override;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ virtual void setProfiler(Profiler* profiler) override;
+
+#endif
+
// ---------- Friendship ----------- //
friend class ConvexTriangleAABBOverlapCallback;
@@ -239,6 +262,18 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId)
mTriangleTestCallback.testTriangle(data[0], data[1], trianglePoints, verticesNormals);
}
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void ConcaveMeshShape::setProfiler(Profiler* profiler) {
+
+ CollisionShape::setProfiler(profiler);
+
+ mDynamicAABBTree.setProfiler(profiler);
+}
+
+#endif
+
}
#endif
diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp
index 2afba401..08ec19a4 100644
--- a/src/collision/shapes/HeightFieldShape.cpp
+++ b/src/collision/shapes/HeightFieldShape.cpp
@@ -217,10 +217,17 @@ bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
// TODO : Implement raycasting without using an AABB for the ray
// but using a dynamic AABB tree or octree instead
- PROFILE("HeightFieldShape::raycast()");
+ PROFILE("HeightFieldShape::raycast()", mProfiler);
TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
+#ifdef IS_PROFILING_ACTIVE
+
+ // Set the profiler
+ triangleCallback.setProfiler(mProfiler);
+
+#endif
+
// Compute the AABB for the ray
const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const AABB rayAABB(Vector3::min(ray.point1, rayEnd), Vector3::max(ray.point1, rayEnd));
@@ -264,6 +271,13 @@ void TriangleOverlapCallback::testTriangle(uint meshSubPart, uint triangleIndex,
verticesNormals, meshSubPart, triangleIndex);
triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
+#ifdef IS_PROFILING_ACTIVE
+
+ // Set the profiler to the triangle shape
+ triangleShape.setProfiler(mProfiler);
+
+#endif
+
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h
index aa0cd787..80c18d9c 100644
--- a/src/collision/shapes/HeightFieldShape.h
+++ b/src/collision/shapes/HeightFieldShape.h
@@ -49,6 +49,13 @@ class TriangleOverlapCallback : public TriangleCallback {
bool mIsHit;
decimal mSmallestHitFraction;
const HeightFieldShape& mHeightFieldShape;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
public:
@@ -66,6 +73,16 @@ class TriangleOverlapCallback : public TriangleCallback {
/// Raycast test between a ray and a triangle of the heightfield
virtual void testTriangle(uint meshSubPart, uint triangleIndex,
const Vector3* trianglePoints, const Vector3* verticesNormals) override;
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler) {
+ mProfiler = profiler;
+ }
+
+#endif
+
};
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index 142ba181..020053b6 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -153,13 +153,12 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3&
return (u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]).getUnit();
}
-
// 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 {
- PROFILE("TriangleShape::raycast()");
+ PROFILE("TriangleShape::raycast()", mProfiler);
const Vector3 pq = ray.point2 - ray.point1;
const Vector3 pa = mPoints[0] - ray.point1;
diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp
index e8f4155a..eeb06305 100644
--- a/src/engine/CollisionWorld.cpp
+++ b/src/engine/CollisionWorld.cpp
@@ -37,6 +37,13 @@ CollisionWorld::CollisionWorld()
mCollisionDetection(this, mPoolAllocator, mSingleFrameAllocator), mCurrentBodyID(0),
mEventListener(nullptr) {
+#ifdef IS_PROFILING_ACTIVE
+
+ // Set the profiler
+ mCollisionDetection.setProfiler(&mProfiler);
+
+#endif
+
}
// Destructor
@@ -75,6 +82,12 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
// Add the collision body to the world
mBodies.insert(collisionBody);
+#ifdef IS_PROFILING_ACTIVE
+
+ collisionBody->setProfiler(&mProfiler);
+
+#endif
+
// Return the pointer to the rigid body
return collisionBody;
}
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index d27b8018..f523d169 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -82,6 +82,12 @@ class CollisionWorld {
/// Pointer to an event listener object
EventListener* mEventListener;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Real-time hierarchical profiler
+ Profiler mProfiler;
+#endif
+
// -------------------- Methods -------------------- //
/// Return the next available body ID
@@ -147,6 +153,12 @@ class CollisionWorld {
/// Test and report collisions between all shapes of the world
void testCollision(CollisionCallback* callback);
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the name of the profiler
+ void setProfilerName(std::string name);
+#endif
+
/// Return the current world-space AABB of given proxy shape
AABB getWorldAABB(const ProxyShape* proxyShape) const;
@@ -236,6 +248,15 @@ inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* ov
mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits);
}
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the name of the profiler
+inline void CollisionWorld::setProfilerName(std::string name) {
+ mProfiler.setName(name);
+}
+
+#endif
+
}
#endif
diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp
index 547aea6e..458c39a7 100644
--- a/src/engine/ConstraintSolver.cpp
+++ b/src/engine/ConstraintSolver.cpp
@@ -34,12 +34,18 @@ ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVe
: mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) {
+#ifdef IS_PROFILING_ACTIVE
+
+ mProfiler = nullptr;
+
+#endif
+
}
// Initialize the constraint solver for a given island
void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
- PROFILE("ConstraintSolver::initializeForIsland()");
+ PROFILE("ConstraintSolver::initializeForIsland()", mProfiler);
assert(island != nullptr);
assert(island->getNbBodies() > 0);
@@ -69,7 +75,7 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
// Solve the velocity constraints
void ConstraintSolver::solveVelocityConstraints(Island* island) {
- PROFILE("ConstraintSolver::solveVelocityConstraints()");
+ PROFILE("ConstraintSolver::solveVelocityConstraints()", mProfiler);
assert(island != nullptr);
assert(island->getNbJoints() > 0);
@@ -86,7 +92,7 @@ void ConstraintSolver::solveVelocityConstraints(Island* island) {
// Solve the position constraints
void ConstraintSolver::solvePositionConstraints(Island* island) {
- PROFILE("ConstraintSolver::solvePositionConstraints()");
+ PROFILE("ConstraintSolver::solvePositionConstraints()", mProfiler);
assert(island != nullptr);
assert(island->getNbJoints() > 0);
diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h
index b40cc983..fbb54ed1 100644
--- a/src/engine/ConstraintSolver.h
+++ b/src/engine/ConstraintSolver.h
@@ -165,6 +165,12 @@ class ConstraintSolver {
/// Constraint solver data used to initialize and solve the constraints
ConstraintSolverData mConstraintSolverData;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+#endif
+
public :
// -------------------- Methods -------------------- //
@@ -197,6 +203,14 @@ class ConstraintSolver {
/// Set the constrained positions/orientations arrays
void setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations);
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler);
+
+#endif
+
};
// Set the constrained velocities arrays
@@ -221,6 +235,15 @@ inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrained
mConstraintSolverData.orientations = constrainedOrientations;
}
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void ConstraintSolver::setProfiler(Profiler* profiler) {
+ mProfiler = profiler;
+}
+
+#endif
+
}
#endif
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 819a1b76..1e5b5401 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -52,7 +52,7 @@ ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocity
// Initialize the contact constraints
void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
- PROFILE("ContactSolver::init()");
+ PROFILE("ContactSolver::init()", mProfiler);
mTimeStep = timeStep;
@@ -98,7 +98,7 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
// Initialize the constraint solver for a given island
void ContactSolver::initializeForIsland(Island* island) {
- PROFILE("ContactSolver::initializeForIsland()");
+ PROFILE("ContactSolver::initializeForIsland()", mProfiler);
assert(island != nullptr);
assert(island->getNbBodies() > 0);
@@ -270,7 +270,7 @@ void ContactSolver::initializeForIsland(Island* island) {
/// the solution of the linear system
void ContactSolver::warmStart() {
- PROFILE("ContactSolver::warmStart()");
+ PROFILE("ContactSolver::warmStart()", mProfiler);
uint contactPointIndex = 0;
@@ -387,7 +387,7 @@ void ContactSolver::warmStart() {
// Solve the contacts
void ContactSolver::solve() {
- PROFILE("ContactSolver::solve()");
+ PROFILE("ContactSolver::solve()", mProfiler);
decimal deltaLambda;
decimal lambdaTemp;
@@ -586,7 +586,7 @@ void ContactSolver::solve() {
// warm start the solver at the next iteration
void ContactSolver::storeImpulses() {
- PROFILE("ContactSolver::storeImpulses()");
+ PROFILE("ContactSolver::storeImpulses()", mProfiler);
uint contactPointIndex = 0;
@@ -614,7 +614,7 @@ void ContactSolver::storeImpulses() {
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
ContactManifoldSolver& contact) const {
- PROFILE("ContactSolver::computeFrictionVectors()");
+ PROFILE("ContactSolver::computeFrictionVectors()", mProfiler);
assert(contact.normal.length() > decimal(0.0));
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index 40e0721d..0a2850f5 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -308,6 +308,13 @@ class ContactSolver {
/// True if the split impulse position correction is active
bool mIsSplitImpulseActive;
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Pointer to the profiler
+ Profiler* mProfiler;
+
+#endif
+
// -------------------- Methods -------------------- //
/// Compute the collision restitution factor from the restitution factor of each body
@@ -367,6 +374,13 @@ class ContactSolver {
/// Activate or Deactivate the split impulses for contacts
void setIsSplitImpulseActive(bool isActive);
+
+#ifdef IS_PROFILING_ACTIVE
+
+ /// Set the profiler
+ void setProfiler(Profiler* profiler);
+
+#endif
};
// Set the split velocities arrays
@@ -425,6 +439,16 @@ inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1,
return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance());
}
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void ContactSolver::setProfiler(Profiler* profiler) {
+
+ mProfiler = profiler;
+}
+
+#endif
+
}
#endif
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 26835166..bb824228 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -29,6 +29,7 @@
#include "constraint/SliderJoint.h"
#include "constraint/HingeJoint.h"
#include "constraint/FixedJoint.h"
+#include <fstream>
// Namespaces
using namespace reactphysics3d;
@@ -53,6 +54,14 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY),
mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) {
+#ifdef IS_PROFILING_ACTIVE
+
+ // Set the profiler
+ mConstraintSolver.setProfiler(&mProfiler);
+ mContactSolver.setProfiler(&mProfiler);
+
+#endif
+
}
// Destructor
@@ -80,10 +89,10 @@ DynamicsWorld::~DynamicsWorld() {
#ifdef IS_PROFILING_ACTIVE
// Print the profiling report
- Profiler::printReport(std::cout);
-
- // Destroy the profiler (release the allocated memory)
- Profiler::destroy();
+ ofstream myfile;
+ myfile.open(mProfiler.getName() + ".txt");
+ mProfiler.printReport(myfile);
+ myfile.close();
#endif
}
@@ -96,10 +105,10 @@ void DynamicsWorld::update(decimal timeStep) {
#ifdef IS_PROFILING_ACTIVE
// Increment the frame counter of the profiler
- Profiler::incrementFrameCounter();
+ mProfiler.incrementFrameCounter();
#endif
- PROFILE("DynamicsWorld::update()");
+ PROFILE("DynamicsWorld::update()", &mProfiler);
mTimeStep = timeStep;
@@ -147,7 +156,7 @@ void DynamicsWorld::update(decimal timeStep) {
/// the sympletic Euler time stepping scheme.
void DynamicsWorld::integrateRigidBodiesPositions() {
- PROFILE("DynamicsWorld::integrateRigidBodiesPositions()");
+ PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", &mProfiler);
// For each island of the world
for (uint i=0; i < mNbIslands; i++) {
@@ -186,7 +195,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
// Update the postion/orientation of the bodies
void DynamicsWorld::updateBodiesState() {
- PROFILE("DynamicsWorld::updateBodiesState()");
+ PROFILE("DynamicsWorld::updateBodiesState()", &mProfiler);
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
@@ -223,7 +232,7 @@ void DynamicsWorld::updateBodiesState() {
// Initialize the bodies velocities arrays for the next simulation step.
void DynamicsWorld::initVelocityArrays() {
- PROFILE("DynamicsWorld::initVelocityArrays()");
+ PROFILE("DynamicsWorld::initVelocityArrays()", &mProfiler);
// Allocate memory for the bodies velocity arrays
uint nbBodies = mRigidBodies.size();
@@ -266,7 +275,7 @@ void DynamicsWorld::initVelocityArrays() {
/// contact solver.
void DynamicsWorld::integrateRigidBodiesVelocities() {
- PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()");
+ PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()", &mProfiler);
// Initialize the bodies velocity arrays
initVelocityArrays();
@@ -328,7 +337,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
// Solve the contacts and constraints
void DynamicsWorld::solveContactsAndConstraints() {
- PROFILE("DynamicsWorld::solveContactsAndConstraints()");
+ PROFILE("DynamicsWorld::solveContactsAndConstraints()", &mProfiler);
// Set the velocities arrays
mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
@@ -401,7 +410,7 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Solve the position error correction of the constraints
void DynamicsWorld::solvePositionCorrection() {
- PROFILE("DynamicsWorld::solvePositionCorrection()");
+ PROFILE("DynamicsWorld::solvePositionCorrection()", &mProfiler);
// Do not continue if there is no constraints
if (mJoints.empty()) return;
@@ -442,6 +451,12 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
mBodies.insert(rigidBody);
mRigidBodies.insert(rigidBody);
+#ifdef IS_PROFILING_ACTIVE
+
+ rigidBody->setProfiler(&mProfiler);
+
+#endif
+
// Return the pointer to the rigid body
return rigidBody;
}
@@ -613,7 +628,7 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
/// it). Then, we create an island with this group of connected bodies.
void DynamicsWorld::computeIslands() {
- PROFILE("DynamicsWorld::computeIslands()");
+ PROFILE("DynamicsWorld::computeIslands()", &mProfiler);
uint nbBodies = mRigidBodies.size();
@@ -757,7 +772,7 @@ void DynamicsWorld::computeIslands() {
/// time, we put all the bodies of the island to sleep.
void DynamicsWorld::updateSleepingBodies() {
- PROFILE("DynamicsWorld::updateSleepingBodies()");
+ PROFILE("DynamicsWorld::updateSleepingBodies()", &mProfiler);
const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity;
const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h
index b632dcf2..366b2770 100644
--- a/src/engine/DynamicsWorld.h
+++ b/src/engine/DynamicsWorld.h
@@ -124,19 +124,9 @@ class DynamicsWorld : public CollisionWorld {
/// Integrate the positions and orientations of rigid bodies.
void integrateRigidBodiesPositions();
- /// Update the AABBs of the bodies
- void updateRigidBodiesAABB();
-
/// Reset the external force and torque applied to the bodies
void resetBodiesForceAndTorque();
- /// Update the position and orientation of a body
- void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
- Vector3 newAngVelocity);
-
- /// Compute and set the interpolation factor to all bodies
- void setInterpolationFactorToAllBodies();
-
/// Initialize the bodies velocities arrays for the next simulation step.
void initVelocityArrays();
@@ -149,9 +139,6 @@ class DynamicsWorld : public CollisionWorld {
/// Solve the position error correction of the constraints
void solvePositionCorrection();
- /// Cleanup the constrained velocities array at each step
- void cleanupConstrainedVelocitiesArray();
-
/// Compute the islands of awake bodies.
void computeIslands();
@@ -478,7 +465,6 @@ inline void DynamicsWorld::setEventListener(EventListener* eventListener) {
mEventListener = eventListener;
}
-
}
#endif
diff --git a/src/engine/Profiler.cpp b/src/engine/Profiler.cpp
index 9437dca0..3212e4a1 100644
--- a/src/engine/Profiler.cpp
+++ b/src/engine/Profiler.cpp
@@ -27,14 +27,12 @@
// Libraries
#include "Profiler.h"
+#include <string>
using namespace reactphysics3d;
// Initialization of static variables
-ProfileNode Profiler::mRootNode("Root", nullptr);
-ProfileNode* Profiler::mCurrentNode = &Profiler::mRootNode;
-long double Profiler::mProfilingStartTime = Timer::getCurrentSystemTime() * 1000.0;
-uint Profiler::mFrameCounter = 0;
+int Profiler::mNbProfilers = 0;
// Constructor
ProfileNode::ProfileNode(const char* name, ProfileNode* parentNode)
@@ -157,6 +155,35 @@ void ProfileNodeIterator::enterParent() {
mCurrentChildNode = mCurrentParentNode->getChildNode();
}
+// Constructor
+Profiler::Profiler(std::string name) :mRootNode("Root", nullptr) {
+
+ // Set the name of the profiler
+ if (name == "") {
+
+ if (mNbProfilers == 0) {
+ mName = "profiler";
+ }
+ else {
+ mName = std::string("profiler") + std::to_string(mNbProfilers);
+ }
+ }
+ else {
+ mName = name;
+ }
+
+ mCurrentNode = &mRootNode;
+ mProfilingStartTime = Timer::getCurrentSystemTime() * 1000.0;
+ mFrameCounter = 0;
+
+ mNbProfilers++;
+}
+
+// Destructor
+Profiler::~Profiler() {
+ destroy();
+}
+
// Method called when we want to start profiling a block of code.
void Profiler::startProfilingBlock(const char* name) {
diff --git a/src/engine/Profiler.h b/src/engine/Profiler.h
index 818d6cc8..45b9b2e9 100644
--- a/src/engine/Profiler.h
+++ b/src/engine/Profiler.h
@@ -184,57 +184,73 @@ class Profiler {
// -------------------- Attributes -------------------- //
+ /// Profiler name
+ std::string mName;
+
+ /// Total number of profilers
+ static int mNbProfilers;
+
/// Root node of the profiler tree
- static ProfileNode mRootNode;
+ ProfileNode mRootNode;
/// Current node in the current execution
- static ProfileNode* mCurrentNode;
+ ProfileNode* mCurrentNode;
/// Frame counter
- static uint mFrameCounter;
+ uint mFrameCounter;
/// Starting profiling time
- static long double mProfilingStartTime;
+ long double mProfilingStartTime;
/// Recursively print the report of a given node of the profiler tree
- static void printRecursiveNodeReport(ProfileNodeIterator* iterator,
- int spacing,
- std::ostream& outputStream);
+ void printRecursiveNodeReport(ProfileNodeIterator* iterator, int spacing, std::ostream& outputStream);
+
+ /// Destroy a previously allocated iterator
+ void destroyIterator(ProfileNodeIterator* iterator);
+
+ /// Destroy the profiler (release the memory)
+ void destroy();
public :
// -------------------- Methods -------------------- //
+ /// Constructor
+ Profiler(std::string name = "");
+
+ /// Destructor
+ ~Profiler();
+
/// Method called when we want to start profiling a block of code.
- static void startProfilingBlock(const char *name);
+ void startProfilingBlock(const char *name);
/// Method called at the end of the scope where the
/// startProfilingBlock() method has been called.
- static void stopProfilingBlock();
+ void stopProfilingBlock();
/// Reset the timing data of the profiler (but not the profiler tree structure)
- static void reset();
+ void reset();
/// Return the number of frames
- static uint getNbFrames();
+ uint getNbFrames();
+
+ /// Get the name of the profiler
+ std::string getName() const;
+
+ /// Set the name of the profiler
+ void setName(std::string name);
/// Return the elasped time since the start/reset of the profiling
- static long double getElapsedTimeSinceStart();
+ long double getElapsedTimeSinceStart();
/// Increment the frame counter
- static void incrementFrameCounter();
+ void incrementFrameCounter();
/// Return an iterator over the profiler tree starting at the root
- static ProfileNodeIterator* getIterator();
+ ProfileNodeIterator* getIterator();
/// Print the report of the profiler in a given output stream
- static void printReport(std::ostream& outputStream);
-
- /// Destroy a previously allocated iterator
- static void destroyIterator(ProfileNodeIterator* iterator);
-
- /// Destroy the profiler (release the memory)
- static void destroy();
+ void printReport(std::ostream& outputStream);
};
// Class ProfileSample
@@ -245,27 +261,33 @@ class Profiler {
*/
class ProfileSample {
+ private:
+
+ Profiler* mProfiler;
+
public :
// -------------------- Methods -------------------- //
/// Constructor
- ProfileSample(const char* name) {
+ ProfileSample(const char* name, Profiler* profiler) :mProfiler(profiler) {
+
+ assert(profiler != nullptr);
// Ask the profiler to start profiling a block of code
- Profiler::startProfilingBlock(name);
+ mProfiler->startProfilingBlock(name);
}
/// Destructor
~ProfileSample() {
// Tell the profiler to stop profiling a block of code
- Profiler::stopProfilingBlock();
+ mProfiler->stopProfilingBlock();
}
};
// Use this macro to start profile a block of code
-#define PROFILE(name) ProfileSample profileSample(name)
+#define PROFILE(name, profiler) ProfileSample profileSample(name, profiler)
// Return true if we are at the root of the profiler tree
inline bool ProfileNodeIterator::isRoot() {
@@ -352,6 +374,16 @@ inline uint Profiler::getNbFrames() {
return mFrameCounter;
}
+// Get the name of the profiler
+inline std::string Profiler::getName() const {
+ return mName;
+}
+
+// Set the name of the profiler
+inline void Profiler::setName(std::string name) {
+ mName = name;
+}
+
// Return the elasped time since the start/reset of the profiling
inline long double Profiler::getElapsedTimeSinceStart() {
long double currentTime = Timer::getCurrentSystemTime() * 1000.0;
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index f00dbdba..1241b701 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -49,6 +49,12 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name, Engine
// Create the dynamics world for the physics simulation
mPhysicsWorld = new rp3d::CollisionWorld();
+#ifdef IS_PROFILING_ACTIVE
+
+ mPhysicsWorld->setProfilerName(name + "_profiler");
+
+#endif
+
// ---------- Sphere 1 ---------- //
// Create a sphere and a corresponding collision body in the dynamics world
@@ -72,6 +78,7 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name, Engine
mSphere2->setSleepingColor(mRedColorDemo);
mPhysicsObjects.push_back(mSphere2);
+
// ---------- Capsule 1 ---------- //
// Create a cylinder and a corresponding collision body in the dynamics world
@@ -94,49 +101,49 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name, Engine
mCapsule2->setSleepingColor(mRedColorDemo);
mPhysicsObjects.push_back(mCapsule2);
+ // ---------- Concave Mesh ---------- //
+
+ // Create a convex mesh and a corresponding collision body in the dynamics world
+ mConcaveMesh = new ConcaveMesh(mPhysicsWorld, mMeshFolderPath + "city.obj");
+ mAllShapes.push_back(mConcaveMesh);
+
+ // Set the color
+ mConcaveMesh->setColor(mGreyColorDemo);
+ mConcaveMesh->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mConcaveMesh);
+
// ---------- Box 1 ---------- //
- // Create a cylinder and a corresponding collision body in the dynamics world
+ // Create a cylinder and a corresponding collision body in the dynamics world
mBox1 = new Box(BOX_SIZE, mPhysicsWorld, mMeshFolderPath);
- mAllShapes.push_back(mBox1);
+ mAllShapes.push_back(mBox1);
- // Set the color
- mBox1->setColor(mGreyColorDemo);
- mBox1->setSleepingColor(mRedColorDemo);
- mPhysicsObjects.push_back(mBox1);
+ // Set the color
+ mBox1->setColor(mGreyColorDemo);
+ mBox1->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mBox1);
- // ---------- Box 2 ---------- //
+ // ---------- Box 2 ---------- //
- // Create a cylinder and a corresponding collision body in the dynamics world
+ // Create a cylinder and a corresponding collision body in the dynamics world
mBox2 = new Box(openglframework::Vector3(3, 2, 5), mPhysicsWorld, mMeshFolderPath);
- mAllShapes.push_back(mBox2);
+ mAllShapes.push_back(mBox2);
- // Set the color
- mBox2->setColor(mGreyColorDemo);
- mBox2->setSleepingColor(mRedColorDemo);
- mPhysicsObjects.push_back(mBox2);
+ // Set the color
+ mBox2->setColor(mGreyColorDemo);
+ mBox2->setSleepingColor(mRedColorDemo);
+ mPhysicsObjects.push_back(mBox2);
// ---------- Convex Mesh ---------- //
// Create a convex mesh and a corresponding collision body in the dynamics world
mConvexMesh = new ConvexMesh(mPhysicsWorld, mMeshFolderPath + "convexmesh.obj");
- mAllShapes.push_back(mConvexMesh);
+ mAllShapes.push_back(mConvexMesh);
// Set the color
mConvexMesh->setColor(mGreyColorDemo);
mConvexMesh->setSleepingColor(mRedColorDemo);
- mPhysicsObjects.push_back(mConvexMesh);
-
- // ---------- Concave Mesh ---------- //
-
- // Create a convex mesh and a corresponding collision body in the dynamics world
- mConcaveMesh = new ConcaveMesh(mPhysicsWorld, mMeshFolderPath + "city.obj");
- mAllShapes.push_back(mConcaveMesh);
-
- // Set the color
- mConcaveMesh->setColor(mGreyColorDemo);
- mConcaveMesh->setSleepingColor(mRedColorDemo);
- mPhysicsObjects.push_back(mConcaveMesh);
+ mPhysicsObjects.push_back(mConvexMesh);
// ---------- Heightfield ---------- //
@@ -154,14 +161,14 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name, Engine
// Reset the scene
void CollisionDetectionScene::reset() {
- mSphere1->setTransform(rp3d::Transform(rp3d::Vector3(12, 0, 0), rp3d::Quaternion::identity()));
- mSphere2->setTransform(rp3d::Transform(rp3d::Vector3(12, 8, 0), rp3d::Quaternion::identity()));
- mCapsule1->setTransform(rp3d::Transform(rp3d::Vector3(-6, 7, 0), rp3d::Quaternion::identity()));
+ mSphere1->setTransform(rp3d::Transform(rp3d::Vector3(15, 5, 0), rp3d::Quaternion::identity()));
+ mSphere2->setTransform(rp3d::Transform(rp3d::Vector3(0, 6, 0), rp3d::Quaternion::identity()));
+ mCapsule1->setTransform(rp3d::Transform(rp3d::Vector3(-8, 7, 0), rp3d::Quaternion::identity()));
mCapsule2->setTransform(rp3d::Transform(rp3d::Vector3(11, -8, 0), rp3d::Quaternion::identity()));
mBox1->setTransform(rp3d::Transform(rp3d::Vector3(-4, -7, 0), rp3d::Quaternion::identity()));
- mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 8, 0), rp3d::Quaternion::identity()));
+ mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 9, 0), rp3d::Quaternion::identity()));
mConvexMesh->setTransform(rp3d::Transform(rp3d::Vector3(-5, 0, 0), rp3d::Quaternion::identity()));
- mConcaveMesh->setTransform(rp3d::Transform(rp3d::Vector3(0, 100, 0), rp3d::Quaternion::identity()));
+ mConcaveMesh->setTransform(rp3d::Transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()));
mHeightField->setTransform(rp3d::Transform(rp3d::Vector3(0, -12, 0), rp3d::Quaternion::identity()));
}
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
old mode 100755
new mode 100644
index 4bea0c0b..e5bd8db9
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -152,7 +152,7 @@ class CollisionDetectionScene : public SceneDemo {
std::vector<PhysicsObject*> mAllShapes;
- uint mSelectedShapeIndex;
+ unsigned int mSelectedShapeIndex;
/// Select the next shape
void selectNextShape();
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
index a6fd6662..8efc13de 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
@@ -46,7 +46,14 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name, EngineSettin
rp3d::Vector3 gravity(0, -9.81f, 0);
// Create the dynamics world for the physics simulation
- mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
+ mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
+
+#ifdef IS_PROFILING_ACTIVE
+
+ mPhysicsWorld->setProfilerName(name + "_profiler");
+
+#endif
+
for (int i=0; i<NB_COMPOUND_SHAPES; i++) {
diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
index d70a02af..77656f92 100644
--- a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
+++ b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
@@ -48,6 +48,13 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name, EngineSettings& sett
// Create the dynamics world for the physics simulation
mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
+#ifdef IS_PROFILING_ACTIVE
+
+ mPhysicsWorld->setProfilerName(name + "_profiler");
+
+#endif
+
+ // ---------- Create the boxes ----------- //
for (int i = 0; i<NB_COMPOUND_SHAPES; i++) {
// Create a convex mesh and a corresponding rigid in the dynamics world
diff --git a/testbed/scenes/cubes/CubesScene.cpp b/testbed/scenes/cubes/CubesScene.cpp
old mode 100755
new mode 100644
index fa7f4ba5..9eebb351
--- a/testbed/scenes/cubes/CubesScene.cpp
+++ b/testbed/scenes/cubes/CubesScene.cpp
@@ -46,6 +46,12 @@ CubesScene::CubesScene(const std::string& name, EngineSettings& settings)
// Create the dynamics world for the physics simulation
mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
+#ifdef IS_PROFILING_ACTIVE
+
+ mPhysicsWorld->setProfilerName(name + "_profiler");
+
+#endif
+
// Create all the cubes of the scene
for (int i=0; i<NB_CUBES; i++) {
diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp
index 238018dd..0d13d40d 100644
--- a/testbed/scenes/heightfield/HeightFieldScene.cpp
+++ b/testbed/scenes/heightfield/HeightFieldScene.cpp
@@ -46,7 +46,14 @@ HeightFieldScene::HeightFieldScene(const std::string& name, EngineSettings& sett
rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0);
// Create the dynamics world for the physics simulation
- mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
+ mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
+
+#ifdef IS_PROFILING_ACTIVE
+
+ mPhysicsWorld->setProfilerName(name + "_profiler");
+
+#endif
+
for (int i = 0; i<NB_COMPOUND_SHAPES; i++) {
diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp
index 3252e90e..271cf1f9 100644
--- a/testbed/scenes/joints/JointsScene.cpp
+++ b/testbed/scenes/joints/JointsScene.cpp
@@ -47,6 +47,12 @@ JointsScene::JointsScene(const std::string& name, EngineSettings& settings)
// Create the dynamics world for the physics simulation
mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
+#ifdef IS_PROFILING_ACTIVE
+
+ mPhysicsWorld->setProfilerName(name + "_profiler");
+
+#endif
+
// Create the Ball-and-Socket joint
createBallAndSocketJoints();
diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp
index bf806183..5705627d 100644
--- a/testbed/scenes/raycast/RaycastScene.cpp
+++ b/testbed/scenes/raycast/RaycastScene.cpp
@@ -47,6 +47,12 @@ RaycastScene::RaycastScene(const std::string& name, EngineSettings& settings)
// Create the dynamics world for the physics simulation
mPhysicsWorld = new rp3d::CollisionWorld();
+#ifdef IS_PROFILING_ACTIVE
+
+ mPhysicsWorld->setProfilerName(name + "_profiler");
+
+#endif
+
// ---------- Dumbbell ---------- //
// Create a convex mesh and a corresponding collision body in the dynamics world
diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h
index e33e9da9..89a9afdd 100644
--- a/testbed/src/Scene.h
+++ b/testbed/src/Scene.h
@@ -53,8 +53,8 @@ struct EngineSettings {
long double elapsedTime; // Elapsed time (in seconds)
float timeStep; // Current time step (in seconds)
- uint nbVelocitySolverIterations; // Nb of velocity solver iterations
- uint nbPositionSolverIterations; // Nb of position solver iterations
+ unsigned int nbVelocitySolverIterations; // Nb of velocity solver iterations
+ unsigned int nbPositionSolverIterations; // Nb of position solver iterations
bool isSleepingEnabled; // True if sleeping technique is enabled
float timeBeforeSleep; // Time of inactivity before a body sleep
float sleepLinearVelocity; // Sleep linear velocity
From 38bd462b91843834a529527b96cad3f1f7ff2ee6 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 13 Nov 2017 18:42:39 +0100
Subject: [PATCH 112/133] Fix issue in SAT algorithm, use the correct
penetration depth for each contact point
---
src/collision/narrowphase/SAT/SATAlgorithm.cpp | 10 +++++++---
1 file changed, 7 insertions(+), 3 deletions(-)
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 47953f1a..327f4465 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -871,8 +871,12 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
bool contactPointsFound = false;
for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
+ // Compute the penetration depth of this contact point (can be different from the minPenetration depth which is
+ // the maximal penetration depth of any contact point for this separating axis
+ decimal penetrationDepth = (referenceFaceVertex - (*itPoints)).dot(axisReferenceSpace);
+
// If the clip point is bellow the reference face
- if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) {
+ if (penetrationDepth > decimal(0.0)) {
contactPointsFound = true;
@@ -889,10 +893,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
- minPenetrationDepth, outWorldNormal);
+ penetrationDepth, outWorldNormal);
// Create a new contact point
- narrowPhaseInfo->addContactPoint(outWorldNormal, minPenetrationDepth,
+ narrowPhaseInfo->addContactPoint(outWorldNormal, penetrationDepth,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
}
From f403a6e8040832a222ff17ad648889b802aacef5 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 22 Nov 2017 22:43:27 +0100
Subject: [PATCH 113/133] Add temporal coherence for convex vs triangle
collision detection
---
src/collision/CollisionCallback.h | 2 +
src/collision/CollisionDetection.cpp | 18 +-
src/collision/ContactManifold.h | 1 +
src/collision/ContactManifoldInfo.cpp | 3 +
src/collision/MiddlePhaseTriangleCallback.cpp | 6 +-
src/collision/MiddlePhaseTriangleCallback.h | 3 +-
src/collision/NarrowPhaseInfo.cpp | 7 +-
src/collision/NarrowPhaseInfo.h | 17 +-
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 15 +-
...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 7 +-
.../narrowphase/GJK/GJKAlgorithm.cpp | 10 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 300 +++++++++---------
.../SphereVsConvexPolyhedronAlgorithm.cpp | 11 +-
src/collision/shapes/CollisionShape.cpp | 2 +-
src/collision/shapes/CollisionShape.h | 11 +
src/collision/shapes/ConcaveMeshShape.cpp | 21 +-
src/collision/shapes/ConcaveMeshShape.h | 5 +-
src/collision/shapes/ConcaveShape.h | 3 +-
src/collision/shapes/HeightFieldShape.cpp | 10 +-
src/collision/shapes/HeightFieldShape.h | 12 +-
src/collision/shapes/SphereShape.cpp | 3 +-
src/collision/shapes/TriangleShape.cpp | 15 +-
src/collision/shapes/TriangleShape.h | 28 +-
src/constraint/ContactPoint.h | 3 +-
src/engine/CollisionWorld.h | 2 +-
src/engine/OverlappingPair.cpp | 73 ++++-
src/engine/OverlappingPair.h | 50 ++-
test/tests/collision/TestRaycast.h | 9 +-
28 files changed, 400 insertions(+), 247 deletions(-)
diff --git a/src/collision/CollisionCallback.h b/src/collision/CollisionCallback.h
index f860b42d..54208fe4 100644
--- a/src/collision/CollisionCallback.h
+++ b/src/collision/CollisionCallback.h
@@ -32,6 +32,8 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
+class OverlappingPair;
+
// Class CollisionCallback
/**
* This class can be used to register a callback for collision test queries.
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 7cb865f9..4808ea65 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -113,6 +113,9 @@ void CollisionDetection::computeMiddlePhase() {
// Make all the contact manifolds and contact points of the pair obsolete
pair->makeContactsObsolete();
+ // Make all the last frame collision info obsolete
+ pair->makeLastFrameCollisionInfosObsolete();
+
ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2();
@@ -192,6 +195,9 @@ void CollisionDetection::computeMiddlePhase() {
// Not handled
continue;
}
+
+ // Remove the obsolete last frame collision infos
+ pair->clearObsoleteLastFrameCollisionInfos();
}
}
}
@@ -263,6 +269,8 @@ void CollisionDetection::computeNarrowPhase() {
// If there is no collision algorithm between those two kinds of shapes, skip it
if (narrowPhaseAlgorithm != nullptr) {
+ LastFrameCollisionInfo* lastCollisionFrameInfo = currentNarrowPhaseInfo->getLastFrameCollisionInfo();
+
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
@@ -271,14 +279,14 @@ void CollisionDetection::computeNarrowPhase() {
// Add the contact points as a potential contact manifold into the pair
currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold();
- currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true;
+ lastCollisionFrameInfo->wasColliding = true;
}
else {
- currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = false;
+ lastCollisionFrameInfo->wasColliding = false;
}
// The previous frame collision info is now valid
- currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true;
+ lastCollisionFrameInfo->isValid = true;
}
currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
@@ -471,6 +479,8 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
NarrowPhaseInfo* narrowPhaseInfo = nullptr;
+ pair->makeLastFrameCollisionInfosObsolete();
+
// If both shapes are convex
if ((isShape1Convex && isShape2Convex)) {
@@ -490,6 +500,8 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
computeConvexVsConcaveMiddlePhase(pair, mPoolAllocator, &narrowPhaseInfo);
}
+ pair->clearObsoleteLastFrameCollisionInfos();
+
return narrowPhaseInfo;
}
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index 80a9f456..8303f35c 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -410,5 +410,6 @@ inline void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoint
}
}
+
#endif
diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp
index f22593ac..6740fe4f 100644
--- a/src/collision/ContactManifoldInfo.cpp
+++ b/src/collision/ContactManifoldInfo.cpp
@@ -62,6 +62,9 @@ void ContactManifoldInfo::reset() {
ContactPointInfo* elementToDelete = element;
element = element->next;
+ // Call the constructor
+ elementToDelete->~ContactPointInfo();
+
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp
index 0bda49d4..58604087 100644
--- a/src/collision/MiddlePhaseTriangleCallback.cpp
+++ b/src/collision/MiddlePhaseTriangleCallback.cpp
@@ -29,14 +29,12 @@
using namespace reactphysics3d;
// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase)
-void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
- const Vector3* verticesNormals) {
+void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) {
// Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the
// destructor of the corresponding NarrowPhaseInfo.
TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
- TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
- verticesNormals, meshSubPart, triangleIndex);
+ TriangleShape(trianglePoints, verticesNormals, shapeId);
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/collision/MiddlePhaseTriangleCallback.h b/src/collision/MiddlePhaseTriangleCallback.h
index 4927c6e0..e04d7a9c 100644
--- a/src/collision/MiddlePhaseTriangleCallback.h
+++ b/src/collision/MiddlePhaseTriangleCallback.h
@@ -82,8 +82,7 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
}
/// Test collision between a triangle and the convex mesh shape
- virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints,
- const Vector3* verticesNormals) override;
+ virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override;
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp
index b4a92a38..660f6704 100644
--- a/src/collision/NarrowPhaseInfo.cpp
+++ b/src/collision/NarrowPhaseInfo.cpp
@@ -35,12 +35,15 @@ using namespace reactphysics3d;
// Constructor
NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
- const Transform& shape2Transform, void** cachedData1, void** cachedData2, Allocator& shapeAllocator)
+ const Transform& shape2Transform, void** cachedData1, void** cachedData2,
+ Allocator& shapeAllocator)
: overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
contactPoints(nullptr), cachedCollisionData1(cachedData1),
- cachedCollisionData2(cachedData2), collisionShapeAllocator(shapeAllocator), next(nullptr) {
+ cachedCollisionData2(cachedData2), next(nullptr), collisionShapeAllocator(shapeAllocator) {
+ // Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
+ overlappingPair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId());
}
// Destructor
diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h
index c9b18094..b100939f 100644
--- a/src/collision/NarrowPhaseInfo.h
+++ b/src/collision/NarrowPhaseInfo.h
@@ -29,11 +29,12 @@
// Libraries
#include "shapes/CollisionShape.h"
#include "collision/ContactManifoldInfo.h"
+#include "engine/OverlappingPair.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
-class OverlappingPair;
+struct LastFrameCollisionInfo;
// Class NarrowPhaseInfo
/**
@@ -70,12 +71,12 @@ struct NarrowPhaseInfo {
// TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
void** cachedCollisionData2;
- /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor)
- Allocator& collisionShapeAllocator;
-
/// Pointer to the next element in the linked list
NarrowPhaseInfo* next;
+ /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor)
+ Allocator& collisionShapeAllocator;
+
/// Constructor
NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
@@ -93,8 +94,16 @@ struct NarrowPhaseInfo {
/// Reset the remaining contact points
void resetContactPoints();
+
+ /// Get the last collision frame info for temporal coherence
+ LastFrameCollisionInfo* getLastFrameCollisionInfo() const;
};
+// Get the last collision frame info for temporal coherence
+inline LastFrameCollisionInfo* NarrowPhaseInfo::getLastFrameCollisionInfo() const {
+ return overlappingPair->getLastFrameCollisionInfo(collisionShape1->getId(), collisionShape2->getId());
+}
+
}
#endif
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index 315dcf7a..300ba5f2 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -50,10 +50,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
#endif
+ // Get the last frame collision info
+ LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
+
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
+ lastFrameCollisionInfo->wasUsingGJK = true;
+ lastFrameCollisionInfo->wasUsingSAT = false;
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
@@ -140,8 +143,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
}
}
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+ lastFrameCollisionInfo->wasUsingSAT = false;
+ lastFrameCollisionInfo->wasUsingGJK = false;
// Return true
return true;
@@ -153,8 +156,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
// Run the SAT algorithm to find the separating axis and compute contact point
bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
+ lastFrameCollisionInfo->wasUsingGJK = false;
+ lastFrameCollisionInfo->wasUsingSAT = true;
return isColliding;
}
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
index 9f2bb47e..507f6043 100644
--- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
@@ -45,10 +45,13 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo*
#endif
+ // Get the last frame collision info
+ LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
+
bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
+ lastFrameCollisionInfo->wasUsingSAT = true;
+ lastFrameCollisionInfo->wasUsingGJK = false;
return isColliding;
}
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index c0f436a6..dcfff45e 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -88,11 +88,13 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
// Create a simplex set
VoronoiSimplex simplex;
+ // Get the last collision frame info
+ LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
+
// Get the previous point V (last cached separating axis)
Vector3 v;
- LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
- if (lastFrameInfo.isValid && lastFrameInfo.wasUsingGJK) {
- v = lastFrameInfo.gjkSeparatingAxis;
+ if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingGJK) {
+ v = lastFrameCollisionInfo->gjkSeparatingAxis;
assert(v.lengthSquare() > decimal(0.000001));
}
else {
@@ -117,7 +119,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().gjkSeparatingAxis = v;
+ lastFrameCollisionInfo->gjkSeparatingAxis = v;
// No intersection, we return
return GJKResult::SEPARATED;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 327f4465..761bd5ec 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -74,7 +74,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint minFaceIndex = 0;
-
// For each face of the convex mesh
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
@@ -110,7 +109,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
minPenetrationDepth, normalWorld);
-
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
@@ -468,156 +466,152 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
Vector3 separatingEdge2A, separatingEdge2B;
Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
- LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
-
// True if the shapes were overlapping in the previous frame and are
// still overlapping on the same axis in this frame
bool isTemporalCoherenceValid = false;
- // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
- // frame collision data per triangle)
- if (polyhedron1->getName() != CollisionShapeName::TRIANGLE && polyhedron2->getName() != CollisionShapeName::TRIANGLE) {
+ LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
- // If the last frame collision info is valid and was also using SAT algorithm
- if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
+ // If the last frame collision info is valid and was also using SAT algorithm
+ if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) {
- // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
- // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
- // the shapes are still separated along this axis, we directly exit with no collision.
+ // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
+ // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
+ // the shapes are still separated along this axis, we directly exit with no collision.
- // If the previous separating axis (or axis with minimum penetration depth)
- // was a face normal of polyhedron 1
- if (lastFrameInfo.satIsAxisFacePolyhedron1) {
+ // If the previous separating axis (or axis with minimum penetration depth)
+ // was a face normal of polyhedron 1
+ if (lastFrameCollisionInfo->satIsAxisFacePolyhedron1) {
- decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2,
- lastFrameInfo.satMinAxisFaceIndex);
- // If the previous axis is a separating axis
- if (penetrationDepth <= decimal(0.0)) {
+ decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2,
+ lastFrameCollisionInfo->satMinAxisFaceIndex);
+ // If the previous axis is a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
- // Return no collision
- return false;
- }
-
- // The two shapes are overlapping as in the previous frame and on the same axis, therefore
- // we will skip the entire SAT algorithm because the minimum separating axis did not change
- isTemporalCoherenceValid = lastFrameInfo.wasColliding;
-
- if (isTemporalCoherenceValid) {
-
- minPenetrationDepth = penetrationDepth;
- minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
- isMinPenetrationFaceNormal = true;
- isMinPenetrationFaceNormalPolyhedron1 = true;
-
- // Compute the contact points between two faces of two convex polyhedra.
- // If contact points have been found, we report them without running the whole SAT algorithm
- if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
- polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
- narrowPhaseInfo, minPenetrationDepth)) {
-
- lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
- lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
- lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
-
- return true;
- }
- else { // Contact points have not been found (the set of clipped points was empty)
-
- // Therefore, we need to run the whole SAT algorithm again
- isTemporalCoherenceValid = false;
- }
- }
+ // Return no collision
+ return false;
}
- else if (lastFrameInfo.satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth)
- // was a face normal of polyhedron 2
- decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1,
- lastFrameInfo.satMinAxisFaceIndex);
- // If the previous axis is a separating axis
- if (penetrationDepth <= decimal(0.0)) {
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding;
- // Return no collision
- return false;
+ if (isTemporalCoherenceValid) {
+
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
+ isMinPenetrationFaceNormal = true;
+ isMinPenetrationFaceNormalPolyhedron1 = true;
+
+ // Compute the contact points between two faces of two convex polyhedra.
+ // If contact points have been found, we report them without running the whole SAT algorithm
+ if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
+ polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
+ narrowPhaseInfo, minPenetrationDepth)) {
+
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
+
+ return true;
}
+ else { // Contact points have not been found (the set of clipped points was empty)
- // The two shapes are overlapping as in the previous frame and on the same axis, therefore
- // we will skip the entire SAT algorithm because the minimum separating axis did not change
- isTemporalCoherenceValid = lastFrameInfo.wasColliding;
-
- if (isTemporalCoherenceValid) {
-
- minPenetrationDepth = penetrationDepth;
- minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
- isMinPenetrationFaceNormal = true;
- isMinPenetrationFaceNormalPolyhedron1 = false;
-
- // Compute the contact points between two faces of two convex polyhedra.
- // If contact points have been found, we report them without running the whole SAT algorithm
- if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
- polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
- narrowPhaseInfo, minPenetrationDepth)) {
-
- lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
- lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
- lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
-
- return true;
- }
- else { // Contact points have not been found (the set of clipped points was empty)
-
- // Therefore, we need to run the whole SAT algorithm again
- isTemporalCoherenceValid = false;
- }
- }
- }
- else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
-
- HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameInfo.satMinEdge1Index);
- HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameInfo.satMinEdge2Index);
-
- Vector3 separatingAxisPolyhedron2Space;
-
- const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
- const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
- const Vector3 edge1Direction = edge1B - edge1A;
- const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
- const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
- const Vector3 edge2Direction = edge2B - edge2A;
-
- // Compute the penetration depth
- decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
- edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
-
- // If the previous axis is a separating axis
- if (penetrationDepth <= decimal(0.0)) {
-
- // Return no collision
- return false;
- }
-
- // The two shapes are overlapping as in the previous frame and on the same axis, therefore
- // we will skip the entire SAT algorithm because the minimum separating axis did not change
- isTemporalCoherenceValid = lastFrameInfo.wasColliding;
-
- // Temporal coherence is valid only if the two edges build a minkowski
- // face (and the cross product is therefore a candidate for separating axis
- if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
+ // Therefore, we need to run the whole SAT algorithm again
isTemporalCoherenceValid = false;
}
+ }
+ }
+ else if (lastFrameCollisionInfo->satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth)
+ // was a face normal of polyhedron 2
- if (isTemporalCoherenceValid) {
+ decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1,
+ lastFrameCollisionInfo->satMinAxisFaceIndex);
- minPenetrationDepth = penetrationDepth;
- isMinPenetrationFaceNormal = false;
- isMinPenetrationFaceNormalPolyhedron1 = false;
- minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index;
- minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index;
- separatingEdge1A = edge1A;
- separatingEdge1B = edge1B;
- separatingEdge2A = edge2A;
- separatingEdge2B = edge2B;
- minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
+ // If the previous axis is a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+
+ // Return no collision
+ return false;
+ }
+
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding;
+
+ if (isTemporalCoherenceValid) {
+
+ minPenetrationDepth = penetrationDepth;
+ minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
+ isMinPenetrationFaceNormal = true;
+ isMinPenetrationFaceNormalPolyhedron1 = false;
+
+ // Compute the contact points between two faces of two convex polyhedra.
+ // If contact points have been found, we report them without running the whole SAT algorithm
+ if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
+ polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
+ narrowPhaseInfo, minPenetrationDepth)) {
+
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
+
+ return true;
}
+ else { // Contact points have not been found (the set of clipped points was empty)
+
+ // Therefore, we need to run the whole SAT algorithm again
+ isTemporalCoherenceValid = false;
+ }
+ }
+ }
+ else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
+
+ HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index);
+ HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index);
+
+ Vector3 separatingAxisPolyhedron2Space;
+
+ const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
+ const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
+ const Vector3 edge1Direction = edge1B - edge1A;
+ const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
+ const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
+ const Vector3 edge2Direction = edge2B - edge2A;
+
+ // Compute the penetration depth
+ decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
+ edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
+
+ // If the previous axis is a separating axis
+ if (penetrationDepth <= decimal(0.0)) {
+
+ // Return no collision
+ return false;
+ }
+
+ // The two shapes are overlapping as in the previous frame and on the same axis, therefore
+ // we will skip the entire SAT algorithm because the minimum separating axis did not change
+ isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding;
+
+ // Temporal coherence is valid only if the two edges build a minkowski
+ // face (and the cross product is therefore a candidate for separating axis
+ if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
+ isTemporalCoherenceValid = false;
+ }
+
+ if (isTemporalCoherenceValid) {
+
+ minPenetrationDepth = penetrationDepth;
+ isMinPenetrationFaceNormal = false;
+ isMinPenetrationFaceNormalPolyhedron1 = false;
+ minSeparatingEdge1Index = lastFrameCollisionInfo->satMinEdge1Index;
+ minSeparatingEdge2Index = lastFrameCollisionInfo->satMinEdge2Index;
+ separatingEdge1A = edge1A;
+ separatingEdge1B = edge1B;
+ separatingEdge2A = edge2A;
+ separatingEdge2B = edge2B;
+ minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
}
}
}
@@ -631,9 +625,9 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
if (penetrationDepth <= decimal(0.0)) {
- lastFrameInfo.satIsAxisFacePolyhedron1 = true;
- lastFrameInfo.satIsAxisFacePolyhedron2 = false;
- lastFrameInfo.satMinAxisFaceIndex = faceIndex;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = true;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
+ lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
// We have found a separating axis
return false;
@@ -649,9 +643,9 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
if (penetrationDepth <= decimal(0.0)) {
- lastFrameInfo.satIsAxisFacePolyhedron1 = false;
- lastFrameInfo.satIsAxisFacePolyhedron2 = true;
- lastFrameInfo.satMinAxisFaceIndex = faceIndex;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = true;
+ lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
// We have found a separating axis
return false;
@@ -694,10 +688,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
if (penetrationDepth <= decimal(0.0)) {
- lastFrameInfo.satIsAxisFacePolyhedron1 = false;
- lastFrameInfo.satIsAxisFacePolyhedron2 = false;
- lastFrameInfo.satMinEdge1Index = i;
- lastFrameInfo.satMinEdge2Index = j;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
+ lastFrameCollisionInfo->satMinEdge1Index = i;
+ lastFrameCollisionInfo->satMinEdge2Index = j;
// We have found a separating axis
return false;
@@ -742,18 +736,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// because of a numerical issue
if (!contactsFound) {
- lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
- lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
- lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// Return no collision
return false;
}
}
- lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
- lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
- lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
+ lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
}
else { // If we have an edge vs edge contact
@@ -781,10 +775,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
}
- lastFrameInfo.satIsAxisFacePolyhedron1 = false;
- lastFrameInfo.satIsAxisFacePolyhedron2 = false;
- lastFrameInfo.satMinEdge1Index = minSeparatingEdge1Index;
- lastFrameInfo.satMinEdge2Index = minSeparatingEdge2Index;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
+ lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
+ lastFrameCollisionInfo->satMinEdge1Index = minSeparatingEdge1Index;
+ lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index;
}
return true;
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index ae3764ab..28a565c1 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -41,6 +41,9 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
+ // Get the last frame collision info
+ LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
+
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
@@ -52,8 +55,8 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
+ lastFrameCollisionInfo->wasUsingGJK = true;
+ lastFrameCollisionInfo->wasUsingSAT = false;
// If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
@@ -76,8 +79,8 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
- narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
+ lastFrameCollisionInfo->wasUsingGJK = false;
+ lastFrameCollisionInfo->wasUsingSAT = true;
return isColliding;
}
diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp
index a89747c9..0b64190e 100644
--- a/src/collision/shapes/CollisionShape.cpp
+++ b/src/collision/shapes/CollisionShape.cpp
@@ -33,7 +33,7 @@ using namespace reactphysics3d;
// Constructor
CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type)
- : mType(type), mName(name), mScaling(1.0, 1.0, 1.0) {
+ : mType(type), mName(name), mScaling(1.0, 1.0, 1.0), mId(0) {
}
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index feb4c502..6792cc09 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -71,6 +71,9 @@ class CollisionShape {
/// Scaling vector of the collision shape
Vector3 mScaling;
+ /// Unique identifier of the shape inside an overlapping pair
+ uint mId;
+
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
@@ -126,6 +129,9 @@ class CollisionShape {
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
+ /// Return the id of the shape
+ uint getId() const;
+
/// Return the local inertia tensor of the collision shapes
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
@@ -171,6 +177,11 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
mScaling = scaling;
}
+// Return the id of the shape
+inline uint CollisionShape::getId() const {
+ return mId;
+}
+
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp
index 646638dd..ee90ce46 100644
--- a/src/collision/shapes/ConcaveMeshShape.cpp
+++ b/src/collision/shapes/ConcaveMeshShape.cpp
@@ -135,6 +135,22 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
return raycastCallback.getIsHit();
}
+// Compute the shape Id for a given triangle of the mesh
+uint ConcaveMeshShape::computeTriangleShapeId(uint subPart, uint triangleIndex) const {
+
+ uint shapeId = 0;
+
+ uint i=0;
+ while (i < subPart) {
+
+ shapeId += mTriangleMesh->getSubpart(i)->getNbTriangles();
+
+ i++;
+ }
+
+ return shapeId + triangleIndex;
+}
+
// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
@@ -162,9 +178,9 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
// Get the vertices normals of the triangle
Vector3 verticesNormals[3];
mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
+
// Create a triangle collision shape
- TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
- verticesNormals, data[0], data[1]);
+ TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]));
triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
#ifdef IS_PROFILING_ACTIVE
@@ -194,5 +210,6 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
smallestHitFraction = raycastInfo.hitFraction;
mIsHit = true;
}
+
}
}
diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h
index 678d5325..2d0a3e9c 100644
--- a/src/collision/shapes/ConcaveMeshShape.h
+++ b/src/collision/shapes/ConcaveMeshShape.h
@@ -155,6 +155,9 @@ class ConcaveMeshShape : public ConcaveShape {
/// Return the three vertex normals (in the array outVerticesNormals) of a triangle
void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const;
+ /// Compute the shape Id for a given triangle of the mesh
+ uint computeTriangleShapeId(uint subPart, uint triangleIndex) const;
+
public:
/// Constructor
@@ -259,7 +262,7 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId)
mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
// Call the callback to test narrow-phase collision with this triangle
- mTriangleTestCallback.testTriangle(data[0], data[1], trianglePoints, verticesNormals);
+ mTriangleTestCallback.testTriangle(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]));
}
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h
index abc3759b..eb00d8f0 100644
--- a/src/collision/shapes/ConcaveShape.h
+++ b/src/collision/shapes/ConcaveShape.h
@@ -46,8 +46,7 @@ class TriangleCallback {
virtual ~TriangleCallback() = default;
/// Report a triangle
- virtual void testTriangle(uint meshSubPart, uint triangleIndex,
- const Vector3* trianglePoints, const Vector3* verticesNormals)=0;
+ virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId)=0;
};
diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp
index 08ec19a4..74276a75 100644
--- a/src/collision/shapes/HeightFieldShape.cpp
+++ b/src/collision/shapes/HeightFieldShape.cpp
@@ -154,7 +154,7 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
Vector3 verticesNormals1[3] = {triangle1Normal, triangle1Normal, triangle1Normal};
// Test collision against the first triangle
- callback.testTriangle(0, 0, trianglePoints, verticesNormals1);
+ callback.testTriangle(trianglePoints, verticesNormals1, computeTriangleShapeId(i, j, 0));
// Generate the second triangle for the current grid rectangle
trianglePoints[0] = p3;
@@ -174,7 +174,7 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
Vector3 verticesNormals2[3] = {triangle2Normal, triangle2Normal, triangle2Normal};
// Test collision against the second triangle
- callback.testTriangle(0, 0, trianglePoints, verticesNormals2);
+ callback.testTriangle(trianglePoints, verticesNormals2, computeTriangleShapeId(i, j, 1));
}
}
}
@@ -263,12 +263,10 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
}
// Raycast test between a ray and a triangle of the heightfield
-void TriangleOverlapCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints,
- const Vector3* verticesNormals) {
+void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) {
// Create a triangle collision shape
- TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2],
- verticesNormals, meshSubPart, triangleIndex);
+ TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId);
triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h
index 80c18d9c..f8b498d5 100644
--- a/src/collision/shapes/HeightFieldShape.h
+++ b/src/collision/shapes/HeightFieldShape.h
@@ -71,8 +71,7 @@ class TriangleOverlapCallback : public TriangleCallback {
bool getIsHit() const {return mIsHit;}
/// Raycast test between a ray and a triangle of the heightfield
- virtual void testTriangle(uint meshSubPart, uint triangleIndex,
- const Vector3* trianglePoints, const Vector3* verticesNormals) override;
+ virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override;
#ifdef IS_PROFILING_ACTIVE
@@ -169,6 +168,9 @@ class HeightFieldShape : public ConcaveShape {
/// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide
void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const;
+ /// Compute the shape Id for a given triangle
+ uint computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const;
+
public:
/// Constructor
@@ -270,6 +272,12 @@ inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decim
0, 0, mass);
}
+// Compute the shape Id for a given triangle
+inline uint HeightFieldShape::computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const {
+
+ return (jIndex * (mNbColumns - 1) + iIndex) * 2 + secondTriangleIncrement;
+}
+
}
#endif
diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp
index 8583e80d..fa6b4f6a 100644
--- a/src/collision/shapes/SphereShape.cpp
+++ b/src/collision/shapes/SphereShape.cpp
@@ -35,7 +35,8 @@ using namespace reactphysics3d;
/**
* @param radius Radius of the sphere (in meters)
*/
-SphereShape::SphereShape(decimal radius) : ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) {
+SphereShape::SphereShape(decimal radius)
+ : ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) {
assert(radius > decimal(0.0));
}
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index 020053b6..77d0929c 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -43,16 +43,15 @@ using namespace reactphysics3d;
* @param verticesNormals The three vertices normals for smooth mesh collision
* @param margin The collision margin (in meters) around the collision shape
*/
-TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
- const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex)
- : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) {
+TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId)
+ : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE) {
- mPoints[0] = point1;
- mPoints[1] = point2;
- mPoints[2] = point3;
+ mPoints[0] = vertices[0];
+ mPoints[1] = vertices[1];
+ mPoints[2] = vertices[2];
// Compute the triangle normal
- mNormal = (point2 - point1).cross(point3 - point1);
+ mNormal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]);
mNormal.normalize();
mVerticesNormals[0] = verticesNormals[0];
@@ -60,6 +59,8 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const
mVerticesNormals[2] = verticesNormals[2];
mRaycastTestType = TriangleRaycastSide::FRONT;
+
+ mId = shapeId;
}
// This method compute the smooth mesh contact with a triangle in case one of the two collision
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index 8360c32b..60ec216b 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -72,12 +72,6 @@ class TriangleShape : public ConvexPolyhedronShape {
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide mRaycastTestType;
- /// Index of the mesh sub part in the original mesh
- uint mMeshSubPart;
-
- /// Triangle index of the triangle in the sub mesh
- uint mTriangleIndex;
-
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
@@ -95,6 +89,9 @@ class TriangleShape : public ConvexPolyhedronShape {
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
+ /// Generate the id of the shape (used for temporal coherence)
+ void generateId();
+
// -------------------- Methods -------------------- //
/// This method implements the technique described in Game Physics Pearl book
@@ -107,8 +104,7 @@ class TriangleShape : public ConvexPolyhedronShape {
// -------------------- Methods -------------------- //
/// Constructor
- TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
- const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex);
+ TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId);
/// Destructor
virtual ~TriangleShape() override = default;
@@ -164,12 +160,6 @@ class TriangleShape : public ConvexPolyhedronShape {
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const override;
- /// Return the index of the sub part mesh of the original mesh
- uint getMeshSubPart() const;
-
- /// Return the triangle index in the original mesh
- uint getTriangleIndex() const;
-
/// This method compute the smooth mesh contact with a triangle in case one of the two collision shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh
static void computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2,
Vector3& localContactPointShape1, Vector3& localContactPointShape2,
@@ -319,16 +309,6 @@ inline Vector3 TriangleShape::getCentroid() const {
return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0);
}
-// Return the index of the sub part mesh of the original mesh
-inline uint TriangleShape::getMeshSubPart() const {
- return mMeshSubPart;
-}
-
-// Return the triangle index in the original mesh
-inline uint TriangleShape::getTriangleIndex() const {
- return mTriangleIndex;
-}
-
// Return the number of half-edges of the polyhedron
inline uint TriangleShape::getNbHalfEdges() const {
return 6;
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index e2a9a5e5..739a9efb 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -28,7 +28,6 @@
// Libraries
#include "body/CollisionBody.h"
-#include "collision/NarrowPhaseInfo.h"
#include "collision/ContactPointInfo.h"
#include "configuration.h"
#include "mathematics/mathematics.h"
@@ -36,6 +35,8 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
+struct NarrowPhaseInfo;
+
// Class ContactPoint
/**
* This class represents a collision contact point between two
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index f523d169..6c558d6d 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -259,4 +259,4 @@ inline void CollisionWorld::setProfilerName(std::string name) {
}
- #endif
+#endif
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index 0055e683..eba2fb80 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -27,20 +27,31 @@
#include <cassert>
#include "OverlappingPair.h"
#include "collision/ContactManifoldInfo.h"
+#include "collision/NarrowPhaseInfo.h"
using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
- Allocator& manifoldsAllocator, Allocator& temporaryMemoryAllocator)
- : mContactManifoldSet(shape1, shape2, manifoldsAllocator), mPotentialContactManifolds(nullptr),
- mTempMemoryAllocator(temporaryMemoryAllocator) {
+ Allocator& persistentMemoryAllocator, Allocator& temporaryMemoryAllocator)
+ : mContactManifoldSet(shape1, shape2, persistentMemoryAllocator), mPotentialContactManifolds(nullptr),
+ mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator) {
}
// Destructor
OverlappingPair::~OverlappingPair() {
assert(mPotentialContactManifolds == nullptr);
+
+ // Remove all the remaining last frame collision info
+ for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) {
+
+ // Call the constructor
+ it->second->~LastFrameCollisionInfo();
+
+ // Release memory
+ mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo));
+ }
}
// Create a new potential contact manifold using contact-points from narrow-phase
@@ -137,3 +148,59 @@ void OverlappingPair::reducePotentialContactManifolds() {
manifold = manifold->getNext();
}
}
+
+
+// Add a new last frame collision info if it does not exist for the given shapes already
+void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) {
+
+ // Try to get the corresponding last frame collision info
+ auto it = mLastFrameCollisionInfos.find(std::make_pair(shapeId1, shapeId2));
+
+ // If there is no collision info for those two shapes already
+ if (it == mLastFrameCollisionInfos.end()) {
+
+ // Create a new collision info
+ LastFrameCollisionInfo* collisionInfo = new (mPersistentAllocator.allocate(sizeof(LastFrameCollisionInfo)))
+ LastFrameCollisionInfo();
+
+ // Add it into the map of collision infos
+ std::map<std::pair<uint, uint>, LastFrameCollisionInfo*>::iterator it;
+ auto ret = mLastFrameCollisionInfos.insert(std::make_pair(std::make_pair(shapeId1, shapeId2), collisionInfo));
+ assert(ret.second);
+ }
+ else {
+
+ // The existing collision info is not obsolete
+ it->second->isObsolete = false;
+ }
+}
+
+
+// Delete all the obsolete last frame collision info
+void OverlappingPair::clearObsoleteLastFrameCollisionInfos() {
+
+ // For each collision info
+ for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ) {
+
+ // If the collision info is obsolete
+ if (it->second->isObsolete) {
+
+ // Delete it
+ it->second->~LastFrameCollisionInfo();
+ mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo));
+
+ mLastFrameCollisionInfos.erase(it++);
+ }
+ else {
+ ++it;
+ }
+ }
+}
+
+// Make all the last frame collision infos obsolete
+void OverlappingPair::makeLastFrameCollisionInfosObsolete() {
+
+ for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) {
+ it->second->isObsolete = true;
+ }
+}
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index ce22bb7e..8c65e633 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -30,6 +30,7 @@
#include "collision/ContactManifoldSet.h"
#include "collision/ProxyShape.h"
#include "collision/shapes/CollisionShape.h"
+#include <map>
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -47,6 +48,9 @@ struct LastFrameCollisionInfo {
/// True if we have information about the previous frame
bool isValid;
+ /// True if the frame info is obsolete (the collision shape are not overlapping in middle phase)
+ bool isObsolete;
+
/// True if the two shapes were colliding in the previous frame
bool wasColliding;
@@ -72,6 +76,7 @@ struct LastFrameCollisionInfo {
LastFrameCollisionInfo() {
isValid = false;
+ isObsolete = false;
wasColliding = false;
wasUsingSAT = false;
wasUsingGJK = false;
@@ -97,12 +102,19 @@ class OverlappingPair {
/// Set of persistent contact manifolds
ContactManifoldSet mContactManifoldSet;
- /// Collision information about the last frame (for temporal coherence)
- LastFrameCollisionInfo mLastFrameCollisionInfo;
+ /// Temporal coherence collision data for each overlapping collision shapes of this pair.
+ /// Temporal coherence data store collision information about the last frame.
+ /// If two convex shapes overlap, we have a single collision data but if one shape is concave,
+ /// we might have collision data for several overlapping triangles. The key in the map is the
+ /// shape Ids of the two collision shapes.
+ std::map<std::pair<uint, uint>, LastFrameCollisionInfo*> mLastFrameCollisionInfos;
/// Linked-list of potential contact manifold
ContactManifoldInfo* mPotentialContactManifolds;
+ /// Persistent memory allocator
+ Allocator& mPersistentAllocator;
+
/// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo
Allocator& mTempMemoryAllocator;
@@ -111,8 +123,8 @@ class OverlappingPair {
// -------------------- Methods -------------------- //
/// Constructor
- OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
- Allocator& memoryAllocator, Allocator& temporaryMemoryAllocator);
+ OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, Allocator& persistentMemoryAllocator,
+ Allocator& temporaryMemoryAllocator);
/// Destructor
~OverlappingPair();
@@ -130,7 +142,7 @@ class OverlappingPair {
ProxyShape* getShape2() const;
/// Return the last frame collision info
- LastFrameCollisionInfo& getLastFrameCollisionInfo();
+ LastFrameCollisionInfo* getLastFrameCollisionInfo(std::pair<uint, uint> shapeIds);
/// Return the a reference to the contact manifold set
const ContactManifoldSet& getContactManifoldSet();
@@ -168,6 +180,18 @@ class OverlappingPair {
/// Reduce the contact manifolds that have too many contact points
void reduceContactManifolds();
+ /// Add a new last frame collision info if it does not exist for the given shapes already
+ void addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2);
+
+ /// Return the last frame collision info for a given pair of shape ids
+ LastFrameCollisionInfo* getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const;
+
+ /// Delete all the obsolete last frame collision info
+ void clearObsoleteLastFrameCollisionInfos();
+
+ /// Make all the last frame collision infos obsolete
+ void makeLastFrameCollisionInfosObsolete();
+
/// Return the pair of bodies index
static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
@@ -194,9 +218,14 @@ inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* conta
mContactManifoldSet.addContactManifold(contactManifoldInfo);
}
-// Return the last frame collision info
-inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() {
- return mLastFrameCollisionInfo;
+// Return the last frame collision info for a given shape id or nullptr if none is found
+inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(std::pair<uint, uint> shapeIds) {
+ std::map<std::pair<uint, uint>, LastFrameCollisionInfo*>::iterator it = mLastFrameCollisionInfos.find(shapeIds);
+ if (it != mLastFrameCollisionInfos.end()) {
+ return it->second;
+ }
+
+ return nullptr;
}
// Return the contact manifold
@@ -265,6 +294,11 @@ inline void OverlappingPair::reduceContactManifolds() {
mContactManifoldSet.reduce();
}
+// Return the last frame collision info for a given pair of shape ids
+inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const {
+ return mLastFrameCollisionInfos.at(std::make_pair(shapeId1, shapeId2));
+}
+
}
#endif
diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h
index f2ae3d6b..c62ca179 100644
--- a/test/tests/collision/TestRaycast.h
+++ b/test/tests/collision/TestRaycast.h
@@ -198,11 +198,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);
+ Vector3 triangleVertices[3];
+ triangleVertices[0] = Vector3(100, 100, 0);
+ triangleVertices[1] = Vector3(105, 100, 0);
+ triangleVertices[2] = Vector3(100, 103, 0);
Vector3 triangleVerticesNormals[3] = {Vector3(0, 0, 1), Vector3(0, 0, 1), Vector3(0, 0, 1)};
- mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3, triangleVerticesNormals, 0, 0);
+ mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0);
mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform);
mCapsuleShape = new CapsuleShape(2, 5);
From f09331c185ddc81a3415ed26e565fd94ae2e71fa Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 22 Nov 2017 22:58:31 +0100
Subject: [PATCH 114/133] Remove unused cachedCollisionData variable
---
src/collision/CollisionDetection.cpp | 6 ++----
src/collision/MiddlePhaseTriangleCallback.cpp | 3 +--
src/collision/NarrowPhaseInfo.cpp | 6 ++----
src/collision/NarrowPhaseInfo.h | 10 +---------
src/collision/ProxyShape.cpp | 7 +------
src/collision/ProxyShape.h | 11 -----------
6 files changed, 7 insertions(+), 36 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 4808ea65..f1a7b80a 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -169,8 +169,7 @@ void CollisionDetection::computeMiddlePhase() {
mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo)))
NarrowPhaseInfo(pair, shape1->getCollisionShape(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
- shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
- shape2->getCachedCollisionData(), mSingleFrameAllocator);
+ shape2->getLocalToWorldTransform(), mSingleFrameAllocator);
mNarrowPhaseInfoList->next = firstNarrowPhaseInfo;
}
@@ -488,8 +487,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
// for the narrow-phase collision detection
narrowPhaseInfo = new (mPoolAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
- shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
- shape2->getCachedCollisionData(), mPoolAllocator);
+ shape2->getLocalToWorldTransform(), mPoolAllocator);
}
// Concave vs Convex algorithm
diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp
index 58604087..f8736a0b 100644
--- a/src/collision/MiddlePhaseTriangleCallback.cpp
+++ b/src/collision/MiddlePhaseTriangleCallback.cpp
@@ -55,7 +55,6 @@ void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, co
isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(),
shape1->getLocalToWorldTransform(),
shape2->getLocalToWorldTransform(),
- shape1->getCachedCollisionData(),
- shape2->getCachedCollisionData(), mAllocator);
+ mAllocator);
narrowPhaseInfoList->next = firstNarrowPhaseInfo;
}
diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp
index 660f6704..a6440367 100644
--- a/src/collision/NarrowPhaseInfo.cpp
+++ b/src/collision/NarrowPhaseInfo.cpp
@@ -35,12 +35,10 @@ using namespace reactphysics3d;
// Constructor
NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
- const Transform& shape2Transform, void** cachedData1, void** cachedData2,
- Allocator& shapeAllocator)
+ const Transform& shape2Transform, Allocator& shapeAllocator)
: overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
- contactPoints(nullptr), cachedCollisionData1(cachedData1),
- cachedCollisionData2(cachedData2), next(nullptr), collisionShapeAllocator(shapeAllocator) {
+ contactPoints(nullptr), next(nullptr), collisionShapeAllocator(shapeAllocator) {
// Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
overlappingPair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId());
diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h
index b100939f..b732480c 100644
--- a/src/collision/NarrowPhaseInfo.h
+++ b/src/collision/NarrowPhaseInfo.h
@@ -63,14 +63,6 @@ struct NarrowPhaseInfo {
/// Linked-list of contact points created during the narrow-phase
ContactPointInfo* contactPoints;
- /// Cached collision data of the proxy shape
- // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
- void** cachedCollisionData1;
-
- /// Cached collision data of the proxy shape
- // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
- void** cachedCollisionData2;
-
/// Pointer to the next element in the linked list
NarrowPhaseInfo* next;
@@ -80,7 +72,7 @@ struct NarrowPhaseInfo {
/// Constructor
NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
- const Transform& shape2Transform, void** cachedData1, void** cachedData2, Allocator& shapeAllocator);
+ const Transform& shape2Transform, Allocator& shapeAllocator);
/// Destructor
~NarrowPhaseInfo();
diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp
index 50418011..6e6eefbd 100644
--- a/src/collision/ProxyShape.cpp
+++ b/src/collision/ProxyShape.cpp
@@ -37,18 +37,13 @@ using namespace reactphysics3d;
*/
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass)
:mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
- mNext(nullptr), mBroadPhaseID(-1), mCachedCollisionData(nullptr), mUserData(nullptr),
- mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
+ mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
}
// Destructor
ProxyShape::~ProxyShape() {
- // Release the cached collision data memory
- if (mCachedCollisionData != nullptr) {
- free(mCachedCollisionData);
- }
}
// Return true if a point is inside the collision shape
diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h
index 574758d4..8e9a9e9f 100644
--- a/src/collision/ProxyShape.h
+++ b/src/collision/ProxyShape.h
@@ -66,9 +66,6 @@ class ProxyShape {
/// Broad-phase ID (node ID in the dynamic AABB tree)
int mBroadPhaseID;
- /// Cached collision data
- void* mCachedCollisionData;
-
/// Pointer to user data
void* mUserData;
@@ -168,9 +165,6 @@ class ProxyShape {
/// Return the next proxy shape in the linked list of proxy shapes
const ProxyShape* getNext() const;
- /// Return the pointer to the cached collision data
- void** getCachedCollisionData();
-
/// Return the local scaling vector of the collision shape
Vector3 getLocalScaling() const;
@@ -201,11 +195,6 @@ class ProxyShape {
};
-// Return the pointer to the cached collision data
-inline void** ProxyShape::getCachedCollisionData() {
- return &mCachedCollisionData;
-}
-
// Return the collision shape
/**
* @return Pointer to the internal collision shape
From fea467f112e29f53dad44e8ada4077f8c4450505 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sat, 25 Nov 2017 17:51:14 +0100
Subject: [PATCH 115/133] Add profiling in SAT algorithm methods
---
src/collision/CollisionDetection.cpp | 6 +++++
.../narrowphase/SAT/SATAlgorithm.cpp | 22 +++++++++++++++++++
2 files changed, 28 insertions(+)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index f1a7b80a..a2c3344d 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -366,6 +366,8 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
void CollisionDetection::addAllContactManifoldsToBodies() {
+ PROFILE("CollisionDetection::addAllContactManifoldsToBodies()", mProfiler);
+
// For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
@@ -413,6 +415,8 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
/// Convert the potential contact into actual contacts
void CollisionDetection::processAllPotentialContacts() {
+ PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler);
+
// For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
@@ -450,6 +454,8 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) {
// Report contacts for all the colliding overlapping pairs
void CollisionDetection::reportAllContacts() {
+ PROFILE("CollisionDetection::reportAllContacts()", mProfiler);
+
// For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 761bd5ec..f12964cf 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -122,6 +122,8 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron,
const SphereShape* sphere, const Vector3& sphereCenter) const {
+ PROFILE("SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth)", mProfiler);
+
// Get the face
HalfEdgeStructure::Face face = polyhedron->getFace(faceIndex);
@@ -297,6 +299,8 @@ decimal SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const Con
const Vector3& edgeDirectionCapsuleSpace,
const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const {
+ PROFILE("SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth)", mProfiler);
+
decimal penetrationDepth = DECIMAL_LARGEST;
// Compute the axis to test (cross product between capsule inner segment and polyhedron edge)
@@ -329,6 +333,8 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe
const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform,
Vector3& outFaceNormalCapsuleSpace) const {
+ PROFILE("SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth", mProfiler);
+
// Get the face
HalfEdgeStructure::Face face = polyhedron->getFace(polyhedronFaceIndex);
@@ -353,6 +359,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const {
+ PROFILE("SATAlgorithm::computeCapsulePolyhedronFaceContactPoints", mProfiler);
+
HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
// Get the face normal
@@ -791,6 +799,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
const Transform& polyhedron1ToPolyhedron2, const Transform& polyhedron2ToPolyhedron1,
uint minFaceIndex, NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const {
+ PROFILE("SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints", mProfiler);
+
const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1;
const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
@@ -903,6 +913,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
// This is used to find the incident face on a polyhedron of a given reference face of another polyhedron
uint SATAlgorithm::findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const {
+ PROFILE("SATAlgorithm::findMostAntiParallelFaceOnPolyhedron", mProfiler);
+
decimal minDotProduct = DECIMAL_LARGEST;
uint mostAntiParallelFace = 0;
@@ -925,6 +937,8 @@ decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const V
const Vector3& edge1Direction, const Vector3& edge2Direction,
Vector3& outSeparatingAxisPolyhedron2Space) const {
+ PROFILE("SATAlgorithm::computeDistanceBetweenEdges", mProfiler);
+
// If the two edges are parallel
if (areParallelVectors(edge1Direction, edge2Direction)) {
@@ -952,6 +966,8 @@ decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const Convex
const Transform& polyhedron1ToPolyhedron2,
uint faceIndex) const {
+ PROFILE("SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron", mProfiler);
+
HalfEdgeStructure::Face face = polyhedron1->getFace(faceIndex);
// Get the face normal
@@ -976,6 +992,8 @@ decimal SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyh
const Transform& polyhedron1ToPolyhedron2,
uint& minFaceIndex) const {
+ PROFILE("SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron", mProfiler);
+
decimal minPenetrationDepth = DECIMAL_LARGEST;
// For each face of the first polyhedron
@@ -1006,6 +1024,8 @@ bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* poly
const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2,
const Transform& polyhedron1ToPolyhedron2) const {
+ PROFILE("SATAlgorithm::testEdgesBuildMinkowskiFace", mProfiler);
+
const Vector3 a = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(edge1.faceIndex);
const Vector3 b = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex);
@@ -1037,6 +1057,8 @@ bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* poly
bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d,
const Vector3& bCrossA, const Vector3& dCrossC) const {
+ PROFILE("SATAlgorithm::testGaussMapArcsIntersect", mProfiler);
+
const decimal cba = c.dot(bCrossA);
const decimal dba = d.dot(bCrossA);
const decimal adc = a.dot(dCrossC);
From c8e9cca9123295e2994a3530a1114917aaf4dc8a Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 26 Nov 2017 12:07:58 +0100
Subject: [PATCH 116/133] Compute the inverse quaternion using its conjugate
---
src/mathematics/Quaternion.h | 24 +++++++-----------------
1 file changed, 7 insertions(+), 17 deletions(-)
diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h
index 28f7b636..b8e75306 100644
--- a/src/mathematics/Quaternion.h
+++ b/src/mathematics/Quaternion.h
@@ -224,16 +224,11 @@ inline void Quaternion::normalize() {
// Inverse the quaternion
inline void Quaternion::inverse() {
- // Get the square length of the quaternion
- decimal lengthSquareQuaternion = lengthSquare();
-
- assert (lengthSquareQuaternion > MACHINE_EPSILON);
-
- // Compute and return the inverse quaternion
- x /= -lengthSquareQuaternion;
- y /= -lengthSquareQuaternion;
- z /= -lengthSquareQuaternion;
- w /= lengthSquareQuaternion;
+ // Use the conjugate of the current quaternion
+ x = -x;
+ y = -y;
+ z = -z;
+ w = w;
}
// Return the unit quaternion
@@ -261,13 +256,8 @@ inline Quaternion Quaternion::getConjugate() const {
// Return the inverse of the quaternion (inline)
inline Quaternion Quaternion::getInverse() const {
- decimal lengthSquareQuaternion = lengthSquare();
-
- assert (lengthSquareQuaternion > MACHINE_EPSILON);
-
- // Compute and return the inverse quaternion
- return Quaternion(-x / lengthSquareQuaternion, -y / lengthSquareQuaternion,
- -z / lengthSquareQuaternion, w / lengthSquareQuaternion);
+ // Return the conjugate quaternion
+ return Quaternion(-x, -y, -z, w);
}
// Scalar product between two quaternions
From 317dea90bd5287085f3b4cd34718c06682db260f Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 28 Nov 2017 17:26:13 +0100
Subject: [PATCH 117/133] Remove Quaternion constructor with Euler angles and
replace it by static fromEulerAngles() method
---
src/mathematics/Quaternion.cpp | 66 ++++++++++---------
src/mathematics/Quaternion.h | 14 ++--
test/tests/collision/TestPointInside.h | 6 +-
test/tests/collision/TestRaycast.h | 6 +-
test/tests/mathematics/TestQuaternion.h | 8 +--
.../CollisionDetectionScene.cpp | 12 ++--
6 files changed, 59 insertions(+), 53 deletions(-)
diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp
index 0b2587f5..d0e1ae61 100644
--- a/src/mathematics/Quaternion.cpp
+++ b/src/mathematics/Quaternion.cpp
@@ -47,14 +47,28 @@ Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z),
}
-// Constructor which convert Euler angles (in radians) to a quaternion
-Quaternion::Quaternion(decimal angleX, decimal angleY, decimal angleZ) {
- initWithEulerAngles(angleX, angleY, angleZ);
+// Constructor with the component w and the vector v=(x y z)
+Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) {
+
}
-// Constructor which convert Euler angles (in radians) to a quaternion
-Quaternion::Quaternion(const Vector3& eulerAngles) {
- initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z);
+// Return a quaternion constructed from Euler angles (in radians)
+Quaternion Quaternion::fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ) {
+
+ Quaternion quaternion;
+ quaternion.initWithEulerAngles(angleX, angleY, angleZ);
+
+ return quaternion;
+}
+
+
+// Return a quaternion constructed from Euler angles (in radians)
+Quaternion Quaternion::fromEulerAngles(const Vector3& eulerAngles) {
+
+ Quaternion quaternion;
+ quaternion.initWithEulerAngles(eulerAngles.x, eulerAngles.y, eulerAngles.z);
+
+ return quaternion;
}
// Copy-constructor
@@ -72,10 +86,10 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
decimal r;
decimal s;
- if (trace < 0.0) {
+ if (trace < decimal(0.0)) {
if (matrix[1][1] > matrix[0][0]) {
if(matrix[2][2] > matrix[1][1]) {
- r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0));
+ r = std::sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0));
s = decimal(0.5) / r;
// Compute the quaternion
@@ -85,7 +99,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
w = (matrix[1][0] - matrix[0][1]) * s;
}
else {
- r = sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + decimal(1.0));
+ r = std::sqrt(matrix[1][1] - matrix[2][2] - matrix[0][0] + decimal(1.0));
s = decimal(0.5) / r;
// Compute the quaternion
@@ -96,7 +110,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
}
}
else if (matrix[2][2] > matrix[0][0]) {
- r = sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0));
+ r = std::sqrt(matrix[2][2] - matrix[0][0] - matrix[1][1] + decimal(1.0));
s = decimal(0.5) / r;
// Compute the quaternion
@@ -106,7 +120,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
w = (matrix[1][0] - matrix[0][1]) * s;
}
else {
- r = sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + decimal(1.0));
+ r = std::sqrt(matrix[0][0] - matrix[1][1] - matrix[2][2] + decimal(1.0));
s = decimal(0.5) / r;
// Compute the quaternion
@@ -117,7 +131,7 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
}
}
else {
- r = sqrt(trace + decimal(1.0));
+ r = std::sqrt(trace + decimal(1.0));
s = decimal(0.5) / r;
// Compute the quaternion
@@ -132,22 +146,12 @@ Quaternion::Quaternion(const Matrix3x3& matrix) {
/// This method is used to get the rotation angle (in radian) and the unit
/// rotation axis of an orientation quaternion.
void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const {
- Quaternion quaternion;
-
- // If the quaternion is unit
- if (length() == 1.0) {
- quaternion = *this;
- }
- else {
- // We compute the unit quaternion
- quaternion = getUnit();
- }
// Compute the roation angle
- angle = acos(quaternion.w) * decimal(2.0);
+ angle = std::acos(w) * decimal(2.0);
// Compute the 3D rotation axis
- Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z);
+ Vector3 rotationAxis(x, y, z);
// Normalize the rotation axis
rotationAxis = rotationAxis.getUnit();
@@ -162,7 +166,7 @@ Matrix3x3 Quaternion::getMatrix() const {
decimal nQ = x*x + y*y + z*z + w*w;
decimal s = 0.0;
- if (nQ > 0.0) {
+ if (nQ > decimal(0.0)) {
s = decimal(2.0) / nQ;
}
@@ -190,7 +194,7 @@ Matrix3x3 Quaternion::getMatrix() const {
/// The t argument has to be such that 0 <= t <= 1. This method is static.
Quaternion Quaternion::slerp(const Quaternion& quaternion1,
const Quaternion& quaternion2, decimal t) {
- assert(t >= 0.0 && t <= 1.0);
+ assert(t >= decimal(0.0) && t <= decimal(1.0));
decimal invert = 1.0;
@@ -198,7 +202,7 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1,
decimal cosineTheta = quaternion1.dot(quaternion2);
// Take care of the sign of cosineTheta
- if (cosineTheta < 0.0) {
+ if (cosineTheta < decimal(0.0)) {
cosineTheta = -cosineTheta;
invert = -1.0;
}
@@ -212,14 +216,14 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1,
}
// Compute the theta angle
- decimal theta = acos(cosineTheta);
+ decimal theta = std::acos(cosineTheta);
// Compute sin(theta)
- decimal sineTheta = sin(theta);
+ decimal sineTheta = std::sin(theta);
// Compute the two coefficients that are in the spherical linear interpolation formula
- decimal coeff1 = sin((decimal(1.0)-t)*theta) / sineTheta;
- decimal coeff2 = sin(t*theta) / sineTheta * invert;
+ decimal coeff1 = std::sin((decimal(1.0)-t)*theta) / sineTheta;
+ decimal coeff2 = std::sin(t*theta) / sineTheta * invert;
// Compute and return the interpolated quaternion
return quaternion1 * coeff1 + quaternion2 * coeff2;
diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h
index b8e75306..74415eb8 100644
--- a/src/mathematics/Quaternion.h
+++ b/src/mathematics/Quaternion.h
@@ -69,11 +69,8 @@ struct Quaternion {
/// Constructor with the component w and the vector v=(x y z)
Quaternion(decimal newW, const Vector3& v);
- /// Constructor which convert Euler angles (in radians) to a quaternion
- Quaternion(decimal angleX, decimal angleY, decimal angleZ);
-
- /// Constructor which convert Euler angles (in radians) to a quaternion
- Quaternion(const Vector3& eulerAngles);
+ /// Constructor with the component w and the vector v=(x y z)
+ Quaternion(const Vector3& v, decimal newW);
/// Copy-constructor
Quaternion(const Quaternion& quaternion);
@@ -123,6 +120,12 @@ struct Quaternion {
/// Return the identity quaternion
static Quaternion identity();
+ /// Return a quaternion constructed from Euler angles (in radians)
+ static Quaternion fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ);
+
+ /// Return a quaternion constructed from Euler angles (in radians)
+ static Quaternion fromEulerAngles(const Vector3& eulerAngles);
+
/// Dot product between two quaternions
decimal dot(const Quaternion& quaternion) const;
@@ -228,7 +231,6 @@ inline void Quaternion::inverse() {
x = -x;
y = -y;
z = -z;
- w = w;
}
// Return the unit quaternion
diff --git a/test/tests/collision/TestPointInside.h b/test/tests/collision/TestPointInside.h
index 608b439b..afdb415c 100644
--- a/test/tests/collision/TestPointInside.h
+++ b/test/tests/collision/TestPointInside.h
@@ -93,7 +93,7 @@ class TestPointInside : public Test {
// Body transform
Vector3 position(-3, 2, 7);
- Quaternion orientation(PI / 5, PI / 6, PI / 7);
+ Quaternion orientation = Quaternion::fromEulerAngles(PI / 5, PI / 6, PI / 7);
mBodyTransform = Transform(position, orientation);
// Create the bodies
@@ -108,7 +108,7 @@ class TestPointInside : public Test {
// Collision shape transform
Vector3 shapePosition(1, -4, -3);
- Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3);
+ Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI / 6 , -PI / 8, PI / 3);
mShapeTransform = Transform(shapePosition, shapeOrientation);
// Compute the the transform from a local shape point to world-space
@@ -165,7 +165,7 @@ class TestPointInside : public Test {
// Compound shape is a capsule and a sphere
Vector3 positionShape2(Vector3(4, 2, -3));
- Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13);
+ Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13);
Transform shapeTransform2(positionShape2, orientationShape2);
mLocalShape2ToWorld = mBodyTransform * shapeTransform2;
mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform);
diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h
index c62ca179..97dd002a 100644
--- a/test/tests/collision/TestRaycast.h
+++ b/test/tests/collision/TestRaycast.h
@@ -169,7 +169,7 @@ class TestRaycast : public Test {
// Body transform
Vector3 position(-3, 2, 7);
- Quaternion orientation(PI / 5, PI / 6, PI / 7);
+ Quaternion orientation = Quaternion::fromEulerAngles(PI / 5, PI / 6, PI / 7);
mBodyTransform = Transform(position, orientation);
// Create the bodies
@@ -185,7 +185,7 @@ class TestRaycast : public Test {
// Collision shape transform
Vector3 shapePosition(1, -4, -3);
- Quaternion shapeOrientation(3 * PI / 6 , -PI / 8, PI / 3);
+ Quaternion shapeOrientation = Quaternion::fromEulerAngles(3 * PI / 6 , -PI / 8, PI / 3);
mShapeTransform = Transform(shapePosition, shapeOrientation);
// Compute the the transform from a local shape point to world-space
@@ -246,7 +246,7 @@ class TestRaycast : public Test {
// Compound shape is a cylinder and a sphere
Vector3 positionShape2(Vector3(4, 2, -3));
- Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13);
+ Quaternion orientationShape2 = Quaternion::fromEulerAngles(-3 *PI / 8, 1.5 * PI/ 3, PI / 13);
Transform shapeTransform2(positionShape2, orientationShape2);
mLocalShape2ToWorld = mBodyTransform * shapeTransform2;
mCompoundCapsuleProxyShape = mCompoundBody->addCollisionShape(mCapsuleShape, mShapeTransform);
diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h
index 5fe0ccd0..dfc65955 100644
--- a/test/tests/mathematics/TestQuaternion.h
+++ b/test/tests/mathematics/TestQuaternion.h
@@ -94,7 +94,7 @@ class TestQuaternion : public Test {
const decimal PI_OVER_2 = PI * decimal(0.5);
const decimal PI_OVER_4 = PI_OVER_2 * decimal(0.5);
- Quaternion quaternion5(PI_OVER_2, 0, 0);
+ Quaternion quaternion5 = Quaternion::fromEulerAngles(PI_OVER_2, 0, 0);
Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4));
quaternionTest5.normalize();
test(approxEqual(quaternion5.x, quaternionTest5.x));
@@ -102,7 +102,7 @@ class TestQuaternion : public Test {
test(approxEqual(quaternion5.z, quaternionTest5.z));
test(approxEqual(quaternion5.w, quaternionTest5.w));
- Quaternion quaternion6(0, PI_OVER_2, 0);
+ Quaternion quaternion6 = Quaternion::fromEulerAngles(0, PI_OVER_2, 0);
Quaternion quaternionTest6(0, std::sin(PI_OVER_4), 0, std::cos(PI_OVER_4));
quaternionTest6.normalize();
test(approxEqual(quaternion6.x, quaternionTest6.x));
@@ -110,7 +110,7 @@ class TestQuaternion : public Test {
test(approxEqual(quaternion6.z, quaternionTest6.z));
test(approxEqual(quaternion6.w, quaternionTest6.w));
- Quaternion quaternion7(Vector3(0, 0, PI_OVER_2));
+ Quaternion quaternion7 = Quaternion::fromEulerAngles(Vector3(0, 0, PI_OVER_2));
Quaternion quaternionTest7(0, 0, std::sin(PI_OVER_4), std::cos(PI_OVER_4));
quaternionTest7.normalize();
test(approxEqual(quaternion7.x, quaternionTest7.x));
@@ -124,7 +124,7 @@ class TestQuaternion : public Test {
// Test method that returns the length
Quaternion quaternion(2, 3, -4, 5);
- test(approxEqual(quaternion.length(), sqrt(decimal(54.0))));
+ test(approxEqual(quaternion.length(), std::sqrt(decimal(54.0))));
// Test method that returns a unit quaternion
test(approxEqual(quaternion.getUnit().length(), 1.0));
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index 1241b701..6f2c65ae 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -282,32 +282,32 @@ bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, i
}
else if (key == GLFW_KEY_A && action == GLFW_PRESS) {
rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(0, stepAngle, 0) * transform.getOrientation());
+ transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, stepAngle, 0) * transform.getOrientation());
mAllShapes[mSelectedShapeIndex]->setTransform(transform);
}
else if (key == GLFW_KEY_D && action == GLFW_PRESS) {
rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(0, -stepAngle, 0) * transform.getOrientation());
+ transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, -stepAngle, 0) * transform.getOrientation());
mAllShapes[mSelectedShapeIndex]->setTransform(transform);
}
else if (key == GLFW_KEY_W && action == GLFW_PRESS) {
rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(stepAngle, 0, 0) * transform.getOrientation());
+ transform.setOrientation(rp3d::Quaternion::fromEulerAngles(stepAngle, 0, 0) * transform.getOrientation());
mAllShapes[mSelectedShapeIndex]->setTransform(transform);
}
else if (key == GLFW_KEY_S && action == GLFW_PRESS) {
rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(-stepAngle, 0, 0) * transform.getOrientation());
+ transform.setOrientation(rp3d::Quaternion::fromEulerAngles(-stepAngle, 0, 0) * transform.getOrientation());
mAllShapes[mSelectedShapeIndex]->setTransform(transform);
}
else if (key == GLFW_KEY_F && action == GLFW_PRESS) {
rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(0, 0, stepAngle) * transform.getOrientation());
+ transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, 0, stepAngle) * transform.getOrientation());
mAllShapes[mSelectedShapeIndex]->setTransform(transform);
}
else if (key == GLFW_KEY_G && action == GLFW_PRESS) {
rp3d::Transform transform = mAllShapes[mSelectedShapeIndex]->getTransform();
- transform.setOrientation(rp3d::Quaternion(0, 0, -stepAngle) * transform.getOrientation());
+ transform.setOrientation(rp3d::Quaternion::fromEulerAngles(0, 0, -stepAngle) * transform.getOrientation());
mAllShapes[mSelectedShapeIndex]->setTransform(transform);
}
From e754711a84264c6f0db646d1bff620c5918ea2ec Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 28 Nov 2017 17:46:45 +0100
Subject: [PATCH 118/133] Remove unnecessary calls to Quaternion.getMatrix()
---
src/collision/narrowphase/GJK/GJKAlgorithm.cpp | 5 ++---
src/mathematics/Transform.h | 5 ++---
2 files changed, 4 insertions(+), 6 deletions(-)
diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
index dcfff45e..62685867 100644
--- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
+++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp
@@ -75,10 +75,9 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase
Transform transform1Inverse = transform1.getInverse();
Transform body2Tobody1 = transform1Inverse * transform2;
- // Matrix that transform a direction from local
+ // Quaternion that transform a direction from local
// space of body 1 into local space of body 2
- Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
- transform1.getOrientation().getMatrix();
+ Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation();
// Initialize the margin (sum of margins of both objects)
decimal margin = shape1->getMargin() + shape2->getMargin();
diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h
index a93734cd..ab17a620 100644
--- a/src/mathematics/Transform.h
+++ b/src/mathematics/Transform.h
@@ -169,8 +169,7 @@ inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const {
// Return the inverse of the transform
inline Transform Transform::getInverse() const {
const Quaternion& invQuaternion = mOrientation.getInverse();
- Matrix3x3 invMatrix = invQuaternion.getMatrix();
- return Transform(invMatrix * (-mPosition), invQuaternion);
+ return Transform(invQuaternion * (-mPosition), invQuaternion);
}
// Return an interpolated transform
@@ -200,7 +199,7 @@ inline Vector3 Transform::operator*(const Vector3& vector) const {
// Operator of multiplication of a transform with another one
inline Transform Transform::operator*(const Transform& transform2) const {
- return Transform(mPosition + mOrientation.getMatrix() * transform2.mPosition,
+ return Transform(mPosition + mOrientation * transform2.mPosition,
mOrientation * transform2.mOrientation);
}
From ebd715d2e080a7547641a1065677e64256ec2f03 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 28 Nov 2017 17:53:50 +0100
Subject: [PATCH 119/133] Add data types
---
src/configuration.h | 9 ++++++---
1 file changed, 6 insertions(+), 3 deletions(-)
diff --git a/src/configuration.h b/src/configuration.h
index 779b45e2..a25c1502 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -54,9 +54,12 @@ using luint = long unsigned int;
using bodyindex = luint;
using bodyindexpair = std::pair<bodyindex, bodyindex>;
-using int8 = int8_t;
-using int16 = int16_t;
-using int32 = int32_t;
+using int8 = std::int8_t;
+using uint8 = std::uint8_t;
+using int16 = std::int16_t;
+using uint16 = std::uint16_t;
+using int32 = std::int32_t;
+using uint32 = std::uint32_t;
// ------------------- Enumerations ------------------- //
From 4cc024b85e9d384f423ec4632975292ac45bdada Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 29 Nov 2017 23:43:55 +0100
Subject: [PATCH 120/133] Rename fields and methods in ContactPoint class
---
src/constraint/ContactPoint.cpp | 8 ++---
src/constraint/ContactPoint.h | 30 +++++++++----------
src/engine/ContactSolver.cpp | 4 +--
.../CollisionDetectionScene.h | 4 +--
testbed/src/SceneDemo.cpp | 2 +-
5 files changed, 24 insertions(+), 24 deletions(-)
diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp
index b99d52f1..37dc34a3 100644
--- a/src/constraint/ContactPoint.cpp
+++ b/src/constraint/ContactPoint.cpp
@@ -34,8 +34,8 @@ using namespace std;
ContactPoint::ContactPoint(const ContactPointInfo* contactInfo)
: mNormal(contactInfo->normal),
mPenetrationDepth(contactInfo->penetrationDepth),
- mLocalPointOnBody1(contactInfo->localPoint1),
- mLocalPointOnBody2(contactInfo->localPoint2),
+ mLocalPointOnShape1(contactInfo->localPoint1),
+ mLocalPointOnShape2(contactInfo->localPoint2),
mIsRestingContact(false), mIsObsolete(false), mNext(nullptr), mPrevious(nullptr) {
assert(mPenetrationDepth > decimal(0.0));
@@ -53,8 +53,8 @@ void ContactPoint::update(const ContactPointInfo* contactInfo) {
mNormal = contactInfo->normal;
mPenetrationDepth = contactInfo->penetrationDepth;
- mLocalPointOnBody1 = contactInfo->localPoint1;
- mLocalPointOnBody2 = contactInfo->localPoint2;
+ mLocalPointOnShape1 = contactInfo->localPoint1;
+ mLocalPointOnShape2 = contactInfo->localPoint2;
mIsObsolete = false;
}
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index 739a9efb..40a3d13c 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -54,11 +54,11 @@ class ContactPoint {
/// Penetration depth
decimal mPenetrationDepth;
- /// Contact point on body 1 in local space of body 1
- Vector3 mLocalPointOnBody1;
+ /// Contact point on proxy shape 1 in local-space of proxy shape 1
+ Vector3 mLocalPointOnShape1;
- /// Contact point on body 2 in local space of body 2
- Vector3 mLocalPointOnBody2;
+ /// Contact point on proxy shape 2 in local-space of proxy shape 2
+ Vector3 mLocalPointOnShape2;
/// True if the contact is a resting contact (exists for more than one time step)
bool mIsRestingContact;
@@ -121,11 +121,11 @@ class ContactPoint {
/// Return the normal vector of the contact
Vector3 getNormal() const;
- /// Return the contact local point on body 1
- Vector3 getLocalPointOnBody1() const;
+ /// Return the contact point on the first proxy shape in the local-space of the proxy shape
+ Vector3 getLocalPointOnShape1() const;
- /// Return the contact local point on body 2
- Vector3 getLocalPointOnBody2() const;
+ /// Return the contact point on the second proxy shape in the local-space of the proxy shape
+ Vector3 getLocalPointOnShape2() const;
/// Return the cached penetration impulse
decimal getPenetrationImpulse() const;
@@ -156,14 +156,14 @@ inline Vector3 ContactPoint::getNormal() const {
return mNormal;
}
-// Return the contact point on body 1
-inline Vector3 ContactPoint::getLocalPointOnBody1() const {
- return mLocalPointOnBody1;
+// Return the contact point on the first proxy shape in the local-space of the proxy shape
+inline Vector3 ContactPoint::getLocalPointOnShape1() const {
+ return mLocalPointOnShape1;
}
-// Return the contact point on body 2
-inline Vector3 ContactPoint::getLocalPointOnBody2() const {
- return mLocalPointOnBody2;
+// Return the contact point on the second proxy shape in the local-space of the proxy shape
+inline Vector3 ContactPoint::getLocalPointOnShape2() const {
+ return mLocalPointOnShape2;
}
// Return the cached penetration impulse
@@ -173,7 +173,7 @@ inline decimal ContactPoint::getPenetrationImpulse() const {
// Return true if the contact point is similar (close enougth) to another given contact point
inline bool ContactPoint::isSimilarWithContactPoint(const ContactPointInfo* localContactPointBody1) const {
- return (localContactPointBody1->localPoint1 - mLocalPointOnBody1).lengthSquare() <= (PERSISTENT_CONTACT_DIST_THRESHOLD *
+ return (localContactPointBody1->localPoint1 - mLocalPointOnShape1).lengthSquare() <= (PERSISTENT_CONTACT_DIST_THRESHOLD *
PERSISTENT_CONTACT_DIST_THRESHOLD);
}
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 1e5b5401..167d37b8 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -157,8 +157,8 @@ void ContactSolver::initializeForIsland(Island* island) {
while (externalContact != nullptr) {
// Get the contact point on the two bodies
- Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnBody1();
- Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnBody2();
+ Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnShape1();
+ Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnShape2();
new (mContactPoints + mNbContactPoints) ContactPointSolver();
mContactPoints[mNbContactPoints].externalContact = externalContact;
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index e5bd8db9..9a90f25c 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -94,13 +94,13 @@ class ContactManager : public rp3d::CollisionCallback {
rp3d::Vector3 normal = contactPoint->getNormal();
openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z);
- rp3d::Vector3 point1 = contactPoint->getLocalPointOnBody1();
+ rp3d::Vector3 point1 = contactPoint->getLocalPointOnShape1();
point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
openglframework::Vector3 position1(point1.x, point1.y, point1.z);
mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red()));
- rp3d::Vector3 point2 = contactPoint->getLocalPointOnBody2();
+ rp3d::Vector3 point2 = contactPoint->getLocalPointOnShape2();
point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
openglframework::Vector3 position2(point2.x, point2.y, point2.z);
mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue()));
diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp
index db5d030e..5ef6fb95 100644
--- a/testbed/src/SceneDemo.cpp
+++ b/testbed/src/SceneDemo.cpp
@@ -435,7 +435,7 @@ std::vector<ContactPoint> SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn
rp3d::ContactPoint* contactPoint = manifold->getContactPoints();
while (contactPoint != nullptr) {
- rp3d::Vector3 point = manifold->getShape1()->getLocalToWorldTransform() * contactPoint->getLocalPointOnBody1();
+ rp3d::Vector3 point = manifold->getShape1()->getLocalToWorldTransform() * contactPoint->getLocalPointOnShape1();
rp3d::Vector3 normalWorld = contactPoint->getNormal();
openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z);
ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red());
From 4f76553c59ae7410afa89f649a7aa089bb34a302 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 4 Dec 2017 22:14:52 +0100
Subject: [PATCH 121/133] Many small optimizations
---
src/collision/CollisionDetection.cpp | 12 +--
src/collision/CollisionDetection.h | 1 -
src/collision/HalfEdgeStructure.h | 18 ++--
src/collision/PolyhedronMesh.cpp | 2 +-
.../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 3 +-
.../narrowphase/CapsuleVsCapsuleAlgorithm.h | 2 +-
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 8 +-
.../CapsuleVsConvexPolyhedronAlgorithm.h | 2 +-
...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 5 +-
...vexPolyhedronVsConvexPolyhedronAlgorithm.h | 2 +-
.../narrowphase/NarrowPhaseAlgorithm.h | 5 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 61 ++++++------
src/collision/narrowphase/SAT/SATAlgorithm.h | 5 +-
.../narrowphase/SphereVsCapsuleAlgorithm.cpp | 3 +-
.../narrowphase/SphereVsCapsuleAlgorithm.h | 2 +-
.../SphereVsConvexPolyhedronAlgorithm.cpp | 5 +-
.../SphereVsConvexPolyhedronAlgorithm.h | 2 +-
.../narrowphase/SphereVsSphereAlgorithm.cpp | 3 +-
.../narrowphase/SphereVsSphereAlgorithm.h | 2 +-
src/collision/shapes/BoxShape.cpp | 7 +-
src/collision/shapes/BoxShape.h | 14 +--
src/collision/shapes/ConvexMeshShape.h | 8 +-
src/collision/shapes/ConvexPolyhedronShape.h | 4 +-
src/collision/shapes/TriangleShape.cpp | 99 ++++++++++---------
src/collision/shapes/TriangleShape.h | 37 +++----
src/mathematics/Quaternion.h | 27 ++++-
src/mathematics/Transform.h | 2 +-
src/mathematics/mathematics_functions.cpp | 31 +++---
src/mathematics/mathematics_functions.h | 5 +-
.../mathematics/TestMathematicsFunctions.h | 62 ++++++------
30 files changed, 235 insertions(+), 204 deletions(-)
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index a2c3344d..c0b4d4e9 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -273,7 +273,7 @@ void CollisionDetection::computeNarrowPhase() {
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true)) {
+ if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mSingleFrameAllocator)) {
// Add the contact points as a potential contact manifold into the pair
currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold();
@@ -593,7 +593,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
+ isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator);
}
}
@@ -688,7 +688,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
+ isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator);
}
}
@@ -767,7 +767,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) {
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
@@ -859,7 +859,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) {
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
@@ -943,7 +943,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) {
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index fc33c00f..d728a95f 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -150,7 +150,6 @@ class CollisionDetection {
/// Process the potential contacts where one collion is a concave shape
void processSmoothMeshContacts(OverlappingPair* pair);
-
public :
diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h
index 6b4e3aa1..ec4fff35 100644
--- a/src/collision/HalfEdgeStructure.h
+++ b/src/collision/HalfEdgeStructure.h
@@ -106,13 +106,13 @@ class HalfEdgeStructure {
uint getNbVertices() const;
/// Return a given face
- Face getFace(uint index) const;
+ const Face& getFace(uint index) const;
/// Return a given edge
- Edge getHalfEdge(uint index) const;
+ const Edge& getHalfEdge(uint index) const;
/// Return a given vertex
- Vertex getVertex(uint index) const;
+ const Vertex& getVertex(uint index) const;
};
@@ -133,33 +133,33 @@ inline void HalfEdgeStructure::addFace(std::vector<uint> faceVertices) {
// Return the number of faces
inline uint HalfEdgeStructure::getNbFaces() const {
- return mFaces.size();
+ return static_cast<uint>(mFaces.size());
}
// Return the number of edges
inline uint HalfEdgeStructure::getNbHalfEdges() const {
- return mEdges.size();
+ return static_cast<uint>(mEdges.size());
}
// Return the number of vertices
inline uint HalfEdgeStructure::getNbVertices() const {
- return mVertices.size();
+ return static_cast<uint>(mVertices.size());
}
// Return a given face
-inline HalfEdgeStructure::Face HalfEdgeStructure::getFace(uint index) const {
+inline const HalfEdgeStructure::Face& HalfEdgeStructure::getFace(uint index) const {
assert(index < mFaces.size());
return mFaces[index];
}
// Return a given edge
-inline HalfEdgeStructure::Edge HalfEdgeStructure::getHalfEdge(uint index) const {
+inline const HalfEdgeStructure::Edge& HalfEdgeStructure::getHalfEdge(uint index) const {
assert(index < mEdges.size());
return mEdges[index];
}
// Return a given vertex
-inline HalfEdgeStructure::Vertex HalfEdgeStructure::getVertex(uint index) const {
+inline const HalfEdgeStructure::Vertex& HalfEdgeStructure::getVertex(uint index) const {
assert(index < mVertices.size());
return mVertices[index];
}
diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp
index b4f82fad..ec8b661d 100644
--- a/src/collision/PolyhedronMesh.cpp
+++ b/src/collision/PolyhedronMesh.cpp
@@ -123,7 +123,7 @@ void PolyhedronMesh::computeFacesNormals() {
// For each face
for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) {
- HalfEdgeStructure::Face face = mHalfEdgeStructure.getFace(f);
+ const HalfEdgeStructure::Face& face = mHalfEdgeStructure.getFace(f);
assert(face.faceVertices.size() >= 3);
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
index b15cb186..bf9eefc2 100755
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
@@ -33,7 +33,8 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two capsules
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
-bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+ Allocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
index 0b19338f..df9a8854 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
@@ -61,7 +61,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two capsules
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
};
}
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index 300ba5f2..f943312b 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -37,11 +37,12 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a capsule and a polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
-bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+ Allocator& memoryAllocator) {
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
- SATAlgorithm satAlgorithm;
+ SATAlgorithm satAlgorithm(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
@@ -85,9 +86,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
// For each face of the polyhedron
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;
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
index c0dd46d5..d7f96d63 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
@@ -61,7 +61,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a capsule and a polyhedron
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
};
}
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
index 507f6043..5cf10b17 100644
--- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
@@ -34,10 +34,11 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two convex polyhedra
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
-bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+ Allocator& memoryAllocator) {
// Run the SAT algorithm to find the separating axis and compute contact point
- SATAlgorithm satAlgorithm;
+ SATAlgorithm satAlgorithm(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
index 1e0c9748..204bc269 100644
--- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
@@ -61,7 +61,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm
ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two convex polyhedra
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
};
}
diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
index 34fa5962..40d3fbc6 100644
--- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h
+++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
@@ -90,8 +90,9 @@ class NarrowPhaseAlgorithm {
/// Deleted assignment operator
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
- /// Compute a contact info if the two bounding volume collide
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0;
+ /// Compute a contact info if the two bounding volumes collide
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+ Allocator& memoryAllocator)=0;
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index f12964cf..7ef99e1f 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -44,6 +44,11 @@ using namespace reactphysics3d;
// Static variables initialization
const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
+// Constructor
+SATAlgorithm::SATAlgorithm(Allocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) {
+
+}
+
// Test collision between a sphere and a convex mesh
bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
@@ -125,7 +130,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd
PROFILE("SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth)", mProfiler);
// Get the face
- HalfEdgeStructure::Face face = polyhedron->getFace(faceIndex);
+ const HalfEdgeStructure::Face& face = polyhedron->getFace(faceIndex);
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(faceIndex);
@@ -200,12 +205,12 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) {
// Get an edge from the polyhedron (convert it into the capsule local-space)
- HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e);
+ const HalfEdgeStructure::Edge& edge = polyhedron->getHalfEdge(e);
const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
- HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
+ const HalfEdgeStructure::Edge& twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex);
const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex);
@@ -336,7 +341,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe
PROFILE("SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth", mProfiler);
// Get the face
- HalfEdgeStructure::Face face = polyhedron->getFace(polyhedronFaceIndex);
+ const HalfEdgeStructure::Face& face = polyhedron->getFace(polyhedronFaceIndex);
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(polyhedronFaceIndex);
@@ -361,7 +366,7 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
PROFILE("SATAlgorithm::computeCapsulePolyhedronFaceContactPoints", mProfiler);
- HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
+ const HalfEdgeStructure::Face& face = polyhedron->getFace(referenceFaceIndex);
// Get the face normal
Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
@@ -375,8 +380,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// For each adjacent edge of the separating face of the polyhedron
do {
- HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex);
- HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
+ const HalfEdgeStructure::Edge& edge = polyhedron->getHalfEdge(edgeIndex);
+ const HalfEdgeStructure::Edge& twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
// Compute the edge vertices and edge direction
Vector3 edgeV1 = polyhedron->getVertexPosition(edge.vertexIndex);
@@ -575,8 +580,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
}
else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
- HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index);
- HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index);
+ const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index);
+ const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index);
Vector3 separatingAxisPolyhedron2Space;
@@ -669,7 +674,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) {
// Get an edge of polyhedron 1
- HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i);
+ const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(i);
const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
@@ -678,7 +683,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) {
// Get an edge of polyhedron 2
- HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j);
+ const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(j);
const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
@@ -816,23 +821,24 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
-(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
// Get the reference face
- HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex);
+ const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex);
// Find the incident face on the other polyhedron (most anti-parallel face)
uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace);
// Get the incident face
- HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
+ const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
- std::vector<Vector3> polygonVertices; // Vertices to clip of the incident face
- std::vector<Vector3> planesNormals; // Normals of the clipping planes
- std::vector<Vector3> planesPoints; // Points on the clipping planes
+ uint nbIncidentFaceVertices = static_cast<uint>(incidentFace.faceVertices.size());
+ List<Vector3> polygonVertices(mMemoryAllocator, nbIncidentFaceVertices); // Vertices to clip of the incident face
+ List<Vector3> planesNormals(mMemoryAllocator, nbIncidentFaceVertices); // Normals of the clipping planes
+ List<Vector3> planesPoints(mMemoryAllocator, nbIncidentFaceVertices); // Points on the clipping planes
// Get all the vertices of the incident face (in the reference local-space)
std::vector<uint>::const_iterator it;
for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) {
const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it);
- polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace);
+ polygonVertices.add(incidentToReferenceTransform * faceVertexIncidentSpace);
}
// Get the reference face clipping planes
@@ -841,10 +847,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
do {
// Get the adjacent edge
- HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
+ const HalfEdgeStructure::Edge& edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
// Get the twin edge
- HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
+ const HalfEdgeStructure::Edge& twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
// Compute the edge vertices and edge direction
Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex);
@@ -855,8 +861,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
// The clipping plane is perpendicular to the edge direction and the reference face normal
Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection);
- planesNormals.push_back(clipPlaneNormal);
- planesPoints.push_back(edgeV1);
+ planesNormals.add(clipPlaneNormal);
+ planesPoints.add(edgeV1);
// Go to the next adjacent edge of the reference face
currentEdgeIndex = edge.nextEdgeIndex;
@@ -867,17 +873,16 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
assert(planesNormals.size() == planesPoints.size());
// Clip the reference faces with the adjacent planes of the reference face
- std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
+ List<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals, mMemoryAllocator);
// We only keep the clipped points that are below the reference face
const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
- std::vector<Vector3>::const_iterator itPoints;
bool contactPointsFound = false;
- for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
+ for (uint i=0; i<clipPolygonVertices.size(); i++) {
// Compute the penetration depth of this contact point (can be different from the minPenetration depth which is
// the maximal penetration depth of any contact point for this separating axis
- decimal penetrationDepth = (referenceFaceVertex - (*itPoints)).dot(axisReferenceSpace);
+ decimal penetrationDepth = (referenceFaceVertex - clipPolygonVertices[i]).dot(axisReferenceSpace);
// If the clip point is bellow the reference face
if (penetrationDepth > decimal(0.0)) {
@@ -887,10 +892,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
Vector3 outWorldNormal = normalWorld;
// Convert the clip incident polyhedron vertex into the incident polyhedron local-space
- Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
+ Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * clipPolygonVertices[i];
// Project the contact point onto the reference face
- Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
+ Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex);
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
@@ -968,7 +973,7 @@ decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const Convex
PROFILE("SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron", mProfiler);
- HalfEdgeStructure::Face face = polyhedron1->getFace(faceIndex);
+ const HalfEdgeStructure::Face& face = polyhedron1->getFace(faceIndex);
// Get the face normal
const Vector3 faceNormal = polyhedron1->getFaceNormal(faceIndex);
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 60f7a091..af49ca09 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -50,6 +50,9 @@ class SATAlgorithm {
/// make sure the contact manifold does not change too much between frames.
static const decimal SAME_SEPARATING_AXIS_BIAS;
+ /// Memory allocator
+ Allocator& mMemoryAllocator;
+
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
@@ -115,7 +118,7 @@ class SATAlgorithm {
// -------------------- Methods -------------------- //
/// Constructor
- SATAlgorithm() = default;
+ SATAlgorithm(Allocator& memoryAllocator);
/// Destructor
~SATAlgorithm() = default;
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
index a5d7a6ac..f5bfc11d 100755
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
@@ -34,7 +34,8 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a capsule
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
-bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+ Allocator& memoryAllocator) {
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
index bb9e9866..b51bba57 100644
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
@@ -61,7 +61,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a capsule
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
};
}
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index 28a565c1..6cc3e578 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -34,7 +34,8 @@ using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
-bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+ Allocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
@@ -69,7 +70,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
- SATAlgorithm satAlgorithm;
+ SATAlgorithm satAlgorithm(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
index 2d9fa801..17055431 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
@@ -61,7 +61,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
};
}
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
index 0b520213..f8820106 100755
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
@@ -30,7 +30,8 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
+bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
+ Allocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
index 1fd885a5..e0741ab8 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
@@ -61,7 +61,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
};
}
diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp
index 6b8c8d6b..37667958 100644
--- a/src/collision/shapes/BoxShape.cpp
+++ b/src/collision/shapes/BoxShape.cpp
@@ -84,10 +84,9 @@ BoxShape::BoxShape(const Vector3& extent)
*/
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
- Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin);
- decimal xSquare = realExtent.x * realExtent.x;
- decimal ySquare = realExtent.y * realExtent.y;
- decimal zSquare = realExtent.z * realExtent.z;
+ decimal xSquare = mExtent.x * mExtent.x;
+ decimal ySquare = mExtent.y * mExtent.y;
+ decimal zSquare = mExtent.z * mExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare));
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index 5643a5c8..d1aa9810 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -101,7 +101,7 @@ class BoxShape : public ConvexPolyhedronShape {
virtual uint getNbFaces() const override;
/// Return a given face of the polyhedron
- virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override;
+ virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override;
/// Return the number of vertices of the polyhedron
virtual uint getNbVertices() const override;
@@ -113,7 +113,7 @@ class BoxShape : public ConvexPolyhedronShape {
virtual uint getNbHalfEdges() const override;
/// Return a given half-edge of the polyhedron
- virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override;
+ virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override;
/// Return the position of a given vertex
virtual Vector3 getVertexPosition(uint vertexIndex) const override;
@@ -130,7 +130,7 @@ class BoxShape : public ConvexPolyhedronShape {
* @return The vector with the three extents of the box shape (in meters)
*/
inline Vector3 BoxShape::getExtent() const {
- return mExtent + Vector3(mMargin, mMargin, mMargin);
+ return mExtent;
}
// Set the scaling vector of the collision shape
@@ -150,7 +150,7 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) {
inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds
- max = mExtent + Vector3(mMargin, mMargin, mMargin);
+ max = mExtent;
// Minimum bounds
min = -max;
@@ -161,7 +161,7 @@ inline size_t BoxShape::getSizeInBytes() const {
return sizeof(BoxShape);
}
-// Return a local support point in a given direction without the objec margin
+// Return a local support point in a given direction without the object margin
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
return Vector3(direction.x < decimal(0.0) ? -mExtent.x : mExtent.x,
@@ -182,7 +182,7 @@ inline uint BoxShape::getNbFaces() const {
}
// Return a given face of the polyhedron
-inline HalfEdgeStructure::Face BoxShape::getFace(uint faceIndex) const {
+inline const HalfEdgeStructure::Face& BoxShape::getFace(uint faceIndex) const {
assert(faceIndex < mHalfEdgeStructure.getNbFaces());
return mHalfEdgeStructure.getFace(faceIndex);
}
@@ -243,7 +243,7 @@ inline uint BoxShape::getNbHalfEdges() const {
}
// Return a given half-edge of the polyhedron
-inline HalfEdgeStructure::Edge BoxShape::getHalfEdge(uint edgeIndex) const {
+inline const HalfEdgeStructure::Edge& BoxShape::getHalfEdge(uint edgeIndex) const {
assert(edgeIndex < getNbHalfEdges());
return mHalfEdgeStructure.getHalfEdge(edgeIndex);
}
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index 71077af2..0ce696af 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -111,7 +111,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
virtual uint getNbFaces() const override;
/// Return a given face of the polyhedron
- virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override;
+ virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override;
/// Return the number of vertices of the polyhedron
virtual uint getNbVertices() const override;
@@ -123,7 +123,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
virtual uint getNbHalfEdges() const override;
/// Return a given half-edge of the polyhedron
- virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override;
+ virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override;
/// Return the position of a given vertex
virtual Vector3 getVertexPosition(uint vertexIndex) const override;
@@ -191,7 +191,7 @@ inline uint ConvexMeshShape::getNbFaces() const {
}
// Return a given face of the polyhedron
-inline HalfEdgeStructure::Face ConvexMeshShape::getFace(uint faceIndex) const {
+inline const HalfEdgeStructure::Face& ConvexMeshShape::getFace(uint faceIndex) const {
assert(faceIndex < getNbFaces());
return mPolyhedronMesh->getHalfEdgeStructure().getFace(faceIndex);
}
@@ -213,7 +213,7 @@ inline uint ConvexMeshShape::getNbHalfEdges() const {
}
// Return a given half-edge of the polyhedron
-inline HalfEdgeStructure::Edge ConvexMeshShape::getHalfEdge(uint edgeIndex) const {
+inline const HalfEdgeStructure::Edge& ConvexMeshShape::getHalfEdge(uint edgeIndex) const {
assert(edgeIndex < getNbHalfEdges());
return mPolyhedronMesh->getHalfEdgeStructure().getHalfEdge(edgeIndex);
}
diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h
index 96707d69..1d944cb8 100644
--- a/src/collision/shapes/ConvexPolyhedronShape.h
+++ b/src/collision/shapes/ConvexPolyhedronShape.h
@@ -62,7 +62,7 @@ class ConvexPolyhedronShape : public ConvexShape {
virtual uint getNbFaces() const=0;
/// Return a given face of the polyhedron
- virtual HalfEdgeStructure::Face getFace(uint faceIndex) const=0;
+ virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const=0;
/// Return the number of vertices of the polyhedron
virtual uint getNbVertices() const=0;
@@ -80,7 +80,7 @@ class ConvexPolyhedronShape : public ConvexShape {
virtual uint getNbHalfEdges() const=0;
/// Return a given half-edge of the polyhedron
- virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const=0;
+ virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const=0;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const override;
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index 77d0929c..e43cc674 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -58,6 +58,57 @@ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNor
mVerticesNormals[1] = verticesNormals[1];
mVerticesNormals[2] = verticesNormals[2];
+ // Faces
+ for (uint i=0; i<2; i++) {
+ mFaces[i].faceVertices.push_back(0);
+ mFaces[i].faceVertices.push_back(1);
+ mFaces[i].faceVertices.push_back(2);
+ mFaces[i].edgeIndex = i;
+ }
+
+ // Edges
+ for (uint i=0; i<6; i++) {
+ switch(i) {
+ case 0:
+ mEdges[0].vertexIndex = 0;
+ mEdges[0].twinEdgeIndex = 1;
+ mEdges[0].faceIndex = 0;
+ mEdges[0].nextEdgeIndex = 2;
+ break;
+ case 1:
+ mEdges[1].vertexIndex = 1;
+ mEdges[1].twinEdgeIndex = 0;
+ mEdges[1].faceIndex = 1;
+ mEdges[1].nextEdgeIndex = 5;
+ break;
+ case 2:
+ mEdges[2].vertexIndex = 1;
+ mEdges[2].twinEdgeIndex = 3;
+ mEdges[2].faceIndex = 0;
+ mEdges[2].nextEdgeIndex = 4;
+ break;
+ case 3:
+ mEdges[3].vertexIndex = 2;
+ mEdges[3].twinEdgeIndex = 2;
+ mEdges[3].faceIndex = 1;
+ mEdges[3].nextEdgeIndex = 1;
+ break;
+ case 4:
+ mEdges[4].vertexIndex = 2;
+ mEdges[4].twinEdgeIndex = 5;
+ mEdges[4].faceIndex = 0;
+ mEdges[4].nextEdgeIndex = 0;
+ break;
+ case 5:
+ mEdges[5].vertexIndex = 0;
+ mEdges[5].twinEdgeIndex = 4;
+ mEdges[5].faceIndex = 1;
+ mEdges[5].nextEdgeIndex = 3;
+ break;
+ }
+ }
+
+
mRaycastTestType = TriangleRaycastSide::FRONT;
mId = shapeId;
@@ -227,51 +278,3 @@ bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
return true;
}
-// Return a given half-edge of the polyhedron
-HalfEdgeStructure::Edge TriangleShape::getHalfEdge(uint edgeIndex) const {
- assert(edgeIndex < getNbHalfEdges());
-
- HalfEdgeStructure::Edge edge;
-
- switch(edgeIndex) {
- case 0:
- edge.vertexIndex = 0;
- edge.twinEdgeIndex = 1;
- edge.faceIndex = 0;
- edge.nextEdgeIndex = 2;
- break;
- case 1:
- edge.vertexIndex = 1;
- edge.twinEdgeIndex = 0;
- edge.faceIndex = 1;
- edge.nextEdgeIndex = 5;
- break;
- case 2:
- edge.vertexIndex = 1;
- edge.twinEdgeIndex = 3;
- edge.faceIndex = 0;
- edge.nextEdgeIndex = 4;
- break;
- case 3:
- edge.vertexIndex = 2;
- edge.twinEdgeIndex = 2;
- edge.faceIndex = 1;
- edge.nextEdgeIndex = 1;
- break;
- case 4:
- edge.vertexIndex = 2;
- edge.twinEdgeIndex = 5;
- edge.faceIndex = 0;
- edge.nextEdgeIndex = 0;
- break;
- case 5:
- edge.vertexIndex = 0;
- edge.twinEdgeIndex = 4;
- edge.faceIndex = 1;
- edge.nextEdgeIndex = 3;
- break;
- }
-
- return edge;
-
-}
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index 60ec216b..a4ce7726 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -72,6 +72,12 @@ class TriangleShape : public ConvexPolyhedronShape {
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide mRaycastTestType;
+ /// Faces information for the two faces of the triangle
+ HalfEdgeStructure::Face mFaces[2];
+
+ /// Edges information for the six edges of the triangle
+ HalfEdgeStructure::Edge mEdges[6];
+
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
@@ -137,7 +143,7 @@ class TriangleShape : public ConvexPolyhedronShape {
virtual uint getNbFaces() const override;
/// Return a given face of the polyhedron
- virtual HalfEdgeStructure::Face getFace(uint faceIndex) const override;
+ virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override;
/// Return the number of vertices of the polyhedron
virtual uint getNbVertices() const override;
@@ -155,7 +161,7 @@ class TriangleShape : public ConvexPolyhedronShape {
virtual uint getNbHalfEdges() const override;
/// Return a given half-edge of the polyhedron
- virtual HalfEdgeStructure::Edge getHalfEdge(uint edgeIndex) const override;
+ virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override;
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const override;
@@ -252,26 +258,9 @@ inline uint TriangleShape::getNbFaces() const {
}
// Return a given face of the polyhedron
-inline HalfEdgeStructure::Face TriangleShape::getFace(uint faceIndex) const {
+inline const HalfEdgeStructure::Face& TriangleShape::getFace(uint faceIndex) const {
assert(faceIndex < 2);
-
- HalfEdgeStructure::Face face;
-
- if (faceIndex == 0) {
- face.faceVertices.push_back(0);
- face.faceVertices.push_back(1);
- face.faceVertices.push_back(2);
- face.edgeIndex = 0;
-
- }
- else {
- face.faceVertices.push_back(0);
- face.faceVertices.push_back(2);
- face.faceVertices.push_back(1);
- face.edgeIndex = 1;
- }
-
- return face;
+ return mFaces[faceIndex];
}
// Return the number of vertices of the polyhedron
@@ -292,6 +281,12 @@ inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) cons
return vertex;
}
+// Return a given half-edge of the polyhedron
+inline const HalfEdgeStructure::Edge& TriangleShape::getHalfEdge(uint edgeIndex) const {
+ assert(edgeIndex < getNbHalfEdges());
+ return mEdges[edgeIndex];
+}
+
// Return the position of a given vertex
inline Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const {
assert(vertexIndex < 3);
diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h
index 74415eb8..d5e771e2 100644
--- a/src/mathematics/Quaternion.h
+++ b/src/mathematics/Quaternion.h
@@ -306,16 +306,35 @@ inline Quaternion Quaternion::operator*(decimal nb) const {
// Overloaded operator for the multiplication of two quaternions
inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
+
+ /* The followin code is equivalent to this
return Quaternion(w * quaternion.w - getVectorV().dot(quaternion.getVectorV()),
- w * quaternion.getVectorV() + quaternion.w * getVectorV() +
- getVectorV().cross(quaternion.getVectorV()));
+ w * quaternion.getVectorV() + quaternion.w * getVectorV() +
+ getVectorV().cross(quaternion.getVectorV()));
+ */
+
+ return Quaternion(w * quaternion.x + quaternion.w * x + y * quaternion.z - z * quaternion.y,
+ w * quaternion.y + quaternion.w * y + z * quaternion.x - x * quaternion.z,
+ w * quaternion.z + quaternion.w * z + x * quaternion.y - y * quaternion.x,
+ w * quaternion.w - x * quaternion.x - y * quaternion.y - z * quaternion.z);
}
// Overloaded operator for the multiplication with a vector.
/// This methods rotates a point given the rotation of a quaternion.
inline Vector3 Quaternion::operator*(const Vector3& point) const {
- Quaternion p(point.x, point.y, point.z, 0.0);
- return (((*this) * p) * getConjugate()).getVectorV();
+
+ /* The following code is equivalent to this
+ * Quaternion p(point.x, point.y, point.z, 0.0);
+ * return (((*this) * p) * getConjugate()).getVectorV();
+ */
+
+ const decimal prodX = w * point.x + y * point.z - z * point.y;
+ const decimal prodY = w * point.y + z * point.x - x * point.z;
+ const decimal prodZ = w * point.z + x * point.y - y * point.x;
+ const decimal prodW = -x * point.x - y * point.y - z * point.z;
+ return Vector3(w * prodX - prodY * z + prodZ * y - prodW * x,
+ w * prodY - prodZ * x + prodX * z - prodW * y,
+ w * prodZ - prodX * y + prodY * x - prodW * z);
}
// Overloaded operator for the assignment
diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h
index ab17a620..52ee663e 100644
--- a/src/mathematics/Transform.h
+++ b/src/mathematics/Transform.h
@@ -194,7 +194,7 @@ inline Transform Transform::identity() {
// Return the transformed vector
inline Vector3 Transform::operator*(const Vector3& vector) const {
- return (mOrientation.getMatrix() * vector) + mPosition;
+ return (mOrientation * vector) + mPosition;
}
// Operator of multiplication of a transform with another one
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 37335abe..e7c9f371 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -198,10 +198,7 @@ decimal reactphysics3d::computePlaneSegmentIntersection(const Vector3& segA, con
const decimal parallelEpsilon = decimal(0.0001);
decimal t = decimal(-1);
- // Segment AB
- const Vector3 ab = segB - segA;
-
- decimal nDotAB = planeNormal.dot(ab);
+ decimal nDotAB = planeNormal.dot(segB - segA);
// If the segment is not parallel to the plane
if (std::abs(nDotAB) > parallelEpsilon) {
@@ -297,23 +294,27 @@ std::vector<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA,
// Clip a polygon against multiple planes and return the clipped polygon vertices
// This method implements the Sutherland–Hodgman clipping algorithm
-std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
- const std::vector<Vector3>& planesNormals) {
+List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,
+ const List<Vector3>& planesNormals, Allocator& allocator) {
assert(planesPoints.size() == planesNormals.size());
- std::vector<Vector3> inputVertices(polygonVertices);
- std::vector<Vector3> outputVertices;
+ uint nbMaxElements = polygonVertices.size() + planesPoints.size();
+ List<Vector3> inputVertices(allocator, nbMaxElements);
+ List<Vector3> outputVertices(allocator, nbMaxElements);
+
+ inputVertices.addRange(polygonVertices);
// For each clipping plane
for (uint p=0; p<planesPoints.size(); p++) {
outputVertices.clear();
- uint vStart = inputVertices.size() - 1;
+ uint nbInputVertices = inputVertices.size();
+ uint vStart = nbInputVertices - 1;
// For each edge of the polygon
- for (uint vEnd = 0; vEnd<inputVertices.size(); vEnd++) {
+ for (uint vEnd = 0; vEnd<nbInputVertices; vEnd++) {
Vector3& v1 = inputVertices[vStart];
Vector3& v2 = inputVertices[vEnd];
@@ -331,15 +332,15 @@ std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vec
decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]);
if (t >= decimal(0) && t <= decimal(1.0)) {
- outputVertices.push_back(v1 + t * (v2 - v1));
+ outputVertices.add(v1 + t * (v2 - v1));
}
else {
- outputVertices.push_back(v2);
+ outputVertices.add(v2);
}
}
// Add the second vertex
- outputVertices.push_back(v2);
+ outputVertices.add(v2);
}
else { // If the second vertex is behind the clipping plane
@@ -350,10 +351,10 @@ std::vector<Vector3> reactphysics3d::clipPolygonWithPlanes(const std::vector<Vec
decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]);
if (t >= decimal(0.0) && t <= decimal(1.0)) {
- outputVertices.push_back(v1 + t * (v2 - v1));
+ outputVertices.add(v1 + t * (v2 - v1));
}
else {
- outputVertices.push_back(v1);
+ outputVertices.add(v1);
}
}
}
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index 9f062826..ff6ac6f4 100755
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -33,6 +33,7 @@
#include <cassert>
#include <cmath>
#include <vector>
+#include "containers/List.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -116,8 +117,8 @@ std::vector<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& s
const std::vector<Vector3>& planesNormals);
/// Clip a polygon against multiple planes and return the clipped polygon vertices
-std::vector<Vector3> clipPolygonWithPlanes(const std::vector<Vector3>& polygonVertices, const std::vector<Vector3>& planesPoints,
- const std::vector<Vector3>& planesNormals);
+List<Vector3> clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,
+ const List<Vector3>& planesNormals, Allocator& allocator);
/// Project a point onto a plane that is given by a point and its unit length normal
Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint);
diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h
index b45415ee..793a79fb 100644
--- a/test/tests/mathematics/TestMathematicsFunctions.h
+++ b/test/tests/mathematics/TestMathematicsFunctions.h
@@ -27,6 +27,8 @@
#define TEST_MATHEMATICS_FUNCTIONS_H
// Libraries
+#include "containers/List.h"
+#include "memory/DefaultAllocator.h"
/// Reactphysics3D namespace
namespace reactphysics3d {
@@ -41,7 +43,7 @@ class TestMathematicsFunctions : public Test {
// ---------- Atributes ---------- //
-
+ DefaultAllocator mAllocator;
public :
@@ -223,37 +225,37 @@ class TestMathematicsFunctions : public Test {
test(clipSegmentVertices.size() == 0);
// Test clipPolygonWithPlanes()
- std::vector<Vector3> polygonVertices;
- polygonVertices.push_back(Vector3(-4, 2, 0));
- polygonVertices.push_back(Vector3(7, 2, 0));
- polygonVertices.push_back(Vector3(7, 4, 0));
- polygonVertices.push_back(Vector3(-4, 4, 0));
+ List<Vector3> polygonVertices(mAllocator);
+ polygonVertices.add(Vector3(-4, 2, 0));
+ polygonVertices.add(Vector3(7, 2, 0));
+ polygonVertices.add(Vector3(7, 4, 0));
+ polygonVertices.add(Vector3(-4, 4, 0));
- planesNormals.clear();
- planesPoints.clear();
- planesNormals.push_back(Vector3(1, 0, 0));
- planesPoints.push_back(Vector3(0, 0, 0));
- planesNormals.push_back(Vector3(0, 1, 0));
- planesPoints.push_back(Vector3(0, 0, 0));
- planesNormals.push_back(Vector3(-1, 0, 0));
- planesPoints.push_back(Vector3(10, 0, 0));
- planesNormals.push_back(Vector3(0, -1, 0));
- planesPoints.push_back(Vector3(10, 5, 0));
+ List<Vector3> polygonPlanesNormals(mAllocator);
+ List<Vector3> polygonPlanesPoints(mAllocator);
+ polygonPlanesNormals.add(Vector3(1, 0, 0));
+ polygonPlanesPoints.add(Vector3(0, 0, 0));
+ polygonPlanesNormals.add(Vector3(0, 1, 0));
+ polygonPlanesPoints.add(Vector3(0, 0, 0));
+ polygonPlanesNormals.add(Vector3(-1, 0, 0));
+ polygonPlanesPoints.add(Vector3(10, 0, 0));
+ polygonPlanesNormals.add(Vector3(0, -1, 0));
+ polygonPlanesPoints.add(Vector3(10, 5, 0));
- clipSegmentVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
- test(clipSegmentVertices.size() == 4);
- test(approxEqual(clipSegmentVertices[0].x, 0, 0.000001));
- test(approxEqual(clipSegmentVertices[0].y, 2, 0.000001));
- test(approxEqual(clipSegmentVertices[0].z, 0, 0.000001));
- test(approxEqual(clipSegmentVertices[1].x, 7, 0.000001));
- test(approxEqual(clipSegmentVertices[1].y, 2, 0.000001));
- test(approxEqual(clipSegmentVertices[1].z, 0, 0.000001));
- test(approxEqual(clipSegmentVertices[2].x, 7, 0.000001));
- test(approxEqual(clipSegmentVertices[2].y, 4, 0.000001));
- test(approxEqual(clipSegmentVertices[2].z, 0, 0.000001));
- test(approxEqual(clipSegmentVertices[3].x, 0, 0.000001));
- test(approxEqual(clipSegmentVertices[3].y, 4, 0.000001));
- test(approxEqual(clipSegmentVertices[3].z, 0, 0.000001));
+ List<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, polygonPlanesPoints, polygonPlanesNormals, mAllocator);
+ test(clipPolygonVertices.size() == 4);
+ test(approxEqual(clipPolygonVertices[0].x, 0, 0.000001));
+ test(approxEqual(clipPolygonVertices[0].y, 2, 0.000001));
+ test(approxEqual(clipPolygonVertices[0].z, 0, 0.000001));
+ test(approxEqual(clipPolygonVertices[1].x, 7, 0.000001));
+ test(approxEqual(clipPolygonVertices[1].y, 2, 0.000001));
+ test(approxEqual(clipPolygonVertices[1].z, 0, 0.000001));
+ test(approxEqual(clipPolygonVertices[2].x, 7, 0.000001));
+ test(approxEqual(clipPolygonVertices[2].y, 4, 0.000001));
+ test(approxEqual(clipPolygonVertices[2].z, 0, 0.000001));
+ test(approxEqual(clipPolygonVertices[3].x, 0, 0.000001));
+ test(approxEqual(clipPolygonVertices[3].y, 4, 0.000001));
+ test(approxEqual(clipPolygonVertices[3].z, 0, 0.000001));
}
From 9d761291d620efdf23749d0ed2d82daee054c45a Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 6 Dec 2017 21:55:50 +0100
Subject: [PATCH 122/133] Small optimizations
---
CMakeLists.txt | 2 ++
.../narrowphase/SAT/SATAlgorithm.cpp | 24 +--------------
src/collision/narrowphase/SAT/SATAlgorithm.h | 3 --
.../shapes/ConvexPolyhedronShape.cpp | 22 ++++++++++++++
src/collision/shapes/ConvexPolyhedronShape.h | 5 ++++
src/constraint/ContactPoint.h | 8 ++---
src/mathematics/mathematics_functions.cpp | 29 ++++++++++---------
7 files changed, 50 insertions(+), 43 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c4cfde6e..e7e2df87 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -192,8 +192,10 @@ SET (REACTPHYSICS3D_SOURCES
"src/memory/PoolAllocator.cpp"
"src/memory/SingleFrameAllocator.h"
"src/memory/SingleFrameAllocator.cpp"
+ "src/memory/DefaultAllocator.h"
"src/containers/Stack.h"
"src/containers/LinkedList.h"
+ "src/containers/List.h"
)
# Create the library
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 7ef99e1f..5e40f45f 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -824,7 +824,7 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex);
// Find the incident face on the other polyhedron (most anti-parallel face)
- uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace);
+ uint incidentFaceIndex = incidentPolyhedron->findMostAntiParallelFace(axisIncidentSpace);
// Get the incident face
const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
@@ -914,28 +914,6 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
return contactPointsFound;
}
-// Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector
-// This is used to find the incident face on a polyhedron of a given reference face of another polyhedron
-uint SATAlgorithm::findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const {
-
- PROFILE("SATAlgorithm::findMostAntiParallelFaceOnPolyhedron", mProfiler);
-
- decimal minDotProduct = DECIMAL_LARGEST;
- uint mostAntiParallelFace = 0;
-
- // For each face of the polyhedron
- for (uint i=0; i < polyhedron->getNbFaces(); i++) {
-
- // Get the face normal
- decimal dotProduct = polyhedron->getFaceNormal(i).dot(direction);
- if (dotProduct < minDotProduct) {
- minDotProduct = dotProduct;
- mostAntiParallelFace = i;
- }
- }
-
- return mostAntiParallelFace;
-}
// Compute and return the distance between the two edges in the direction of the candidate separating axis
decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid,
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index af49ca09..52884f8e 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -72,9 +72,6 @@ class SATAlgorithm {
const Vector3& c, const Vector3& d,
const Vector3& bCrossA, const Vector3& dCrossC) const;
- // Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector
- uint findMostAntiParallelFaceOnPolyhedron(const ConvexPolyhedronShape* polyhedron, const Vector3& direction) const;
-
/// Compute and return the distance between the two edges in the direction of the candidate separating axis
decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid,
const Vector3& edge1Direction, const Vector3& edge2Direction,
diff --git a/src/collision/shapes/ConvexPolyhedronShape.cpp b/src/collision/shapes/ConvexPolyhedronShape.cpp
index ab93584d..24f80de6 100644
--- a/src/collision/shapes/ConvexPolyhedronShape.cpp
+++ b/src/collision/shapes/ConvexPolyhedronShape.cpp
@@ -35,3 +35,25 @@ ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name)
: ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON) {
}
+
+// Find and return the index of the polyhedron face with the most anti-parallel face
+// normal given a direction vector. This is used to find the incident face on
+// a polyhedron of a given reference face of another polyhedron
+uint ConvexPolyhedronShape::findMostAntiParallelFace(const Vector3& direction) const {
+
+ decimal minDotProduct = DECIMAL_LARGEST;
+ uint mostAntiParallelFace = 0;
+
+ // For each face of the polyhedron
+ for (uint i=0; i < getNbFaces(); i++) {
+
+ // Get the face normal
+ decimal dotProduct = getFaceNormal(i).dot(direction);
+ if (dotProduct < minDotProduct) {
+ minDotProduct = dotProduct;
+ mostAntiParallelFace = i;
+ }
+ }
+
+ return mostAntiParallelFace;
+}
diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h
index 1d944cb8..50c6df89 100644
--- a/src/collision/shapes/ConvexPolyhedronShape.h
+++ b/src/collision/shapes/ConvexPolyhedronShape.h
@@ -87,6 +87,10 @@ class ConvexPolyhedronShape : public ConvexShape {
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const=0;
+
+ /// Find and return the index of the polyhedron face with the most anti-parallel face
+ /// normal given a direction vector
+ uint findMostAntiParallelFace(const Vector3& direction) const;
};
// Return true if the collision shape is a polyhedron
@@ -94,6 +98,7 @@ inline bool ConvexPolyhedronShape::isPolyhedron() const {
return true;
}
+
}
#endif
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index 40a3d13c..d78659f8 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -122,10 +122,10 @@ class ContactPoint {
Vector3 getNormal() const;
/// Return the contact point on the first proxy shape in the local-space of the proxy shape
- Vector3 getLocalPointOnShape1() const;
+ const Vector3& getLocalPointOnShape1() const;
/// Return the contact point on the second proxy shape in the local-space of the proxy shape
- Vector3 getLocalPointOnShape2() const;
+ const Vector3& getLocalPointOnShape2() const;
/// Return the cached penetration impulse
decimal getPenetrationImpulse() const;
@@ -157,12 +157,12 @@ inline Vector3 ContactPoint::getNormal() const {
}
// Return the contact point on the first proxy shape in the local-space of the proxy shape
-inline Vector3 ContactPoint::getLocalPointOnShape1() const {
+inline const Vector3& ContactPoint::getLocalPointOnShape1() const {
return mLocalPointOnShape1;
}
// Return the contact point on the second proxy shape in the local-space of the proxy shape
-inline Vector3 ContactPoint::getLocalPointOnShape2() const {
+inline const Vector3& ContactPoint::getLocalPointOnShape2() const {
return mLocalPointOnShape2;
}
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index e7c9f371..b82c9fbd 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -300,24 +300,26 @@ List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygon
assert(planesPoints.size() == planesNormals.size());
uint nbMaxElements = polygonVertices.size() + planesPoints.size();
- List<Vector3> inputVertices(allocator, nbMaxElements);
- List<Vector3> outputVertices(allocator, nbMaxElements);
+ List<Vector3> list1(allocator, nbMaxElements);
+ List<Vector3> list2(allocator, nbMaxElements);
+ List<decimal> dotProducts(allocator, nbMaxElements * 2);
- inputVertices.addRange(polygonVertices);
+ const List<Vector3>* inputVertices = &polygonVertices;
+ List<Vector3>* outputVertices = &list2;
// For each clipping plane
for (uint p=0; p<planesPoints.size(); p++) {
- outputVertices.clear();
+ outputVertices->clear();
- uint nbInputVertices = inputVertices.size();
+ uint nbInputVertices = inputVertices->size();
uint vStart = nbInputVertices - 1;
// For each edge of the polygon
for (uint vEnd = 0; vEnd<nbInputVertices; vEnd++) {
- Vector3& v1 = inputVertices[vStart];
- Vector3& v2 = inputVertices[vEnd];
+ const Vector3& v1 = (*inputVertices)[vStart];
+ const Vector3& v2 = (*inputVertices)[vEnd];
decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
@@ -332,15 +334,15 @@ List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygon
decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]);
if (t >= decimal(0) && t <= decimal(1.0)) {
- outputVertices.add(v1 + t * (v2 - v1));
+ outputVertices->add(v1 + t * (v2 - v1));
}
else {
- outputVertices.add(v2);
+ outputVertices->add(v2);
}
}
// Add the second vertex
- outputVertices.add(v2);
+ outputVertices->add(v2);
}
else { // If the second vertex is behind the clipping plane
@@ -351,10 +353,10 @@ List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygon
decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]);
if (t >= decimal(0.0) && t <= decimal(1.0)) {
- outputVertices.add(v1 + t * (v2 - v1));
+ outputVertices->add(v1 + t * (v2 - v1));
}
else {
- outputVertices.add(v1);
+ outputVertices->add(v1);
}
}
}
@@ -363,9 +365,10 @@ List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygon
}
inputVertices = outputVertices;
+ outputVertices = p % 2 == 0 ? &list1 : &list2;
}
- return outputVertices;
+ return *outputVertices;
}
// Project a point onto a plane that is given by a point and its unit length normal
From cf42e9f04c35cdacb3b7ae54881812d8bd187d7d Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 12 Dec 2017 07:29:29 +0100
Subject: [PATCH 123/133] Optimizations in contact solver
---
src/engine/ContactSolver.cpp | 300 +++++++++++++++++++++++++++--------
1 file changed, 233 insertions(+), 67 deletions(-)
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 167d37b8..7bd74e8a 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -163,22 +163,48 @@ void ContactSolver::initializeForIsland(Island* island) {
new (mContactPoints + mNbContactPoints) ContactPointSolver();
mContactPoints[mNbContactPoints].externalContact = externalContact;
mContactPoints[mNbContactPoints].normal = externalContact->getNormal();
- mContactPoints[mNbContactPoints].r1 = p1 - x1;
- mContactPoints[mNbContactPoints].r2 = p2 - x2;
+ mContactPoints[mNbContactPoints].r1.x = p1.x - x1.x;
+ mContactPoints[mNbContactPoints].r1.y = p1.y - x1.y;
+ mContactPoints[mNbContactPoints].r1.z = p1.z - x1.z;
+ mContactPoints[mNbContactPoints].r2.x = p2.x - x2.x;
+ mContactPoints[mNbContactPoints].r2.y = p2.y - x2.y;
+ mContactPoints[mNbContactPoints].r2.z = p2.z - x2.z;
mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth();
mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact();
externalContact->setIsRestingContact(true);
mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse();
mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0;
- mContactConstraints[mNbContactManifolds].frictionPointBody1 += p1;
- mContactConstraints[mNbContactManifolds].frictionPointBody2 += p2;
+ mContactConstraints[mNbContactManifolds].frictionPointBody1.x += p1.x;
+ mContactConstraints[mNbContactManifolds].frictionPointBody1.y += p1.y;
+ mContactConstraints[mNbContactManifolds].frictionPointBody1.z += p1.z;
+ mContactConstraints[mNbContactManifolds].frictionPointBody2.x += p2.x;
+ mContactConstraints[mNbContactManifolds].frictionPointBody2.y += p2.y;
+ mContactConstraints[mNbContactManifolds].frictionPointBody2.z += p2.z;
// Compute the velocity difference
- Vector3 deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1);
+ //deltaV = v2 + w2.cross(mContactPoints[mNbContactPoints].r2) - v1 - w1.cross(mContactPoints[mNbContactPoints].r1);
+ Vector3 deltaV(v2.x + w2.y * mContactPoints[mNbContactPoints].r2.z - w2.z * mContactPoints[mNbContactPoints].r2.y
+ - v1.x - w1.y * mContactPoints[mNbContactPoints].r1.z - w1.z * mContactPoints[mNbContactPoints].r1.y,
+ v2.y + w2.z * mContactPoints[mNbContactPoints].r2.x - w2.x * mContactPoints[mNbContactPoints].r2.z
+ - v1.y - w1.z * mContactPoints[mNbContactPoints].r1.x - w1.x * mContactPoints[mNbContactPoints].r1.z,
+ v2.z + w2.x * mContactPoints[mNbContactPoints].r2.y - w2.y * mContactPoints[mNbContactPoints].r2.x
+ - v1.z - w1.x * mContactPoints[mNbContactPoints].r1.y - w1.y * mContactPoints[mNbContactPoints].r1.x);
- Vector3 r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
- Vector3 r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
+ // r1CrossN = mContactPoints[mNbContactPoints].r1.cross(mContactPoints[mNbContactPoints].normal);
+ Vector3 r1CrossN(mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.z -
+ mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.y,
+ mContactPoints[mNbContactPoints].r1.z * mContactPoints[mNbContactPoints].normal.x -
+ mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.z,
+ mContactPoints[mNbContactPoints].r1.x * mContactPoints[mNbContactPoints].normal.y -
+ mContactPoints[mNbContactPoints].r1.y * mContactPoints[mNbContactPoints].normal.x);
+ // r2CrossN = mContactPoints[mNbContactPoints].r2.cross(mContactPoints[mNbContactPoints].normal);
+ Vector3 r2CrossN(mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.z -
+ mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.y,
+ mContactPoints[mNbContactPoints].r2.z * mContactPoints[mNbContactPoints].normal.x -
+ mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.z,
+ mContactPoints[mNbContactPoints].r2.x * mContactPoints[mNbContactPoints].normal.y -
+ mContactPoints[mNbContactPoints].r2.y * mContactPoints[mNbContactPoints].normal.x);
mContactPoints[mNbContactPoints].i1TimesR1CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 * r1CrossN;
mContactPoints[mNbContactPoints].i2TimesR2CrossN = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 * r2CrossN;
@@ -194,13 +220,18 @@ void ContactSolver::initializeForIsland(Island* island) {
// at the beginning of the contact. Note that if it is a resting contact (normal
// velocity bellow a given threshold), we do not add a restitution velocity bias
mContactPoints[mNbContactPoints].restitutionBias = 0.0;
- decimal deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal);
+ // deltaVDotN = deltaV.dot(mContactPoints[mNbContactPoints].normal);
+ decimal deltaVDotN = deltaV.x * mContactPoints[mNbContactPoints].normal.x +
+ deltaV.y * mContactPoints[mNbContactPoints].normal.y +
+ deltaV.z * mContactPoints[mNbContactPoints].normal.z;
const decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2);
if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
mContactPoints[mNbContactPoints].restitutionBias = restitutionFactor * deltaVDotN;
}
- mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal;
+ mContactConstraints[mNbContactManifolds].normal.x += mContactPoints[mNbContactPoints].normal.x;
+ mContactConstraints[mNbContactManifolds].normal.y += mContactPoints[mNbContactPoints].normal.y;
+ mContactConstraints[mNbContactManifolds].normal.z += mContactPoints[mNbContactPoints].normal.z;
mNbContactPoints++;
@@ -209,8 +240,12 @@ void ContactSolver::initializeForIsland(Island* island) {
mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
mContactConstraints[mNbContactManifolds].frictionPointBody2 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
- mContactConstraints[mNbContactManifolds].r1Friction = mContactConstraints[mNbContactManifolds].frictionPointBody1 - x1;
- mContactConstraints[mNbContactManifolds].r2Friction = mContactConstraints[mNbContactManifolds].frictionPointBody2 - x2;
+ mContactConstraints[mNbContactManifolds].r1Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody1.x - x1.x;
+ mContactConstraints[mNbContactManifolds].r1Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody1.y - x1.y;
+ mContactConstraints[mNbContactManifolds].r1Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody1.z - x1.z;
+ mContactConstraints[mNbContactManifolds].r2Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody2.x - x2.x;
+ mContactConstraints[mNbContactManifolds].r2Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody2.y - x2.y;
+ mContactConstraints[mNbContactManifolds].r2Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody2.z - x2.z;
mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1();
mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2();
@@ -230,8 +265,20 @@ void ContactSolver::initializeForIsland(Island* island) {
mContactConstraints[mNbContactManifolds].normal.normalize();
- Vector3 deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) -
- v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction);
+ // deltaVFrictionPoint = v2 + w2.cross(mContactConstraints[mNbContactManifolds].r2Friction) -
+ // v1 - w1.cross(mContactConstraints[mNbContactManifolds].r1Friction);
+ Vector3 deltaVFrictionPoint(v2.x + w2.y * mContactConstraints[mNbContactManifolds].r2Friction.z -
+ w2.z * mContactConstraints[mNbContactManifolds].r2Friction.y -
+ v1.x - w1.y * mContactConstraints[mNbContactManifolds].r1Friction.z -
+ w1.z * mContactConstraints[mNbContactManifolds].r1Friction.y,
+ v2.y + w2.z * mContactConstraints[mNbContactManifolds].r2Friction.x -
+ w2.x * mContactConstraints[mNbContactManifolds].r2Friction.z -
+ v1.y - w1.z * mContactConstraints[mNbContactManifolds].r1Friction.x -
+ w1.x * mContactConstraints[mNbContactManifolds].r1Friction.z,
+ v2.z + w2.x * mContactConstraints[mNbContactManifolds].r2Friction.y -
+ w2.y * mContactConstraints[mNbContactManifolds].r2Friction.x -
+ v1.z - w1.x * mContactConstraints[mNbContactManifolds].r1Friction.y -
+ w1.y * mContactConstraints[mNbContactManifolds].r1Friction.x);
// Compute the friction vectors
computeFrictionVectors(deltaVFrictionPoint, mContactConstraints[mNbContactManifolds]);
@@ -289,13 +336,25 @@ void ContactSolver::warmStart() {
// --------- Penetration --------- //
// Update the velocities of the body 1 by applying the impulse P
- Vector3 impulsePenetration = mContactPoints[contactPointIndex].normal * mContactPoints[contactPointIndex].penetrationImpulse;
- mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * impulsePenetration;
- mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * mContactPoints[contactPointIndex].penetrationImpulse;
+ Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse,
+ mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse,
+ mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse);
+ mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
+ mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
+ mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z;
+
+ mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
+ mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
+ mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
// Update the velocities of the body 2 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * impulsePenetration;
- mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * mContactPoints[contactPointIndex].penetrationImpulse;
+ mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
+ mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
+ mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z;
+
+ mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
+ mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
+ mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
}
else { // If it is a new contact point
@@ -312,20 +371,27 @@ void ContactSolver::warmStart() {
// Project the old friction impulses (with old friction vectors) into the new friction
// vectors to get the new friction impulses
- Vector3 oldFrictionImpulse = mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1 +
- mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2;
+ Vector3 oldFrictionImpulse(mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.x +
+ mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.x,
+ mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.y +
+ mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.y,
+ mContactConstraints[c].friction1Impulse * mContactConstraints[c].oldFrictionVector1.z +
+ mContactConstraints[c].friction2Impulse * mContactConstraints[c].oldFrictionVector2.z);
mContactConstraints[c].friction1Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector1);
mContactConstraints[c].friction2Impulse = oldFrictionImpulse.dot(mContactConstraints[c].frictionVector2);
// ------ First friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
- Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 *
- mContactConstraints[c].friction1Impulse;
- Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 *
- mContactConstraints[c].friction1Impulse;
- Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 *
- mContactConstraints[c].friction1Impulse;
+ Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * mContactConstraints[c].friction1Impulse,
+ -mContactConstraints[c].r1CrossT1.y * mContactConstraints[c].friction1Impulse,
+ -mContactConstraints[c].r1CrossT1.z * mContactConstraints[c].friction1Impulse);
+ Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * mContactConstraints[c].friction1Impulse,
+ mContactConstraints[c].frictionVector1.y * mContactConstraints[c].friction1Impulse,
+ mContactConstraints[c].frictionVector1.z * mContactConstraints[c].friction1Impulse);
+ Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * mContactConstraints[c].friction1Impulse,
+ mContactConstraints[c].r2CrossT1.y * mContactConstraints[c].friction1Impulse,
+ mContactConstraints[c].r2CrossT1.z * mContactConstraints[c].friction1Impulse);
// Update the velocities of the body 1 by applying the impulse P
mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
@@ -338,23 +404,40 @@ void ContactSolver::warmStart() {
// ------ Second friction constraint at the center of the contact manifold ----- //
// Compute the impulse P = J^T * lambda
- angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * mContactConstraints[c].friction2Impulse;
- linearImpulseBody2 = mContactConstraints[c].frictionVector2 * mContactConstraints[c].friction2Impulse;
- angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * mContactConstraints[c].friction2Impulse;
+ angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * mContactConstraints[c].friction2Impulse;
+ angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * mContactConstraints[c].friction2Impulse;
+ angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * mContactConstraints[c].friction2Impulse;
+ linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * mContactConstraints[c].friction2Impulse;
+ linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * mContactConstraints[c].friction2Impulse;
+ linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * mContactConstraints[c].friction2Impulse;
+ angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * mContactConstraints[c].friction2Impulse;
+ angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * mContactConstraints[c].friction2Impulse;
+ angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
+ mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+ mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+ mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+ mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+ mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+ mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda
- angularImpulseBody1 = -mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
- angularImpulseBody2 = mContactConstraints[c].normal * mContactConstraints[c].frictionTwistImpulse;
+ angularImpulseBody1.x = -mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse;
+ angularImpulseBody1.y = -mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse;
+ angularImpulseBody1.z = -mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
+
+ angularImpulseBody2.x = mContactConstraints[c].normal.x * mContactConstraints[c].frictionTwistImpulse;
+ angularImpulseBody2.y = mContactConstraints[c].normal.y * mContactConstraints[c].frictionTwistImpulse;
+ angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse;
// Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
@@ -409,8 +492,15 @@ void ContactSolver::solve() {
// --------- Penetration --------- //
// Compute J*v
- Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1);
- decimal deltaVDotN = deltaV.dot(mContactPoints[contactPointIndex].normal);
+ //Vector3 deltaV = v2 + w2.cross(mContactPoints[contactPointIndex].r2) - v1 - w1.cross(mContactPoints[contactPointIndex].r1);
+ Vector3 deltaV(v2.x + w2.y * mContactPoints[contactPointIndex].r2.z - w2.z * mContactPoints[contactPointIndex].r2.y - v1.x -
+ w1.y * mContactPoints[contactPointIndex].r1.z + w1.z * mContactPoints[contactPointIndex].r1.y,
+ v2.y + w2.z * mContactPoints[contactPointIndex].r2.x - w2.x * mContactPoints[contactPointIndex].r2.z - v1.y -
+ w1.z * mContactPoints[contactPointIndex].r1.x + w1.x * mContactPoints[contactPointIndex].r1.z,
+ v2.z + w2.x * mContactPoints[contactPointIndex].r2.y - w2.y * mContactPoints[contactPointIndex].r2.x - v1.z -
+ w1.x * mContactPoints[contactPointIndex].r1.y + w1.y * mContactPoints[contactPointIndex].r1.x);
+ decimal deltaVDotN = deltaV.x * mContactPoints[contactPointIndex].normal.x + deltaV.y * mContactPoints[contactPointIndex].normal.y +
+ deltaV.z * mContactPoints[contactPointIndex].normal.z;
decimal Jv = deltaVDotN;
// Compute the bias "b" of the constraint
@@ -433,15 +523,27 @@ void ContactSolver::solve() {
deltaLambda, decimal(0.0));
deltaLambda = mContactPoints[contactPointIndex].penetrationImpulse - lambdaTemp;
- Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambda;
+ Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambda,
+ mContactPoints[contactPointIndex].normal.y * deltaLambda,
+ mContactPoints[contactPointIndex].normal.z * deltaLambda);
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse;
- mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambda;
+ mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
+ mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
+ mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
+
+ mAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
+ mAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
+ mAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
// Update the velocities of the body 2 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
- mAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambda;
+ mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
+ mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
+ mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
+
+ mAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
+ mAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
+ mAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
@@ -453,9 +555,17 @@ void ContactSolver::solve() {
const Vector3& w1Split = mSplitAngularVelocities[mContactConstraints[c].indexBody1];
const Vector3& v2Split = mSplitLinearVelocities[mContactConstraints[c].indexBody2];
const Vector3& w2Split = mSplitAngularVelocities[mContactConstraints[c].indexBody2];
- Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) -
- v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
- decimal JvSplit = deltaVSplit.dot(mContactPoints[contactPointIndex].normal);
+
+ //Vector3 deltaVSplit = v2Split + w2Split.cross(mContactPoints[contactPointIndex].r2) - v1Split - w1Split.cross(mContactPoints[contactPointIndex].r1);
+ Vector3 deltaVSplit(v2Split.x + w2Split.y * mContactPoints[contactPointIndex].r2.z - w2Split.z * mContactPoints[contactPointIndex].r2.y - v1Split.x -
+ w1Split.y * mContactPoints[contactPointIndex].r1.z + w1Split.z * mContactPoints[contactPointIndex].r1.y,
+ v2Split.y + w2Split.z * mContactPoints[contactPointIndex].r2.x - w2Split.x * mContactPoints[contactPointIndex].r2.z - v1Split.y -
+ w1Split.z * mContactPoints[contactPointIndex].r1.x + w1Split.x * mContactPoints[contactPointIndex].r1.z,
+ v2Split.z + w2Split.x * mContactPoints[contactPointIndex].r2.y - w2Split.y * mContactPoints[contactPointIndex].r2.x - v1Split.z -
+ w1Split.x * mContactPoints[contactPointIndex].r1.y + w1Split.y * mContactPoints[contactPointIndex].r1.x);
+ decimal JvSplit = deltaVSplit.x * mContactPoints[contactPointIndex].normal.x +
+ deltaVSplit.y * mContactPoints[contactPointIndex].normal.y +
+ deltaVSplit.z * mContactPoints[contactPointIndex].normal.z;
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) *
mContactPoints[contactPointIndex].inversePenetrationMass;
decimal lambdaTempSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse;
@@ -464,15 +574,27 @@ void ContactSolver::solve() {
deltaLambdaSplit, decimal(0.0));
deltaLambdaSplit = mContactPoints[contactPointIndex].penetrationSplitImpulse - lambdaTempSplit;
- Vector3 linearImpulse = mContactPoints[contactPointIndex].normal * deltaLambdaSplit;
+ Vector3 linearImpulse(mContactPoints[contactPointIndex].normal.x * deltaLambdaSplit,
+ mContactPoints[contactPointIndex].normal.y * deltaLambdaSplit,
+ mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit);
// Update the velocities of the body 1 by applying the impulse P
- mSplitLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulse;
- mSplitAngularVelocities[mContactConstraints[c].indexBody1] -= mContactPoints[contactPointIndex].i1TimesR1CrossN * deltaLambdaSplit;
+ mSplitLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
+ mSplitLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
+ mSplitLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z;
+
+ mSplitAngularVelocities[mContactConstraints[c].indexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
+ mSplitAngularVelocities[mContactConstraints[c].indexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
+ mSplitAngularVelocities[mContactConstraints[c].indexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
// Update the velocities of the body 1 by applying the impulse P
- mSplitLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulse;
- mSplitAngularVelocities[mContactConstraints[c].indexBody2] += mContactPoints[contactPointIndex].i2TimesR2CrossN * deltaLambdaSplit;
+ mSplitLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
+ mSplitLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
+ mSplitLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z;
+
+ mSplitAngularVelocities[mContactConstraints[c].indexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
+ mSplitAngularVelocities[mContactConstraints[c].indexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
+ mSplitAngularVelocities[mContactConstraints[c].indexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
}
contactPointIndex++;
@@ -481,9 +603,16 @@ void ContactSolver::solve() {
// ------ First friction constraint at the center of the contact manifol ------ //
// Compute J*v
- Vector3 deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction)
- - v1 - w1.cross(mContactConstraints[c].r1Friction);
- decimal Jv = deltaV.dot(mContactConstraints[c].frictionVector1);
+ // deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
+ Vector3 deltaV(v2.x + w2.y * mContactConstraints[c].r2Friction.z - w2.z * mContactConstraints[c].r2Friction.y - v1.x -
+ w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y,
+ v2.y + w2.z * mContactConstraints[c].r2Friction.x - w2.x * mContactConstraints[c].r2Friction.z - v1.y -
+ w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z,
+ v2.z + w2.x * mContactConstraints[c].r2Friction.y - w2.y * mContactConstraints[c].r2Friction.x - v1.z -
+ w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x);
+ decimal Jv = deltaV.x * mContactConstraints[c].frictionVector1.x +
+ deltaV.y * mContactConstraints[c].frictionVector1.y +
+ deltaV.z * mContactConstraints[c].frictionVector1.z;
// Compute the Lagrange multiplier lambda
decimal deltaLambda = -Jv * mContactConstraints[c].inverseFriction1Mass;
@@ -495,23 +624,42 @@ void ContactSolver::solve() {
deltaLambda = mContactConstraints[c].friction1Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
- Vector3 angularImpulseBody1 = -mContactConstraints[c].r1CrossT1 * deltaLambda;
- Vector3 linearImpulseBody2 = mContactConstraints[c].frictionVector1 * deltaLambda;
- Vector3 angularImpulseBody2 = mContactConstraints[c].r2CrossT1 * deltaLambda;
+ Vector3 angularImpulseBody1(-mContactConstraints[c].r1CrossT1.x * deltaLambda,
+ -mContactConstraints[c].r1CrossT1.y * deltaLambda,
+ -mContactConstraints[c].r1CrossT1.z * deltaLambda);
+ Vector3 linearImpulseBody2(mContactConstraints[c].frictionVector1.x * deltaLambda,
+ mContactConstraints[c].frictionVector1.y * deltaLambda,
+ mContactConstraints[c].frictionVector1.z * deltaLambda);
+ Vector3 angularImpulseBody2(mContactConstraints[c].r2CrossT1.x * deltaLambda,
+ mContactConstraints[c].r2CrossT1.y * deltaLambda,
+ mContactConstraints[c].r2CrossT1.z * deltaLambda);
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
+ mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+ mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+ mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+ mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+ mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+ mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifol ----- //
// Compute J*v
- deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
- Jv = deltaV.dot(mContactConstraints[c].frictionVector2);
+ //deltaV = v2 + w2.cross(mContactConstraints[c].r2Friction) - v1 - w1.cross(mContactConstraints[c].r1Friction);
+ deltaV.x = v2.x + w2.y * mContactConstraints[c].r2Friction.z - v2.z * mContactConstraints[c].r2Friction.y - v1.x -
+ w1.y * mContactConstraints[c].r1Friction.z + w1.z * mContactConstraints[c].r1Friction.y;
+ deltaV.y = v2.y + w2.z * mContactConstraints[c].r2Friction.x - v2.x * mContactConstraints[c].r2Friction.z - v1.y -
+ w1.z * mContactConstraints[c].r1Friction.x + w1.x * mContactConstraints[c].r1Friction.z;
+ deltaV.z = v2.z + w2.x * mContactConstraints[c].r2Friction.y - v2.y * mContactConstraints[c].r2Friction.x - v1.z -
+ w1.x * mContactConstraints[c].r1Friction.y + w1.y * mContactConstraints[c].r1Friction.x;
+ Jv = deltaV.x * mContactConstraints[c].frictionVector2.x + deltaV.y * mContactConstraints[c].frictionVector2.y +
+ deltaV.z * mContactConstraints[c].frictionVector2.z;
// Compute the Lagrange multiplier lambda
deltaLambda = -Jv * mContactConstraints[c].inverseFriction2Mass;
@@ -523,23 +671,36 @@ void ContactSolver::solve() {
deltaLambda = mContactConstraints[c].friction2Impulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
- angularImpulseBody1 = -mContactConstraints[c].r1CrossT2 * deltaLambda;
- linearImpulseBody2 = mContactConstraints[c].frictionVector2 * deltaLambda;
- angularImpulseBody2 = mContactConstraints[c].r2CrossT2 * deltaLambda;
+ angularImpulseBody1.x = -mContactConstraints[c].r1CrossT2.x * deltaLambda;
+ angularImpulseBody1.y = -mContactConstraints[c].r1CrossT2.y * deltaLambda;
+ angularImpulseBody1.z = -mContactConstraints[c].r1CrossT2.z * deltaLambda;
+
+ linearImpulseBody2.x = mContactConstraints[c].frictionVector2.x * deltaLambda;
+ linearImpulseBody2.y = mContactConstraints[c].frictionVector2.y * deltaLambda;
+ linearImpulseBody2.z = mContactConstraints[c].frictionVector2.z * deltaLambda;
+
+ angularImpulseBody2.x = mContactConstraints[c].r2CrossT2.x * deltaLambda;
+ angularImpulseBody2.y = mContactConstraints[c].r2CrossT2.y * deltaLambda;
+ angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
+ mLinearVelocities[mContactConstraints[c].indexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+ mLinearVelocities[mContactConstraints[c].indexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+ mLinearVelocities[mContactConstraints[c].indexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+ mLinearVelocities[mContactConstraints[c].indexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+ mLinearVelocities[mContactConstraints[c].indexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+ mLinearVelocities[mContactConstraints[c].indexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifol ------ //
// Compute J*v
deltaV = w2 - w1;
- Jv = deltaV.dot(mContactConstraints[c].normal);
+ Jv = deltaV.x * mContactConstraints[c].normal.x + deltaV.y * mContactConstraints[c].normal.y +
+ deltaV.z * mContactConstraints[c].normal.z;
deltaLambda = -Jv * (mContactConstraints[c].inverseTwistFrictionMass);
frictionLimit = mContactConstraints[c].frictionCoefficient * sumPenetrationImpulse;
@@ -550,7 +711,9 @@ void ContactSolver::solve() {
deltaLambda = mContactConstraints[c].frictionTwistImpulse - lambdaTemp;
// Compute the impulse P=J^T * lambda
- angularImpulseBody2 = mContactConstraints[c].normal * deltaLambda;
+ angularImpulseBody2.x = mContactConstraints[c].normal.x * deltaLambda;
+ angularImpulseBody2.y = mContactConstraints[c].normal.y * deltaLambda;
+ angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
mAngularVelocities[mContactConstraints[c].indexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
@@ -619,8 +782,11 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
assert(contact.normal.length() > decimal(0.0));
// Compute the velocity difference vector in the tangential plane
- Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal;
- Vector3 tangentVelocity = deltaVelocity - normalVelocity;
+ Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x,
+ deltaVelocity.y * contact.normal.y * contact.normal.y,
+ deltaVelocity.z * contact.normal.z * contact.normal.z);
+ Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y,
+ deltaVelocity.z - normalVelocity.z);
// If the velocty difference in the tangential plane is not zero
decimal lengthTangenVelocity = tangentVelocity.length();
From 9066264189e27c85bd7a16b04cd3bb32c6a8ef8c Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 12 Dec 2017 22:36:19 +0100
Subject: [PATCH 124/133] Remove commented code
---
src/engine/DynamicsWorld.cpp | 23 -----------------------
1 file changed, 23 deletions(-)
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index bb824228..66b13a8e 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -358,20 +358,6 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Check if there are contacts and constraints to solve
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
- //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
- //if (!isConstraintsToSolve && !isContactsToSolve) continue;
-
- // If there are contacts in the current island
-// if (isContactsToSolve) {
-
-// // Initialize the solver
-// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
-
-// // Warm start the contact solver
-// if (mContactSolver.IsWarmStartingActive()) {
-// mContactSolver.warmStart();
-// }
-// }
// If there are constraints
if (isConstraintsToSolve) {
@@ -393,15 +379,6 @@ void DynamicsWorld::solveContactsAndConstraints() {
}
mContactSolver.solve();
-
- // Solve the contacts
-// if (isContactsToSolve) {
-
-// mContactSolver.resetTotalPenetrationImpulse();
-
-// mContactSolver.solvePenetrationConstraints();
-// mContactSolver.solveFrictionConstraints();
-// }
}
mContactSolver.storeImpulses();
From 2d0cb2753832059abb460ebb317bbbfd9e8f0eab Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Tue, 12 Dec 2017 07:38:56 +0100
Subject: [PATCH 125/133] Add List and DefaultAllocator classes
---
src/containers/List.h | 191 ++++++++++++++++++++++++++++++++++
src/memory/DefaultAllocator.h | 66 ++++++++++++
2 files changed, 257 insertions(+)
create mode 100644 src/containers/List.h
create mode 100644 src/memory/DefaultAllocator.h
diff --git a/src/containers/List.h b/src/containers/List.h
new file mode 100644
index 00000000..4915f32d
--- /dev/null
+++ b/src/containers/List.h
@@ -0,0 +1,191 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_LIST_H
+#define REACTPHYSICS3D_LIST_H
+
+// Libraries
+#include "configuration.h"
+#include "memory/Allocator.h"
+#include <cstring>
+
+namespace reactphysics3d {
+
+// Class List
+/**
+ * This class represents a simple generic list with custom memory allocator.
+ */
+template<typename T>
+class List {
+
+ private:
+
+ // -------------------- Attributes -------------------- //
+
+ /// Pointer to the first element of the list
+ T* mElements;
+
+ /// Number of elements in the list
+ uint mSize;
+
+ /// Number of allocated elements in the list
+ uint mCapacity;
+
+ /// Memory allocator
+ Allocator& mAllocator;
+
+ // -------------------- Methods -------------------- //
+
+ /// Allocate more memory for the elements of the list
+ void allocateMemory(uint nbElementsToAllocate) {
+
+ assert(nbElementsToAllocate > mCapacity);
+
+ // Allocate memory for the new array
+ void* newMemory = mAllocator.allocate(nbElementsToAllocate * sizeof(T));
+
+ if (mElements != nullptr) {
+
+ // Copy the elements to the new allocated memory location
+ std::memcpy(newMemory, static_cast<void*>(mElements), mSize * sizeof(T));
+
+ // Release the previously allocated memory
+ mAllocator.release(mElements, mCapacity * sizeof(T));
+ }
+
+ mElements = static_cast<T*>(newMemory);
+
+ mCapacity = nbElementsToAllocate;
+ }
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ List(Allocator& allocator, uint capacity = 0)
+ : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) {
+
+ if (capacity > 0) {
+
+ // Allocate memory
+ allocateMemory(capacity);
+ }
+ }
+
+ /// Copy constructor
+ List(const List<T>& list) : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) {
+
+ // All all the elements of the list to the current one
+ addRange(list);
+ }
+
+ /// Destructor
+ ~List() {
+
+ // If elements have been allocated
+ if (mCapacity > 0) {
+
+ // Clear the list
+ clear();
+
+ // Release the memory allocated on the heap
+ mAllocator.release(static_cast<void*>(mElements), mCapacity * sizeof(T));
+ }
+ }
+
+ /// Add an element into the list
+ void add(const T& element) {
+
+ // If we need to allocate more memory
+ if (mSize == mCapacity) {
+ allocateMemory(mCapacity == 0 ? 1 : mCapacity * 2);
+ }
+
+ mElements[mSize] = element;
+ mSize++;
+ }
+
+ /// Append another list to the current one
+ void addRange(const List<T>& list) {
+
+ // If we need to allocate more memory
+ if (mSize + list.size() > mCapacity) {
+
+ // Allocate memory
+ allocateMemory(mSize + list.size());
+ }
+
+ // Add the elements of the list to the current one
+ for(uint i=0; i<list.size(); i++) {
+ mElements[mSize] = list[i];
+ mSize++;
+ }
+ }
+
+ /// Clear the list
+ void clear() {
+
+ // Call the destructor of each element
+ for (uint i=0; i < mSize; i++) {
+ mElements[i].~T();
+ }
+
+ mSize = 0;
+ }
+
+ /// Return the number of elments in the list
+ uint size() const {
+ return mSize;
+ }
+
+ /// Overloaded index operator
+ T& operator[](const uint index) {
+ assert(index >= 0 && index < mSize);
+ return mElements[index];
+ }
+
+ /// Overloaded const index operator
+ const T& operator[](const uint index) const {
+ assert(index >= 0 && index < mSize);
+ return mElements[index];
+ }
+
+ /// Overloaded assignment operator
+ List<T>& operator=(const List<T>& list) {
+
+ // Clear all the elements
+ clear();
+
+ // Add all the elements of the list to the current one
+ addRange(list);
+
+ return *this;
+ }
+};
+
+}
+
+#endif
diff --git a/src/memory/DefaultAllocator.h b/src/memory/DefaultAllocator.h
new file mode 100644
index 00000000..367171bd
--- /dev/null
+++ b/src/memory/DefaultAllocator.h
@@ -0,0 +1,66 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_DEFAULT_ALLOCATOR_H
+#define REACTPHYSICS3D_DEFAULT_ALLOCATOR_H
+
+// Libraries
+#include "memory/Allocator.h"
+#include <cstdlib>
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class DefaultAllocator
+/**
+ * This class represents a default memory allocator that uses default malloc/free methods
+ */
+class DefaultAllocator : public Allocator {
+
+ public:
+
+ /// Destructor
+ virtual ~DefaultAllocator() = default;
+
+ /// Allocate memory of a given size (in bytes) and return a pointer to the
+ /// allocated memory.
+ virtual void* allocate(size_t size) override {
+ return malloc(size);
+ }
+
+ /// Release previously allocated memory.
+ virtual void release(void* pointer, size_t size) override {
+ free(pointer);
+ }
+
+ /// Return true if memory needs to be release with this allocator
+ virtual bool isReleaseNeeded() const override {
+ return true;
+ }
+};
+
+}
+
+#endif
From 5392948518ef415991feb55e0595bc15b48d737c Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 13 Dec 2017 17:51:38 +0100
Subject: [PATCH 126/133] Use inline constructors for mathematics objects
(vectors, quaternion, matrices, ...)
---
src/mathematics/Matrix2x2.cpp | 25 ------------------
src/mathematics/Matrix2x2.h | 25 ++++++++++++++++++
src/mathematics/Matrix3x3.cpp | 26 -------------------
src/mathematics/Matrix3x3.h | 26 +++++++++++++++++++
src/mathematics/Quaternion.cpp | 21 ----------------
src/mathematics/Quaternion.h | 25 ++++++++++++++++--
src/mathematics/Transform.cpp | 22 ----------------
src/mathematics/Transform.h | 46 ++++++++++++++++++++++++++++++++--
src/mathematics/Vector2.cpp | 15 -----------
src/mathematics/Vector2.h | 16 ++++++++++++
src/mathematics/Vector3.cpp | 15 -----------
src/mathematics/Vector3.h | 15 +++++++++++
12 files changed, 149 insertions(+), 128 deletions(-)
diff --git a/src/mathematics/Matrix2x2.cpp b/src/mathematics/Matrix2x2.cpp
index c501c1db..bf4adbe5 100644
--- a/src/mathematics/Matrix2x2.cpp
+++ b/src/mathematics/Matrix2x2.cpp
@@ -28,31 +28,6 @@
using namespace reactphysics3d;
-// Constructor of the class Matrix2x2
-Matrix2x2::Matrix2x2() {
-
- // Initialize all values in the matrix to zero
- setAllValues(0.0, 0.0, 0.0, 0.0);
-}
-
-// Constructor
-Matrix2x2::Matrix2x2(decimal value) {
- setAllValues(value, value, value, value);
-}
-
-// Constructor with arguments
-Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) {
-
- // Initialize the matrix with the values
- setAllValues(a1, a2, b1, b2);
-}
-
-// Copy-constructor
-Matrix2x2::Matrix2x2(const Matrix2x2& matrix) {
- setAllValues(matrix.mRows[0][0], matrix.mRows[0][1],
- matrix.mRows[1][0], matrix.mRows[1][1]);
-}
-
// Assignment operator
Matrix2x2& Matrix2x2::operator=(const Matrix2x2& matrix) {
diff --git a/src/mathematics/Matrix2x2.h b/src/mathematics/Matrix2x2.h
index ee31b07a..8315eb99 100644
--- a/src/mathematics/Matrix2x2.h
+++ b/src/mathematics/Matrix2x2.h
@@ -147,6 +147,31 @@ class Matrix2x2 {
Vector2& operator[](int row);
};
+// Constructor of the class Matrix2x2
+inline Matrix2x2::Matrix2x2() {
+
+ // Initialize all values in the matrix to zero
+ setAllValues(0.0, 0.0, 0.0, 0.0);
+}
+
+// Constructor
+inline Matrix2x2::Matrix2x2(decimal value) {
+ setAllValues(value, value, value, value);
+}
+
+// Constructor with arguments
+inline Matrix2x2::Matrix2x2(decimal a1, decimal a2, decimal b1, decimal b2) {
+
+ // Initialize the matrix with the values
+ setAllValues(a1, a2, b1, b2);
+}
+
+// Copy-constructor
+inline Matrix2x2::Matrix2x2(const Matrix2x2& matrix) {
+ setAllValues(matrix.mRows[0][0], matrix.mRows[0][1],
+ matrix.mRows[1][0], matrix.mRows[1][1]);
+}
+
// Method to set all the values in the matrix
inline void Matrix2x2::setAllValues(decimal a1, decimal a2,
decimal b1, decimal b2) {
diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp
index a5491ed4..e86e63a1 100644
--- a/src/mathematics/Matrix3x3.cpp
+++ b/src/mathematics/Matrix3x3.cpp
@@ -30,32 +30,6 @@
// Namespaces
using namespace reactphysics3d;
-// Constructor of the class Matrix3x3
-Matrix3x3::Matrix3x3() {
- // Initialize all values in the matrix to zero
- setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
-}
-
-// Constructor
-Matrix3x3::Matrix3x3(decimal value) {
- setAllValues(value, value, value, value, value, value, value, value, value);
-}
-
-// Constructor with arguments
-Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3,
- decimal b1, decimal b2, decimal b3,
- decimal c1, decimal c2, decimal c3) {
- // Initialize the matrix with the values
- setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3);
-}
-
-// Copy-constructor
-Matrix3x3::Matrix3x3(const Matrix3x3& matrix) {
- setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2],
- matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2],
- matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]);
-}
-
// Assignment operator
Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix) {
diff --git a/src/mathematics/Matrix3x3.h b/src/mathematics/Matrix3x3.h
index ebb8792d..6897a2cf 100644
--- a/src/mathematics/Matrix3x3.h
+++ b/src/mathematics/Matrix3x3.h
@@ -155,6 +155,32 @@ class Matrix3x3 {
Vector3& operator[](int row);
};
+// Constructor of the class Matrix3x3
+inline Matrix3x3::Matrix3x3() {
+ // Initialize all values in the matrix to zero
+ setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+}
+
+// Constructor
+inline Matrix3x3::Matrix3x3(decimal value) {
+ setAllValues(value, value, value, value, value, value, value, value, value);
+}
+
+// Constructor with arguments
+inline Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3,
+ decimal b1, decimal b2, decimal b3,
+ decimal c1, decimal c2, decimal c3) {
+ // Initialize the matrix with the values
+ setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3);
+}
+
+// Copy-constructor
+inline Matrix3x3::Matrix3x3(const Matrix3x3& matrix) {
+ setAllValues(matrix.mRows[0][0], matrix.mRows[0][1], matrix.mRows[0][2],
+ matrix.mRows[1][0], matrix.mRows[1][1], matrix.mRows[1][2],
+ matrix.mRows[2][0], matrix.mRows[2][1], matrix.mRows[2][2]);
+}
+
// Method to set all the values in the matrix
inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3,
decimal b1, decimal b2, decimal b3,
diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp
index d0e1ae61..0e877612 100644
--- a/src/mathematics/Quaternion.cpp
+++ b/src/mathematics/Quaternion.cpp
@@ -31,27 +31,6 @@
// Namespace
using namespace reactphysics3d;
-// Constructor of the class
-Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) {
-
-}
-
-// Constructor with arguments
-Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW)
- :x(newX), y(newY), z(newZ), w(newW) {
-
-}
-
-// Constructor with the component w and the vector v=(x y z)
-Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) {
-
-}
-
-// Constructor with the component w and the vector v=(x y z)
-Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) {
-
-}
-
// Return a quaternion constructed from Euler angles (in radians)
Quaternion Quaternion::fromEulerAngles(decimal angleX, decimal angleY, decimal angleZ) {
diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h
index d5e771e2..dd0726dc 100644
--- a/src/mathematics/Quaternion.h
+++ b/src/mathematics/Quaternion.h
@@ -169,7 +169,28 @@ struct Quaternion {
void initWithEulerAngles(decimal angleX, decimal angleY, decimal angleZ);
};
-/// Set all the values
+// Constructor of the class
+inline Quaternion::Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) {
+
+}
+
+// Constructor with arguments
+inline Quaternion::Quaternion(decimal newX, decimal newY, decimal newZ, decimal newW)
+ :x(newX), y(newY), z(newZ), w(newW) {
+
+}
+
+// Constructor with the component w and the vector v=(x y z)
+inline Quaternion::Quaternion(decimal newW, const Vector3& v) : x(v.x), y(v.y), z(v.z), w(newW) {
+
+}
+
+// Constructor with the component w and the vector v=(x y z)
+inline Quaternion::Quaternion(const Vector3& v, decimal newW) : x(v.x), y(v.y), z(v.z), w(newW) {
+
+}
+
+// Set all the values
inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, decimal newW) {
x = newX;
y = newY;
@@ -177,7 +198,7 @@ inline void Quaternion::setAllValues(decimal newX, decimal newY, decimal newZ, d
w = newW;
}
-/// Set the quaternion to zero
+// Set the quaternion to zero
inline void Quaternion::setToZero() {
x = 0;
y = 0;
diff --git a/src/mathematics/Transform.cpp b/src/mathematics/Transform.cpp
index 4bc91253..d5cf0399 100644
--- a/src/mathematics/Transform.cpp
+++ b/src/mathematics/Transform.cpp
@@ -29,25 +29,3 @@
// Namespaces
using namespace reactphysics3d;
-// Constructor
-Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) {
-
-}
-
-// Constructor
-Transform::Transform(const Vector3& position, const Matrix3x3& orientation)
- : mPosition(position), mOrientation(Quaternion(orientation)) {
-
-}
-
-// Constructor
-Transform::Transform(const Vector3& position, const Quaternion& orientation)
- : mPosition(position), mOrientation(orientation) {
-
-}
-
-// Copy-constructor
-Transform::Transform(const Transform& transform)
- : mPosition(transform.mPosition), mOrientation(transform.mOrientation) {
-
-}
diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h
index 52ee663e..a5248fc2 100644
--- a/src/mathematics/Transform.h
+++ b/src/mathematics/Transform.h
@@ -118,6 +118,29 @@ class Transform {
Transform& operator=(const Transform& transform);
};
+// Constructor
+inline Transform::Transform() : mPosition(Vector3(0.0, 0.0, 0.0)), mOrientation(Quaternion::identity()) {
+
+}
+
+// Constructor
+inline Transform::Transform(const Vector3& position, const Matrix3x3& orientation)
+ : mPosition(position), mOrientation(Quaternion(orientation)) {
+
+}
+
+// Constructor
+inline Transform::Transform(const Vector3& position, const Quaternion& orientation)
+ : mPosition(position), mOrientation(orientation) {
+
+}
+
+// Copy-constructor
+inline Transform::Transform(const Transform& transform)
+ : mPosition(transform.mPosition), mOrientation(transform.mOrientation) {
+
+}
+
// Return the position of the transform
inline const Vector3& Transform::getPosition() const {
return mPosition;
@@ -199,8 +222,27 @@ inline Vector3 Transform::operator*(const Vector3& vector) const {
// Operator of multiplication of a transform with another one
inline Transform Transform::operator*(const Transform& transform2) const {
- return Transform(mPosition + mOrientation * transform2.mPosition,
- mOrientation * transform2.mOrientation);
+
+ const decimal prodX = mOrientation.w * transform2.mPosition.x + mOrientation.w * transform2.mPosition.z
+ - mOrientation.z * transform2.mPosition.y;
+ const decimal prodY = mOrientation.w * transform2.mPosition.y + mOrientation.z * transform2.mPosition.x
+ - mOrientation.x * transform2.mPosition.z;
+ const decimal prodZ = mOrientation.w * transform2.mPosition.z + mOrientation.x * transform2.mPosition.y
+ - mOrientation.y * transform2.mPosition.x;
+ const decimal prodW = -mOrientation.x * transform2.mPosition.x - mOrientation.y * transform2.mPosition.y
+ - mOrientation.z * transform2.mPosition.z;
+
+ return Transform(Vector3(mPosition.x + mOrientation.w * prodX - prodY * mOrientation.z + prodZ * mOrientation.y - prodW * mOrientation.x,
+ mPosition.y + mOrientation.w * prodY - prodZ * mOrientation.x + prodX * mOrientation.z - prodW * mOrientation.y,
+ mPosition.z + mOrientation.w * prodZ - prodX * mOrientation.y + prodY * mOrientation.x - prodW * mOrientation.z),
+ Quaternion(mOrientation.w * transform2.mOrientation.x + transform2.mOrientation.w * mOrientation.x
+ + mOrientation.y * transform2.mOrientation.z - mOrientation.z * transform2.mOrientation.y,
+ mOrientation.w * transform2.mOrientation.y + transform2.mOrientation.w * mOrientation.y
+ + mOrientation.z * transform2.mOrientation.x - mOrientation.x * transform2.mOrientation.z,
+ mOrientation.w * transform2.mOrientation.z + transform2.mOrientation.w * mOrientation.z
+ + mOrientation.x * transform2.mOrientation.y - mOrientation.y * transform2.mOrientation.x,
+ mOrientation.w * transform2.mOrientation.w - mOrientation.x * transform2.mOrientation.x
+ - mOrientation.y * transform2.mOrientation.y - mOrientation.z * transform2.mOrientation.z));
}
// Return true if the two transforms are equal
diff --git a/src/mathematics/Vector2.cpp b/src/mathematics/Vector2.cpp
index 2f225636..271fd82f 100644
--- a/src/mathematics/Vector2.cpp
+++ b/src/mathematics/Vector2.cpp
@@ -30,21 +30,6 @@
// Namespaces
using namespace reactphysics3d;
-// Constructor
-Vector2::Vector2() : x(0.0), y(0.0) {
-
-}
-
-// Constructor with arguments
-Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) {
-
-}
-
-// Copy-constructor
-Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) {
-
-}
-
// Return the corresponding unit vector
Vector2 Vector2::getUnit() const {
decimal lengthVector = length();
diff --git a/src/mathematics/Vector2.h b/src/mathematics/Vector2.h
index 4b0e32f1..bc5f8872 100644
--- a/src/mathematics/Vector2.h
+++ b/src/mathematics/Vector2.h
@@ -156,6 +156,22 @@ struct Vector2 {
friend Vector2 operator/(const Vector2& vector1, const Vector2& vector2);
};
+// Constructor
+inline Vector2::Vector2() : x(0.0), y(0.0) {
+
+}
+
+// Constructor with arguments
+inline Vector2::Vector2(decimal newX, decimal newY) : x(newX), y(newY) {
+
+}
+
+// Copy-constructor
+inline Vector2::Vector2(const Vector2& vector) : x(vector.x), y(vector.y) {
+
+}
+
+
// Set the vector to zero
inline void Vector2::setToZero() {
x = 0;
diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp
index ab2d126d..33b760ba 100644
--- a/src/mathematics/Vector3.cpp
+++ b/src/mathematics/Vector3.cpp
@@ -31,21 +31,6 @@
// Namespaces
using namespace reactphysics3d;
-// Constructor of the class Vector3D
-Vector3::Vector3() : x(0.0), y(0.0), z(0.0) {
-
-}
-
-// Constructor with arguments
-Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) {
-
-}
-
-// Copy-constructor
-Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) {
-
-}
-
// Return the corresponding unit vector
Vector3 Vector3::getUnit() const {
decimal lengthVector = length();
diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h
index 1ca1adfd..95255fdb 100644
--- a/src/mathematics/Vector3.h
+++ b/src/mathematics/Vector3.h
@@ -168,6 +168,21 @@ struct Vector3 {
friend Vector3 operator/(const Vector3& vector1, const Vector3& vector2);
};
+// Constructor of the class Vector3D
+inline Vector3::Vector3() : x(0.0), y(0.0), z(0.0) {
+
+}
+
+// Constructor with arguments
+inline Vector3::Vector3(decimal newX, decimal newY, decimal newZ) : x(newX), y(newY), z(newZ) {
+
+}
+
+// Copy-constructor
+inline Vector3::Vector3(const Vector3& vector) : x(vector.x), y(vector.y), z(vector.z) {
+
+}
+
// Set the vector to zero
inline void Vector3::setToZero() {
x = 0;
From 7d20a746e9a8822d4530e6b7b44ef3286d8aed68 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 14 Dec 2017 15:09:56 +0100
Subject: [PATCH 127/133] Do not std::map to store mapping from rigid body to
index in array
---
src/body/RigidBody.cpp | 2 +-
src/body/RigidBody.h | 7 ++++++-
src/constraint/BallAndSocketJoint.cpp | 4 ++--
src/constraint/FixedJoint.cpp | 4 ++--
src/constraint/HingeJoint.cpp | 4 ++--
src/constraint/SliderJoint.cpp | 4 ++--
src/engine/ConstraintSolver.cpp | 4 +---
src/engine/ConstraintSolver.h | 16 +++-------------
src/engine/ContactSolver.cpp | 8 +++-----
src/engine/ContactSolver.h | 6 +-----
src/engine/DynamicsWorld.cpp | 26 +++++++++-----------------
src/engine/DynamicsWorld.h | 3 ---
12 files changed, 32 insertions(+), 56 deletions(-)
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index d0f0c782..01e1694b 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -42,7 +42,7 @@ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyinde
: CollisionBody(transform, world, id), mInitMass(decimal(1.0)),
mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
- mJointsList(nullptr) {
+ mJointsList(nullptr), mArrayIndex(0) {
// Compute the inverse mass
mMassInverse = decimal(1.0) / mInitMass;
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index 6ba7a596..e12cab05 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -50,6 +50,11 @@ class DynamicsWorld;
*/
class RigidBody : public CollisionBody {
+ private :
+
+ /// Index of the body in arrays for contact/constraint solver
+ uint mArrayIndex;
+
protected :
// -------------------- Attributes -------------------- //
@@ -102,7 +107,7 @@ class RigidBody : public CollisionBody {
decimal mAngularDamping;
/// First element of the linked list of joints involving this body
- JointListElement* mJointsList;
+ JointListElement* mJointsList;
// -------------------- Methods -------------------- //
diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp
index 7a5fce1b..272d922e 100644
--- a/src/constraint/BallAndSocketJoint.cpp
+++ b/src/constraint/BallAndSocketJoint.cpp
@@ -45,8 +45,8 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
- mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
- mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+ mIndexBody1 = mBody1->mArrayIndex;
+ mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies center of mass and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;
diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp
index 8f0b105c..01fe3957 100644
--- a/src/constraint/FixedJoint.cpp
+++ b/src/constraint/FixedJoint.cpp
@@ -53,8 +53,8 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
- mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
- mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+ mIndexBody1 = mBody1->mArrayIndex;
+ mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;
diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp
index 1634bf27..7b400a2a 100644
--- a/src/constraint/HingeJoint.cpp
+++ b/src/constraint/HingeJoint.cpp
@@ -68,8 +68,8 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
- mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
- mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+ mIndexBody1 = mBody1->mArrayIndex;
+ mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;
diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp
index 919d91c0..7697013d 100644
--- a/src/constraint/SliderJoint.cpp
+++ b/src/constraint/SliderJoint.cpp
@@ -67,8 +67,8 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the veloc ity array
- mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
- mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
+ mIndexBody1 = mBody1->mArrayIndex;
+ mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;
diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp
index 458c39a7..5345cbaf 100644
--- a/src/engine/ConstraintSolver.cpp
+++ b/src/engine/ConstraintSolver.cpp
@@ -30,9 +30,7 @@
using namespace reactphysics3d;
// Constructor
-ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
- : mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
- mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) {
+ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) {
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h
index fbb54ed1..105045e0 100644
--- a/src/engine/ConstraintSolver.h
+++ b/src/engine/ConstraintSolver.h
@@ -60,18 +60,12 @@ struct ConstraintSolverData {
/// Reference to the bodies orientations
Quaternion* orientations;
- /// Reference to the map that associates rigid body to their index
- /// in the constrained velocities array
- const std::map<RigidBody*, uint>& mapBodyToConstrainedVelocityIndex;
-
/// True if warm starting of the solver is active
bool isWarmStartingActive;
/// Constructor
- ConstraintSolverData(const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
- :linearVelocities(nullptr), angularVelocities(nullptr),
- positions(nullptr), orientations(nullptr),
- mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
+ ConstraintSolverData() :linearVelocities(nullptr), angularVelocities(nullptr),
+ positions(nullptr), orientations(nullptr) {
}
@@ -152,10 +146,6 @@ class ConstraintSolver {
// -------------------- Attributes -------------------- //
- /// Reference to the map that associates rigid body to their index in
- /// the constrained velocities array
- const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
-
/// Current time step
decimal mTimeStep;
@@ -176,7 +166,7 @@ class ConstraintSolver {
// -------------------- Methods -------------------- //
/// Constructor
- ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
+ ConstraintSolver();
/// Destructor
~ConstraintSolver() = default;
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 7bd74e8a..6361badd 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -39,12 +39,10 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor
-ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
- SingleFrameAllocator& allocator)
+ContactSolver::ContactSolver(SingleFrameAllocator& allocator)
:mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr),
mContactConstraints(nullptr), mSingleFrameAllocator(allocator),
mLinearVelocities(nullptr), mAngularVelocities(nullptr),
- mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsSplitImpulseActive(true) {
}
@@ -131,8 +129,8 @@ void ContactSolver::initializeForIsland(Island* island) {
// Initialize the internal contact manifold structure using the external
// contact manifold
new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
- mContactConstraints[mNbContactManifolds].indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
- mContactConstraints[mNbContactManifolds].indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
+ mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex;
+ mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex;
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index 0a2850f5..05bd569c 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -302,9 +302,6 @@ class ContactSolver {
/// Array of angular velocities
Vector3* mAngularVelocities;
- /// Reference to the map of rigid body to their index in the constrained velocities array
- const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
-
/// True if the split impulse position correction is active
bool mIsSplitImpulseActive;
@@ -342,8 +339,7 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Constructor
- ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex,
- SingleFrameAllocator& allocator);
+ ContactSolver(SingleFrameAllocator& allocator);
/// Destructor
~ContactSolver() = default;
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 66b13a8e..054c4dc5 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -41,8 +41,7 @@ using namespace std;
*/
DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
: CollisionWorld(),
- mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator),
- mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
+ mContactSolver(mSingleFrameAllocator),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
@@ -167,7 +166,7 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
// Get the constrained velocity
- uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
+ uint indexArray = bodies[b]->mArrayIndex;
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
@@ -205,7 +204,7 @@ void DynamicsWorld::updateBodiesState() {
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
- uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
+ uint index = bodies[b]->mArrayIndex;
// Update the linear and angular velocity of the body
bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index];
@@ -250,21 +249,14 @@ void DynamicsWorld::initVelocityArrays() {
assert(mConstrainedPositions != nullptr);
assert(mConstrainedOrientations != nullptr);
- // Reset the velocities arrays
- for (uint i=0; i<nbBodies; i++) {
+ // Initialize the map of body indexes in the velocity arrays
+ uint i = 0;
+ for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+
mSplitLinearVelocities[i].setToZero();
mSplitAngularVelocities[i].setToZero();
- }
- // Initialize the map of body indexes in the velocity arrays
- mMapBodyToConstrainedVelocityIndex.clear();
- std::set<RigidBody*>::const_iterator it;
- uint indexBody = 0;
- for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
-
- // Add the body into the map
- mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody));
- indexBody++;
+ (*it)->mArrayIndex = i++;
}
}
@@ -289,7 +281,7 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
// Insert the body into the map of constrained velocities
- uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
+ uint indexBody = bodies[b]->mArrayIndex;
assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h
index 366b2770..0c614b65 100644
--- a/src/engine/DynamicsWorld.h
+++ b/src/engine/DynamicsWorld.h
@@ -100,9 +100,6 @@ class DynamicsWorld : public CollisionWorld {
/// Array of constrained rigid bodies orientation (for position error correction)
Quaternion* mConstrainedOrientations;
- /// Map body to their index in the constrained velocities array
- std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
-
/// Number of islands in the world
uint mNbIslands;
From 47869627d13d71f3997adea53731890e55a10de4 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 14 Dec 2017 20:24:19 +0100
Subject: [PATCH 128/133] Fix issue in Transform
---
src/mathematics/Transform.h | 6 +++++-
1 file changed, 5 insertions(+), 1 deletion(-)
diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h
index a5248fc2..aeaf4234 100644
--- a/src/mathematics/Transform.h
+++ b/src/mathematics/Transform.h
@@ -223,7 +223,11 @@ inline Vector3 Transform::operator*(const Vector3& vector) const {
// Operator of multiplication of a transform with another one
inline Transform Transform::operator*(const Transform& transform2) const {
- const decimal prodX = mOrientation.w * transform2.mPosition.x + mOrientation.w * transform2.mPosition.z
+ // The following code is equivalent to this
+ //return Transform(mPosition + mOrientation * transform2.mPosition,
+ // mOrientation * transform2.mOrientation);
+
+ const decimal prodX = mOrientation.w * transform2.mPosition.x + mOrientation.y * transform2.mPosition.z
- mOrientation.z * transform2.mPosition.y;
const decimal prodY = mOrientation.w * transform2.mPosition.y + mOrientation.z * transform2.mPosition.x
- mOrientation.x * transform2.mPosition.z;
From f2ee00ca68b409234b84d134cdc9845c9c55e2c3 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 14 Dec 2017 22:25:52 +0100
Subject: [PATCH 129/133] Use List instead of std::vector compute segment
clipping in SAT algorithm
---
src/body/RigidBody.cpp | 4 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 10 ++---
src/mathematics/mathematics_functions.cpp | 43 +++++++++++--------
src/mathematics/mathematics_functions.h | 7 +--
4 files changed, 36 insertions(+), 28 deletions(-)
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index 01e1694b..e4ce4458 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -39,10 +39,10 @@ using namespace reactphysics3d;
* @param id The ID of the body
*/
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id)
- : CollisionBody(transform, world, id), mInitMass(decimal(1.0)),
+ : CollisionBody(transform, world, id), mArrayIndex(0), mInitMass(decimal(1.0)),
mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
- mJointsList(nullptr), mArrayIndex(0) {
+ mJointsList(nullptr) {
// Compute the inverse mass
mMassInverse = decimal(1.0) / mInitMass;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 5e40f45f..2605eaf9 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -374,8 +374,8 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
uint firstEdgeIndex = face.edgeIndex;
uint edgeIndex = firstEdgeIndex;
- std::vector<Vector3> planesPoints;
- std::vector<Vector3> planesNormals;
+ List<Vector3> planesPoints(mMemoryAllocator, 2);
+ List<Vector3> planesNormals(mMemoryAllocator, 2);
// For each adjacent edge of the separating face of the polyhedron
do {
@@ -393,15 +393,15 @@ bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
Vector3 clipPlaneNormal = faceNormal.cross(edgeDirection);
// Construct a clipping plane for each adjacent edge of the separating face of the polyhedron
- planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex));
- planesNormals.push_back(clipPlaneNormal);
+ planesPoints.add(polyhedron->getVertexPosition(edge.vertexIndex));
+ planesNormals.add(clipPlaneNormal);
edgeIndex = edge.nextEdgeIndex;
} while(edgeIndex != firstEdgeIndex);
// First we clip the inner segment of the capsule with the four planes of the adjacent faces
- std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals);
+ List<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals, mMemoryAllocator);
// Project the two clipped points into the polyhedron face
const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index b82c9fbd..0d8994f4 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -222,27 +222,34 @@ decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, co
// Clip a segment against multiple planes and return the clipped segment vertices
// This method implements the Sutherland–Hodgman clipping algorithm
-std::vector<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
- const std::vector<Vector3>& planesPoints,
- const std::vector<Vector3>& planesNormals) {
+List<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
+ const List<Vector3>& planesPoints,
+ const List<Vector3>& planesNormals,
+ Allocator& allocator) {
assert(planesPoints.size() == planesNormals.size());
- std::vector<Vector3> inputVertices = {segA, segB};
- std::vector<Vector3> outputVertices;
+ List<Vector3> list1(allocator, 2);
+ List<Vector3> list2(allocator, 2);
+
+ List<Vector3>* inputVertices = &list1;
+ List<Vector3>* outputVertices = &list2;
+
+ inputVertices->add(segA);
+ inputVertices->add(segB);
// For each clipping plane
for (uint p=0; p<planesPoints.size(); p++) {
// If there is no more vertices, stop
- if (inputVertices.empty()) return inputVertices;
+ if (inputVertices->size() == 0) return *inputVertices;
- assert(inputVertices.size() == 2);
+ assert(inputVertices->size() == 2);
- outputVertices.clear();
+ outputVertices->clear();
- Vector3& v1 = inputVertices[0];
- Vector3& v2 = inputVertices[1];
+ Vector3& v1 = (*inputVertices)[0];
+ Vector3& v2 = (*inputVertices)[1];
decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]);
decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]);
@@ -257,39 +264,40 @@ std::vector<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA,
decimal t = computePlaneSegmentIntersection(v1, v2, planesNormals[p].dot(planesPoints[p]), planesNormals[p]);
if (t >= decimal(0) && t <= decimal(1.0)) {
- outputVertices.push_back(v1 + t * (v2 - v1));
+ outputVertices->add(v1 + t * (v2 - v1));
}
else {
- outputVertices.push_back(v2);
+ outputVertices->add(v2);
}
}
else {
- outputVertices.push_back(v1);
+ outputVertices->add(v1);
}
// Add the second vertex
- outputVertices.push_back(v2);
+ outputVertices->add(v2);
}
else { // If the second vertex is behind the clipping plane
// If the first vertex is in front of the clippling plane
if (v1DotN >= decimal(0.0)) {
- outputVertices.push_back(v1);
+ outputVertices->add(v1);
// The first point we keep is the intersection between the segment v1, v2 and the clipping plane
decimal t = computePlaneSegmentIntersection(v1, v2, -planesNormals[p].dot(planesPoints[p]), -planesNormals[p]);
if (t >= decimal(0.0) && t <= decimal(1.0)) {
- outputVertices.push_back(v1 + t * (v2 - v1));
+ outputVertices->add(v1 + t * (v2 - v1));
}
}
}
inputVertices = outputVertices;
+ outputVertices = p % 2 == 0 ? &list1 : &list2;
}
- return outputVertices;
+ return *outputVertices;
}
// Clip a polygon against multiple planes and return the clipped polygon vertices
@@ -302,7 +310,6 @@ List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygon
uint nbMaxElements = polygonVertices.size() + planesPoints.size();
List<Vector3> list1(allocator, nbMaxElements);
List<Vector3> list2(allocator, nbMaxElements);
- List<decimal> dotProducts(allocator, nbMaxElements * 2);
const List<Vector3>* inputVertices = &polygonVertices;
List<Vector3>* outputVertices = &list2;
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index ff6ac6f4..5081d741 100755
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -112,9 +112,10 @@ decimal computePlaneSegmentIntersection(const Vector3& segA, const Vector3& segB
decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& linePointB, const Vector3& point);
/// Clip a segment against multiple planes and return the clipped segment vertices
-std::vector<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
- const std::vector<Vector3>& planesPoints,
- const std::vector<Vector3>& planesNormals);
+List<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
+ const List<Vector3>& planesPoints,
+ const List<Vector3>& planesNormals,
+ Allocator& allocator);
/// Clip a polygon against multiple planes and return the clipped polygon vertices
List<Vector3> clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,
From 8f126a75d6d38dd4152739fab4257bd0f427e514 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Wed, 27 Dec 2017 20:53:09 +0100
Subject: [PATCH 130/133] Use List in HalfEdgeStructure with some changes in
memory allocation
---
CMakeLists.txt | 2 +
src/body/CollisionBody.cpp | 2 +-
src/body/RigidBody.cpp | 2 +-
src/collision/CollisionDetection.cpp | 7 +
src/collision/ContactManifoldSet.cpp | 1 -
src/collision/HalfEdgeStructure.cpp | 4 +-
src/collision/HalfEdgeStructure.h | 29 +-
src/collision/MiddlePhaseTriangleCallback.cpp | 2 +-
src/collision/NarrowPhaseInfo.cpp | 6 +-
src/collision/PolyhedronMesh.cpp | 11 +-
src/collision/PolyhedronMesh.h | 1 +
src/collision/ProxyShape.cpp | 6 +-
src/collision/ProxyShape.h | 5 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 5 +-
src/collision/shapes/BoxShape.cpp | 33 +-
src/collision/shapes/BoxShape.h | 4 +-
src/collision/shapes/CapsuleShape.cpp | 2 +-
src/collision/shapes/CapsuleShape.h | 2 +-
src/collision/shapes/CollisionShape.h | 2 +-
src/collision/shapes/ConcaveMeshShape.cpp | 8 +-
src/collision/shapes/ConcaveMeshShape.h | 7 +-
src/collision/shapes/ConvexMeshShape.cpp | 2 +-
src/collision/shapes/ConvexMeshShape.h | 2 +-
src/collision/shapes/HeightFieldShape.cpp | 8 +-
src/collision/shapes/HeightFieldShape.h | 7 +-
src/collision/shapes/SphereShape.cpp | 2 +-
src/collision/shapes/SphereShape.h | 2 +-
src/collision/shapes/TriangleShape.cpp | 15 +-
src/collision/shapes/TriangleShape.h | 7 +-
src/containers/List.h | 105 ++--
src/engine/CollisionWorld.cpp | 1 -
src/engine/CollisionWorld.h | 1 +
src/engine/OverlappingPair.cpp | 20 +-
src/memory/MemoryManager.cpp | 32 ++
src/memory/MemoryManager.h | 74 +++
src/memory/PoolAllocator.cpp | 2 +-
src/memory/PoolAllocator.h | 2 +-
test/tests/collision/TestCollisionWorld.h | 499 +++++++++++++-----
test/tests/collision/TestDynamicAABBTree.h | 29 +
test/tests/collision/TestHalfEdgeStructure.h | 47 +-
test/tests/collision/TestRaycast.h | 4 +-
.../mathematics/TestMathematicsFunctions.h | 18 +-
42 files changed, 730 insertions(+), 290 deletions(-)
create mode 100644 src/memory/MemoryManager.cpp
create mode 100644 src/memory/MemoryManager.h
mode change 100644 => 100755 test/tests/collision/TestCollisionWorld.h
mode change 100644 => 100755 test/tests/collision/TestDynamicAABBTree.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e7e2df87..e585e0d1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -193,6 +193,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/memory/SingleFrameAllocator.h"
"src/memory/SingleFrameAllocator.cpp"
"src/memory/DefaultAllocator.h"
+ "src/memory/MemoryManager.h"
+ "src/memory/MemoryManager.cpp"
"src/containers/Stack.h"
"src/containers/LinkedList.h"
"src/containers/List.h"
diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index e588f93c..56493d28 100644
--- a/src/body/CollisionBody.cpp
+++ b/src/body/CollisionBody.cpp
@@ -72,7 +72,7 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
- transform, decimal(1));
+ transform, decimal(1), mWorld.mPoolAllocator);
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index e4ce4458..7419fc4c 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -226,7 +226,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
- transform, mass);
+ transform, mass, mWorld.mPoolAllocator);
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index c0b4d4e9..e57c5fd8 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -288,7 +288,14 @@ void CollisionDetection::computeNarrowPhase() {
lastCollisionFrameInfo->isValid = true;
}
+ NarrowPhaseInfo* narrowPhaseInfoToDelete = currentNarrowPhaseInfo;
currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
+
+ // Call the destructor
+ narrowPhaseInfoToDelete->~NarrowPhaseInfo();
+
+ // Release the allocated memory for the narrow phase info
+ mSingleFrameAllocator.release(narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo));
}
// Convert the potential contact into actual contacts
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index ce3e4492..5f70bbc0 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -225,7 +225,6 @@ void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
// Delete the contact manifold
manifold->~ContactManifold();
mMemoryAllocator.release(manifold, sizeof(ContactManifold));
-
mNbManifolds--;
}
diff --git a/src/collision/HalfEdgeStructure.cpp b/src/collision/HalfEdgeStructure.cpp
index 0f7b8f6e..70a731d5 100644
--- a/src/collision/HalfEdgeStructure.cpp
+++ b/src/collision/HalfEdgeStructure.cpp
@@ -96,8 +96,8 @@ void HalfEdgeStructure::init() {
mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1));
mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex));
- mEdges.push_back(itEdge->second);
- mEdges.push_back(edge);
+ mEdges.add(itEdge->second);
+ mEdges.add(edge);
}
currentFaceEdges.push_back(pairV1V2);
diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h
index ec4fff35..e398913f 100644
--- a/src/collision/HalfEdgeStructure.h
+++ b/src/collision/HalfEdgeStructure.h
@@ -50,14 +50,14 @@ class HalfEdgeStructure {
};
struct Face {
- uint edgeIndex; // Index of an half-edge of the face
- std::vector<uint> faceVertices; // Index of the vertices of the face
+ uint edgeIndex; // Index of an half-edge of the face
+ List<uint> faceVertices; // Index of the vertices of the face
/// Constructor
- Face() {}
+ Face(Allocator& allocator) : faceVertices(allocator) {}
/// Constructor
- Face(std::vector<uint> vertices) : faceVertices(vertices) {}
+ Face(List<uint> vertices) : faceVertices(vertices) {}
};
struct Vertex {
@@ -70,19 +70,24 @@ class HalfEdgeStructure {
private:
+ /// Reference to a memory allocator
+ Allocator& mAllocator;
+
/// All the faces
- std::vector<Face> mFaces;
+ List<Face> mFaces;
/// All the vertices
- std::vector<Vertex> mVertices;
+ List<Vertex> mVertices;
/// All the half-edges
- std::vector<Edge> mEdges;
+ List<Edge> mEdges;
public:
/// Constructor
- HalfEdgeStructure() = default;
+ HalfEdgeStructure(Allocator& allocator, uint facesCapacity, uint verticesCapacity,
+ uint edgesCapacity) :mAllocator(allocator), mFaces(allocator, facesCapacity),
+ mVertices(allocator, verticesCapacity), mEdges(allocator, edgesCapacity) {}
/// Destructor
~HalfEdgeStructure() = default;
@@ -94,7 +99,7 @@ class HalfEdgeStructure {
uint addVertex(uint vertexPointIndex);
/// Add a face
- void addFace(std::vector<uint> faceVertices);
+ void addFace(List<uint> faceVertices);
/// Return the number of faces
uint getNbFaces() const;
@@ -119,16 +124,16 @@ class HalfEdgeStructure {
// Add a vertex
inline uint HalfEdgeStructure::addVertex(uint vertexPointIndex) {
Vertex vertex(vertexPointIndex);
- mVertices.push_back(vertex);
+ mVertices.add(vertex);
return mVertices.size() - 1;
}
// Add a face
-inline void HalfEdgeStructure::addFace(std::vector<uint> faceVertices) {
+inline void HalfEdgeStructure::addFace(List<uint> faceVertices) {
// Create a new face
Face face(faceVertices);
- mFaces.push_back(face);
+ mFaces.add(face);
}
// Return the number of faces
diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp
index f8736a0b..a53271a4 100644
--- a/src/collision/MiddlePhaseTriangleCallback.cpp
+++ b/src/collision/MiddlePhaseTriangleCallback.cpp
@@ -34,7 +34,7 @@ void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, co
// Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the
// destructor of the corresponding NarrowPhaseInfo.
TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
- TriangleShape(trianglePoints, verticesNormals, shapeId);
+ TriangleShape(trianglePoints, verticesNormals, shapeId, mAllocator);
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp
index a6440367..0189db99 100644
--- a/src/collision/NarrowPhaseInfo.cpp
+++ b/src/collision/NarrowPhaseInfo.cpp
@@ -52,11 +52,13 @@ NarrowPhaseInfo::~NarrowPhaseInfo() {
// Release the memory of the TriangleShape (this memory was allocated in the
// MiddlePhaseTriangleCallback::testTriangle() method)
if (collisionShape1->getName() == CollisionShapeName::TRIANGLE) {
+ collisionShape1->~CollisionShape();
collisionShapeAllocator.release(collisionShape1, sizeof(TriangleShape));
}
if (collisionShape2->getName() == CollisionShapeName::TRIANGLE) {
- collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape));
- }
+ collisionShape2->~CollisionShape();
+ collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape));
+ }
}
// Add a new contact point
diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp
index ec8b661d..08a84e7d 100644
--- a/src/collision/PolyhedronMesh.cpp
+++ b/src/collision/PolyhedronMesh.cpp
@@ -25,6 +25,7 @@
// Libraries
#include "PolyhedronMesh.h"
+#include "memory/MemoryManager.h"
using namespace reactphysics3d;
@@ -34,7 +35,11 @@ using namespace reactphysics3d;
* Create a polyhedron mesh given an array of polygons.
* @param polygonVertexArray Pointer to the array of polygons and their vertices
*/
-PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) {
+PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray)
+ : mHalfEdgeStructure(MemoryManager::getDefaultAllocator(),
+ polygonVertexArray->getNbFaces(),
+ polygonVertexArray->getNbVertices(),
+ (polygonVertexArray->getNbFaces() + polygonVertexArray->getNbVertices() - 2) * 2) {
mPolygonVertexArray = polygonVertexArray;
@@ -70,11 +75,11 @@ void PolyhedronMesh::createHalfEdgeStructure() {
// Get the polygon face
PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f);
- std::vector<uint> faceVertices;
+ List<uint> faceVertices(MemoryManager::getDefaultAllocator(), face->nbVertices);
// For each vertex of the face
for (uint v=0; v < face->nbVertices; v++) {
- faceVertices.push_back(mPolygonVertexArray->getVertexIndexInFace(f, v));
+ faceVertices.add(mPolygonVertexArray->getVertexIndexInFace(f, v));
}
assert(faceVertices.size() >= 3);
diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h
index a64db670..b459723a 100644
--- a/src/collision/PolyhedronMesh.h
+++ b/src/collision/PolyhedronMesh.h
@@ -30,6 +30,7 @@
#include "mathematics/mathematics.h"
#include "HalfEdgeStructure.h"
#include "collision/PolygonVertexArray.h"
+#include "memory/DefaultAllocator.h"
#include <vector>
namespace reactphysics3d {
diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp
index 6e6eefbd..f782b2a8 100644
--- a/src/collision/ProxyShape.cpp
+++ b/src/collision/ProxyShape.cpp
@@ -35,9 +35,9 @@ using namespace reactphysics3d;
* @param transform Transformation from collision shape local-space to body local-space
* @param mass Mass of the collision shape (in kilograms)
*/
-ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass)
+ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass, Allocator& allocator)
:mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
- mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
+ mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF), mAllocator(allocator) {
}
@@ -76,7 +76,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
worldToLocalTransform * ray.point2,
ray.maxFraction);
- bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this);
+ bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this, mAllocator);
// Convert the raycast info into world-space
raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;
diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h
index 8e9a9e9f..0ba34698 100644
--- a/src/collision/ProxyShape.h
+++ b/src/collision/ProxyShape.h
@@ -82,6 +82,9 @@ class ProxyShape {
/// proxy shape will collide with every collision categories by default.
unsigned short mCollideWithMaskBits;
+ /// Memory allocator
+ Allocator& mAllocator;
+
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
@@ -100,7 +103,7 @@ class ProxyShape {
/// Constructor
ProxyShape(CollisionBody* body, CollisionShape* shape,
- const Transform& transform, decimal mass);
+ const Transform& transform, decimal mass, Allocator& allocator);
/// Destructor
virtual ~ProxyShape();
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 2605eaf9..1992cbc7 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -835,9 +835,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
List<Vector3> planesPoints(mMemoryAllocator, nbIncidentFaceVertices); // Points on the clipping planes
// Get all the vertices of the incident face (in the reference local-space)
- std::vector<uint>::const_iterator it;
- for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) {
- const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it);
+ for (uint i=0; i < incidentFace.faceVertices.size(); i++) {
+ const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(incidentFace.faceVertices[i]);
polygonVertices.add(incidentToReferenceTransform * faceVertexIncidentSpace);
}
diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp
index 37667958..079b15c5 100644
--- a/src/collision/shapes/BoxShape.cpp
+++ b/src/collision/shapes/BoxShape.cpp
@@ -27,6 +27,7 @@
#include "BoxShape.h"
#include "collision/ProxyShape.h"
#include "configuration.h"
+#include "memory/MemoryManager.h"
#include <vector>
#include <cassert>
@@ -37,7 +38,9 @@ using namespace reactphysics3d;
* @param extent The vector with the three extents of the box (in meters)
*/
BoxShape::BoxShape(const Vector3& extent)
- : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent) {
+ : ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent),
+ mHalfEdgeStructure(MemoryManager::getDefaultAllocator(), 6, 8, 24) {
+
assert(extent.x > decimal(0.0));
assert(extent.y > decimal(0.0));
assert(extent.z > decimal(0.0));
@@ -52,19 +55,21 @@ BoxShape::BoxShape(const Vector3& extent)
mHalfEdgeStructure.addVertex(6);
mHalfEdgeStructure.addVertex(7);
+ DefaultAllocator& allocator = MemoryManager::getDefaultAllocator();
+
// Faces
- std::vector<uint> face0;
- face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3);
- std::vector<uint> face1;
- face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2);
- std::vector<uint> face2;
- face2.push_back(4); face2.push_back(7); face2.push_back(6); face2.push_back(5);
- std::vector<uint> face3;
- face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7);
- std::vector<uint> face4;
- face4.push_back(4); face4.push_back(5); face4.push_back(1); face4.push_back(0);
- std::vector<uint> face5;
- face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3);
+ List<uint> face0(allocator, 4);
+ face0.add(0); face0.add(1); face0.add(2); face0.add(3);
+ List<uint> face1(allocator, 4);
+ face1.add(1); face1.add(5); face1.add(6); face1.add(2);
+ List<uint> face2(allocator, 4);
+ face2.add(4); face2.add(7); face2.add(6); face2.add(5);
+ List<uint> face3(allocator, 4);
+ face3.add(4); face3.add(0); face3.add(3); face3.add(7);
+ List<uint> face4(allocator, 4);
+ face4.add(4); face4.add(5); face4.add(1); face4.add(0);
+ List<uint> face5(allocator, 4);
+ face5.add(2); face5.add(6); face5.add(7); face5.add(3);
mHalfEdgeStructure.addFace(face0);
mHalfEdgeStructure.addFace(face1);
@@ -93,7 +98,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
}
// Raycast method with feedback information
-bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
Vector3 rayDirection = ray.point2 - ray.point1;
decimal tMin = DECIMAL_SMALLEST;
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index d1aa9810..b3e094bd 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -31,7 +31,7 @@
#include "ConvexPolyhedronShape.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
-
+#include "memory/DefaultAllocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -64,7 +64,7 @@ class BoxShape : public ConvexPolyhedronShape {
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp
index 32ea146c..da29fede 100644
--- a/src/collision/shapes/CapsuleShape.cpp
+++ b/src/collision/shapes/CapsuleShape.cpp
@@ -85,7 +85,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
}
// Raycast method with feedback information
-bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
const Vector3 n = ray.point2 - ray.point1;
diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h
index 8aebff58..0da2d1f4 100644
--- a/src/collision/shapes/CapsuleShape.h
+++ b/src/collision/shapes/CapsuleShape.h
@@ -62,7 +62,7 @@ class CapsuleShape : public ConvexShape {
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
/// Raycasting method between a ray one of the two spheres end cap of the capsule
bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index 6792cc09..4381b05c 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -87,7 +87,7 @@ class CollisionShape {
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const=0;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const = 0;
diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp
index ee90ce46..0b49f20c 100644
--- a/src/collision/shapes/ConcaveMeshShape.cpp
+++ b/src/collision/shapes/ConcaveMeshShape.cpp
@@ -111,12 +111,12 @@ 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 {
+bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
PROFILE("ConcaveMeshShape::raycast()", mProfiler);
// Create the callback object that will compute ray casting against triangles
- ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray);
+ ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray, allocator);
#ifdef IS_PROFILING_ACTIVE
@@ -180,7 +180,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
// Create a triangle collision shape
- TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]));
+ TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]), mAllocator);
triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
#ifdef IS_PROFILING_ACTIVE
@@ -192,7 +192,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
- bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
+ bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator);
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h
index 2d0a3e9c..52d49a02 100644
--- a/src/collision/shapes/ConcaveMeshShape.h
+++ b/src/collision/shapes/ConcaveMeshShape.h
@@ -77,6 +77,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
RaycastInfo& mRaycastInfo;
const Ray& mRay;
bool mIsHit;
+ Allocator& mAllocator;
#ifdef IS_PROFILING_ACTIVE
@@ -89,9 +90,9 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
// Constructor
ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape,
- ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray)
+ ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, Allocator& allocator)
: mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape),
- mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false) {
+ mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false), mAllocator(allocator) {
}
@@ -141,7 +142,7 @@ class ConcaveMeshShape : public ConcaveShape {
// -------------------- Methods -------------------- //
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp
index f6011375..19ef5016 100644
--- a/src/collision/shapes/ConvexMeshShape.cpp
+++ b/src/collision/shapes/ConvexMeshShape.cpp
@@ -112,7 +112,7 @@ void ConvexMeshShape::recalculateBounds() {
}
// Raycast method with feedback information
-bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo);
}
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index 0ce696af..1c33d919 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -77,7 +77,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp
index 74276a75..f067a827 100644
--- a/src/collision/shapes/HeightFieldShape.cpp
+++ b/src/collision/shapes/HeightFieldShape.cpp
@@ -212,14 +212,14 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor
// 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 HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
// TODO : Implement raycasting without using an AABB for the ray
// but using a dynamic AABB tree or octree instead
PROFILE("HeightFieldShape::raycast()", mProfiler);
- TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
+ TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this, allocator);
#ifdef IS_PROFILING_ACTIVE
@@ -266,7 +266,7 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) {
// Create a triangle collision shape
- TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId);
+ TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId, mAllocator);
triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
#ifdef IS_PROFILING_ACTIVE
@@ -278,7 +278,7 @@ void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
- bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
+ bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator);
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) {
diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h
index f8b498d5..4190c3c7 100644
--- a/src/collision/shapes/HeightFieldShape.h
+++ b/src/collision/shapes/HeightFieldShape.h
@@ -49,6 +49,7 @@ class TriangleOverlapCallback : public TriangleCallback {
bool mIsHit;
decimal mSmallestHitFraction;
const HeightFieldShape& mHeightFieldShape;
+ Allocator& mAllocator;
#ifdef IS_PROFILING_ACTIVE
@@ -61,9 +62,9 @@ class TriangleOverlapCallback : public TriangleCallback {
// Constructor
TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo,
- const HeightFieldShape& heightFieldShape)
+ const HeightFieldShape& heightFieldShape, Allocator& allocator)
: mRay(ray), mProxyShape(proxyShape), mRaycastInfo(raycastInfo),
- mHeightFieldShape (heightFieldShape) {
+ mHeightFieldShape (heightFieldShape), mAllocator(allocator) {
mIsHit = false;
mSmallestHitFraction = mRay.maxFraction;
}
@@ -143,7 +144,7 @@ class HeightFieldShape : public ConcaveShape {
// -------------------- Methods -------------------- //
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp
index fa6b4f6a..7d155f97 100644
--- a/src/collision/shapes/SphereShape.cpp
+++ b/src/collision/shapes/SphereShape.cpp
@@ -41,7 +41,7 @@ SphereShape::SphereShape(decimal radius)
}
// Raycast method with feedback information
-bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
+bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
const Vector3 m = ray.point1;
decimal c = m.dot(m) - mMargin * mMargin;
diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h
index 70f651de..18883d79 100644
--- a/src/collision/shapes/SphereShape.h
+++ b/src/collision/shapes/SphereShape.h
@@ -55,7 +55,7 @@ class SphereShape : public ConvexShape {
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index e43cc674..4a7eb013 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -33,6 +33,7 @@
using namespace reactphysics3d;
+
// Constructor
/**
* Do not use this constructor. It is supposed to be used internally only.
@@ -43,8 +44,9 @@ using namespace reactphysics3d;
* @param verticesNormals The three vertices normals for smooth mesh collision
* @param margin The collision margin (in meters) around the collision shape
*/
-TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId)
- : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE) {
+TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId,
+ Allocator& allocator)
+ : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mFaces{HalfEdgeStructure::Face(allocator), HalfEdgeStructure::Face(allocator)} {
mPoints[0] = vertices[0];
mPoints[1] = vertices[1];
@@ -60,9 +62,10 @@ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNor
// Faces
for (uint i=0; i<2; i++) {
- mFaces[i].faceVertices.push_back(0);
- mFaces[i].faceVertices.push_back(1);
- mFaces[i].faceVertices.push_back(2);
+ mFaces[i].faceVertices.reserve(3);
+ mFaces[i].faceVertices.add(0);
+ mFaces[i].faceVertices.add(1);
+ mFaces[i].faceVertices.add(2);
mFaces[i].edgeIndex = i;
}
@@ -208,7 +211,7 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3&
// 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 {
+bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
PROFILE("TriangleShape::raycast()", mProfiler);
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index a4ce7726..9a2c756e 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -60,6 +60,7 @@ class TriangleShape : public ConvexPolyhedronShape {
// -------------------- Attribute -------------------- //
+
/// Three points of the triangle
Vector3 mPoints[3];
@@ -90,7 +91,8 @@ class TriangleShape : public ConvexPolyhedronShape {
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
+ Allocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
@@ -110,7 +112,8 @@ class TriangleShape : public ConvexPolyhedronShape {
// -------------------- Methods -------------------- //
/// Constructor
- TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId);
+ TriangleShape(const Vector3* vertices, const Vector3* verticesNormals,
+ uint shapeId, Allocator& allocator);
/// Destructor
virtual ~TriangleShape() override = default;
diff --git a/src/containers/List.h b/src/containers/List.h
index 4915f32d..71ae7ef7 100644
--- a/src/containers/List.h
+++ b/src/containers/List.h
@@ -44,59 +44,38 @@ class List {
// -------------------- Attributes -------------------- //
- /// Pointer to the first element of the list
- T* mElements;
+ /// Buffer for the list elements
+ void* mBuffer;
/// Number of elements in the list
- uint mSize;
+ size_t mSize;
/// Number of allocated elements in the list
- uint mCapacity;
+ size_t mCapacity;
/// Memory allocator
Allocator& mAllocator;
// -------------------- Methods -------------------- //
- /// Allocate more memory for the elements of the list
- void allocateMemory(uint nbElementsToAllocate) {
-
- assert(nbElementsToAllocate > mCapacity);
-
- // Allocate memory for the new array
- void* newMemory = mAllocator.allocate(nbElementsToAllocate * sizeof(T));
-
- if (mElements != nullptr) {
-
- // Copy the elements to the new allocated memory location
- std::memcpy(newMemory, static_cast<void*>(mElements), mSize * sizeof(T));
-
- // Release the previously allocated memory
- mAllocator.release(mElements, mCapacity * sizeof(T));
- }
-
- mElements = static_cast<T*>(newMemory);
-
- mCapacity = nbElementsToAllocate;
- }
public:
// -------------------- Methods -------------------- //
/// Constructor
- List(Allocator& allocator, uint capacity = 0)
- : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) {
+ List(Allocator& allocator, size_t capacity = 0)
+ : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) {
if (capacity > 0) {
// Allocate memory
- allocateMemory(capacity);
+ reserve(capacity);
}
}
/// Copy constructor
- List(const List<T>& list) : mElements(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) {
+ List(const List<T>& list) : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) {
// All all the elements of the list to the current one
addRange(list);
@@ -112,22 +91,66 @@ class List {
clear();
// Release the memory allocated on the heap
- mAllocator.release(static_cast<void*>(mElements), mCapacity * sizeof(T));
+ mAllocator.release(mBuffer, mCapacity * sizeof(T));
}
}
+ /// Allocate memory for a given number of elements
+ void reserve(size_t capacity) {
+
+ if (capacity <= mCapacity) return;
+
+ // Allocate memory for the new array
+ void* newMemory = mAllocator.allocate(capacity * sizeof(T));
+
+ if (mBuffer != nullptr) {
+
+ // Copy the elements to the new allocated memory location
+ std::memcpy(newMemory, mBuffer, mSize * sizeof(T));
+
+ // Release the previously allocated memory
+ mAllocator.release(mBuffer, mCapacity * sizeof(T));
+ }
+
+ mBuffer = newMemory;
+ assert(mBuffer != nullptr);
+
+ mCapacity = capacity;
+ }
+
/// Add an element into the list
void add(const T& element) {
// If we need to allocate more memory
if (mSize == mCapacity) {
- allocateMemory(mCapacity == 0 ? 1 : mCapacity * 2);
+ reserve(mCapacity == 0 ? 1 : mCapacity * 2);
}
- mElements[mSize] = element;
+ // Use the copy-constructor to construct the element
+ new (static_cast<char*>(mBuffer) + mSize * sizeof(T)) T(element);
+
mSize++;
}
+ /// Remove an element from the list at a given index
+ void remove(uint index) {
+
+ assert(index >= 0 && index < mSize);
+
+ // Call the destructor
+ (static_cast<T*>(mBuffer)[index]).~T();
+
+ mSize--;
+
+ if (index != mSize) {
+
+ // Move the elements to fill in the empty slot
+ char* dest = static_cast<char*>(mBuffer) + index * sizeof(T);
+ char* src = dest + sizeof(T);
+ std::memcpy(static_cast<void*>(dest), static_cast<void*>(src), (mSize - index) * sizeof(T));
+ }
+ }
+
/// Append another list to the current one
void addRange(const List<T>& list) {
@@ -135,12 +158,13 @@ class List {
if (mSize + list.size() > mCapacity) {
// Allocate memory
- allocateMemory(mSize + list.size());
+ reserve(mSize + list.size());
}
// Add the elements of the list to the current one
for(uint i=0; i<list.size(); i++) {
- mElements[mSize] = list[i];
+
+ new (static_cast<char*>(mBuffer) + mSize * sizeof(T)) T(list[i]);
mSize++;
}
}
@@ -150,27 +174,32 @@ class List {
// Call the destructor of each element
for (uint i=0; i < mSize; i++) {
- mElements[i].~T();
+ (static_cast<T*>(mBuffer)[i]).~T();
}
mSize = 0;
}
/// Return the number of elments in the list
- uint size() const {
+ size_t size() const {
return mSize;
}
+ /// Return the capacity of the list
+ size_t capacity() const {
+ return mCapacity;
+ }
+
/// Overloaded index operator
T& operator[](const uint index) {
assert(index >= 0 && index < mSize);
- return mElements[index];
+ return (static_cast<T*>(mBuffer)[index]);
}
/// Overloaded const index operator
const T& operator[](const uint index) const {
assert(index >= 0 && index < mSize);
- return mElements[index];
+ return (static_cast<T*>(mBuffer)[index]);
}
/// Overloaded assignment operator
diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp
index eeb06305..8b29e4a1 100644
--- a/src/engine/CollisionWorld.cpp
+++ b/src/engine/CollisionWorld.cpp
@@ -187,4 +187,3 @@ AABB CollisionWorld::getWorldAABB(const ProxyShape* proxyShape) const {
return mCollisionDetection.getWorldAABB(proxyShape);
}
-
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index 6c558d6d..ead42710 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -39,6 +39,7 @@
#include "collision/CollisionDetection.h"
#include "constraint/Joint.h"
#include "constraint/ContactPoint.h"
+#include "memory/DefaultAllocator.h"
#include "memory/PoolAllocator.h"
#include "EventListener.h"
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index eba2fb80..431476ed 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -116,20 +116,16 @@ void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo
// Clear all the potential contact manifolds
void OverlappingPair::clearPotentialContactManifolds() {
- // Do we need to release memory
- if (mTempMemoryAllocator.isReleaseNeeded()) {
+ ContactManifoldInfo* element = mPotentialContactManifolds;
+ while(element != nullptr) {
- ContactManifoldInfo* element = mPotentialContactManifolds;
- while(element != nullptr) {
+ // Remove the proxy collision shape
+ ContactManifoldInfo* elementToRemove = element;
+ element = element->getNext();
- // Remove the proxy collision shape
- ContactManifoldInfo* elementToRemove = element;
- element = element->getNext();
-
- // Delete the element
- elementToRemove->~ContactManifoldInfo();
- mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo));
- }
+ // Delete the element
+ elementToRemove->~ContactManifoldInfo();
+ mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo));
}
mPotentialContactManifolds = nullptr;
diff --git a/src/memory/MemoryManager.cpp b/src/memory/MemoryManager.cpp
new file mode 100644
index 00000000..85832cb1
--- /dev/null
+++ b/src/memory/MemoryManager.cpp
@@ -0,0 +1,32 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2015 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "MemoryManager.h"
+
+using namespace reactphysics3d;
+
+// Static variables
+DefaultAllocator MemoryManager::mDefaultAllocator;
diff --git a/src/memory/MemoryManager.h b/src/memory/MemoryManager.h
new file mode 100644
index 00000000..922a5a17
--- /dev/null
+++ b/src/memory/MemoryManager.h
@@ -0,0 +1,74 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_MEMORY_MANAGER_H
+#define REACTPHYSICS3D_MEMORY_MANAGER_H
+
+// Libraries
+#include "memory/DefaultAllocator.h"
+
+/// Namespace ReactPhysics3D
+namespace reactphysics3d {
+
+// Class MemoryManager
+/**
+ * The memory manager is used to store the different memory allocators that are used
+ * by the library.
+ */
+class MemoryManager {
+
+ private:
+
+ /// Default memory allocator
+ static DefaultAllocator mDefaultAllocator;
+
+ public:
+
+ /// Memory allocation types
+ enum class AllocationType {
+ Default, // Default memory allocator
+ Pool, // Memory pool allocator
+ Frame, // Single frame memory allocator
+ };
+
+ /// Constructor
+ MemoryManager();
+
+ /// Destructor
+ ~MemoryManager();
+
+ /// Return the default memory allocator
+ static DefaultAllocator& getDefaultAllocator();
+};
+
+// Return the default memory allocator
+inline DefaultAllocator& MemoryManager::getDefaultAllocator() {
+ return mDefaultAllocator;
+}
+
+}
+
+#endif
+
diff --git a/src/memory/PoolAllocator.cpp b/src/memory/PoolAllocator.cpp
index dc1306bc..53cee0a1 100644
--- a/src/memory/PoolAllocator.cpp
+++ b/src/memory/PoolAllocator.cpp
@@ -126,7 +126,7 @@ void* PoolAllocator::allocate(size_t size) {
}
else { // If there is no more free memory units in the corresponding heap
- // If we need to allocate more memory to containsthe blocks
+ // If we need to allocate more memory to contains the blocks
if (mNbCurrentMemoryBlocks == mNbAllocatedMemoryBlocks) {
// Allocate more memory to contain the blocks
diff --git a/src/memory/PoolAllocator.h b/src/memory/PoolAllocator.h
index 0664e559..b06778d5 100644
--- a/src/memory/PoolAllocator.h
+++ b/src/memory/PoolAllocator.h
@@ -94,7 +94,7 @@ class PoolAllocator : public Allocator {
/// Size of the memory units that each heap is responsible to allocate
static size_t mUnitSizes[NB_HEAPS];
- /// Lookup table that mape size to allocate to the index of the
+ /// Lookup table that map the size to allocate to the index of the
/// corresponding heap we will use for the allocation.
static int mMapSizeToHeapIndex[MAX_UNIT_SIZE + 1];
diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h
old mode 100644
new mode 100755
index 678b5f21..ad0c92cf
--- a/test/tests/collision/TestCollisionWorld.h
+++ b/test/tests/collision/TestCollisionWorld.h
@@ -40,21 +40,108 @@ enum CollisionCategory {
CATEGORY_3 = 0x0004
};
-// TODO : Add test for concave shape collision here
+// Contact point collision data
+struct CollisionPointData {
+
+ Vector3 localPointBody1;
+ Vector3 localPointBody2;
+ decimal penetrationDepth;
+
+ CollisionPointData(const Vector3& point1, const Vector3& point2, decimal penDepth) {
+ localPointBody1 = point1;
+ localPointBody2 = point2;
+ penetrationDepth = penDepth;
+ }
+
+ bool isContactPointSimilarTo(const Vector3& pointBody1, const Vector3& pointBody2, decimal penDepth, decimal epsilon = 0.001) {
+
+ return approxEqual(pointBody1, localPointBody1, epsilon) &&
+ approxEqual(pointBody2, localPointBody2, epsilon) &&
+ approxEqual(penetrationDepth, penDepth, epsilon);
+ }
+};
+
+// Contact manifold collision data
+struct CollisionManifoldData {
+
+ std::vector<CollisionPointData> contactPoints;
+
+ int getNbContactPoints() const {
+ return contactPoints.size();
+ }
+
+ bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) {
+
+ std::vector<CollisionPointData>::iterator it;
+ for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
+
+ if (it->isContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) {
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+};
+
+// Collision data between two proxy shapes
+struct CollisionData {
+
+ std::pair<const ProxyShape*, const ProxyShape*> proxyShapes;
+ std::pair<CollisionBody*, CollisionBody*> bodies;
+ std::vector<CollisionManifoldData> contactManifolds;
+
+ int getNbContactManifolds() const {
+ return contactManifolds.size();
+ }
+
+ int getTotalNbContactPoints() const {
+
+ int nbPoints = 0;
+
+ std::vector<CollisionManifoldData>::const_iterator it;
+ for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) {
+
+ nbPoints += it->getNbContactPoints();
+ }
+
+ return nbPoints;
+ }
+
+ bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) {
+
+ std::vector<CollisionManifoldData>::iterator it;
+ for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) {
+
+ if (it->hasContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) {
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+};
// Class
class WorldCollisionCallback : public CollisionCallback
{
+ private:
+
+ std::vector<std::pair<const ProxyShape*, const ProxyShape*>> mCollisionData;
+
+ std::pair<const ProxyShape*, const ProxyShape*> getCollisionKeyPair(std::pair<const ProxyShape*, const ProxyShape*> pair) const {
+
+ if (pair.first > pair.second) {
+ return std::make_pair(pair.second, pair.first);
+ }
+
+ return pair;
+ }
+
public:
- bool boxCollideWithSphere1;
- bool sphere1CollideWithSphere2;
-
- CollisionBody* boxBody;
- CollisionBody* sphere1Body;
- CollisionBody* sphere2Body;
- CollisionBody* cylinderBody;
-
WorldCollisionCallback()
{
reset();
@@ -62,30 +149,79 @@ class WorldCollisionCallback : public CollisionCallback
void reset()
{
- boxCollideWithSphere1 = false;
- sphere1CollideWithSphere2 = false;
+ mCollisionData.clear();
}
- // This method will be called for contact
+ bool hasContacts() const {
+ return mCollisionData.size() > 0;
+ }
+
+ bool areProxyShapesColliding(const ProxyShape* proxyShape1, const ProxyShape* proxyShape2) {
+ return std::find(mCollisionData.begin(), mCollisionData.end(), getCollisionKeyPair(std::make_pair(proxyShape1, proxyShape2))) != mCollisionData.end();
+ }
+
+ // This method will be called for each contact
virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
- if (isContactBetweenBodies(boxBody, sphere1Body, collisionCallbackInfo)) {
- boxCollideWithSphere1 = true;
- }
- else if (isContactBetweenBodies(sphere1Body, sphere2Body, collisionCallbackInfo)) {
- sphere1CollideWithSphere2 = true;
- }
- }
+ CollisionData collisionData;
+ collisionData.bodies = std::make_pair(collisionCallbackInfo.body1, collisionCallbackInfo.body2);
+ collisionData.proxyShapes = std::make_pair(collisionCallbackInfo.proxyShape1, collisionCallbackInfo.proxyShape2);
- bool isContactBetweenBodies(const CollisionBody* body1, const CollisionBody* body2,
- const CollisionCallbackInfo& collisionCallbackInfo) {
- return (collisionCallbackInfo.body1->getID() == body1->getID() &&
- collisionCallbackInfo.body2->getID() == body2->getID()) ||
- (collisionCallbackInfo.body2->getID() == body1->getID() &&
- collisionCallbackInfo.body1->getID() == body2->getID());
+ ContactManifoldListElement* element = collisionCallbackInfo.contactManifoldElements;
+ while (element != nullptr) {
+
+ ContactManifold* contactManifold = element->getContactManifold();
+
+ CollisionManifoldData collisionManifold;
+
+ ContactPoint* contactPoint = contactManifold->getContactPoints();
+ while (contactPoint != nullptr) {
+
+ CollisionPointData collisionPoint(contactPoint->getLocalPointOnShape1(), contactPoint->getLocalPointOnShape2(), contactPoint->getPenetrationDepth());
+ collisionManifold.contactPoints.push_back(collisionPoint);
+
+ contactPoint = contactPoint->getNext();
+ }
+
+ collisionData.contactManifolds.push_back(collisionManifold);
+
+ element = element->getNext();
+ }
}
};
+/// Overlap callback
+class WorldOverlapCallback : public OverlapCallback {
+
+ private:
+
+ CollisionBody* mOverlapBody;
+
+ public:
+
+ /// Destructor
+ virtual ~WorldOverlapCallback() {
+ reset();
+ }
+
+ /// This method will be called for each reported overlapping bodies
+ virtual void notifyOverlap(CollisionBody* collisionBody) override {
+
+ }
+
+ void reset() {
+ mOverlapBody = nullptr;
+ }
+
+ bool hasOverlap() const {
+ return mOverlapBody != nullptr;
+ }
+
+ CollisionBody* getOverlapBody() {
+ return mOverlapBody;
+ }
+};
+
// Class TestCollisionWorld
/**
* Unit test for the CollisionWorld class.
@@ -100,22 +236,29 @@ class TestCollisionWorld : public Test {
CollisionWorld* mWorld;
// Bodies
- CollisionBody* mBoxBody;
- CollisionBody* mSphere1Body;
- CollisionBody* mSphere2Body;
+ CollisionBody* mBoxBody1;
+ CollisionBody* mBoxBody2;
+ CollisionBody* mSphereBody1;
+ CollisionBody* mSphereBody2;
// Collision shapes
- BoxShape* mBoxShape;
- SphereShape* mSphereShape;
+ BoxShape* mBoxShape1;
+ BoxShape* mBoxShape2;
+ SphereShape* mSphereShape1;
+ SphereShape* mSphereShape2;
// Proxy shapes
- ProxyShape* mBoxProxyShape;
- ProxyShape* mSphere1ProxyShape;
- ProxyShape* mSphere2ProxyShape;
+ ProxyShape* mBoxProxyShape1;
+ ProxyShape* mBoxProxyShape2;
+ ProxyShape* mSphereProxyShape1;
+ ProxyShape* mSphereProxyShape2;
- // Collision callback class
+ // Collision callback
WorldCollisionCallback mCollisionCallback;
+ // Overlap callback
+ WorldOverlapCallback mOverlapCallback;
+
public :
// ---------- Methods ---------- //
@@ -123,147 +266,243 @@ class TestCollisionWorld : public Test {
/// Constructor
TestCollisionWorld(const std::string& name) : Test(name) {
- // Create the world
+ // Create the collision world
mWorld = new CollisionWorld();
- // Create the bodies
- Transform boxTransform(Vector3(10, 0, 0), Quaternion::identity());
- mBoxBody = mWorld->createCollisionBody(boxTransform);
- mBoxShape = new BoxShape(Vector3(3, 3, 3));
- mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, Transform::identity());
+ // ---------- Boxes ---------- //
+ Transform boxTransform1(Vector3(-20, 20, 0), Quaternion::identity());
+ mBoxBody1 = mWorld->createCollisionBody(boxTransform1);
+ mBoxShape1 = new BoxShape(Vector3(3, 3, 3));
+ mBoxProxyShape1 = mBoxBody1->addCollisionShape(mBoxShape1, Transform::identity());
- mSphereShape = new SphereShape(3.0);
- Transform sphere1Transform(Vector3(10,5, 0), Quaternion::identity());
- mSphere1Body = mWorld->createCollisionBody(sphere1Transform);
- mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, Transform::identity());
+ Transform boxTransform2(Vector3(-10, 20, 0), Quaternion::identity());
+ mBoxBody2 = mWorld->createCollisionBody(boxTransform2);
+ mBoxShape2 = new BoxShape(Vector3(2, 2, 2));
+ mBoxProxyShape2 = mBoxBody2->addCollisionShape(mBoxShape1, Transform::identity());
- Transform sphere2Transform(Vector3(30, 10, 10), Quaternion::identity());
- mSphere2Body = mWorld->createCollisionBody(sphere2Transform);
- mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity());
+ // ---------- Spheres ---------- //
+ mSphereShape1 = new SphereShape(3.0);
+ Transform sphereTransform1(Vector3(10, 20, 0), Quaternion::identity());
+ mSphereBody1 = mWorld->createCollisionBody(sphereTransform1);
+ mSphereProxyShape1 = mSphereBody1->addCollisionShape(mSphereShape1, Transform::identity());
- // Assign collision categories to proxy shapes
- mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1);
- mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1);
- mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2);
-
- mCollisionCallback.boxBody = mBoxBody;
- mCollisionCallback.sphere1Body = mSphere1Body;
- mCollisionCallback.sphere2Body = mSphere2Body;
+ mSphereShape2 = new SphereShape(5.0);
+ Transform sphereTransform2(Vector3(20, 20, 0), Quaternion::identity());
+ mSphereBody2 = mWorld->createCollisionBody(sphereTransform2);
+ mSphereProxyShape2 = mSphereBody2->addCollisionShape(mSphereShape2, Transform::identity());
}
/// Destructor
virtual ~TestCollisionWorld() {
- delete mBoxShape;
- delete mSphereShape;
+
+ delete mBoxShape1;
+ delete mBoxShape2;
+
+ delete mSphereShape1;
+ delete mSphereShape2;
+
+ delete mWorld;
}
/// Run the tests
void run() {
- testCollisions();
+ testNoCollisions();
+ testNoOverlap();
+ testNoAABBOverlap();
+
+ testAABBOverlap();
+
+ testSphereVsSphereCollision();
+ testSphereVsBoxCollision();
+
+ testMultipleCollisions();
}
- void testCollisions() {
+ void testNoCollisions() {
- mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
- test(mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.sphere1CollideWithSphere2);
+ // All the shapes of the world are not touching when they are created.
+ // Here we test that at the beginning, there is no collision at all.
- test(mWorld->testAABBOverlap(mBoxBody, mSphere1Body));
- test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
+ // ---------- Global test ---------- //
- test(mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB()));
- test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB()));
+ mCollisionCallback.reset();
+ mWorld->testCollision(&mCollisionCallback);
+ test(!mCollisionCallback.hasContacts());
- mCollisionCallback.reset();
- test(!mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.sphere1CollideWithSphere2);
+ // ---------- Single body test ---------- //
- mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback);
- test(mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.sphere1CollideWithSphere2);
+ mCollisionCallback.reset();
+ mWorld->testCollision(mBoxBody1, &mCollisionCallback);
+ test(!mCollisionCallback.hasContacts());
- mCollisionCallback.reset();
- test(!mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.sphere1CollideWithSphere2);
+ mCollisionCallback.reset();
+ mWorld->testCollision(mBoxBody2, &mCollisionCallback);
+ test(!mCollisionCallback.hasContacts());
- // Move sphere 1 to collide with sphere 2
- mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
+ mCollisionCallback.reset();
+ mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ test(!mCollisionCallback.hasContacts());
- mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
- test(!mCollisionCallback.boxCollideWithSphere1);
- test(mCollisionCallback.sphere1CollideWithSphere2);
+ mCollisionCallback.reset();
+ mWorld->testCollision(mSphereBody2, &mCollisionCallback);
+ test(!mCollisionCallback.hasContacts());
- mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody, mSphere1Body, &mCollisionCallback);
- test(!mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.sphere1CollideWithSphere2);
+ // Two bodies test
- mCollisionCallback.reset();
- test(!mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.sphere1CollideWithSphere2);
+ mCollisionCallback.reset();
+ mWorld->testCollision(mBoxBody1, mBoxBody2, &mCollisionCallback);
+ test(!mCollisionCallback.hasContacts());
+
+ mCollisionCallback.reset();
+ mWorld->testCollision(mSphereBody1, mSphereBody2, &mCollisionCallback);
+ test(!mCollisionCallback.hasContacts());
+
+ mCollisionCallback.reset();
+ mWorld->testCollision(mBoxBody1, mSphereBody1, &mCollisionCallback);
+ test(!mCollisionCallback.hasContacts());
+
+ mCollisionCallback.reset();
+ mWorld->testCollision(mBoxBody1, mSphereBody2, &mCollisionCallback);
+ test(!mCollisionCallback.hasContacts());
+
+ mCollisionCallback.reset();
+ mWorld->testCollision(mBoxBody2, mSphereBody1, &mCollisionCallback);
+ test(!mCollisionCallback.hasContacts());
+
+ mCollisionCallback.reset();
+ mWorld->testCollision(mBoxBody2, mSphereBody2, &mCollisionCallback);
+ test(!mCollisionCallback.hasContacts());
+
+ }
+
+ void testNoOverlap() {
+
+ // All the shapes of the world are not touching when they are created.
+ // Here we test that at the beginning, there is no overlap at all.
+
+ // ---------- Single body test ---------- //
+
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mBoxBody1, &mOverlapCallback);
+ test(!mOverlapCallback.hasOverlap());
+
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mBoxBody2, &mOverlapCallback);
+ test(!mOverlapCallback.hasOverlap());
+
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
+ test(!mOverlapCallback.hasOverlap());
+
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody2, &mOverlapCallback);
+ test(!mOverlapCallback.hasOverlap());
+
+ // Two bodies test
+
+ test(!mWorld->testOverlap(mBoxBody1, mBoxBody2));
+ test(!mWorld->testOverlap(mSphereBody1, mSphereBody2));
+ test(!mWorld->testOverlap(mBoxBody1, mSphereBody1));
+ test(!mWorld->testOverlap(mBoxBody1, mSphereBody2));
+ test(!mWorld->testOverlap(mBoxBody2, mSphereBody1));
+ test(!mWorld->testOverlap(mBoxBody2, mSphereBody2));
+ }
+
+ void testNoAABBOverlap() {
+
+ // All the shapes of the world are not touching when they are created.
+ // Here we test that at the beginning, there is no AABB overlap at all.
+
+ // Two bodies test
+
+ test(!mWorld->testAABBOverlap(mBoxBody1, mBoxBody2));
+ test(!mWorld->testAABBOverlap(mSphereBody1, mSphereBody2));
+ test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody1));
+ test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody2));
+ test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody1));
+ test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody2));
+ }
+
+ void testAABBOverlap() {
+
+ // TODO : Test the CollisionWorld::testAABBOverlap() method here
+ }
+
+ void testSphereVsSphereCollision() {
+
+
+
+ // Move sphere 1 to collide with sphere 2
+ mSphereBody1->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
+
+ }
+
+ void testSphereVsBoxCollision() {
// Move sphere 1 to collide with box
- mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity()));
+ mSphereBody1->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity()));
// --------- Test collision with inactive bodies --------- //
mCollisionCallback.reset();
- mBoxBody->setIsActive(false);
- mSphere1Body->setIsActive(false);
- mSphere2Body->setIsActive(false);
+ mBoxBody1->setIsActive(false);
+ mSphereBody1->setIsActive(false);
+ mSphereBody2->setIsActive(false);
mWorld->testCollision(&mCollisionCallback);
- test(!mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.sphere1CollideWithSphere2);
+
- test(!mWorld->testAABBOverlap(mBoxBody, mSphere1Body));
- test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
-
- test(!mBoxProxyShape->testAABBOverlap(mSphere1ProxyShape->getWorldAABB()));
- test(!mSphere1ProxyShape->testAABBOverlap(mSphere2ProxyShape->getWorldAABB()));
-
- mBoxBody->setIsActive(true);
- mSphere1Body->setIsActive(true);
- mSphere2Body->setIsActive(true);
+ mBoxBody1->setIsActive(true);
+ mSphereBody1->setIsActive(true);
+ mSphereBody2->setIsActive(true);
// --------- Test collision with collision filtering -------- //
- mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3);
- mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2);
- mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1);
+ //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3);
+ //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2);
+ //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1);
- mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
- test(mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.sphere1CollideWithSphere2);
+ //mCollisionCallback.reset();
+ //mWorld->testCollision(&mCollisionCallback);
+ //test(mCollisionCallback.boxCollideWithSphere1);
+ //test(!mCollisionCallback.sphere1CollideWithSphere2);
- // Move sphere 1 to collide with sphere 2
- mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
+ //// Move sphere 1 to collide with sphere 2
+ //mSphere1Body->setTransform(Transform(Vector3(30, 15, 10), Quaternion::identity()));
- mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
- test(!mCollisionCallback.boxCollideWithSphere1);
- test(mCollisionCallback.sphere1CollideWithSphere2);
+ //mCollisionCallback.reset();
+ //mWorld->testCollision(&mCollisionCallback);
+ //test(!mCollisionCallback.boxCollideWithSphere1);
+ //test(mCollisionCallback.sphere1CollideWithSphere2);
- mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2);
- mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2);
- mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3);
+ //mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2);
+ //mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2);
+ //mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3);
- mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
- test(!mCollisionCallback.boxCollideWithSphere1);
- test(!mCollisionCallback.sphere1CollideWithSphere2);
+ //mCollisionCallback.reset();
+ //mWorld->testCollision(&mCollisionCallback);
+ //test(!mCollisionCallback.boxCollideWithSphere1);
+ //test(!mCollisionCallback.sphere1CollideWithSphere2);
- // Move sphere 1 to collide with box
- mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity()));
+ //// Move sphere 1 to collide with box
+ //mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity()));
- mBoxProxyShape->setCollideWithMaskBits(0xFFFF);
- mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF);
- mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF);
+ //mBoxProxyShape->setCollideWithMaskBits(0xFFFF);
+ //mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF);
+ //mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF);
}
+
+ void testMultipleCollisions() {
+
+ // TODO : Test collisions without categories set
+
+ // TODO : Test colliisons with categories set
+
+ // Assign collision categories to proxy shapes
+ //mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1);
+ //mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1);
+ //mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2);
+ }
};
}
diff --git a/test/tests/collision/TestDynamicAABBTree.h b/test/tests/collision/TestDynamicAABBTree.h
old mode 100644
new mode 100755
index 7ab3202b..53f5e618
--- a/test/tests/collision/TestDynamicAABBTree.h
+++ b/test/tests/collision/TestDynamicAABBTree.h
@@ -114,6 +114,12 @@ class TestDynamicAABBTree : public Test {
// Dynamic AABB Tree
DynamicAABBTree tree;
+
+#ifdef IS_PROFILING_ACTIVE
+ /// Pointer to the profiler
+ Profiler* profiler = new Profiler();
+ tree.setProfiler(profiler);
+#endif
int object1Data = 56;
int object2Data = 23;
@@ -152,6 +158,10 @@ class TestDynamicAABBTree : public Test {
test(*(int*)(tree.getNodeDataPointer(object2Id)) == object2Data);
test(*(int*)(tree.getNodeDataPointer(object3Id)) == object3Data);
test(*(int*)(tree.getNodeDataPointer(object4Id)) == object4Data);
+
+#ifdef IS_PROFILING_ACTIVE
+ delete profiler;
+#endif
}
void testOverlapping() {
@@ -161,6 +171,12 @@ class TestDynamicAABBTree : public Test {
// Dynamic AABB Tree
DynamicAABBTree tree;
+#ifdef IS_PROFILING_ACTIVE
+ /// Pointer to the profiler
+ Profiler* profiler = new Profiler();
+ tree.setProfiler(profiler);
+#endif
+
int object1Data = 56;
int object2Data = 23;
int object3Data = 13;
@@ -342,6 +358,9 @@ class TestDynamicAABBTree : public Test {
test(!mOverlapCallback.isOverlapping(object3Id));
test(!mOverlapCallback.isOverlapping(object4Id));
+#ifdef IS_PROFILING_ACTIVE
+ delete profiler;
+#endif
}
void testRaycast() {
@@ -351,6 +370,12 @@ class TestDynamicAABBTree : public Test {
// Dynamic AABB Tree
DynamicAABBTree tree;
+#ifdef IS_PROFILING_ACTIVE
+ /// Pointer to the profiler
+ Profiler* profiler = new Profiler();
+ tree.setProfiler(profiler);
+#endif
+
int object1Data = 56;
int object2Data = 23;
int object3Data = 13;
@@ -513,6 +538,10 @@ class TestDynamicAABBTree : public Test {
test(!mRaycastCallback.isHit(object2Id));
test(mRaycastCallback.isHit(object3Id));
test(mRaycastCallback.isHit(object4Id));
+
+#ifdef IS_PROFILING_ACTIVE
+ delete profiler;
+#endif
}
};
diff --git a/test/tests/collision/TestHalfEdgeStructure.h b/test/tests/collision/TestHalfEdgeStructure.h
index 4c4a2b39..35f357b3 100644
--- a/test/tests/collision/TestHalfEdgeStructure.h
+++ b/test/tests/collision/TestHalfEdgeStructure.h
@@ -19,6 +19,9 @@ class TestHalfEdgeStructure : public Test {
// ---------- Atributes ---------- //
+ /// Memory allocator
+ DefaultAllocator mAllocator;
+
public :
@@ -43,7 +46,7 @@ class TestHalfEdgeStructure : public Test {
void testCube() {
// Create the half-edge structure for a cube
- rp3d::HalfEdgeStructure cubeStructure;
+ rp3d::HalfEdgeStructure cubeStructure(mAllocator, 6, 8, 24);
rp3d::Vector3 vertices[8] = {
rp3d::Vector3(-0.5, -0.5, 0.5),
@@ -67,18 +70,18 @@ class TestHalfEdgeStructure : public Test {
cubeStructure.addVertex(7);
// Faces
- std::vector<uint> face0;
- face0.push_back(0); face0.push_back(1); face0.push_back(2); face0.push_back(3);
- std::vector<uint> face1;
- face1.push_back(1); face1.push_back(5); face1.push_back(6); face1.push_back(2);
- std::vector<uint> face2;
- face2.push_back(5); face2.push_back(4); face2.push_back(7); face2.push_back(6);
- std::vector<uint> face3;
- face3.push_back(4); face3.push_back(0); face3.push_back(3); face3.push_back(7);
- std::vector<uint> face4;
- face4.push_back(0); face4.push_back(4); face4.push_back(5); face4.push_back(1);
- std::vector<uint> face5;
- face5.push_back(2); face5.push_back(6); face5.push_back(7); face5.push_back(3);
+ List<uint> face0(mAllocator, 4);
+ face0.add(0); face0.add(1); face0.add(2); face0.add(3);
+ List<uint> face1(mAllocator, 4);
+ face1.add(1); face1.add(5); face1.add(6); face1.add(2);
+ List<uint> face2(mAllocator, 4);
+ face2.add(5); face2.add(4); face2.add(7); face2.add(6);
+ List<uint> face3(mAllocator, 4);
+ face3.add(4); face3.add(0); face3.add(3); face3.add(7);
+ List<uint> face4(mAllocator, 4);
+ face4.add(0); face4.add(4); face4.add(5); face4.add(1);
+ List<uint> face5(mAllocator, 4);
+ face5.add(2); face5.add(6); face5.add(7); face5.add(3);
cubeStructure.addFace(face0);
cubeStructure.addFace(face1);
@@ -168,7 +171,7 @@ class TestHalfEdgeStructure : public Test {
// Create the half-edge structure for a tetrahedron
std::vector<std::vector<uint>> faces;
- rp3d::HalfEdgeStructure tetrahedron;
+ rp3d::HalfEdgeStructure tetrahedron(mAllocator, 4, 4, 12);
// Vertices
rp3d::Vector3 vertices[4] = {
@@ -184,14 +187,14 @@ class TestHalfEdgeStructure : public Test {
tetrahedron.addVertex(3);
// Faces
- std::vector<uint> face0;
- face0.push_back(0); face0.push_back(1); face0.push_back(2);
- std::vector<uint> face1;
- face1.push_back(0); face1.push_back(3); face1.push_back(1);
- std::vector<uint> face2;
- face2.push_back(1); face2.push_back(3); face2.push_back(2);
- std::vector<uint> face3;
- face3.push_back(0); face3.push_back(2); face3.push_back(3);
+ List<uint> face0(mAllocator, 3);
+ face0.add(0); face0.add(1); face0.add(2);
+ List<uint> face1(mAllocator, 3);
+ face1.add(0); face1.add(3); face1.add(1);
+ List<uint> face2(mAllocator, 3);
+ face2.add(1); face2.add(3); face2.add(2);
+ List<uint> face3(mAllocator, 3);
+ face3.add(0); face3.add(2); face3.add(3);
tetrahedron.addFace(face0);
tetrahedron.addFace(face1);
diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h
index 97dd002a..d02ab847 100644
--- a/test/tests/collision/TestRaycast.h
+++ b/test/tests/collision/TestRaycast.h
@@ -99,6 +99,8 @@ class TestRaycast : public Test {
// Raycast callback class
WorldRaycastCallback mCallback;
+ DefaultAllocator mAllocator;
+
// Epsilon
decimal epsilon;
@@ -203,7 +205,7 @@ class TestRaycast : public Test {
triangleVertices[1] = Vector3(105, 100, 0);
triangleVertices[2] = Vector3(100, 103, 0);
Vector3 triangleVerticesNormals[3] = {Vector3(0, 0, 1), Vector3(0, 0, 1), Vector3(0, 0, 1)};
- mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0);
+ mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0, mAllocator);
mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform);
mCapsuleShape = new CapsuleShape(2, 5);
diff --git a/test/tests/mathematics/TestMathematicsFunctions.h b/test/tests/mathematics/TestMathematicsFunctions.h
index 793a79fb..b76cb78c 100644
--- a/test/tests/mathematics/TestMathematicsFunctions.h
+++ b/test/tests/mathematics/TestMathematicsFunctions.h
@@ -176,13 +176,13 @@ class TestMathematicsFunctions : public Test {
segmentVertices.push_back(Vector3(-6, 3, 0));
segmentVertices.push_back(Vector3(8, 3, 0));
- std::vector<Vector3> planesNormals;
- std::vector<Vector3> planesPoints;
- planesNormals.push_back(Vector3(-1, 0, 0));
- planesPoints.push_back(Vector3(4, 0, 0));
+ List<Vector3> planesNormals(mAllocator, 2);
+ List<Vector3> planesPoints(mAllocator, 2);
+ planesNormals.add(Vector3(-1, 0, 0));
+ planesPoints.add(Vector3(4, 0, 0));
- std::vector<Vector3> clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1],
- planesPoints, planesNormals);
+ List<Vector3> clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1],
+ planesPoints, planesNormals, mAllocator);
test(clipSegmentVertices.size() == 2);
test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001));
test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001));
@@ -195,7 +195,7 @@ class TestMathematicsFunctions : public Test {
segmentVertices.push_back(Vector3(8, 3, 0));
segmentVertices.push_back(Vector3(-6, 3, 0));
- clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals);
+ clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator);
test(clipSegmentVertices.size() == 2);
test(approxEqual(clipSegmentVertices[0].x, 4, 0.000001));
test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001));
@@ -208,7 +208,7 @@ class TestMathematicsFunctions : public Test {
segmentVertices.push_back(Vector3(-6, 3, 0));
segmentVertices.push_back(Vector3(3, 3, 0));
- clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals);
+ clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator);
test(clipSegmentVertices.size() == 2);
test(approxEqual(clipSegmentVertices[0].x, -6, 0.000001));
test(approxEqual(clipSegmentVertices[0].y, 3, 0.000001));
@@ -221,7 +221,7 @@ class TestMathematicsFunctions : public Test {
segmentVertices.push_back(Vector3(5, 3, 0));
segmentVertices.push_back(Vector3(8, 3, 0));
- clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals);
+ clipSegmentVertices = clipSegmentWithPlanes(segmentVertices[0], segmentVertices[1], planesPoints, planesNormals, mAllocator);
test(clipSegmentVertices.size() == 0);
// Test clipPolygonWithPlanes()
From 261ffef616b8d6af37eefd0beec52d2c3bb7a5ff Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 1 Jan 2018 18:35:57 +0100
Subject: [PATCH 131/133] Refactor memory allocation
---
CMakeLists.txt | 2 +-
src/body/CollisionBody.cpp | 12 +--
src/body/RigidBody.cpp | 12 ++-
src/body/RigidBody.h | 4 +-
src/collision/CollisionCallback.cpp | 12 ++-
src/collision/CollisionCallback.h | 6 +-
src/collision/CollisionDetection.cpp | 97 ++++++++++---------
src/collision/CollisionDetection.h | 19 ++--
src/collision/ContactManifold.cpp | 2 +-
src/collision/ContactManifold.h | 4 +-
src/collision/ContactManifoldInfo.cpp | 2 +-
src/collision/ContactManifoldInfo.h | 6 +-
src/collision/ContactManifoldSet.cpp | 2 +-
src/collision/ContactManifoldSet.h | 4 +-
src/collision/HalfEdgeStructure.h | 6 +-
src/collision/MiddlePhaseTriangleCallback.h | 4 +-
src/collision/NarrowPhaseInfo.cpp | 6 +-
src/collision/NarrowPhaseInfo.h | 4 +-
src/collision/PolyhedronMesh.cpp | 4 +-
src/collision/ProxyShape.cpp | 8 +-
src/collision/ProxyShape.h | 9 +-
.../broadphase/BroadPhaseAlgorithm.cpp | 4 +-
.../broadphase/BroadPhaseAlgorithm.h | 2 +-
.../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 2 +-
.../narrowphase/CapsuleVsCapsuleAlgorithm.h | 2 +-
.../CapsuleVsConvexPolyhedronAlgorithm.cpp | 2 +-
.../CapsuleVsConvexPolyhedronAlgorithm.h | 2 +-
...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 2 +-
...vexPolyhedronVsConvexPolyhedronAlgorithm.h | 2 +-
.../narrowphase/NarrowPhaseAlgorithm.h | 2 +-
.../narrowphase/SAT/SATAlgorithm.cpp | 2 +-
src/collision/narrowphase/SAT/SATAlgorithm.h | 4 +-
.../narrowphase/SphereVsCapsuleAlgorithm.cpp | 2 +-
.../narrowphase/SphereVsCapsuleAlgorithm.h | 2 +-
.../SphereVsConvexPolyhedronAlgorithm.cpp | 2 +-
.../SphereVsConvexPolyhedronAlgorithm.h | 2 +-
.../narrowphase/SphereVsSphereAlgorithm.cpp | 2 +-
.../narrowphase/SphereVsSphereAlgorithm.h | 2 +-
src/collision/shapes/BoxShape.cpp | 6 +-
src/collision/shapes/BoxShape.h | 2 +-
src/collision/shapes/CapsuleShape.cpp | 2 +-
src/collision/shapes/CapsuleShape.h | 2 +-
src/collision/shapes/CollisionShape.h | 2 +-
src/collision/shapes/ConcaveMeshShape.cpp | 2 +-
src/collision/shapes/ConcaveMeshShape.h | 6 +-
src/collision/shapes/ConvexMeshShape.cpp | 2 +-
src/collision/shapes/ConvexMeshShape.h | 2 +-
src/collision/shapes/HeightFieldShape.cpp | 2 +-
src/collision/shapes/HeightFieldShape.h | 6 +-
src/collision/shapes/SphereShape.cpp | 2 +-
src/collision/shapes/SphereShape.h | 2 +-
src/collision/shapes/TriangleShape.cpp | 4 +-
src/collision/shapes/TriangleShape.h | 4 +-
src/configuration.h | 2 +-
src/containers/LinkedList.h | 6 +-
src/containers/List.h | 6 +-
src/engine/CollisionWorld.cpp | 8 +-
src/engine/CollisionWorld.h | 10 +-
src/engine/ContactSolver.cpp | 12 ++-
src/engine/ContactSolver.h | 9 +-
src/engine/DynamicsWorld.cpp | 63 +++++++-----
src/engine/Island.cpp | 11 ++-
src/engine/Island.h | 2 +-
src/engine/OverlappingPair.cpp | 2 +-
src/engine/OverlappingPair.h | 12 +--
src/mathematics/mathematics_functions.cpp | 4 +-
src/mathematics/mathematics_functions.h | 4 +-
src/memory/DefaultAllocator.h | 12 +--
src/memory/{Allocator.h => MemoryAllocator.h} | 19 ++--
src/memory/MemoryManager.cpp | 2 +
src/memory/MemoryManager.h | 92 +++++++++++++++---
src/memory/PoolAllocator.cpp | 35 ++++---
src/memory/PoolAllocator.h | 16 +--
src/memory/SingleFrameAllocator.cpp | 75 ++++++++++++--
src/memory/SingleFrameAllocator.h | 34 ++++---
75 files changed, 449 insertions(+), 296 deletions(-)
rename src/memory/{Allocator.h => MemoryAllocator.h} (87%)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e585e0d1..bc11aebd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -187,7 +187,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/mathematics/Vector3.h"
"src/mathematics/Ray.h"
"src/mathematics/Vector3.cpp"
- "src/memory/Allocator.h"
+ "src/memory/MemoryAllocator.h"
"src/memory/PoolAllocator.h"
"src/memory/PoolAllocator.cpp"
"src/memory/SingleFrameAllocator.h"
diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index 56493d28..bb8b0408 100644
--- a/src/body/CollisionBody.cpp
+++ b/src/body/CollisionBody.cpp
@@ -70,9 +70,9 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform) {
// Create a new proxy collision shape to attach the collision shape to the body
- ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate(
+ ProxyShape* proxyShape = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
- transform, decimal(1), mWorld.mPoolAllocator);
+ transform, decimal(1), mWorld.mMemoryManager);
#ifdef IS_PROFILING_ACTIVE
@@ -123,7 +123,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
}
current->~ProxyShape();
- mWorld.mPoolAllocator.release(current, sizeof(ProxyShape));
+ mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, current, sizeof(ProxyShape));
mNbCollisionShapes--;
return;
}
@@ -143,7 +143,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
}
elementToRemove->~ProxyShape();
- mWorld.mPoolAllocator.release(elementToRemove, sizeof(ProxyShape));
+ mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, elementToRemove, sizeof(ProxyShape));
mNbCollisionShapes--;
return;
}
@@ -169,7 +169,7 @@ void CollisionBody::removeAllCollisionShapes() {
}
current->~ProxyShape();
- mWorld.mPoolAllocator.release(current, sizeof(ProxyShape));
+ mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, current, sizeof(ProxyShape));
// Get the next element in the list
current = nextElement;
@@ -188,7 +188,7 @@ void CollisionBody::resetContactManifoldsList() {
// Delete the current element
currentElement->~ContactManifoldListElement();
- mWorld.mPoolAllocator.release(currentElement, sizeof(ContactManifoldListElement));
+ mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, currentElement, sizeof(ContactManifoldListElement));
currentElement = nextElement;
}
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index 7419fc4c..36c362dc 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -176,7 +176,7 @@ void RigidBody::setMass(decimal mass) {
}
// Remove a joint from the joints list
-void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint) {
+void RigidBody::removeJointFromJointsList(MemoryManager& memoryManager, const Joint* joint) {
assert(joint != nullptr);
assert(mJointsList != nullptr);
@@ -186,7 +186,8 @@ void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const
JointListElement* elementToRemove = mJointsList;
mJointsList = elementToRemove->next;
elementToRemove->~JointListElement();
- memoryAllocator.release(elementToRemove, sizeof(JointListElement));
+ memoryManager.release(MemoryManager::AllocationType::Pool,
+ elementToRemove, sizeof(JointListElement));
}
else { // If the element to remove is not the first one in the list
JointListElement* currentElement = mJointsList;
@@ -195,7 +196,8 @@ void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const
JointListElement* elementToRemove = currentElement->next;
currentElement->next = elementToRemove->next;
elementToRemove->~JointListElement();
- memoryAllocator.release(elementToRemove, sizeof(JointListElement));
+ memoryManager.release(MemoryManager::AllocationType::Pool,
+ elementToRemove, sizeof(JointListElement));
break;
}
currentElement = currentElement->next;
@@ -224,9 +226,9 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
decimal mass) {
// Create a new proxy collision shape to attach the collision shape to the body
- ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate(
+ ProxyShape* proxyShape = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
- transform, mass, mWorld.mPoolAllocator);
+ transform, mass, mWorld.mMemoryManager);
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index e12cab05..23603bda 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -31,7 +31,7 @@
#include "CollisionBody.h"
#include "engine/Material.h"
#include "mathematics/mathematics.h"
-#include "memory/PoolAllocator.h"
+#include "memory/MemoryManager.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
@@ -112,7 +112,7 @@ class RigidBody : public CollisionBody {
// -------------------- Methods -------------------- //
/// Remove a joint from the joints list
- void removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint);
+ void removeJointFromJointsList(reactphysics3d::MemoryManager& memoryManager, const Joint* joint);
/// Update the transform of the body after a change of the center of mass
void updateTransformWithCenterOfMass();
diff --git a/src/collision/CollisionCallback.cpp b/src/collision/CollisionCallback.cpp
index db820db4..1702e40a 100644
--- a/src/collision/CollisionCallback.cpp
+++ b/src/collision/CollisionCallback.cpp
@@ -26,18 +26,18 @@
// Libraries
#include "collision/CollisionCallback.h"
#include "engine/OverlappingPair.h"
-#include "memory/Allocator.h"
+#include "memory/MemoryAllocator.h"
#include "collision/ContactManifold.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
-CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator) :
+CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager) :
contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()),
body2(pair->getShape2()->getBody()),
proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()),
- mMemoryAllocator(allocator) {
+ mMemoryManager(memoryManager) {
assert(pair != nullptr);
@@ -52,7 +52,8 @@ CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair*
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
- ContactManifoldListElement* element = new (mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)))
+ ContactManifoldListElement* element = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(ContactManifoldListElement)))
ContactManifoldListElement(contactManifold,
contactManifoldElements);
contactManifoldElements = element;
@@ -72,7 +73,8 @@ CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() {
// Delete and release memory
element->~ContactManifoldListElement();
- mMemoryAllocator.release(element, sizeof(ContactManifoldListElement));
+ mMemoryManager.release(MemoryManager::AllocationType::Pool, element,
+ sizeof(ContactManifoldListElement));
element = nextElement;
}
diff --git a/src/collision/CollisionCallback.h b/src/collision/CollisionCallback.h
index 54208fe4..57ae0f04 100644
--- a/src/collision/CollisionCallback.h
+++ b/src/collision/CollisionCallback.h
@@ -69,11 +69,11 @@ class CollisionCallback {
/// Pointer to the proxy shape of second body
const ProxyShape* proxyShape2;
- /// Memory allocator
- Allocator& mMemoryAllocator;
+ /// Memory manager
+ MemoryManager& mMemoryManager;
// Constructor
- CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator);
+ CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager);
// Destructor
~CollisionCallbackInfo();
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index e57c5fd8..66cf1eb5 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -47,9 +47,8 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
-CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator)
- : mPoolAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator),
- mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this),
+CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager)
+ : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this),
mIsCollisionShapesAdded(false) {
// Set the default collision dispatch configuration
@@ -95,7 +94,7 @@ void CollisionDetection::computeBroadPhase() {
// Ask the broad-phase to recompute the overlapping pairs of collision
// shapes. This call can only add new overlapping pairs in the collision
// detection.
- mBroadPhaseAlgorithm.computeOverlappingPairs(mPoolAllocator);
+ mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryManager);
}
}
@@ -133,7 +132,7 @@ void CollisionDetection::computeMiddlePhase() {
// Destroy the overlapping pair
itToRemove->second->~OverlappingPair();
- mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair));
+ mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove);
continue;
}
@@ -166,10 +165,10 @@ void CollisionDetection::computeMiddlePhase() {
// No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection
NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList;
- mNarrowPhaseInfoList = new (mSingleFrameAllocator.allocate(sizeof(NarrowPhaseInfo)))
+ mNarrowPhaseInfoList = new (mMemoryManager.allocate(MemoryManager::AllocationType::Frame, sizeof(NarrowPhaseInfo)))
NarrowPhaseInfo(pair, shape1->getCollisionShape(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
- shape2->getLocalToWorldTransform(), mSingleFrameAllocator);
+ shape2->getLocalToWorldTransform(), mMemoryManager.getSingleFrameAllocator());
mNarrowPhaseInfoList->next = firstNarrowPhaseInfo;
}
@@ -177,7 +176,7 @@ void CollisionDetection::computeMiddlePhase() {
else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) {
NarrowPhaseInfo* narrowPhaseInfo = nullptr;
- computeConvexVsConcaveMiddlePhase(pair, mSingleFrameAllocator, &narrowPhaseInfo);
+ computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), &narrowPhaseInfo);
// Add all the narrow-phase info object reported by the callback into the
// list of all the narrow-phase info object
@@ -202,7 +201,7 @@ void CollisionDetection::computeMiddlePhase() {
}
// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
-void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator,
+void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator,
NarrowPhaseInfo** firstNarrowPhaseInfo) {
ProxyShape* shape1 = pair->getShape1();
@@ -273,7 +272,7 @@ void CollisionDetection::computeNarrowPhase() {
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mSingleFrameAllocator)) {
+ if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mMemoryManager.getSingleFrameAllocator())) {
// Add the contact points as a potential contact manifold into the pair
currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold();
@@ -295,7 +294,7 @@ void CollisionDetection::computeNarrowPhase() {
narrowPhaseInfoToDelete->~NarrowPhaseInfo();
// Release the allocated memory for the narrow phase info
- mSingleFrameAllocator.release(narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo));
+ mMemoryManager.release(MemoryManager::AllocationType::Frame, narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo));
}
// Convert the potential contact into actual contacts
@@ -327,8 +326,9 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
// Create the overlapping pair and add it into the set of overlapping pairs
- OverlappingPair* newPair = new (mPoolAllocator.allocate(sizeof(OverlappingPair)))
- OverlappingPair(shape1, shape2, mPoolAllocator, mSingleFrameAllocator);
+ OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair)))
+ OverlappingPair(shape1, shape2, mMemoryManager.getPoolAllocator(),
+ mMemoryManager.getSingleFrameAllocator());
assert(newPair != nullptr);
#ifndef NDEBUG
@@ -359,7 +359,7 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
// Destroy the overlapping pair
itToRemove->second->~OverlappingPair();
- mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair));
+ mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove);
}
else {
@@ -403,14 +403,16 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
- ContactManifoldListElement* listElement1 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement)))
+ ContactManifoldListElement* listElement1 = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(ContactManifoldListElement)))
ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList);
body1->mContactManifoldsList = listElement1;
// Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body
- ContactManifoldListElement* listElement2 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement)))
+ ContactManifoldListElement* listElement2 = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(ContactManifoldListElement)))
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
@@ -470,7 +472,7 @@ void CollisionDetection::reportAllContacts() {
// If there is a user callback
if (mWorld->mEventListener != nullptr && it->second->hasContacts()) {
- CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator);
+ CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mMemoryManager);
// Trigger a callback event to report the new contact to the user
mWorld->mEventListener->newContact(collisionInfo);
@@ -498,9 +500,10 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
// No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection
- narrowPhaseInfo = new (mPoolAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(),
+ narrowPhaseInfo = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(),
shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
- shape2->getLocalToWorldTransform(), mPoolAllocator);
+ shape2->getLocalToWorldTransform(), mMemoryManager.getPoolAllocator());
}
// Concave vs Convex algorithm
@@ -508,7 +511,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
// Run the middle-phase collision detection algorithm to find the triangles of the concave
// shape we need to use during the narrow-phase collision detection
- computeConvexVsConcaveMiddlePhase(pair, mPoolAllocator, &narrowPhaseInfo);
+ computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getPoolAllocator(), &narrowPhaseInfo);
}
pair->clearObsoleteLastFrameCollisionInfos();
@@ -524,7 +527,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over
std::unordered_set<bodyindex> reportedBodies;
// Ask the broad-phase to get all the overlapping shapes
- LinkedList<int> overlappingNodes(mPoolAllocator);
+ LinkedList<int> overlappingNodes(mMemoryManager.getPoolAllocator());
mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes);
// For each overlaping proxy shape
@@ -575,7 +578,8 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
if (aabb1.testCollision(aabb2)) {
// Create a temporary overlapping pair
- OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator);
+ OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(),
+ mMemoryManager.getPoolAllocator());
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -600,7 +604,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator);
+ isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mMemoryManager.getPoolAllocator());
}
}
@@ -611,7 +615,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
currentNarrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
- mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Return if we have found a narrow-phase collision
@@ -648,7 +652,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
// Ask the broad-phase to get all the overlapping shapes
- LinkedList<int> overlappingNodes(mPoolAllocator);
+ LinkedList<int> overlappingNodes(mMemoryManager.getPoolAllocator());
mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
const bodyindex bodyId = body->getID();
@@ -670,7 +674,8 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
// Create a temporary overlapping pair
- OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
+ OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(),
+ mMemoryManager.getPoolAllocator());
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -695,7 +700,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mPoolAllocator);
+ isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mMemoryManager.getPoolAllocator());
}
}
@@ -706,7 +711,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
currentNarrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
- mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Return if we have found a narrow-phase collision
@@ -754,7 +759,8 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
if (aabb1.testCollision(aabb2)) {
// Create a temporary overlapping pair
- OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator);
+ OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(),
+ mMemoryManager.getPoolAllocator());
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -774,7 +780,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) {
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) {
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
@@ -788,7 +794,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
currentNarrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
- mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Process the potential contacts
@@ -797,7 +803,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
if (pair.hasContacts()) {
// Report the contacts to the user
- CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
+ CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager);
collisionCallback->notifyContact(collisionInfo);
}
}
@@ -826,7 +832,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
// Ask the broad-phase to get all the overlapping shapes
- LinkedList<int> overlappingNodes(mPoolAllocator);
+ LinkedList<int> overlappingNodes(mMemoryManager.getPoolAllocator());
mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
const bodyindex bodyId = body->getID();
@@ -846,7 +852,8 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
// Create a temporary overlapping pair
- OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
+ OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(),
+ mMemoryManager.getPoolAllocator());
// Compute the middle-phase collision detection between the two shapes
NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -866,7 +873,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) {
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) {
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
@@ -880,7 +887,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
currentNarrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
- mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Process the potential contacts
@@ -889,7 +896,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
if (pair.hasContacts()) {
// Report the contacts to the user
- CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
+ CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager);
callback->notifyContact(collisionInfo);
}
}
@@ -920,8 +927,8 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
OverlappingPair* originalPair = it->second;
// Create a new overlapping pair so that we do not work on the original one
- OverlappingPair pair(originalPair->getShape1(), originalPair->getShape2(), mPoolAllocator,
- mPoolAllocator);
+ OverlappingPair pair(originalPair->getShape1(), originalPair->getShape2(), mMemoryManager.getPoolAllocator(),
+ mMemoryManager.getPoolAllocator());
ProxyShape* shape1 = pair.getShape1();
ProxyShape* shape2 = pair.getShape2();
@@ -950,7 +957,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
- if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mPoolAllocator)) {
+ if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) {
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
@@ -964,7 +971,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
currentNarrowPhaseInfo->~NarrowPhaseInfo();
// Release the allocated memory
- mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+ mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
}
// Process the potential contacts
@@ -973,7 +980,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
if (pair.hasContacts()) {
// Report the contacts to the user
- CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
+ CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager);
callback->notifyContact(collisionInfo);
}
}
@@ -996,12 +1003,6 @@ EventListener* CollisionDetection::getWorldEventListener() {
return mWorld->mEventListener;
}
-// Return a reference to the world memory allocator
-PoolAllocator& CollisionDetection::getWorldMemoryAllocator() {
- return mWorld->mPoolAllocator;
-}
-
-
// Return the world-space AABB of a given proxy shape
const AABB CollisionDetection::getWorldAABB(const ProxyShape* proxyShape) const {
assert(proxyShape->mBroadPhaseID > -1);
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index d728a95f..6aeac709 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -32,8 +32,7 @@
#include "engine/OverlappingPair.h"
#include "engine/EventListener.h"
#include "narrowphase/DefaultCollisionDispatch.h"
-#include "memory/PoolAllocator.h"
-#include "memory/SingleFrameAllocator.h"
+#include "memory/MemoryManager.h"
#include "constraint/ContactPoint.h"
#include <vector>
#include <set>
@@ -62,6 +61,9 @@ class CollisionDetection {
// -------------------- Attributes -------------------- //
+ /// Memory manager
+ MemoryManager& mMemoryManager;
+
/// Collision Detection Dispatch configuration
CollisionDispatch* mCollisionDispatch;
@@ -71,12 +73,6 @@ class CollisionDetection {
/// Collision detection matrix (algorithms to use)
NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES];
- /// Reference to the memory allocator
- PoolAllocator& mPoolAllocator;
-
- /// Reference to the single frame memory allocator
- SingleFrameAllocator& mSingleFrameAllocator;
-
/// Pointer to the physics world
CollisionWorld* mWorld;
@@ -133,7 +129,7 @@ class CollisionDetection {
void addAllContactManifoldsToBodies();
/// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
- void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator,
+ void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator,
NarrowPhaseInfo** firstNarrowPhaseInfo);
/// Compute the middle-phase collision detection between two proxy shapes
@@ -156,7 +152,7 @@ class CollisionDetection {
// -------------------- Methods -------------------- //
/// Constructor
- CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator);
+ CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager);
/// Destructor
~CollisionDetection() = default;
@@ -223,9 +219,6 @@ class CollisionDetection {
/// Return the world event listener
EventListener* getWorldEventListener();
- /// Return a reference to the world memory allocator
- PoolAllocator& getWorldMemoryAllocator();
-
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index c2d60422..ba1bcb1e 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -30,7 +30,7 @@
using namespace reactphysics3d;
// Constructor
-ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, Allocator& memoryAllocator)
+ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator)
: mShape1(shape1), mShape2(shape2), mContactPoints(nullptr),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index 8303f35c..f513ad82 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -129,7 +129,7 @@ class ContactManifold {
bool mIsAlreadyInIsland;
/// Reference to the memory allocator
- Allocator& mMemoryAllocator;
+ MemoryAllocator& mMemoryAllocator;
/// Pointer to the next contact manifold in the linked-list
ContactManifold* mNext;
@@ -217,7 +217,7 @@ class ContactManifold {
/// Constructor
ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
- Allocator& memoryAllocator);
+ MemoryAllocator& memoryAllocator);
/// Destructor
~ContactManifold();
diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp
index 6740fe4f..a28e3817 100644
--- a/src/collision/ContactManifoldInfo.cpp
+++ b/src/collision/ContactManifoldInfo.cpp
@@ -29,7 +29,7 @@
using namespace reactphysics3d;
// Constructor
-ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator)
+ContactManifoldInfo::ContactManifoldInfo(MemoryAllocator& allocator)
: mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator) {
}
diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h
index 4126f4c8..b2687da5 100644
--- a/src/collision/ContactManifoldInfo.h
+++ b/src/collision/ContactManifoldInfo.h
@@ -28,7 +28,7 @@
// Libraries
#include "collision/ContactPointInfo.h"
-#include "memory/Allocator.h"
+#include "memory/MemoryAllocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -57,14 +57,14 @@ class ContactManifoldInfo {
ContactManifoldInfo* mNext;
/// Reference the the memory allocator where the contact point infos have been allocated
- Allocator& mAllocator;
+ MemoryAllocator& mAllocator;
public:
// -------------------- Methods -------------------- //
/// Constructor
- ContactManifoldInfo(Allocator& allocator);
+ ContactManifoldInfo(MemoryAllocator& allocator);
/// Destructor
~ContactManifoldInfo();
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
index 5f70bbc0..c63e6a11 100644
--- a/src/collision/ContactManifoldSet.cpp
+++ b/src/collision/ContactManifoldSet.cpp
@@ -30,7 +30,7 @@ using namespace reactphysics3d;
// Constructor
ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
- Allocator& memoryAllocator)
+ MemoryAllocator& memoryAllocator)
: mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr) {
diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h
index 13d27b70..56b09b04 100644
--- a/src/collision/ContactManifoldSet.h
+++ b/src/collision/ContactManifoldSet.h
@@ -61,7 +61,7 @@ class ContactManifoldSet {
ProxyShape* mShape2;
/// Reference to the memory allocator for the contact manifolds
- Allocator& mMemoryAllocator;
+ MemoryAllocator& mMemoryAllocator;
/// Contact manifolds of the set
ContactManifold* mManifolds;
@@ -95,7 +95,7 @@ class ContactManifoldSet {
/// Constructor
ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
- Allocator& memoryAllocator);
+ MemoryAllocator& memoryAllocator);
/// Destructor
~ContactManifoldSet();
diff --git a/src/collision/HalfEdgeStructure.h b/src/collision/HalfEdgeStructure.h
index e398913f..15658a93 100644
--- a/src/collision/HalfEdgeStructure.h
+++ b/src/collision/HalfEdgeStructure.h
@@ -54,7 +54,7 @@ class HalfEdgeStructure {
List<uint> faceVertices; // Index of the vertices of the face
/// Constructor
- Face(Allocator& allocator) : faceVertices(allocator) {}
+ Face(MemoryAllocator& allocator) : faceVertices(allocator) {}
/// Constructor
Face(List<uint> vertices) : faceVertices(vertices) {}
@@ -71,7 +71,7 @@ class HalfEdgeStructure {
private:
/// Reference to a memory allocator
- Allocator& mAllocator;
+ MemoryAllocator& mAllocator;
/// All the faces
List<Face> mFaces;
@@ -85,7 +85,7 @@ class HalfEdgeStructure {
public:
/// Constructor
- HalfEdgeStructure(Allocator& allocator, uint facesCapacity, uint verticesCapacity,
+ HalfEdgeStructure(MemoryAllocator& allocator, uint facesCapacity, uint verticesCapacity,
uint edgesCapacity) :mAllocator(allocator), mFaces(allocator, facesCapacity),
mVertices(allocator, verticesCapacity), mEdges(allocator, edgesCapacity) {}
diff --git a/src/collision/MiddlePhaseTriangleCallback.h b/src/collision/MiddlePhaseTriangleCallback.h
index e04d7a9c..b4798a2c 100644
--- a/src/collision/MiddlePhaseTriangleCallback.h
+++ b/src/collision/MiddlePhaseTriangleCallback.h
@@ -56,7 +56,7 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
const ConcaveShape* mConcaveShape;
/// Reference to the single-frame memory allocator
- Allocator& mAllocator;
+ MemoryAllocator& mAllocator;
#ifdef IS_PROFILING_ACTIVE
@@ -74,7 +74,7 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair,
ProxyShape* concaveProxyShape,
ProxyShape* convexProxyShape, const ConcaveShape* concaveShape,
- Allocator& allocator)
+ MemoryAllocator& allocator)
:mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape),
mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape),
mAllocator(allocator), narrowPhaseInfoList(nullptr) {
diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp
index 0189db99..2c7a226c 100644
--- a/src/collision/NarrowPhaseInfo.cpp
+++ b/src/collision/NarrowPhaseInfo.cpp
@@ -35,7 +35,7 @@ using namespace reactphysics3d;
// Constructor
NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
- const Transform& shape2Transform, Allocator& shapeAllocator)
+ const Transform& shape2Transform, MemoryAllocator& shapeAllocator)
: overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
contactPoints(nullptr), next(nullptr), collisionShapeAllocator(shapeAllocator) {
@@ -68,7 +68,7 @@ void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penD
assert(penDepth > decimal(0.0));
// Get the memory allocator
- Allocator& allocator = overlappingPair->getTemporaryAllocator();
+ MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator();
// Create the contact point info
ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo)))
@@ -89,7 +89,7 @@ void NarrowPhaseInfo::addContactPointsAsPotentialContactManifold() {
void NarrowPhaseInfo::resetContactPoints() {
// Get the memory allocator
- Allocator& allocator = overlappingPair->getTemporaryAllocator();
+ MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator();
// For each remaining contact point info
ContactPointInfo* element = contactPoints;
diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h
index b732480c..f9ea1ac9 100644
--- a/src/collision/NarrowPhaseInfo.h
+++ b/src/collision/NarrowPhaseInfo.h
@@ -67,12 +67,12 @@ struct NarrowPhaseInfo {
NarrowPhaseInfo* next;
/// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor)
- Allocator& collisionShapeAllocator;
+ MemoryAllocator& collisionShapeAllocator;
/// Constructor
NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
- const Transform& shape2Transform, Allocator& shapeAllocator);
+ const Transform& shape2Transform, MemoryAllocator& shapeAllocator);
/// Destructor
~NarrowPhaseInfo();
diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp
index 08a84e7d..64db474e 100644
--- a/src/collision/PolyhedronMesh.cpp
+++ b/src/collision/PolyhedronMesh.cpp
@@ -36,7 +36,7 @@ using namespace reactphysics3d;
* @param polygonVertexArray Pointer to the array of polygons and their vertices
*/
PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray)
- : mHalfEdgeStructure(MemoryManager::getDefaultAllocator(),
+ : mHalfEdgeStructure(MemoryManager::getBaseAllocator(),
polygonVertexArray->getNbFaces(),
polygonVertexArray->getNbVertices(),
(polygonVertexArray->getNbFaces() + polygonVertexArray->getNbVertices() - 2) * 2) {
@@ -75,7 +75,7 @@ void PolyhedronMesh::createHalfEdgeStructure() {
// Get the polygon face
PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f);
- List<uint> faceVertices(MemoryManager::getDefaultAllocator(), face->nbVertices);
+ List<uint> faceVertices(MemoryManager::getBaseAllocator(), face->nbVertices);
// For each vertex of the face
for (uint v=0; v < face->nbVertices; v++) {
diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp
index f782b2a8..8847d094 100644
--- a/src/collision/ProxyShape.cpp
+++ b/src/collision/ProxyShape.cpp
@@ -35,9 +35,9 @@ using namespace reactphysics3d;
* @param transform Transformation from collision shape local-space to body local-space
* @param mass Mass of the collision shape (in kilograms)
*/
-ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass, Allocator& allocator)
- :mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
- mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF), mAllocator(allocator) {
+ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass, MemoryManager& memoryManager)
+ :mMemoryManager(memoryManager), mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
+ mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
}
@@ -76,7 +76,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
worldToLocalTransform * ray.point2,
ray.maxFraction);
- bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this, mAllocator);
+ bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this, mMemoryManager.getPoolAllocator());
// Convert the raycast info into world-space
raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;
diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h
index 0ba34698..0da79e28 100644
--- a/src/collision/ProxyShape.h
+++ b/src/collision/ProxyShape.h
@@ -29,6 +29,7 @@
// Libraries
#include "body/CollisionBody.h"
#include "shapes/CollisionShape.h"
+#include "memory/MemoryManager.h"
namespace reactphysics3d {
@@ -48,6 +49,9 @@ class ProxyShape {
// -------------------- Attributes -------------------- //
+ /// Reference to the memory manager
+ MemoryManager& mMemoryManager;
+
/// Pointer to the parent body
CollisionBody* mBody;
@@ -82,9 +86,6 @@ class ProxyShape {
/// proxy shape will collide with every collision categories by default.
unsigned short mCollideWithMaskBits;
- /// Memory allocator
- Allocator& mAllocator;
-
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
@@ -103,7 +104,7 @@ class ProxyShape {
/// Constructor
ProxyShape(CollisionBody* body, CollisionShape* shape,
- const Transform& transform, decimal mass, Allocator& allocator);
+ const Transform& transform, decimal mass, MemoryManager& memoryManager);
/// Destructor
virtual ~ProxyShape();
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp
index c1484434..8998859e 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp
@@ -185,14 +185,14 @@ void BroadPhaseAlgorithm::reportAllShapesOverlappingWithAABB(const AABB& aabb,
}
// Compute all the overlapping pairs of collision shapes
-void BroadPhaseAlgorithm::computeOverlappingPairs(Allocator& allocator) {
+void BroadPhaseAlgorithm::computeOverlappingPairs(MemoryManager& memoryManager) {
// TODO : Try to see if we can allocate potential pairs in single frame allocator
// Reset the potential overlapping pairs
mNbPotentialPairs = 0;
- LinkedList<int> overlappingNodes(allocator);
+ LinkedList<int> overlappingNodes(memoryManager.getPoolAllocator());
// For all collision shapes that have moved (or have been created) during the
// last simulation step
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h
index 7e1ff262..30a67920 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.h
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.h
@@ -210,7 +210,7 @@ class BroadPhaseAlgorithm {
void reportAllShapesOverlappingWithAABB(const AABB& aabb, LinkedList<int>& overlappingNodes) const;
/// Compute all the overlapping pairs of collision shapes
- void computeOverlappingPairs(Allocator& allocator);
+ void computeOverlappingPairs(MemoryManager& memoryManager);
/// Return the proxy shape corresponding to the broad-phase node id in parameter
ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const;
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
index bf9eefc2..dce9b04b 100755
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
@@ -34,7 +34,7 @@ using namespace reactphysics3d;
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
- Allocator& memoryAllocator) {
+ MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
index df9a8854..b122ca14 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
@@ -61,7 +61,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two capsules
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index f943312b..33a381f0 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -38,7 +38,7 @@ using namespace reactphysics3d;
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
- Allocator& memoryAllocator) {
+ MemoryAllocator& memoryAllocator) {
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
index d7f96d63..58bb14fb 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
@@ -61,7 +61,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a capsule and a polyhedron
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
index 5cf10b17..375814cd 100644
--- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
@@ -35,7 +35,7 @@ using namespace reactphysics3d;
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
- Allocator& memoryAllocator) {
+ MemoryAllocator& memoryAllocator) {
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm(memoryAllocator);
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
index 204bc269..ddd10585 100644
--- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
@@ -61,7 +61,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm
ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two convex polyhedra
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
index 40d3fbc6..3088d874 100644
--- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h
+++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h
@@ -92,7 +92,7 @@ class NarrowPhaseAlgorithm {
/// Compute a contact info if the two bounding volumes collide
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
- Allocator& memoryAllocator)=0;
+ MemoryAllocator& memoryAllocator)=0;
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 1992cbc7..66be7d06 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -45,7 +45,7 @@ using namespace reactphysics3d;
const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
// Constructor
-SATAlgorithm::SATAlgorithm(Allocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) {
+SATAlgorithm::SATAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) {
}
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 52884f8e..0402067f 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -51,7 +51,7 @@ class SATAlgorithm {
static const decimal SAME_SEPARATING_AXIS_BIAS;
/// Memory allocator
- Allocator& mMemoryAllocator;
+ MemoryAllocator& mMemoryAllocator;
#ifdef IS_PROFILING_ACTIVE
@@ -115,7 +115,7 @@ class SATAlgorithm {
// -------------------- Methods -------------------- //
/// Constructor
- SATAlgorithm(Allocator& memoryAllocator);
+ SATAlgorithm(MemoryAllocator& memoryAllocator);
/// Destructor
~SATAlgorithm() = default;
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
index f5bfc11d..2e86dfe3 100755
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
@@ -35,7 +35,7 @@ using namespace reactphysics3d;
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
- Allocator& memoryAllocator) {
+ MemoryAllocator& memoryAllocator) {
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
index b51bba57..c7c4b39e 100644
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
@@ -61,7 +61,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a capsule
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index 6cc3e578..8d4f562c 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -35,7 +35,7 @@ using namespace reactphysics3d;
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
- Allocator& memoryAllocator) {
+ MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
index 17055431..1929f43c 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
@@ -61,7 +61,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
index f8820106..8103380e 100755
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
@@ -31,7 +31,7 @@
using namespace reactphysics3d;
bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
- Allocator& memoryAllocator) {
+ MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
index e0741ab8..142053c4 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
@@ -61,7 +61,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
- virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, Allocator& memoryAllocator) override;
+ virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp
index 079b15c5..c13e32f4 100644
--- a/src/collision/shapes/BoxShape.cpp
+++ b/src/collision/shapes/BoxShape.cpp
@@ -39,7 +39,7 @@ using namespace reactphysics3d;
*/
BoxShape::BoxShape(const Vector3& extent)
: ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent),
- mHalfEdgeStructure(MemoryManager::getDefaultAllocator(), 6, 8, 24) {
+ mHalfEdgeStructure(MemoryManager::getBaseAllocator(), 6, 8, 24) {
assert(extent.x > decimal(0.0));
assert(extent.y > decimal(0.0));
@@ -55,7 +55,7 @@ BoxShape::BoxShape(const Vector3& extent)
mHalfEdgeStructure.addVertex(6);
mHalfEdgeStructure.addVertex(7);
- DefaultAllocator& allocator = MemoryManager::getDefaultAllocator();
+ MemoryAllocator& allocator = MemoryManager::getBaseAllocator();
// Faces
List<uint> face0(allocator, 4);
@@ -98,7 +98,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
}
// Raycast method with feedback information
-bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
+bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
Vector3 rayDirection = ray.point2 - ray.point1;
decimal tMin = DECIMAL_SMALLEST;
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index b3e094bd..2ebcdce1 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -64,7 +64,7 @@ class BoxShape : public ConvexPolyhedronShape {
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp
index da29fede..3f89d090 100644
--- a/src/collision/shapes/CapsuleShape.cpp
+++ b/src/collision/shapes/CapsuleShape.cpp
@@ -85,7 +85,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
}
// Raycast method with feedback information
-bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
+bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
const Vector3 n = ray.point2 - ray.point1;
diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h
index 0da2d1f4..7843f139 100644
--- a/src/collision/shapes/CapsuleShape.h
+++ b/src/collision/shapes/CapsuleShape.h
@@ -62,7 +62,7 @@ class CapsuleShape : public ConvexShape {
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Raycasting method between a ray one of the two spheres end cap of the capsule
bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index 4381b05c..8d84ab55 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -87,7 +87,7 @@ class CollisionShape {
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const=0;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const=0;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const = 0;
diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp
index 0b49f20c..8f215fc2 100644
--- a/src/collision/shapes/ConcaveMeshShape.cpp
+++ b/src/collision/shapes/ConcaveMeshShape.cpp
@@ -111,7 +111,7 @@ 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, Allocator& allocator) const {
+bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
PROFILE("ConcaveMeshShape::raycast()", mProfiler);
diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h
index 52d49a02..c8c91972 100644
--- a/src/collision/shapes/ConcaveMeshShape.h
+++ b/src/collision/shapes/ConcaveMeshShape.h
@@ -77,7 +77,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
RaycastInfo& mRaycastInfo;
const Ray& mRay;
bool mIsHit;
- Allocator& mAllocator;
+ MemoryAllocator& mAllocator;
#ifdef IS_PROFILING_ACTIVE
@@ -90,7 +90,7 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
// Constructor
ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape,
- ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, Allocator& allocator)
+ ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, MemoryAllocator& allocator)
: mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape),
mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false), mAllocator(allocator) {
@@ -142,7 +142,7 @@ class ConcaveMeshShape : public ConcaveShape {
// -------------------- Methods -------------------- //
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp
index 19ef5016..5b6c70d6 100644
--- a/src/collision/shapes/ConvexMeshShape.cpp
+++ b/src/collision/shapes/ConvexMeshShape.cpp
@@ -112,7 +112,7 @@ void ConvexMeshShape::recalculateBounds() {
}
// Raycast method with feedback information
-bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
+bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo);
}
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index 1c33d919..44d01df4 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -77,7 +77,7 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp
index f067a827..badfb68d 100644
--- a/src/collision/shapes/HeightFieldShape.cpp
+++ b/src/collision/shapes/HeightFieldShape.cpp
@@ -212,7 +212,7 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor
// 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 HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
+bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
// TODO : Implement raycasting without using an AABB for the ray
// but using a dynamic AABB tree or octree instead
diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h
index 4190c3c7..fc587d4c 100644
--- a/src/collision/shapes/HeightFieldShape.h
+++ b/src/collision/shapes/HeightFieldShape.h
@@ -49,7 +49,7 @@ class TriangleOverlapCallback : public TriangleCallback {
bool mIsHit;
decimal mSmallestHitFraction;
const HeightFieldShape& mHeightFieldShape;
- Allocator& mAllocator;
+ MemoryAllocator& mAllocator;
#ifdef IS_PROFILING_ACTIVE
@@ -62,7 +62,7 @@ class TriangleOverlapCallback : public TriangleCallback {
// Constructor
TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo,
- const HeightFieldShape& heightFieldShape, Allocator& allocator)
+ const HeightFieldShape& heightFieldShape, MemoryAllocator& allocator)
: mRay(ray), mProxyShape(proxyShape), mRaycastInfo(raycastInfo),
mHeightFieldShape (heightFieldShape), mAllocator(allocator) {
mIsHit = false;
@@ -144,7 +144,7 @@ class HeightFieldShape : public ConcaveShape {
// -------------------- Methods -------------------- //
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp
index 7d155f97..3ad7afe7 100644
--- a/src/collision/shapes/SphereShape.cpp
+++ b/src/collision/shapes/SphereShape.cpp
@@ -41,7 +41,7 @@ SphereShape::SphereShape(decimal radius)
}
// Raycast method with feedback information
-bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const {
+bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
const Vector3 m = ray.point1;
decimal c = m.dot(m) - mMargin * mMargin;
diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h
index 18883d79..90b20d2d 100644
--- a/src/collision/shapes/SphereShape.h
+++ b/src/collision/shapes/SphereShape.h
@@ -55,7 +55,7 @@ class SphereShape : public ConvexShape {
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
- virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, Allocator& allocator) const override;
+ virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp
index 4a7eb013..2257c0e6 100644
--- a/src/collision/shapes/TriangleShape.cpp
+++ b/src/collision/shapes/TriangleShape.cpp
@@ -45,7 +45,7 @@ using namespace reactphysics3d;
* @param margin The collision margin (in meters) around the collision shape
*/
TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId,
- Allocator& allocator)
+ MemoryAllocator& allocator)
: ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mFaces{HalfEdgeStructure::Face(allocator), HalfEdgeStructure::Face(allocator)} {
mPoints[0] = vertices[0];
@@ -211,7 +211,7 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3&
// 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, Allocator& allocator) const {
+bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
PROFILE("TriangleShape::raycast()", mProfiler);
diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h
index 9a2c756e..23f26adc 100644
--- a/src/collision/shapes/TriangleShape.h
+++ b/src/collision/shapes/TriangleShape.h
@@ -92,7 +92,7 @@ class TriangleShape : public ConvexPolyhedronShape {
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
- Allocator& allocator) const override;
+ MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
@@ -113,7 +113,7 @@ class TriangleShape : public ConvexPolyhedronShape {
/// Constructor
TriangleShape(const Vector3* vertices, const Vector3* verticesNormals,
- uint shapeId, Allocator& allocator);
+ uint shapeId, MemoryAllocator& allocator);
/// Destructor
virtual ~TriangleShape() override = default;
diff --git a/src/configuration.h b/src/configuration.h
index a25c1502..96391e76 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -152,7 +152,7 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
constexpr decimal COS_ANGLE_SIMILAR_CONTACT_MANIFOLD = decimal(0.95);
/// Size (in bytes) of the single frame allocator
-constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 15728640; // 15 Mb
+constexpr size_t INIT_SINGLE_FRAME_ALLOCATOR_BYTES = 1048576; // 1Mb
}
diff --git a/src/containers/LinkedList.h b/src/containers/LinkedList.h
index 281c134c..997035b7 100644
--- a/src/containers/LinkedList.h
+++ b/src/containers/LinkedList.h
@@ -27,7 +27,7 @@
#define REACTPHYSICS3D_LINKED_LIST_H
// Libraries
-#include "memory/Allocator.h"
+#include "memory/MemoryAllocator.h"
namespace reactphysics3d {
@@ -64,14 +64,14 @@ class LinkedList {
ListElement* mListHead;
/// Memory allocator used to allocate the list elements
- Allocator& mAllocator;
+ MemoryAllocator& mAllocator;
public:
// -------------------- Methods -------------------- //
/// Constructor
- LinkedList(Allocator& allocator) : mListHead(nullptr), mAllocator(allocator) {
+ LinkedList(MemoryAllocator& allocator) : mListHead(nullptr), mAllocator(allocator) {
}
diff --git a/src/containers/List.h b/src/containers/List.h
index 71ae7ef7..176766a5 100644
--- a/src/containers/List.h
+++ b/src/containers/List.h
@@ -28,7 +28,7 @@
// Libraries
#include "configuration.h"
-#include "memory/Allocator.h"
+#include "memory/MemoryAllocator.h"
#include <cstring>
namespace reactphysics3d {
@@ -54,7 +54,7 @@ class List {
size_t mCapacity;
/// Memory allocator
- Allocator& mAllocator;
+ MemoryAllocator& mAllocator;
// -------------------- Methods -------------------- //
@@ -64,7 +64,7 @@ class List {
// -------------------- Methods -------------------- //
/// Constructor
- List(Allocator& allocator, size_t capacity = 0)
+ List(MemoryAllocator& allocator, size_t capacity = 0)
: mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) {
if (capacity > 0) {
diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp
index 8b29e4a1..da5bf609 100644
--- a/src/engine/CollisionWorld.cpp
+++ b/src/engine/CollisionWorld.cpp
@@ -33,8 +33,7 @@ using namespace std;
// Constructor
CollisionWorld::CollisionWorld()
- : mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES),
- mCollisionDetection(this, mPoolAllocator, mSingleFrameAllocator), mCurrentBodyID(0),
+ : mCollisionDetection(this, mMemoryManager), mCurrentBodyID(0),
mEventListener(nullptr) {
#ifdef IS_PROFILING_ACTIVE
@@ -74,7 +73,8 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
// Create the collision body
- CollisionBody* collisionBody = new (mPoolAllocator.allocate(sizeof(CollisionBody)))
+ CollisionBody* collisionBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(CollisionBody)))
CollisionBody(transform, *this, bodyID);
assert(collisionBody != nullptr);
@@ -111,7 +111,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
mBodies.erase(collisionBody);
// Free the object from the memory allocator
- mPoolAllocator.release(collisionBody, sizeof(CollisionBody));
+ mMemoryManager.release(MemoryManager::AllocationType::Pool, collisionBody, sizeof(CollisionBody));
}
// Return the next available body ID
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index ead42710..9f1fea58 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -39,8 +39,7 @@
#include "collision/CollisionDetection.h"
#include "constraint/Joint.h"
#include "constraint/ContactPoint.h"
-#include "memory/DefaultAllocator.h"
-#include "memory/PoolAllocator.h"
+#include "memory/MemoryManager.h"
#include "EventListener.h"
/// Namespace reactphysics3d
@@ -62,11 +61,8 @@ class CollisionWorld {
// -------------------- Attributes -------------------- //
- /// Pool Memory allocator
- PoolAllocator mPoolAllocator;
-
- /// Single frame Memory allocator
- SingleFrameAllocator mSingleFrameAllocator;
+ /// Memory manager
+ MemoryManager mMemoryManager;
/// Reference to the collision detection
CollisionDetection mCollisionDetection;
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 6361badd..f4762c49 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -39,9 +39,9 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor
-ContactSolver::ContactSolver(SingleFrameAllocator& allocator)
- :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr),
- mContactConstraints(nullptr), mSingleFrameAllocator(allocator),
+ContactSolver::ContactSolver(MemoryManager& memoryManager)
+ :mMemoryManager(memoryManager), mSplitLinearVelocities(nullptr),
+ mSplitAngularVelocities(nullptr), mContactConstraints(nullptr),
mLinearVelocities(nullptr), mAngularVelocities(nullptr),
mIsSplitImpulseActive(true) {
@@ -75,10 +75,12 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
if (nbContactManifolds == 0 || nbContactPoints == 0) return;
// TODO : Count exactly the number of constraints to allocate here
- mContactPoints = static_cast<ContactPointSolver*>(mSingleFrameAllocator.allocate(sizeof(ContactPointSolver) * nbContactPoints));
+ mContactPoints = static_cast<ContactPointSolver*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+ sizeof(ContactPointSolver) * nbContactPoints));
assert(mContactPoints != nullptr);
- mContactConstraints = static_cast<ContactManifoldSolver*>(mSingleFrameAllocator.allocate(sizeof(ContactManifoldSolver) * nbContactManifolds));
+ mContactConstraints = static_cast<ContactManifoldSolver*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+ sizeof(ContactManifoldSolver) * nbContactManifolds));
assert(mContactConstraints != nullptr);
// For each island of the world
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index 05bd569c..8799a1ad 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -272,6 +272,10 @@ class ContactSolver {
// -------------------- Attributes -------------------- //
+ /// Memory manager
+ MemoryManager& mMemoryManager;
+
+
/// Split linear velocities for the position contact solver (split impulse)
Vector3* mSplitLinearVelocities;
@@ -293,9 +297,6 @@ class ContactSolver {
/// Number of contact constraints
uint mNbContactManifolds;
- /// Single frame memory allocator
- SingleFrameAllocator& mSingleFrameAllocator;
-
/// Array of linear velocities
Vector3* mLinearVelocities;
@@ -339,7 +340,7 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Constructor
- ContactSolver(SingleFrameAllocator& allocator);
+ ContactSolver(MemoryManager& memoryManager);
/// Destructor
~ContactSolver() = default;
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 054c4dc5..35b1ba04 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -41,7 +41,7 @@ using namespace std;
*/
DynamicsWorld::DynamicsWorld(const Vector3 &gravity)
: CollisionWorld(),
- mContactSolver(mSingleFrameAllocator),
+ mContactSolver(mMemoryManager),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
mIsSleepingEnabled(SLEEPING_ENABLED), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
@@ -147,7 +147,7 @@ void DynamicsWorld::update(decimal timeStep) {
resetBodiesForceAndTorque();
// Reset the single frame memory allocator
- mSingleFrameAllocator.reset();
+ mMemoryManager.resetFrameAllocator();
}
// Integrate position and orientation of the rigid bodies.
@@ -236,12 +236,18 @@ void DynamicsWorld::initVelocityArrays() {
// Allocate memory for the bodies velocity arrays
uint nbBodies = mRigidBodies.size();
- mSplitLinearVelocities = static_cast<Vector3*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3)));
- mSplitAngularVelocities = static_cast<Vector3*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3)));
- mConstrainedLinearVelocities = static_cast<Vector3*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3)));
- mConstrainedAngularVelocities = static_cast<Vector3*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3)));
- mConstrainedPositions = static_cast<Vector3*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Vector3)));
- mConstrainedOrientations = static_cast<Quaternion*>(mSingleFrameAllocator.allocate(nbBodies * sizeof(Quaternion)));
+ mSplitLinearVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+ nbBodies * sizeof(Vector3)));
+ mSplitAngularVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+ nbBodies * sizeof(Vector3)));
+ mConstrainedLinearVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+ nbBodies * sizeof(Vector3)));
+ mConstrainedAngularVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+ nbBodies * sizeof(Vector3)));
+ mConstrainedPositions = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+ nbBodies * sizeof(Vector3)));
+ mConstrainedOrientations = static_cast<Quaternion*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+ nbBodies * sizeof(Quaternion)));
assert(mSplitLinearVelocities != nullptr);
assert(mSplitAngularVelocities != nullptr);
assert(mConstrainedLinearVelocities != nullptr);
@@ -412,8 +418,8 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
// Create the rigid body
- RigidBody* rigidBody = new (mPoolAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
- *this, bodyID);
+ RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(RigidBody))) RigidBody(transform, *this, bodyID);
assert(rigidBody != nullptr);
// Add the rigid body to the physics world
@@ -459,7 +465,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
mRigidBodies.erase(rigidBody);
// Free the object from the memory allocator
- mPoolAllocator.release(rigidBody, sizeof(RigidBody));
+ mMemoryManager.release(MemoryManager::AllocationType::Pool, rigidBody, sizeof(RigidBody));
}
// Create a joint between two bodies in the world and return a pointer to the new joint
@@ -477,7 +483,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
// Ball-and-Socket joint
case JointType::BALLSOCKETJOINT:
{
- void* allocatedMemory = mPoolAllocator.allocate(sizeof(BallAndSocketJoint));
+ void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(BallAndSocketJoint));
const BallAndSocketJointInfo& info = static_cast<const BallAndSocketJointInfo&>(
jointInfo);
newJoint = new (allocatedMemory) BallAndSocketJoint(info);
@@ -487,7 +494,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
// Slider joint
case JointType::SLIDERJOINT:
{
- void* allocatedMemory = mPoolAllocator.allocate(sizeof(SliderJoint));
+ void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(SliderJoint));
const SliderJointInfo& info = static_cast<const SliderJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) SliderJoint(info);
break;
@@ -496,7 +504,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
// Hinge joint
case JointType::HINGEJOINT:
{
- void* allocatedMemory = mPoolAllocator.allocate(sizeof(HingeJoint));
+ void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(HingeJoint));
const HingeJointInfo& info = static_cast<const HingeJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) HingeJoint(info);
break;
@@ -505,7 +514,8 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
// Fixed joint
case JointType::FIXEDJOINT:
{
- void* allocatedMemory = mPoolAllocator.allocate(sizeof(FixedJoint));
+ void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(FixedJoint));
const FixedJointInfo& info = static_cast<const FixedJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) FixedJoint(info);
break;
@@ -558,8 +568,8 @@ void DynamicsWorld::destroyJoint(Joint* joint) {
mJoints.erase(joint);
// Remove the joint from the joint list of the bodies involved in the joint
- joint->mBody1->removeJointFromJointsList(mPoolAllocator, joint);
- joint->mBody2->removeJointFromJointsList(mPoolAllocator, joint);
+ joint->mBody1->removeJointFromJointsList(mMemoryManager, joint);
+ joint->mBody2->removeJointFromJointsList(mMemoryManager, joint);
size_t nbBytes = joint->getSizeInBytes();
@@ -567,7 +577,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) {
joint->~Joint();
// Release the allocated memory
- mPoolAllocator.release(joint, nbBytes);
+ mMemoryManager.release(MemoryManager::AllocationType::Pool, joint, nbBytes);
}
// Add the joint to the list of joints of the two bodies involved in the joint
@@ -576,13 +586,15 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
assert(joint != nullptr);
// Add the joint at the beginning of the linked list of joints of the first body
- void* allocatedMemory1 = mPoolAllocator.allocate(sizeof(JointListElement));
+ void* allocatedMemory1 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(JointListElement));
JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint,
joint->mBody1->mJointsList);
joint->mBody1->mJointsList = jointListElement1;
// Add the joint at the beginning of the linked list of joints of the second body
- void* allocatedMemory2 = mPoolAllocator.allocate(sizeof(JointListElement));
+ void* allocatedMemory2 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
+ sizeof(JointListElement));
JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint,
joint->mBody2->mJointsList);
joint->mBody2->mJointsList = jointListElement2;
@@ -603,7 +615,8 @@ void DynamicsWorld::computeIslands() {
// Allocate and create the array of islands pointer. This memory is allocated
// in the single frame allocator
- mIslands = static_cast<Island**>(mSingleFrameAllocator.allocate(sizeof(Island*) * nbBodies));
+ mIslands = static_cast<Island**>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+ sizeof(Island*) * nbBodies));
mNbIslands = 0;
int nbContactManifolds = 0;
@@ -619,7 +632,8 @@ void DynamicsWorld::computeIslands() {
// Create a stack (using an array) for the rigid bodies to visit during the Depth First Search
size_t nbBytesStack = sizeof(RigidBody*) * nbBodies;
- RigidBody** stackBodiesToVisit = static_cast<RigidBody**>(mSingleFrameAllocator.allocate(nbBytesStack));
+ RigidBody** stackBodiesToVisit = static_cast<RigidBody**>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+ nbBytesStack));
// For each rigid body of the world
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
@@ -642,9 +656,10 @@ void DynamicsWorld::computeIslands() {
body->mIsAlreadyInIsland = true;
// Create the new island
- void* allocatedMemoryIsland = mSingleFrameAllocator.allocate(sizeof(Island));
+ void* allocatedMemoryIsland = mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
+ sizeof(Island));
mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, mJoints.size(),
- mSingleFrameAllocator);
+ mMemoryManager);
// While there are still some bodies to visit in the stack
while (stackIndex > 0) {
diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp
index 19eef304..34b65540 100644
--- a/src/engine/Island.cpp
+++ b/src/engine/Island.cpp
@@ -29,14 +29,17 @@
using namespace reactphysics3d;
// Constructor
-Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, SingleFrameAllocator& allocator)
+Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, MemoryManager& memoryManager)
: mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0),
mNbContactManifolds(0), mNbJoints(0) {
// Allocate memory for the arrays on the single frame allocator
- mBodies = static_cast<RigidBody**>(allocator.allocate(sizeof(RigidBody*) * nbMaxBodies));
- mContactManifolds = static_cast<ContactManifold**>(allocator.allocate(sizeof(ContactManifold*) * nbMaxContactManifolds));
- mJoints = static_cast<Joint**>(allocator.allocate(sizeof(Joint*) * nbMaxJoints));
+ mBodies = static_cast<RigidBody**>(memoryManager.allocate(MemoryManager::AllocationType::Frame,
+ sizeof(RigidBody*) * nbMaxBodies));
+ mContactManifolds = static_cast<ContactManifold**>(memoryManager.allocate(MemoryManager::AllocationType::Frame,
+ sizeof(ContactManifold*) * nbMaxContactManifolds));
+ mJoints = static_cast<Joint**>(memoryManager.allocate(MemoryManager::AllocationType::Frame,
+ sizeof(Joint*) * nbMaxJoints));
}
// Destructor
diff --git a/src/engine/Island.h b/src/engine/Island.h
index 5f370004..ca1e6fdd 100644
--- a/src/engine/Island.h
+++ b/src/engine/Island.h
@@ -69,7 +69,7 @@ class Island {
/// Constructor
Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
- SingleFrameAllocator& allocator);
+ MemoryManager& memoryManager);
/// Destructor
~Island();
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index 431476ed..7a95be95 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -33,7 +33,7 @@ using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
- Allocator& persistentMemoryAllocator, Allocator& temporaryMemoryAllocator)
+ MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator)
: mContactManifoldSet(shape1, shape2, persistentMemoryAllocator), mPotentialContactManifolds(nullptr),
mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator) {
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index 8c65e633..f3631775 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -113,18 +113,18 @@ class OverlappingPair {
ContactManifoldInfo* mPotentialContactManifolds;
/// Persistent memory allocator
- Allocator& mPersistentAllocator;
+ MemoryAllocator& mPersistentAllocator;
/// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo
- Allocator& mTempMemoryAllocator;
+ MemoryAllocator& mTempMemoryAllocator;
public:
// -------------------- Methods -------------------- //
/// Constructor
- OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, Allocator& persistentMemoryAllocator,
- Allocator& temporaryMemoryAllocator);
+ OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& persistentMemoryAllocator,
+ MemoryAllocator& temporaryMemoryAllocator);
/// Destructor
~OverlappingPair();
@@ -157,7 +157,7 @@ class OverlappingPair {
void addContactManifold(const ContactManifoldInfo* contactManifoldInfo);
/// Return a reference to the temporary memory allocator
- Allocator& getTemporaryAllocator();
+ MemoryAllocator& getTemporaryAllocator();
/// Return true if one of the shapes of the pair is a concave shape
bool hasConcaveShape() const;
@@ -264,7 +264,7 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body
}
// Return a reference to the temporary memory allocator
-inline Allocator& OverlappingPair::getTemporaryAllocator() {
+inline MemoryAllocator& OverlappingPair::getTemporaryAllocator() {
return mTempMemoryAllocator;
}
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 0d8994f4..9fa13083 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -225,7 +225,7 @@ decimal reactphysics3d::computePointToLineDistance(const Vector3& linePointA, co
List<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
const List<Vector3>& planesPoints,
const List<Vector3>& planesNormals,
- Allocator& allocator) {
+ MemoryAllocator& allocator) {
assert(planesPoints.size() == planesNormals.size());
@@ -303,7 +303,7 @@ List<Vector3> reactphysics3d::clipSegmentWithPlanes(const Vector3& segA, const V
// Clip a polygon against multiple planes and return the clipped polygon vertices
// This method implements the Sutherland–Hodgman clipping algorithm
List<Vector3> reactphysics3d::clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,
- const List<Vector3>& planesNormals, Allocator& allocator) {
+ const List<Vector3>& planesNormals, MemoryAllocator& allocator) {
assert(planesPoints.size() == planesNormals.size());
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index 5081d741..eda0fdd3 100755
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -115,11 +115,11 @@ decimal computePointToLineDistance(const Vector3& linePointA, const Vector3& lin
List<Vector3> clipSegmentWithPlanes(const Vector3& segA, const Vector3& segB,
const List<Vector3>& planesPoints,
const List<Vector3>& planesNormals,
- Allocator& allocator);
+ MemoryAllocator& allocator);
/// Clip a polygon against multiple planes and return the clipped polygon vertices
List<Vector3> clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const List<Vector3>& planesPoints,
- const List<Vector3>& planesNormals, Allocator& allocator);
+ const List<Vector3>& planesNormals, MemoryAllocator& allocator);
/// Project a point onto a plane that is given by a point and its unit length normal
Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint);
diff --git a/src/memory/DefaultAllocator.h b/src/memory/DefaultAllocator.h
index 367171bd..ac4ddd04 100644
--- a/src/memory/DefaultAllocator.h
+++ b/src/memory/DefaultAllocator.h
@@ -27,7 +27,7 @@
#define REACTPHYSICS3D_DEFAULT_ALLOCATOR_H
// Libraries
-#include "memory/Allocator.h"
+#include "memory/MemoryAllocator.h"
#include <cstdlib>
/// ReactPhysics3D namespace
@@ -37,13 +37,16 @@ namespace reactphysics3d {
/**
* This class represents a default memory allocator that uses default malloc/free methods
*/
-class DefaultAllocator : public Allocator {
+class DefaultAllocator : public MemoryAllocator {
public:
/// Destructor
virtual ~DefaultAllocator() = default;
+ /// Assignment operator
+ DefaultAllocator& operator=(DefaultAllocator& allocator) = default;
+
/// Allocate memory of a given size (in bytes) and return a pointer to the
/// allocated memory.
virtual void* allocate(size_t size) override {
@@ -54,11 +57,6 @@ class DefaultAllocator : public Allocator {
virtual void release(void* pointer, size_t size) override {
free(pointer);
}
-
- /// Return true if memory needs to be release with this allocator
- virtual bool isReleaseNeeded() const override {
- return true;
- }
};
}
diff --git a/src/memory/Allocator.h b/src/memory/MemoryAllocator.h
similarity index 87%
rename from src/memory/Allocator.h
rename to src/memory/MemoryAllocator.h
index 0440a5c9..c0d86a33 100644
--- a/src/memory/Allocator.h
+++ b/src/memory/MemoryAllocator.h
@@ -23,8 +23,8 @@
* *
********************************************************************************/
-#ifndef REACTPHYSICS3D_ALLOCATOR_H
-#define REACTPHYSICS3D_ALLOCATOR_H
+#ifndef REACTPHYSICS3D_MEMORY_ALLOCATOR_H
+#define REACTPHYSICS3D_MEMORY_ALLOCATOR_H
// Libraries
#include <cstring>
@@ -32,16 +32,22 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
-// Class Allocator
+// Class MemoryAllocator
/**
* Abstract class with the basic interface of all the derived memory allocators
*/
-class Allocator {
+class MemoryAllocator {
public:
+ /// Constructor
+ MemoryAllocator() = default;
+
/// Destructor
- virtual ~Allocator() = default;
+ virtual ~MemoryAllocator() = default;
+
+ /// Assignment operator
+ MemoryAllocator& operator=(MemoryAllocator& allocator) = default;
/// Allocate memory of a given size (in bytes) and return a pointer to the
/// allocated memory.
@@ -49,9 +55,6 @@ class Allocator {
/// Release previously allocated memory.
virtual void release(void* pointer, size_t size)=0;
-
- /// Return true if memory needs to be release with this allocator
- virtual bool isReleaseNeeded() const=0;
};
}
diff --git a/src/memory/MemoryManager.cpp b/src/memory/MemoryManager.cpp
index 85832cb1..99a32655 100644
--- a/src/memory/MemoryManager.cpp
+++ b/src/memory/MemoryManager.cpp
@@ -30,3 +30,5 @@ using namespace reactphysics3d;
// Static variables
DefaultAllocator MemoryManager::mDefaultAllocator;
+MemoryAllocator* MemoryManager::mBaseAllocator = &mDefaultAllocator;
+
diff --git a/src/memory/MemoryManager.h b/src/memory/MemoryManager.h
index 922a5a17..8d986a31 100644
--- a/src/memory/MemoryManager.h
+++ b/src/memory/MemoryManager.h
@@ -27,7 +27,10 @@
#define REACTPHYSICS3D_MEMORY_MANAGER_H
// Libraries
+#include "memory/MemoryAllocator.h"
#include "memory/DefaultAllocator.h"
+#include "memory/PoolAllocator.h"
+#include "memory/SingleFrameAllocator.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@@ -41,31 +44,98 @@ class MemoryManager {
private:
- /// Default memory allocator
+ /// Default malloc/free memory allocator
static DefaultAllocator mDefaultAllocator;
+ /// Pointer to the base memory allocator to use
+ static MemoryAllocator* mBaseAllocator;
+
+ /// Memory pool allocator
+ PoolAllocator mPoolAllocator;
+
+ /// Single frame stack allocator
+ SingleFrameAllocator mSingleFrameAllocator;
+
public:
/// Memory allocation types
enum class AllocationType {
- Default, // Default memory allocator
+ Base, // Base memory allocator
Pool, // Memory pool allocator
Frame, // Single frame memory allocator
};
- /// Constructor
- MemoryManager();
+ /// Constructor
+ MemoryManager() = default;
- /// Destructor
- ~MemoryManager();
+ /// Destructor
+ ~MemoryManager() = default;
- /// Return the default memory allocator
- static DefaultAllocator& getDefaultAllocator();
+ /// Allocate memory of a given type
+ void* allocate(AllocationType allocationType, size_t size);
+
+ /// Release previously allocated memory.
+ void release(AllocationType allocationType, void* pointer, size_t size);
+
+ /// Return the pool allocator
+ PoolAllocator& getPoolAllocator();
+
+ /// Return the single frame stack allocator
+ SingleFrameAllocator& getSingleFrameAllocator();
+
+ /// Return the base memory allocator
+ static MemoryAllocator& getBaseAllocator();
+
+ /// Set the base memory allocator
+ static void setBaseAllocator(MemoryAllocator* memoryAllocator);
+
+ /// Reset the single frame allocator
+ void resetFrameAllocator();
};
-// Return the default memory allocator
-inline DefaultAllocator& MemoryManager::getDefaultAllocator() {
- return mDefaultAllocator;
+// Allocate memory of a given type
+inline void* MemoryManager::allocate(AllocationType allocationType, size_t size) {
+
+ switch (allocationType) {
+ case AllocationType::Base: return mBaseAllocator->allocate(size); break;
+ case AllocationType::Pool: return mPoolAllocator.allocate(size); break;
+ case AllocationType::Frame: return mSingleFrameAllocator.allocate(size); break;
+ }
+}
+
+// Release previously allocated memory.
+inline void MemoryManager::release(AllocationType allocationType, void* pointer, size_t size) {
+
+ switch (allocationType) {
+ case AllocationType::Base: mBaseAllocator->release(pointer, size); break;
+ case AllocationType::Pool: mPoolAllocator.release(pointer, size); break;
+ case AllocationType::Frame: mSingleFrameAllocator.release(pointer, size); break;
+ }
+}
+
+// Return the pool allocator
+inline PoolAllocator& MemoryManager::getPoolAllocator() {
+ return mPoolAllocator;
+}
+
+// Return the single frame stack allocator
+inline SingleFrameAllocator& MemoryManager::getSingleFrameAllocator() {
+ return mSingleFrameAllocator;
+}
+
+// Return the base memory allocator
+inline MemoryAllocator& MemoryManager::getBaseAllocator() {
+ return *mBaseAllocator;
+}
+
+// Set the base memory allocator
+inline void MemoryManager::setBaseAllocator(MemoryAllocator* baseAllocator) {
+ mBaseAllocator = baseAllocator;
+}
+
+// Reset the single frame allocator
+inline void MemoryManager::resetFrameAllocator() {
+ mSingleFrameAllocator.reset();
}
}
diff --git a/src/memory/PoolAllocator.cpp b/src/memory/PoolAllocator.cpp
index 53cee0a1..2a9507c4 100644
--- a/src/memory/PoolAllocator.cpp
+++ b/src/memory/PoolAllocator.cpp
@@ -25,6 +25,7 @@
// Libraries
#include "PoolAllocator.h"
+#include "MemoryManager.h"
#include <cstdlib>
#include <cassert>
@@ -42,7 +43,7 @@ PoolAllocator::PoolAllocator() {
mNbAllocatedMemoryBlocks = 64;
mNbCurrentMemoryBlocks = 0;
const size_t sizeToAllocate = mNbAllocatedMemoryBlocks * sizeof(MemoryBlock);
- mMemoryBlocks = (MemoryBlock*) malloc(sizeToAllocate);
+ mMemoryBlocks = static_cast<MemoryBlock*>(MemoryManager::getBaseAllocator().allocate(sizeToAllocate));
memset(mMemoryBlocks, 0, sizeToAllocate);
memset(mFreeMemoryUnits, 0, sizeof(mFreeMemoryUnits));
@@ -55,7 +56,7 @@ PoolAllocator::PoolAllocator() {
// Initialize the array that contains the sizes the memory units that will
// be allocated in each different heap
- for (int i=0; i < NB_HEAPS; i++) {
+ for (uint i=0; i < NB_HEAPS; i++) {
mUnitSizes[i] = (i+1) * 8;
}
@@ -82,16 +83,17 @@ PoolAllocator::~PoolAllocator() {
// Release the memory allocated for each block
for (uint i=0; i<mNbCurrentMemoryBlocks; i++) {
- free(mMemoryBlocks[i].memoryUnits);
+ MemoryManager::getBaseAllocator().release(mMemoryBlocks[i].memoryUnits, BLOCK_SIZE);
}
- free(mMemoryBlocks);
+ MemoryManager::getBaseAllocator().release(mMemoryBlocks, mNbAllocatedMemoryBlocks * sizeof(MemoryBlock));
#ifndef NDEBUG
// Check that the allocate() and release() methods have been called the same
// number of times to avoid memory leaks.
assert(mNbTimesAllocateMethodCalled == 0);
#endif
+
}
// Allocate memory of a given size (in bytes) and return a pointer to the
@@ -108,8 +110,8 @@ void* PoolAllocator::allocate(size_t size) {
// If we need to allocate more than the maximum memory unit size
if (size > MAX_UNIT_SIZE) {
- // Allocate memory using standard malloc() function
- return malloc(size);
+ // Allocate memory using default allocation
+ return MemoryManager::getBaseAllocator().allocate(size);
}
// Get the index of the heap that will take care of the allocation request
@@ -132,7 +134,7 @@ void* PoolAllocator::allocate(size_t size) {
// Allocate more memory to contain the blocks
MemoryBlock* currentMemoryBlocks = mMemoryBlocks;
mNbAllocatedMemoryBlocks += 64;
- mMemoryBlocks = (MemoryBlock*) malloc(mNbAllocatedMemoryBlocks * sizeof(MemoryBlock));
+ mMemoryBlocks = static_cast<MemoryBlock*>(MemoryManager::getBaseAllocator().allocate(mNbAllocatedMemoryBlocks * sizeof(MemoryBlock)));
memcpy(mMemoryBlocks, currentMemoryBlocks,mNbCurrentMemoryBlocks * sizeof(MemoryBlock));
memset(mMemoryBlocks + mNbCurrentMemoryBlocks, 0, 64 * sizeof(MemoryBlock));
free(currentMemoryBlocks);
@@ -141,17 +143,22 @@ void* PoolAllocator::allocate(size_t size) {
// Allocate a new memory blocks for the corresponding heap and divide it in many
// memory units
MemoryBlock* newBlock = mMemoryBlocks + mNbCurrentMemoryBlocks;
- newBlock->memoryUnits = (MemoryUnit*) malloc(BLOCK_SIZE);
+ newBlock->memoryUnits = static_cast<MemoryUnit*>(MemoryManager::getBaseAllocator().allocate(BLOCK_SIZE));
assert(newBlock->memoryUnits != nullptr);
size_t unitSize = mUnitSizes[indexHeap];
uint nbUnits = BLOCK_SIZE / unitSize;
assert(nbUnits * unitSize <= BLOCK_SIZE);
+ void* memoryUnitsStart = static_cast<void*>(newBlock->memoryUnits);
+ char* memoryUnitsStartChar = static_cast<char*>(memoryUnitsStart);
for (size_t i=0; i < nbUnits - 1; i++) {
- MemoryUnit* unit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * i);
- MemoryUnit* nextUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (i+1));
+ void* unitPointer = static_cast<void*>(memoryUnitsStartChar + unitSize * i);
+ void* nextUnitPointer = static_cast<void*>(memoryUnitsStartChar + unitSize * (i+1));
+ MemoryUnit* unit = static_cast<MemoryUnit*>(unitPointer);
+ MemoryUnit* nextUnit = static_cast<MemoryUnit*>(nextUnitPointer);
unit->nextUnit = nextUnit;
}
- MemoryUnit* lastUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize*(nbUnits-1));
+ void* lastUnitPointer = static_cast<void*>(memoryUnitsStartChar + unitSize*(nbUnits-1));
+ MemoryUnit* lastUnit = static_cast<MemoryUnit*>(lastUnitPointer);
lastUnit->nextUnit = nullptr;
// Add the new allocated block into the list of free memory units in the heap
@@ -176,8 +183,8 @@ void PoolAllocator::release(void* pointer, size_t size) {
// If the size is larger than the maximum memory unit size
if (size > MAX_UNIT_SIZE) {
- // Release the memory using the standard free() function
- free(pointer);
+ // Release the memory using the default deallocation
+ MemoryManager::getBaseAllocator().release(pointer, size);
return;
}
@@ -187,7 +194,7 @@ void PoolAllocator::release(void* pointer, size_t size) {
// Insert the released memory unit into the list of free memory units of the
// corresponding heap
- MemoryUnit* releasedUnit = (MemoryUnit*) pointer;
+ MemoryUnit* releasedUnit = static_cast<MemoryUnit*>(pointer);
releasedUnit->nextUnit = mFreeMemoryUnits[indexHeap];
mFreeMemoryUnits[indexHeap] = releasedUnit;
}
diff --git a/src/memory/PoolAllocator.h b/src/memory/PoolAllocator.h
index b06778d5..25f31198 100644
--- a/src/memory/PoolAllocator.h
+++ b/src/memory/PoolAllocator.h
@@ -28,7 +28,7 @@
// Libraries
#include "configuration.h"
-#include "Allocator.h"
+#include "MemoryAllocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -40,7 +40,7 @@ namespace reactphysics3d {
* efficiently. This implementation is inspired by the small block allocator
* described here : http://www.codeproject.com/useritems/Small_Block_Allocator.asp
*/
-class PoolAllocator : public Allocator {
+class PoolAllocator : public MemoryAllocator {
private :
@@ -131,23 +131,17 @@ class PoolAllocator : public Allocator {
/// Destructor
virtual ~PoolAllocator() override;
+ /// Assignment operator
+ PoolAllocator& operator=(PoolAllocator& allocator) = default;
+
/// Allocate memory of a given size (in bytes) and return a pointer to the
/// allocated memory.
virtual void* allocate(size_t size) override;
/// Release previously allocated memory.
virtual void release(void* pointer, size_t size) override;
-
- /// Return true if memory needs to be release with this allocator
- virtual bool isReleaseNeeded() const override;
-
};
-// Return true if memory needs to be release with this allocator
-inline bool PoolAllocator::isReleaseNeeded() const {
- return true;
-}
-
}
#endif
diff --git a/src/memory/SingleFrameAllocator.cpp b/src/memory/SingleFrameAllocator.cpp
index a2e436cd..c9d6533a 100644
--- a/src/memory/SingleFrameAllocator.cpp
+++ b/src/memory/SingleFrameAllocator.cpp
@@ -25,17 +25,19 @@
// Libraries
#include "SingleFrameAllocator.h"
+#include "MemoryManager.h"
#include <cstdlib>
#include <cassert>
using namespace reactphysics3d;
// Constructor
-SingleFrameAllocator::SingleFrameAllocator(size_t totalSizeBytes)
- : mTotalSizeBytes(totalSizeBytes), mCurrentOffset(0) {
+SingleFrameAllocator::SingleFrameAllocator()
+ : mTotalSizeBytes(INIT_SINGLE_FRAME_ALLOCATOR_BYTES),
+ mCurrentOffset(0), mNbFramesTooMuchAllocated(0), mNeedToAllocatedMore(false) {
// Allocate a whole block of memory at the beginning
- mMemoryBufferStart = static_cast<char*>(malloc(mTotalSizeBytes));
+ mMemoryBufferStart = static_cast<char*>(MemoryManager::getBaseAllocator().allocate(mTotalSizeBytes));
assert(mMemoryBufferStart != nullptr);
}
@@ -43,7 +45,7 @@ SingleFrameAllocator::SingleFrameAllocator(size_t totalSizeBytes)
SingleFrameAllocator::~SingleFrameAllocator() {
// Release the memory allocated at the beginning
- free(mMemoryBufferStart);
+ MemoryManager::getBaseAllocator().release(mMemoryBufferStart, mTotalSizeBytes);
}
@@ -52,14 +54,13 @@ SingleFrameAllocator::~SingleFrameAllocator() {
void* SingleFrameAllocator::allocate(size_t size) {
// Check that there is enough remaining memory in the buffer
- if (static_cast<size_t>(mCurrentOffset) + size > mTotalSizeBytes) {
+ if (mCurrentOffset + size > mTotalSizeBytes) {
- // This should never occur. If it does, you must increase the initial
- // size of memory of this allocator
- assert(false);
+ // We need to allocate more memory next time reset() is called
+ mNeedToAllocatedMore = true;
- // Return null
- return nullptr;
+ // Return default memory allocation
+ return MemoryManager::getBaseAllocator().allocate(size);
}
// Next available memory location
@@ -72,9 +73,63 @@ void* SingleFrameAllocator::allocate(size_t size) {
return nextAvailableMemory;
}
+// Release previously allocated memory.
+void SingleFrameAllocator::release(void* pointer, size_t size) {
+
+ // If allocated memory is not within the single frame allocation range
+ char* p = static_cast<char*>(pointer);
+ if (p < mMemoryBufferStart || p > mMemoryBufferStart + mTotalSizeBytes) {
+
+ // Use default deallocation
+ MemoryManager::getBaseAllocator().release(pointer, size);
+ }
+}
+
// Reset the marker of the current allocated memory
void SingleFrameAllocator::reset() {
+ // If too much memory is allocated
+ if (mCurrentOffset < mTotalSizeBytes / 2) {
+
+ mNbFramesTooMuchAllocated++;
+
+ if (mNbFramesTooMuchAllocated > NB_FRAMES_UNTIL_SHRINK) {
+
+ // Release the memory allocated at the beginning
+ MemoryManager::getBaseAllocator().release(mMemoryBufferStart, mTotalSizeBytes);
+
+ // Divide the total memory to allocate by two
+ mTotalSizeBytes /= 2;
+ if (mTotalSizeBytes <= 0) mTotalSizeBytes = 1;
+
+ // Allocate a whole block of memory at the beginning
+ mMemoryBufferStart = static_cast<char*>(MemoryManager::getBaseAllocator().allocate(mTotalSizeBytes));
+ assert(mMemoryBufferStart != nullptr);
+
+ mNbFramesTooMuchAllocated = 0;
+ }
+ }
+ else {
+ mNbFramesTooMuchAllocated = 0;
+ }
+
+ // If we need to allocate more memory
+ if (mNeedToAllocatedMore) {
+
+ // Release the memory allocated at the beginning
+ MemoryManager::getBaseAllocator().release(mMemoryBufferStart, mTotalSizeBytes);
+
+ // Multiply the total memory to allocate by two
+ mTotalSizeBytes *= 2;
+
+ // Allocate a whole block of memory at the beginning
+ mMemoryBufferStart = static_cast<char*>(MemoryManager::getBaseAllocator().allocate(mTotalSizeBytes));
+ assert(mMemoryBufferStart != nullptr);
+
+ mNeedToAllocatedMore = false;
+ mNbFramesTooMuchAllocated = 0;
+ }
+
// Reset the current offset at the beginning of the block
mCurrentOffset = 0;
}
diff --git a/src/memory/SingleFrameAllocator.h b/src/memory/SingleFrameAllocator.h
index 426c2a2d..d3142908 100644
--- a/src/memory/SingleFrameAllocator.h
+++ b/src/memory/SingleFrameAllocator.h
@@ -27,7 +27,7 @@
#define REACTPHYSICS3D_SINGLE_FRAME_ALLOCATOR_H
// Libraries
-#include "Allocator.h"
+#include "MemoryAllocator.h"
#include "configuration.h"
/// ReactPhysics3D namespace
@@ -38,10 +38,16 @@ namespace reactphysics3d {
* This class represent a memory allocator used to efficiently allocate
* memory on the heap that is used during a single frame.
*/
-class SingleFrameAllocator : public Allocator {
+class SingleFrameAllocator : public MemoryAllocator {
private :
+ // -------------------- Constants -------------------- //
+
+ /// Number of frames to wait before shrinking the allocated
+ /// memory if too much is allocated
+ static const int NB_FRAMES_UNTIL_SHRINK = 120;
+
// -------------------- Attributes -------------------- //
/// Total size (in bytes) of memory of the allocator
@@ -51,36 +57,38 @@ class SingleFrameAllocator : public Allocator {
char* mMemoryBufferStart;
/// Pointer to the next available memory location in the buffer
- int mCurrentOffset;
+ size_t mCurrentOffset;
+
+ /// Current number of frames since we detected too much memory
+ /// is allocated
+ size_t mNbFramesTooMuchAllocated;
+
+ /// True if we need to allocate more memory in the next reset() call
+ bool mNeedToAllocatedMore;
public :
// -------------------- Methods -------------------- //
/// Constructor
- SingleFrameAllocator(size_t totalSizeBytes);
+ SingleFrameAllocator();
/// Destructor
virtual ~SingleFrameAllocator() override;
+ /// Assignment operator
+ SingleFrameAllocator& operator=(SingleFrameAllocator& allocator) = default;
+
/// Allocate memory of a given size (in bytes)
virtual void* allocate(size_t size) override;
/// Release previously allocated memory.
- virtual void release(void* pointer, size_t size) override { }
+ virtual void release(void* pointer, size_t size) override;
/// Reset the marker of the current allocated memory
virtual void reset();
-
- /// Return true if memory needs to be release with this allocator
- virtual bool isReleaseNeeded() const override;
};
-// Return true if memory needs to be release with this allocator
-inline bool SingleFrameAllocator::isReleaseNeeded() const {
- return false;
-}
-
}
#endif
From ceb27760cb15830c006fe2d939e8628557f74896 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 14 Jan 2018 10:47:39 +0100
Subject: [PATCH 132/133] Working on containers
---
CMakeLists.txt | 1 +
src/containers/List.h | 133 +++++-
src/containers/Map.h | 532 ++++++++++++++++++++++
src/mathematics/mathematics_functions.cpp | 24 +
src/mathematics/mathematics_functions.h | 3 +
test/main.cpp | 7 +
test/tests/containers/TestList.h | 331 ++++++++++++++
test/tests/containers/TestMap.h | 319 +++++++++++++
test/tests/mathematics/TestTransform.h | 15 +-
9 files changed, 1353 insertions(+), 12 deletions(-)
create mode 100644 src/containers/Map.h
create mode 100644 test/tests/containers/TestList.h
create mode 100644 test/tests/containers/TestMap.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index bc11aebd..169476c9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -198,6 +198,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/containers/Stack.h"
"src/containers/LinkedList.h"
"src/containers/List.h"
+ "src/containers/Map.h"
)
# Create the library
diff --git a/src/containers/List.h b/src/containers/List.h
index 176766a5..6391d52b 100644
--- a/src/containers/List.h
+++ b/src/containers/List.h
@@ -30,6 +30,7 @@
#include "configuration.h"
#include "memory/MemoryAllocator.h"
#include <cstring>
+#include <iterator>
namespace reactphysics3d {
@@ -61,6 +62,102 @@ class List {
public:
+ /// Class Iterator
+ /**
+ * This class represents an iterator for the List
+ */
+ class Iterator {
+
+ private:
+
+ size_t mCurrentIndex;
+ T* mBuffer;
+ size_t mSize;
+
+ public:
+
+ // Iterator traits
+ using value_type = T;
+ using difference_type = std::ptrdiff_t;
+ using pointer = T*;
+ using reference = T&;
+ using iterator_category = std::bidirectional_iterator_tag;
+
+ /// Constructor
+ Iterator() = default;
+
+ /// Constructor
+ Iterator(T* buffer, size_t index, size_t size)
+ :mCurrentIndex(index), mBuffer(buffer), mSize(size) {
+
+ }
+
+ /// Copy constructor
+ Iterator(const Iterator& it)
+ :mCurrentIndex(it.mCurrentIndex), mBuffer(it.mBuffer), mSize(it.size) {
+
+ }
+
+ /// Deferencable
+ reference operator*() const {
+ assert(mCurrentIndex >= 0 && mCurrentIndex < mSize);
+ return mBuffer[mCurrentIndex];
+ }
+
+ /// Deferencable
+ pointer operator->() const {
+ assert(mCurrentIndex >= 0 && mCurrentIndex < mSize);
+ return &(mBuffer[mCurrentIndex]);
+ }
+
+ /// Post increment (it++)
+ Iterator& operator++() {
+ assert(mCurrentIndex < mSize - 1);
+ mCurrentIndex++;
+ return *this;
+ }
+
+ /// Pre increment (++it)
+ Iterator operator++(int number) {
+ assert(mCurrentIndex < mSize - 1);
+ Iterator tmp = *this;
+ mCurrentIndex++;
+ return tmp;
+ }
+
+ /// Post decrement (it--)
+ Iterator& operator--() {
+ assert(mCurrentIndex > 0);
+ mCurrentIndex--;
+ return *this;
+ }
+
+ /// Pre decrement (--it)
+ Iterator operator--(int number) {
+ assert(mCurrentIndex > 0);
+ Iterator tmp = *this;
+ mCurrentIndex--;
+ return tmp;
+ }
+
+ /// Equality operator (it == end())
+ bool operator==(const Iterator& iterator) const {
+ assert(mCurrentIndex >= 0 && mCurrentIndex <= mSize);
+
+ // If both iterators points to the end of the list
+ if (mCurrentIndex == mSize && iterator.mCurrentIndex == iterator.mSize) {
+ return true;
+ }
+
+ return &(mBuffer[mCurrentIndex]) == &(iterator.mBuffer[mCurrentIndex]);
+ }
+
+ /// Inequality operator (it != end())
+ bool operator!=(const Iterator& iterator) const {
+ return !(*this == iterator);
+ }
+ };
+
// -------------------- Methods -------------------- //
/// Constructor
@@ -151,7 +248,7 @@ class List {
}
}
- /// Append another list to the current one
+ /// Append another list at the end of the current one
void addRange(const List<T>& list) {
// If we need to allocate more memory
@@ -180,7 +277,7 @@ class List {
mSize = 0;
}
- /// Return the number of elments in the list
+ /// Return the number of elements in the list
size_t size() const {
return mSize;
}
@@ -202,14 +299,38 @@ class List {
return (static_cast<T*>(mBuffer)[index]);
}
+ /// Overloaded equality operator
+ bool operator==(const List<T>& list) const {
+
+ if (mSize != list.mSize) return false;
+
+ T* items = static_cast<T*>(mBuffer);
+ for (int i=0; i < mSize; i++) {
+ if (items[i] != list[i]) {
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ /// Overloaded not equal operator
+ bool operator!=(const List<T>& list) const {
+
+ return !((*this) == list);
+ }
+
/// Overloaded assignment operator
List<T>& operator=(const List<T>& list) {
- // Clear all the elements
- clear();
+ if (this != &list) {
- // Add all the elements of the list to the current one
- addRange(list);
+ // Clear all the elements
+ clear();
+
+ // Add all the elements of the list to the current one
+ addRange(list);
+ }
return *this;
}
diff --git a/src/containers/Map.h b/src/containers/Map.h
new file mode 100644
index 00000000..5510b98d
--- /dev/null
+++ b/src/containers/Map.h
@@ -0,0 +1,532 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_MAP_H
+#define REACTPHYSICS3D_MAP_H
+
+// Libraries
+#include "memory/MemoryAllocator.h"
+#include "mathematics/mathematics_functions.h"
+#include <cstring>
+#include <limits>
+
+namespace reactphysics3d {
+
+// Class Map
+/**
+ * This class represents a simple generic associative map
+ */
+template<typename K, typename V>
+class Map {
+
+ private:
+
+ /// An entry of the map
+ struct Entry {
+
+ size_t hashCode; // Hash code of the entry
+ int next; // Index of the next entry
+ std::pair<K, V>* keyValue; // Pointer to the pair with key and value
+
+ /// Constructor
+ Entry() {
+ next = -1;
+ keyValue = nullptr;
+ }
+
+ /// Destructor
+ ~Entry() {
+
+ assert(keyValue == nullptr);
+ }
+
+ };
+
+ // -------------------- Constants -------------------- //
+
+ /// Number of prime numbers in array
+ static constexpr int NB_PRIMES = 70;
+
+ /// Array of prime numbers for the size of the map
+ static const int PRIMES[NB_PRIMES];
+
+ /// Largest prime number
+ static int LARGEST_PRIME;
+
+ // -------------------- Attributes -------------------- //
+
+ /// Current number of used entries in the map
+ int mNbUsedEntries;
+
+ /// Number of free entries among the used ones
+ int mNbFreeEntries;
+
+ /// Current capacity of the map
+ int mCapacity;
+
+ /// Array with all the buckets
+ int* mBuckets;
+
+ /// Array with all the entries
+ Entry* mEntries;
+
+ /// Memory allocator
+ MemoryAllocator& mAllocator;
+
+ /// Index to the fist free entry
+ int mFreeIndex;
+
+ // -------------------- Methods -------------------- //
+
+ /// Initialize the map
+ void initialize(int capacity) {
+
+ // Compute the next larger prime size
+ mCapacity = getPrimeSize(capacity);
+
+ // Allocate memory for the buckets
+ mBuckets = static_cast<int*>(mAllocator.allocate(mCapacity * sizeof(int)));
+
+ // Allocate memory for the entries
+ mEntries = static_cast<Entry*>(mAllocator.allocate(mCapacity * sizeof(Entry)));
+
+ // Initialize the buckets and entries
+ for (int i=0; i<mCapacity; i++) {
+
+ mBuckets[i] = -1;
+
+ // Construct the entry
+ new (&mEntries[i]) Entry();
+ }
+
+ mNbUsedEntries = 0;
+ mNbFreeEntries = 0;
+ mFreeIndex = -1;
+ }
+
+ /// Expand the capacity of the map
+ void expand(int newCapacity) {
+
+ assert(newCapacity > mCapacity);
+ assert(isPrimeNumber(newCapacity));
+
+ // Allocate memory for the buckets
+ int* newBuckets = static_cast<int*>(mAllocator.allocate(newCapacity * sizeof(int)));
+
+ // Allocate memory for the entries
+ Entry* newEntries = static_cast<Entry*>(mAllocator.allocate(newCapacity * sizeof(Entry)));
+
+ // Initialize the new buckets
+ for (int i=0; i<newCapacity; i++) {
+ newBuckets[i] = -1;
+ }
+
+ // Copy the old entries to the new allocated memory location
+ std::memcpy(newEntries, mEntries, mNbUsedEntries * sizeof(Entry));
+
+ // Construct the new entries
+ for (int i=mNbUsedEntries; i<newCapacity; i++) {
+
+ // Construct the entry
+ new (static_cast<void*>(&newEntries[i])) Entry();
+ }
+
+ // For each used entry
+ for (int i=0; i<mNbUsedEntries; i++) {
+
+ // If the entry is not free
+ if (newEntries[i].hashCode != -1) {
+
+ // Get the corresponding bucket
+ int bucket = newEntries[i].hashCode % newCapacity;
+
+ newEntries[i].next = newBuckets[bucket];
+ newBuckets[bucket] = i;
+ }
+ }
+
+ // Release previously allocated memory
+ mAllocator.release(mBuckets, mCapacity * sizeof(int));
+ mAllocator.release(mEntries, mCapacity * sizeof(Entry));
+
+ mCapacity = newCapacity;
+ mBuckets = newBuckets;
+ mEntries = newEntries;
+ }
+
+ /// Return the index of the entry with a given key or -1 if there is no entry with this key
+ int findEntry(const K& key) const {
+
+ if (mCapacity > 0) {
+
+ size_t hashCode = std::hash<K>()(key);
+ int bucket = hashCode % mCapacity;
+
+ for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) {
+ if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == key) {
+ return i;
+ }
+ }
+ }
+
+ return -1;
+ }
+
+ /// Return the prime number that is larger or equal to the number in parameter
+ /// for the size of the map
+ static int getPrimeSize(int number) {
+
+ // Check if the next larger prime number is in the precomputed array of primes
+ for (int i = 0; i < NB_PRIMES; i++) {
+ if (PRIMES[i] >= number) return PRIMES[i];
+ }
+
+ // Manually compute the next larger prime number
+ for (int i = (number | 1); i < std::numeric_limits<int>::max(); i+=2) {
+
+ if (isPrimeNumber(i)) {
+ return i;
+ }
+ }
+
+ return number;
+ }
+
+ /// Clear and reset the map
+ void reset() {
+
+ // If elements have been allocated
+ if (mCapacity > 0) {
+
+ // Clear the list
+ clear();
+
+ // Destroy the entries
+ for (int i=0; i < mCapacity; i++) {
+ mEntries[i].~Entry();
+ }
+
+ mAllocator.release(mBuckets, mCapacity * sizeof(int));
+ mAllocator.release(mEntries, mCapacity * sizeof(Entry));
+
+ mNbUsedEntries = 0;
+ mNbFreeEntries = 0;
+ mCapacity = 0;
+ mBuckets = nullptr;
+ mEntries = nullptr;
+ mFreeIndex = -1;
+ }
+ }
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ Map(MemoryAllocator& allocator, size_t capacity = 0)
+ : mNbUsedEntries(0), mNbFreeEntries(0), mCapacity(0), mBuckets(nullptr),
+ mEntries(nullptr), mAllocator(allocator), mFreeIndex(-1) {
+
+ // If the largest prime has not been computed yet
+ if (LARGEST_PRIME == -1) {
+
+ // Compute the largest prime number (largest map capacity)
+ LARGEST_PRIME = getPrimeSize(PRIMES[NB_PRIMES - 1] + 2);
+ }
+
+ if (capacity > 0) {
+
+ initialize(capacity);
+ }
+ }
+
+ /// Copy constructor
+ Map(const Map<K, V>& map)
+ :mNbUsedEntries(map.mNbUsedEntries), mNbFreeEntries(map.mNbFreeEntries), mCapacity(map.mCapacity),
+ mAllocator(map.mAllocator), mFreeIndex(map.mFreeIndex) {
+
+ // Allocate memory for the buckets
+ mBuckets = static_cast<int*>(mAllocator.allocate(mCapacity * sizeof(int)));
+
+ // Allocate memory for the entries
+ mEntries = static_cast<Entry*>(mAllocator.allocate(mCapacity * sizeof(Entry)));
+
+ // Copy the buckets
+ std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int));
+
+ // Copy the entries
+ std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry));
+ }
+
+ /// Destructor
+ ~Map() {
+
+ reset();
+ }
+
+ /// Allocate memory for a given number of elements
+ void reserve(size_t capacity) {
+
+ if (capacity <= mCapacity) return;
+
+ if (capacity > LARGEST_PRIME && LARGEST_PRIME > mCapacity) {
+ capacity = LARGEST_PRIME;
+ }
+ else {
+ capacity = getPrimeSize(capacity);
+ }
+
+ expand(capacity);
+ }
+
+ /// Return true if the map contains an item with the given key
+ bool containsKey(const K& key) const {
+ return findEntry(key) != -1;
+ }
+
+ /// Add an element into the map
+ void add(const std::pair<K,V>& keyValue) {
+
+ if (mCapacity == 0) {
+ initialize(0);
+ }
+
+ // Compute the hash code of the key
+ size_t hashCode = std::hash<K>()(keyValue.first);
+
+ // Compute the corresponding bucket index
+ int bucket = hashCode % mCapacity;
+
+ // Check if the item is already in the map
+ for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) {
+
+ // If there is already an item with the same key in the map
+ if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == keyValue.first) {
+
+ throw std::runtime_error("The key and value pair already exists in the map");
+ }
+ }
+
+ size_t entryIndex;
+
+ // If there are free entries to use
+ if (mNbFreeEntries > 0) {
+ assert(mFreeIndex >= 0);
+ entryIndex = mFreeIndex;
+ mFreeIndex = mEntries[entryIndex].next;
+ mNbFreeEntries--;
+ }
+ else {
+
+ // If we need to allocator more entries
+ if (mNbUsedEntries == mCapacity) {
+
+ // Allocate more memory
+ reserve(mCapacity * 2);
+
+ // Recompute the bucket index
+ bucket = hashCode % mCapacity;
+ }
+
+ entryIndex = mNbUsedEntries;
+ mNbUsedEntries++;
+ }
+
+ assert(mEntries[entryIndex].keyValue == nullptr);
+ mEntries[entryIndex].hashCode = hashCode;
+ mEntries[entryIndex].next = mBuckets[bucket];
+ mEntries[entryIndex].keyValue = static_cast<std::pair<K,V>*>(mAllocator.allocate(sizeof(std::pair<K,V>)));
+ assert(mEntries[entryIndex].keyValue != nullptr);
+ new (mEntries[entryIndex].keyValue) std::pair<K,V>(keyValue);
+ mBuckets[bucket] = entryIndex;
+ }
+
+ /// Remove the element from the map with a given key
+ bool remove(const K& key) {
+
+ if (mCapacity > 0) {
+
+ size_t hashcode = std::hash<K>()(key);
+ int bucket = hashcode % mCapacity;
+ int last = -1;
+ for (int i = mBuckets[bucket]; i >= 0; last = i, i = mEntries[i].next) {
+
+ if (mEntries[i].hashCode == hashcode && mEntries[i].keyValue->first == key) {
+
+ if (last < 0 ) {
+ mBuckets[bucket] = mEntries[i].next;
+ }
+ else {
+ mEntries[last].next = mEntries[i].next;
+ }
+
+ // Release memory for the key/value pair if any
+ if (mEntries[i].keyValue != nullptr) {
+ mEntries[i].keyValue->~pair<K,V>();
+ mAllocator.release(mEntries[i].keyValue, sizeof(std::pair<K,V>));
+ mEntries[i].keyValue = nullptr;
+ }
+ mEntries[i].hashCode = -1;
+ mEntries[i].next = mFreeIndex;
+ mFreeIndex = i;
+ mNbFreeEntries++;
+
+ return true;
+ }
+ }
+ }
+
+ return false;
+ }
+
+ /// Clear the list
+ void clear() {
+
+ if (mNbUsedEntries > 0) {
+
+ for (int i=0; i < mCapacity; i++) {
+ mBuckets[i] = -1;
+ mEntries[i].next = -1;
+ if (mEntries[i].keyValue != nullptr) {
+ mEntries[i].keyValue->~pair<K,V>();
+ mAllocator.release(mEntries[i].keyValue, sizeof(std::pair<K,V>));
+ mEntries[i].keyValue = nullptr;
+ }
+ }
+
+ mFreeIndex = -1;
+ mNbUsedEntries = 0;
+ mNbFreeEntries = 0;
+ }
+
+ assert(size() == 0);
+ }
+
+ /// Return the number of elements in the map
+ int size() const {
+ return mNbUsedEntries - mNbFreeEntries;
+ }
+
+ /// Return the capacity of the map
+ int capacity() const {
+ return mCapacity;
+ }
+
+ /// Overloaded index operator
+ V& operator[](const K& key) {
+
+ int entry = -1;
+
+ if (mCapacity > 0) {
+ entry = findEntry(key);
+ }
+
+ if (entry == -1) {
+ throw std::runtime_error("No item with given key has been found in the map");
+ }
+
+ assert(mEntries[entry].keyValue != nullptr);
+
+ return mEntries[entry].keyValue->second;
+ }
+
+ /// Overloaded index operator
+ const V& operator[](const K& key) const {
+
+ int entry = -1;
+
+ if (mCapacity > 0) {
+ entry = findEntry(key);
+ }
+
+ if (entry == -1) {
+ throw std::runtime_error("No item with given key has been found in the map");
+ }
+
+ return mEntries[entry];
+ }
+
+ /// Overloaded equality operator
+ bool operator==(const Map<K,V>& map) const {
+
+ // TODO : Implement this
+ return false;
+ }
+
+ /// Overloaded not equal operator
+ bool operator!=(const Map<K,V>& map) const {
+
+ return !((*this) == map);
+ }
+
+ /// Overloaded assignment operator
+ Map<K,V>& operator=(const Map<K, V>& map) {
+
+ // Check for self assignment
+ if (this != &map) {
+
+ // Reset the map
+ reset();
+
+ if (map.mCapacity > 0) {
+
+ // Compute the next larger prime size
+ mCapacity = getPrimeSize(map.mCapacity);
+
+ // Allocate memory for the buckets
+ mBuckets = static_cast<int*>(mAllocator.allocate(mCapacity * sizeof(int)));
+
+ // Allocate memory for the entries
+ mEntries = static_cast<Entry*>(mAllocator.allocate(mCapacity * sizeof(Entry)));
+
+ // Copy the buckets
+ std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int));
+
+ // Copy the entries
+ std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry));
+
+ mNbUsedEntries = map.mNbUsedEntries;
+ mNbFreeEntries = map.mNbFreeEntries;
+ mFreeIndex = map.mFreeIndex;
+ }
+ }
+
+ return *this;
+ }
+};
+
+template<typename K, typename V>
+const int Map<K,V>::PRIMES[NB_PRIMES] = {3, 7, 11, 17, 23, 29, 37, 47, 59, 71, 89, 107, 131, 163, 197, 239, 293, 353, 431, 521, 631, 761, 919,
+ 1103, 1327, 1597, 1931, 2333, 2801, 3371, 4049, 4861, 5839, 7013, 8419, 10103, 12143, 14591,
+ 17519, 21023, 25229, 30293, 36353, 43627, 52361, 62851, 75431, 90523, 108631, 130363, 156437,
+ 187751, 225307, 270371, 324449, 389357, 467237, 560689, 672827, 807403, 968897, 1162687, 1395263,
+ 1674319, 2009191, 2411033, 2893249, 3471899, 4166287, 4999559};
+
+template<typename K, typename V>
+int Map<K,V>::LARGEST_PRIME = -1;
+
+}
+
+#endif
diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp
index 9fa13083..7e4a1427 100755
--- a/src/mathematics/mathematics_functions.cpp
+++ b/src/mathematics/mathematics_functions.cpp
@@ -383,4 +383,28 @@ Vector3 reactphysics3d::projectPointOntoPlane(const Vector3& point, const Vector
return point - unitPlaneNormal.dot(point - planePoint) * unitPlaneNormal;
}
+// Return true if the given number is prime
+bool reactphysics3d::isPrimeNumber(int number) {
+
+ // If it's a odd number
+ if ((number & 1) != 0) {
+
+ int limit = static_cast<int>(std::sqrt(number));
+
+ for (int divisor = 3; divisor <= limit; divisor += 2) {
+
+ // If we have found a divisor
+ if ((number % divisor) == 0) {
+
+ // It is not a prime number
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ return number == 2;
+}
+
diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h
index eda0fdd3..a72b0077 100755
--- a/src/mathematics/mathematics_functions.h
+++ b/src/mathematics/mathematics_functions.h
@@ -124,6 +124,9 @@ List<Vector3> clipPolygonWithPlanes(const List<Vector3>& polygonVertices, const
/// Project a point onto a plane that is given by a point and its unit length normal
Vector3 projectPointOntoPlane(const Vector3& point, const Vector3& planeNormal, const Vector3& planePoint);
+/// Return true if the given number is prime
+bool isPrimeNumber(int number);
+
}
diff --git a/test/main.cpp b/test/main.cpp
index 26d62803..8bb62ea4 100644
--- a/test/main.cpp
+++ b/test/main.cpp
@@ -39,6 +39,8 @@
#include "tests/collision/TestDynamicAABBTree.h"
#include "tests/collision/TestHalfEdgeStructure.h"
#include "tests/collision/TestTriangleVertexArray.h"
+#include "tests/containers/TestList.h"
+#include "tests/containers/TestMap.h"
using namespace reactphysics3d;
@@ -46,6 +48,11 @@ int main() {
TestSuite testSuite("ReactPhysics3D Tests");
+ // ---------- Containers tests ---------- //
+
+ testSuite.addTest(new TestList("List"));
+ testSuite.addTest(new TestMap("Map"));
+
// ---------- Mathematics tests ---------- //
testSuite.addTest(new TestVector2("Vector2"));
diff --git a/test/tests/containers/TestList.h b/test/tests/containers/TestList.h
new file mode 100644
index 00000000..ba1fbf61
--- /dev/null
+++ b/test/tests/containers/TestList.h
@@ -0,0 +1,331 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef TEST_LIST_H
+#define TEST_LIST_H
+
+// Libraries
+#include "Test.h"
+#include "containers/List.h"
+#include "memory/DefaultAllocator.h"
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Class TestList
+/**
+ * Unit test for the List class
+ */
+class TestList : public Test {
+
+ private :
+
+ // ---------- Atributes ---------- //
+
+ DefaultAllocator mAllocator;
+
+ public :
+
+ // ---------- Methods ---------- //
+
+ /// Constructor
+ TestList(const std::string& name) : Test(name) {
+
+ }
+
+ /// Run the tests
+ void run() {
+
+ testConstructors();
+ testAddRemoveClear();
+ testAssignment();
+ testIndexing();
+ testEquality();
+ testReserve();
+ testIteration();
+ }
+
+ void testConstructors() {
+
+ // ----- Constructors ----- //
+
+ List<int> list1(mAllocator);
+ test(list1.capacity() == 0);
+ test(list1.size() == 0);
+
+ List<int> list2(mAllocator, 100);
+ test(list2.capacity() == 100);
+ test(list2.size() == 0);
+
+ List<int> list3(mAllocator);
+ list3.add(1);
+ list3.add(2);
+ list3.add(3);
+ test(list3.capacity() == 4);
+ test(list3.size() == 3);
+
+ // ----- Copy Constructors ----- //
+
+ List<int> list4(list1);
+ test(list4.capacity() == 0);
+ test(list4.size() == 0);
+
+ List<int> list5(list3);
+ test(list5.capacity() == list3.size());
+ test(list5.size() == list3.size());
+ for (uint i=0; i<list3.size(); i++) {
+ test(list5[i] == list3[i]);
+ }
+
+ // ----- Test capacity grow ----- //
+ List<std::string> list6(mAllocator, 20);
+ test(list6.capacity() == 20);
+ for (uint i=0; i<20; i++) {
+ list6.add("test");
+ }
+ test(list6.capacity() == 20);
+ list6.add("test");
+ test(list6.capacity() == 40);
+ }
+
+ void testAddRemoveClear() {
+
+ // ----- Test add() ----- //
+
+ List<int> list1(mAllocator);
+ list1.add(4);
+ test(list1.size() == 1);
+ test(list1[0] == 4);
+ list1.add(9);
+ test(list1.size() == 2);
+ test(list1[0] == 4);
+ test(list1[1] == 9);
+
+ const int arraySize = 15;
+ int arrayTest[arraySize] = {3, 145, -182, 34, 12, 95, -1834, 4143, -111, -111, 4343, 234, 22983, -3432, 753};
+ List<int> list2(mAllocator);
+ for (uint i=0; i<arraySize; i++) {
+ list2.add(arrayTest[i]);
+ }
+ test(list2.size() == arraySize);
+ for (uint i=0; i<arraySize; i++) {
+ test(list2[i] == arrayTest[i]);
+ }
+
+ // ----- Test remove() ----- //
+
+ List<int> list3(mAllocator);
+ list3.add(1);
+ list3.add(2);
+ list3.add(3);
+ list3.add(4);
+
+ list3.remove(3);
+ test(list3.size() == 3);
+ test(list3.capacity() == 4);
+ test(list3[0] = 1);
+ test(list3[1] = 2);
+ test(list3[2] = 3);
+
+ list3.remove(1);
+ test(list3.size() == 2);
+ test(list3.capacity() == 4);
+ test(list3[0] = 1);
+ test(list3[1] = 3);
+
+ list3.remove(0);
+ test(list3.size() == 1);
+ test(list3.capacity() == 4);
+ test(list3[0] = 3);
+
+ list3.remove(0);
+ test(list3.size() == 0);
+ test(list3.capacity() == 4);
+
+ // ----- Test addRange() ----- //
+
+ List<int> list4(mAllocator);
+ list4.add(1);
+ list4.add(2);
+ list4.add(3);
+
+ List<int> list5(mAllocator);
+ list5.add(4);
+ list5.add(5);
+
+ List<int> list6(mAllocator);
+ list6.addRange(list5);
+ test(list6.size() == list5.size());
+ test(list6[0] == 4);
+ test(list6[1] == 5);
+
+ list4.addRange(list5);
+ test(list4.size() == 3 + list5.size());
+ test(list4[0] == 1);
+ test(list4[1] == 2);
+ test(list4[2] == 3);
+ test(list4[3] == 4);
+ test(list4[4] == 5);
+
+ // ----- Test clear() ----- //
+
+ List<std::string> list7(mAllocator);
+ list7.add("test1");
+ list7.add("test2");
+ list7.add("test3");
+ list7.clear();
+ test(list7.size() == 0);
+ list7.add("new");
+ test(list7.size() == 1);
+ test(list7[0] == "new");
+ }
+
+ void testAssignment() {
+
+ List<int> list1(mAllocator);
+ list1.add(1);
+ list1.add(2);
+ list1.add(3);
+
+ List<int> list2(mAllocator);
+ list2.add(5);
+ list2.add(6);
+
+ List<int> list3(mAllocator);
+ List<int> list4(mAllocator);
+ list4.add(1);
+ list4.add(2);
+
+ List<int> list5(mAllocator);
+ list5.add(1);
+ list5.add(2);
+ list5.add(3);
+
+ list3 = list2;
+ test(list2.size() == list3.size());
+ test(list2[0] == list3[0]);
+ test(list2[1] == list3[1]);
+
+ list4 = list1;
+ test(list4.size() == list1.size())
+ test(list4[0] = list1[0]);
+ test(list4[1] = list1[1]);
+ test(list4[2] = list1[2]);
+
+ list5 = list2;
+ test(list5.size() == list2.size());
+ test(list5[0] = list2[0]);
+ test(list5[1] = list2[1]);
+ }
+
+ void testIndexing() {
+
+ List<int> list1(mAllocator);
+ list1.add(1);
+ list1.add(2);
+ list1.add(3);
+
+ test(list1[0] == 1);
+ test(list1[1] == 2);
+ test(list1[2] == 3);
+
+ list1[0] = 6;
+ list1[1] = 7;
+ list1[2] = 8;
+
+ test(list1[0] == 6);
+ test(list1[1] == 7);
+ test(list1[2] == 8);
+
+ const int a = list1[0];
+ const int b = list1[1];
+ test(a == 6);
+ test(b == 7);
+
+ list1[0]++;
+ list1[1]++;
+ test(list1[0] == 7);
+ test(list1[1] == 8);
+ }
+
+ void testEquality() {
+
+ List<int> list1(mAllocator);
+ list1.add(1);
+ list1.add(2);
+ list1.add(3);
+
+ List<int> list2(mAllocator);
+ list2.add(1);
+ list2.add(2);
+
+ List<int> list3(mAllocator);
+ list3.add(1);
+ list3.add(2);
+ list3.add(3);
+
+ List<int> list4(mAllocator);
+ list4.add(1);
+ list4.add(5);
+ list4.add(3);
+
+ test(list1 == list1);
+ test(list1 != list2);
+ test(list1 == list3);
+ test(list1 != list4);
+ test(list2 != list4);
+ }
+
+ void testReserve() {
+
+ List<int> list1(mAllocator);
+ list1.reserve(10);
+ test(list1.size() == 0);
+ test(list1.capacity() == 10);
+ list1.add(1);
+ list1.add(2);
+ test(list1.capacity() == 10);
+ test(list1.size() == 2);
+ test(list1[0] == 1);
+ test(list1[1] == 2);
+
+ list1.reserve(1);
+ test(list1.capacity() == 10);
+
+ list1.reserve(100);
+ test(list1.capacity() == 100);
+ test(list1[0] == 1);
+ test(list1[1] == 2);
+ }
+
+ void testIteration() {
+ // TODO : Implement this
+ }
+
+ };
+
+}
+
+#endif
diff --git a/test/tests/containers/TestMap.h b/test/tests/containers/TestMap.h
new file mode 100644
index 00000000..d7e084dc
--- /dev/null
+++ b/test/tests/containers/TestMap.h
@@ -0,0 +1,319 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef TEST_MAP_H
+#define TEST_MAP_H
+
+// Libraries
+#include "Test.h"
+#include "containers/Map.h"
+#include "memory/DefaultAllocator.h"
+
+/// Reactphysics3D namespace
+namespace reactphysics3d {
+
+// Class TestMap
+/**
+ * Unit test for the Map class
+ */
+class TestMap : public Test {
+
+ private :
+
+ // ---------- Atributes ---------- //
+
+ DefaultAllocator mAllocator;
+
+ public :
+
+ // ---------- Methods ---------- //
+
+ /// Constructor
+ TestMap(const std::string& name) : Test(name) {
+
+ }
+
+ /// Run the tests
+ void run() {
+
+ testConstructors();
+ testReserve();
+ testAddRemoveClear();
+ testContainsKey();
+ testIndexing();
+ testEquality();
+ testAssignment();
+ testIteration();
+ }
+
+ void testConstructors() {
+
+ // ----- Constructors ----- //
+
+ Map<int, std::string> map1(mAllocator);
+ test(map1.capacity() == 0);
+ test(map1.size() == 0);
+
+ Map<int, std::string> map2(mAllocator, 100);
+ test(map2.capacity() >= 100);
+ test(map2.size() == 0);
+
+ // ----- Copy Constructors ----- //
+/*
+ Map<int, std::string> map3(map1);
+ test(map3.capacity() == map1.capacity());
+ test(map3.size() == map1.size());
+
+ Map<int, int> map4(mAllocator);
+ map4.add(std::make_pair(1, 10));
+ map4.add(std::make_pair(2, 20));
+ map4.add(std::make_pair(3, 30));
+ test(map4.capacity() >= 3);
+ test(map4.size() == 3);
+
+ Map<int, int> map5(map4);
+ test(map5.capacity() == map4.capacity());
+ test(map5.size() == map4.size());
+ test(map5[1] == 10);
+ test(map5[2] == 20);
+ test(map5[3] == 30);
+ */
+ }
+
+ void testReserve() {
+
+ Map<int, std::string> map1(mAllocator);
+ map1.reserve(15);
+ test(map1.capacity() >= 15);
+ map1.add(std::make_pair(1, "test1"));
+ map1.add(std::make_pair(2, "test2"));
+ test(map1.capacity() >= 15);
+
+ map1.reserve(10);
+ test(map1.capacity() >= 15);
+
+ map1.reserve(100);
+ test(map1.capacity() >= 100);
+ test(map1[1] == "test1");
+ test(map1[2] == "test2");
+ }
+
+ void testAddRemoveClear() {
+
+ // ----- Test add() ----- //
+
+ Map<int, int> map1(mAllocator);
+ map1.add(std::make_pair(1, 10));
+ map1.add(std::make_pair(8, 80));
+ map1.add(std::make_pair(13, 130));
+ test(map1[1] == 10);
+ test(map1[8] == 80);
+ test(map1[13] == 130);
+ test(map1.size() == 3);
+
+ Map<int, int> map2(mAllocator, 15);
+ for (int i = 0; i < 1000000; i++) {
+ map2.add(std::make_pair(i, i * 100));
+ }
+ bool isValid = true;
+ for (int i = 0; i < 1000000; i++) {
+ if (map2[i] != i * 100) isValid = false;
+ }
+ test(isValid);
+
+ map1.remove(1);
+ map1.add(std::make_pair(1, 10));
+ test(map1.size() == 3);
+ test(map1[1] == 10);
+
+ // ----- Test remove() ----- //
+
+ map1.remove(1);
+ test(!map1.containsKey(1));
+ test(map1.containsKey(8));
+ test(map1.containsKey(13));
+ test(map1.size() == 2);
+
+ map1.remove(13);
+ test(!map1.containsKey(8));
+ test(map1.containsKey(13));
+ test(map1.size() == 1);
+
+ map1.remove(8);
+ test(!map1.containsKey(8));
+ test(map1.size() == 0);
+
+ isValid = true;
+ for (int i = 0; i < 1000000; i++) {
+ map2.remove(i);
+ }
+ for (int i = 0; i < 1000000; i++) {
+ if (map2.containsKey(i)) isValid = false;
+ }
+ test(isValid);
+ test(map2.size() == 0);
+
+ Map<int, int> map3(mAllocator);
+ for (int i=0; i < 1000000; i++) {
+ map3.add(std::make_pair(i, i * 10));
+ map3.remove(i);
+ }
+
+ // ----- Test clear() ----- //
+
+ Map<int, int> map4(mAllocator);
+ map4.add(std::make_pair(2, 20));
+ map4.add(std::make_pair(4, 40));
+ map4.add(std::make_pair(6, 60));
+ map4.clear();
+ test(map4.size() == 0);
+ map4.add(std::make_pair(2, 20));
+ test(map4.size() == 1);
+ test(map4[2] == 20);
+ map4.clear();
+ test(map4.size() == 0);
+
+ Map<int, int> map5(mAllocator);
+ map5.clear();
+ test(map5.size() == 0);
+ }
+
+ void testContainsKey() {
+
+ Map<int, int> map1(mAllocator);
+
+ test(!map1.containsKey(2));
+ test(!map1.containsKey(4));
+ test(!map1.containsKey(6));
+
+ map1.add(std::make_pair(2, 20));
+ map1.add(std::make_pair(4, 40));
+ map1.add(std::make_pair(6, 60));
+
+ test(map1.containsKey(2));
+ test(map1.containsKey(4));
+ test(map1.containsKey(6));
+
+ map1.remove(4);
+ test(!map1.containsKey(4));
+ test(map1.containsKey(2));
+ test(map1.containsKey(6));
+
+ map1.clear();
+ test(!map1.containsKey(2));
+ test(!map1.containsKey(6));
+ }
+
+ void testIndexing() {
+
+ Map<int, int> map1(mAllocator);
+ map1.add(std::make_pair(2, 20));
+ map1.add(std::make_pair(4, 40));
+ map1.add(std::make_pair(6, 60));
+ test(map1[2] == 20);
+ test(map1[4] == 40);
+ test(map1[6] == 60);
+
+ map1[2] = 10;
+ map1[4] = 20;
+ map1[6] = 30;
+
+ test(map1[2] == 10);
+ test(map1[4] == 20);
+ test(map1[6] == 30);
+ }
+
+ void testEquality() {
+
+ Map<std::string, int> map1(mAllocator, 10);
+ Map<std::string, int> map2(mAllocator, 2);
+
+ test(map1 == map2);
+
+ map1.add(std::make_pair("a", 1));
+ map1.add(std::make_pair("b", 2));
+ map1.add(std::make_pair("c", 3));
+
+ map2.add(std::make_pair("a", 1));
+ map2.add(std::make_pair("b", 2));
+ map2.add(std::make_pair("c", 4));
+
+ test(map1 == map1);
+ test(map2 == map2);
+ test(map1 != map2);
+
+ map2["c"] = 3;
+
+ test(map1 == map2);
+
+ Map<std::string, int> map3(mAllocator);
+ map3.add(std::make_pair("a", 1));
+
+ test(map1 != map3);
+ test(map2 != map3);
+ }
+
+ void testAssignment() {
+
+ Map<int, int> map1(mAllocator);
+ map1.add(std::make_pair(1, 3));
+ map1.add(std::make_pair(2, 6));
+ map1.add(std::make_pair(10, 30));
+/*
+ Map<int, int> map2(mAllocator);
+ map2 = map1;
+ test(map2.size() == map1.size());
+ test(map2[1] == 3);
+ test(map2[2] == 6);
+ test(map2[10] == 30);
+
+ Map<int, int> map3(mAllocator, 100);
+ map3 = map1;
+ test(map3.size() == map1.size());
+ test(map3[1] == 3);
+ test(map3[2] == 6);
+ test(map3[10] == 30);
+
+ Map<int, int> map4(mAllocator);
+ map3 = map4;
+ test(map3.size() == 0);
+*/
+ Map<int, int> map5(mAllocator);
+ map5.add(std::make_pair(7, 8));
+ map5.add(std::make_pair(19, 70));
+ map1 = map5;
+ test(map5.size() == map1.size());
+ test(map1[7] == 8);
+ test(map1[19] == 70);
+ }
+
+ void testIteration() {
+
+ }
+ };
+
+}
+
+#endif
diff --git a/test/tests/mathematics/TestTransform.h b/test/tests/mathematics/TestTransform.h
index 581b9c5e..815e3d54 100644
--- a/test/tests/mathematics/TestTransform.h
+++ b/test/tests/mathematics/TestTransform.h
@@ -61,13 +61,16 @@ class TestTransform : public Test {
mIdentityTransform.setToIdentity();
- decimal sinA = sin(PI/8.0f);
- decimal cosA = cos(PI/8.0f);
- mTransform1 = Transform(Vector3(4, 5, 6), Quaternion(sinA, sinA, sinA, cosA));
+ Vector3 unitVec(1, 1, 1);
+ unitVec.normalize();
- decimal sinB = sin(PI/3.0f);
- decimal cosB = cos(PI/3.0f);
- mTransform2 = Transform(Vector3(8, 45, -6), Quaternion(sinB, sinB, sinB, cosB));
+ decimal sinA = std::sin(PI/8.0f);
+ decimal cosA = std::cos(PI/8.0f);
+ mTransform1 = Transform(Vector3(4, 5, 6), Quaternion(sinA * unitVec, cosA));
+
+ decimal sinB = std::sin(PI/3.0f);
+ decimal cosB = std::cos(PI/3.0f);
+ mTransform2 = Transform(Vector3(8, 45, -6), Quaternion(sinB * unitVec, cosB));
}
/// Run the tests
From 584b28a91c67745030df1482a3ab573063f2f6ad Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 14 Jan 2018 10:51:38 +0100
Subject: [PATCH 133/133] Add Cube stack scene in the testbed application
---
testbed/CMakeLists.txt | 2 +
testbed/common/AABB.h | 3 -
.../CollisionDetectionScene.cpp | 4 +-
testbed/scenes/concavemesh/ConcaveMeshScene.h | 8 +-
testbed/scenes/cubestack/CubeStackScene.cpp | 146 ++++++++++++++++++
testbed/scenes/cubestack/CubeStackScene.h | 82 ++++++++++
testbed/src/TestbedApplication.cpp | 6 +
7 files changed, 242 insertions(+), 9 deletions(-)
create mode 100644 testbed/scenes/cubestack/CubeStackScene.cpp
create mode 100644 testbed/scenes/cubestack/CubeStackScene.h
diff --git a/testbed/CMakeLists.txt b/testbed/CMakeLists.txt
index e5e3b312..818cb026 100644
--- a/testbed/CMakeLists.txt
+++ b/testbed/CMakeLists.txt
@@ -114,6 +114,8 @@ SET(SCENES_SOURCES
scenes/concavemesh/ConcaveMeshScene.cpp
scenes/heightfield/HeightFieldScene.h
scenes/heightfield/HeightFieldScene.cpp
+ scenes/cubestack/CubeStackScene.h
+ scenes/cubestack/CubeStackScene.cpp
)
# Add .user file to set debug path in Visual Studio
diff --git a/testbed/common/AABB.h b/testbed/common/AABB.h
index e73326b5..2e8f82e1 100644
--- a/testbed/common/AABB.h
+++ b/testbed/common/AABB.h
@@ -37,9 +37,6 @@ class AABB {
// -------------------- Attributes -------------------- //
- /// Size of each side of the box
- float mSize[3];
-
/// Scaling matrix (applied to a cube to obtain the correct box dimensions)
openglframework::Matrix4 mScalingMatrix;
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index 6f2c65ae..02e8acc1 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -168,8 +168,8 @@ void CollisionDetectionScene::reset() {
mBox1->setTransform(rp3d::Transform(rp3d::Vector3(-4, -7, 0), rp3d::Quaternion::identity()));
mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 9, 0), rp3d::Quaternion::identity()));
mConvexMesh->setTransform(rp3d::Transform(rp3d::Vector3(-5, 0, 0), rp3d::Quaternion::identity()));
- mConcaveMesh->setTransform(rp3d::Transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()));
- mHeightField->setTransform(rp3d::Transform(rp3d::Vector3(0, -12, 0), rp3d::Quaternion::identity()));
+ mConcaveMesh->setTransform(rp3d::Transform(rp3d::Vector3(0, 15, 0), rp3d::Quaternion::identity()));
+ mHeightField->setTransform(rp3d::Transform(rp3d::Vector3(0, -22, 0), rp3d::Quaternion::identity()));
}
// Destructor
diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.h b/testbed/scenes/concavemesh/ConcaveMeshScene.h
index a8d566a1..beca8f71 100644
--- a/testbed/scenes/concavemesh/ConcaveMeshScene.h
+++ b/testbed/scenes/concavemesh/ConcaveMeshScene.h
@@ -42,10 +42,10 @@ namespace trianglemeshscene {
// Constants
const float SCENE_RADIUS = 70.0f; // Radius of the scene in meters
-static const int NB_BOXES = 10;
-static const int NB_SPHERES = 5;
-static const int NB_CAPSULES = 5;
-static const int NB_MESHES = 3;
+static const int NB_BOXES = 50;
+static const int NB_SPHERES = 40;
+static const int NB_CAPSULES = 20;
+static const int NB_MESHES = 15;
static const int NB_COMPOUND_SHAPES = 3;
const openglframework::Vector3 BOX_SIZE(2, 2, 2);
const float SPHERE_RADIUS = 1.5f;
diff --git a/testbed/scenes/cubestack/CubeStackScene.cpp b/testbed/scenes/cubestack/CubeStackScene.cpp
new file mode 100644
index 00000000..e4a0f859
--- /dev/null
+++ b/testbed/scenes/cubestack/CubeStackScene.cpp
@@ -0,0 +1,146 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+// Libraries
+#include "CubeStackScene.h"
+
+// Namespaces
+using namespace openglframework;
+using namespace cubestackscene;
+
+// Constructor
+CubeStackScene::CubeStackScene(const std::string& name, EngineSettings& settings)
+ : SceneDemo(name, settings, SCENE_RADIUS) {
+
+ // Compute the radius and the center of the scene
+ openglframework::Vector3 center(0, 5, 0);
+
+ // Set the center of the scene
+ setScenePosition(center, SCENE_RADIUS);
+
+ // Gravity vector in the dynamics world
+ rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0);
+
+ // Create the dynamics world for the physics simulation
+ mPhysicsWorld = new rp3d::DynamicsWorld(gravity);
+
+#ifdef IS_PROFILING_ACTIVE
+
+ mPhysicsWorld->setProfilerName(name + "_profiler");
+
+#endif
+
+ // Create all the cubes of the scene
+ for (int i=1; i<=NB_FLOORS; i++) {
+
+ for (int j=0; j<i; j++) {
+
+ // Create a cube and a corresponding rigid in the dynamics world
+ Box* cube = new Box(BOX_SIZE, BOX_MASS, getDynamicsWorld(), mMeshFolderPath);
+
+ // Set the box color
+ cube->setColor(mDemoColors[i % mNbDemoColors]);
+ cube->setSleepingColor(mRedColorDemo);
+
+ // Change the material properties of the rigid body
+ rp3d::Material& material = cube->getRigidBody()->getMaterial();
+ material.setBounciness(rp3d::decimal(0.4));
+
+ // Add the box the list of box in the scene
+ mBoxes.push_back(cube);
+ mPhysicsObjects.push_back(cube);
+ }
+ }
+
+ // ------------------------- FLOOR ----------------------- //
+
+ // Create the floor
+ mFloor = new Box(FLOOR_SIZE, FLOOR_MASS, getDynamicsWorld(), mMeshFolderPath);
+ mFloor->setColor(mGreyColorDemo);
+ mFloor->setSleepingColor(mGreyColorDemo);
+
+ // The floor must be a static rigid body
+ mFloor->getRigidBody()->setType(rp3d::BodyType::STATIC);
+ mPhysicsObjects.push_back(mFloor);
+
+ // Get the physics engine parameters
+ mEngineSettings.isGravityEnabled = getDynamicsWorld()->isGravityEnabled();
+ rp3d::Vector3 gravityVector = getDynamicsWorld()->getGravity();
+ mEngineSettings.gravity = openglframework::Vector3(gravityVector.x, gravityVector.y, gravityVector.z);
+ mEngineSettings.isSleepingEnabled = getDynamicsWorld()->isSleepingEnabled();
+ mEngineSettings.sleepLinearVelocity = getDynamicsWorld()->getSleepLinearVelocity();
+ mEngineSettings.sleepAngularVelocity = getDynamicsWorld()->getSleepAngularVelocity();
+ mEngineSettings.nbPositionSolverIterations = getDynamicsWorld()->getNbIterationsPositionSolver();
+ mEngineSettings.nbVelocitySolverIterations = getDynamicsWorld()->getNbIterationsVelocitySolver();
+ mEngineSettings.timeBeforeSleep = getDynamicsWorld()->getTimeBeforeSleep();
+}
+
+// Destructor
+CubeStackScene::~CubeStackScene() {
+
+ // Destroy all the cubes of the scene
+ for (std::vector<Box*>::iterator it = mBoxes.begin(); it != mBoxes.end(); ++it) {
+
+ // Destroy the corresponding rigid body from the dynamics world
+ getDynamicsWorld()->destroyRigidBody((*it)->getRigidBody());
+
+ // Destroy the cube
+ delete (*it);
+ }
+
+ // Destroy the rigid body of the floor
+ getDynamicsWorld()->destroyRigidBody(mFloor->getRigidBody());
+
+ // Destroy the floor
+ delete mFloor;
+
+ // Destroy the dynamics world
+ delete getDynamicsWorld();
+}
+
+// Reset the scene
+void CubeStackScene::reset() {
+
+ int index = 0;
+ for (int i=NB_FLOORS; i > 0; i--) {
+
+ for (int j=0; j<i; j++) {
+
+ // Create all the cubes of the scene
+ Box* box = mBoxes[index];
+
+ // Position of the cubes
+ rp3d::Vector3 position((-i * 0.5f + j) * (0.1f + BOX_SIZE.x),
+ BOX_SIZE.y + (NB_FLOORS - i) * (BOX_SIZE.y + 0.1f),
+ 0);
+
+ box->setTransform(rp3d::Transform(position, rp3d::Quaternion::identity()));
+
+ index++;
+ }
+ }
+
+ mFloor->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()));
+}
diff --git a/testbed/scenes/cubestack/CubeStackScene.h b/testbed/scenes/cubestack/CubeStackScene.h
new file mode 100644
index 00000000..9bbc1c01
--- /dev/null
+++ b/testbed/scenes/cubestack/CubeStackScene.h
@@ -0,0 +1,82 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2016 Daniel Chappuis *
+*********************************************************************************
+* *
+* This software is provided 'as-is', without any express or implied warranty. *
+* In no event will the authors be held liable for any damages arising from the *
+* use of this software. *
+* *
+* Permission is granted to anyone to use this software for any purpose, *
+* including commercial applications, and to alter it and redistribute it *
+* freely, subject to the following restrictions: *
+* *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+* that you wrote the original software. If you use this software in a *
+* product, an acknowledgment in the product documentation would be *
+* appreciated but is not required. *
+* *
+* 2. Altered source versions must be plainly marked as such, and must not be *
+* misrepresented as being the original software. *
+* *
+* 3. This notice may not be removed or altered from any source distribution. *
+* *
+********************************************************************************/
+
+#ifndef CUBESTACK_SCENE_H
+#define CUBESTACK_SCENE_H
+
+// Libraries
+#include "openglframework.h"
+#include "reactphysics3d.h"
+#include "Box.h"
+#include "SceneDemo.h"
+
+namespace cubestackscene {
+
+// Constants
+const float SCENE_RADIUS = 30.0f; // Radius of the scene in meters
+const int NB_FLOORS = 15; // Number of boxes in the scene
+const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters
+const openglframework::Vector3 FLOOR_SIZE(50, 1, 50); // Floor dimensions in meters
+const float BOX_MASS = 1.0f; // Box mass in kilograms
+const float FLOOR_MASS = 100.0f; // Floor mass in kilograms
+
+// Class CubeStackScene
+class CubeStackScene : public SceneDemo {
+
+ protected :
+
+ // -------------------- Attributes -------------------- //
+
+ /// All the boxes of the scene
+ std::vector<Box*> mBoxes;
+
+ /// Box for the floor
+ Box* mFloor;
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ CubeStackScene(const std::string& name, EngineSettings& settings);
+
+ /// Destructor
+ virtual ~CubeStackScene() override;
+
+ /// Reset the scene
+ virtual void reset() override;
+
+ /// Return all the contact points of the scene
+ virtual std::vector<ContactPoint> getContactPoints() const override;
+};
+
+// Return all the contact points of the scene
+inline std::vector<ContactPoint> CubeStackScene::getContactPoints() const {
+ return computeContactPointsOfWorld(getDynamicsWorld());
+}
+
+}
+
+#endif
diff --git a/testbed/src/TestbedApplication.cpp b/testbed/src/TestbedApplication.cpp
index f2a43bc1..63230e0c 100644
--- a/testbed/src/TestbedApplication.cpp
+++ b/testbed/src/TestbedApplication.cpp
@@ -36,6 +36,7 @@
#include "heightfield/HeightFieldScene.h"
#include "raycast/RaycastScene.h"
#include "concavemesh/ConcaveMeshScene.h"
+#include "cubestack/CubeStackScene.h"
using namespace openglframework;
using namespace jointsscene;
@@ -45,6 +46,7 @@ using namespace collisionshapesscene;
using namespace trianglemeshscene;
using namespace heightfieldscene;
using namespace collisiondetectionscene;
+using namespace cubestackscene;
// Initialization of static variables
const float TestbedApplication::SCROLL_SENSITIVITY = 0.08f;
@@ -102,6 +104,10 @@ void TestbedApplication::createScenes() {
CubesScene* cubeScene = new CubesScene("Cubes", mEngineSettings);
mScenes.push_back(cubeScene);
+ // Cube Stack scene
+ CubeStackScene* cubeStackScene = new CubeStackScene("Cube Stack", mEngineSettings);
+ mScenes.push_back(cubeStackScene);
+
// Joints scene
JointsScene* jointsScene = new JointsScene("Joints", mEngineSettings);
mScenes.push_back(jointsScene);