diff --git a/CHANGELOG.md b/CHANGELOG.md
index cedb3ff7..7ff8f1d9 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,5 +1,18 @@
# Changelog
+## Develop
+
+### Changed
+
+ - The CollisionWorld::testCollision() methods do not have the 'categoryMaskBits' parameter anymore.
+ - The CollisionWorld::testOverlap() methods do not have the 'categoryMaskBits' parameter anymore.
+ - Many methods in the EventListener class have changed. Check the user manual for more information.
+ - The way to retrieve contacts from a CollisionCallbackInfo object has changed. Check the user manual for more information.
+
+### Removed
+
+ - DynamicsWorld::getContactsList(). You need to use the EventListener class to retrieve contacts now.
+
## Release Candidate
### Fixed
@@ -25,6 +38,7 @@
- The methods CollisionBody::getProxyShapesList() has been remove. You can now use the
CollisionBody::getNbProxyShapes() method to know the number of proxy-shapes of a body and the
CollisionBody::getProxyShape(uint proxyShapeIndex) method to get a given proxy-shape of the body.
+ - The CollisionWorld::testAABBOverlap() methods have been removed.
## Version 0.7.0 (May 1, 2018)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 63cc8a17..2057916f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -81,6 +81,8 @@ SET (REACTPHYSICS3D_HEADERS
"src/body/CollisionBody.h"
"src/body/RigidBody.h"
"src/collision/ContactPointInfo.h"
+ "src/collision/ContactManifoldInfo.h"
+ "src/collision/ContactPair.h"
"src/collision/broadphase/DynamicAABBTree.h"
"src/collision/narrowphase/CollisionDispatch.h"
"src/collision/narrowphase/GJK/VoronoiSimplex.h"
@@ -119,7 +121,6 @@ SET (REACTPHYSICS3D_HEADERS
"src/collision/HalfEdgeStructure.h"
"src/collision/CollisionDetection.h"
"src/collision/ContactManifold.h"
- "src/collision/ContactManifoldSet.h"
"src/collision/MiddlePhaseTriangleCallback.h"
"src/constraint/BallAndSocketJoint.h"
"src/constraint/ContactPoint.h"
@@ -135,6 +136,7 @@ SET (REACTPHYSICS3D_HEADERS
"src/engine/DynamicsWorld.h"
"src/engine/EventListener.h"
"src/engine/Island.h"
+ "src/engine/Islands.h"
"src/engine/Material.h"
"src/engine/OverlappingPair.h"
"src/engine/Timer.h"
@@ -213,7 +215,6 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/HalfEdgeStructure.cpp"
"src/collision/CollisionDetection.cpp"
"src/collision/ContactManifold.cpp"
- "src/collision/ContactManifoldSet.cpp"
"src/collision/MiddlePhaseTriangleCallback.cpp"
"src/constraint/BallAndSocketJoint.cpp"
"src/constraint/ContactPoint.cpp"
@@ -238,6 +239,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/components/ProxyShapeComponents.cpp"
"src/components/DynamicsComponents.cpp"
"src/collision/CollisionCallback.cpp"
+ "src/collision/OverlapCallback.cpp"
"src/mathematics/mathematics_functions.cpp"
"src/mathematics/Matrix2x2.cpp"
"src/mathematics/Matrix3x3.cpp"
diff --git a/src/body/Body.cpp b/src/body/Body.cpp
index c322c53e..1117572f 100644
--- a/src/body/Body.cpp
+++ b/src/body/Body.cpp
@@ -36,7 +36,7 @@ using namespace reactphysics3d;
* @param id ID of the new body
*/
Body::Body(Entity entity, bodyindex id)
- : mID(id), mEntity(entity), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true),
+ : mID(id), mEntity(entity), mIsAllowedToSleep(true), mIsActive(true),
mIsSleeping(false), mSleepTime(0), mUserData(nullptr) {
#ifdef IS_LOGGING_ACTIVE
diff --git a/src/body/Body.h b/src/body/Body.h
index bdee42c6..4c7513a5 100644
--- a/src/body/Body.h
+++ b/src/body/Body.h
@@ -57,9 +57,6 @@ class Body {
/// Identifier of the entity in the ECS
Entity mEntity;
- /// True if the body has already been added in an island (for sleeping technique)
- bool mIsAlreadyInIsland;
-
/// True if the body is allowed to go to sleep for better efficiency
bool mIsAllowedToSleep;
@@ -75,8 +72,10 @@ class Body {
bool mIsActive;
/// True if the body is sleeping (for sleeping technique)
+ // TODO : DELETE THIS
bool mIsSleeping;
+ // TODO : Move this into the body components
/// Elapsed time since the body velocity was bellow the sleep velocity
decimal mSleepTime;
@@ -141,6 +140,8 @@ class Body {
void setLogger(Logger* logger);
#endif
+ // TODO : Check if those operators are still used
+
/// Smaller than operator
bool operator<(const Body& body2) const;
diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index 5ff628e4..e7023f3a 100644
--- a/src/body/CollisionBody.cpp
+++ b/src/body/CollisionBody.cpp
@@ -40,8 +40,7 @@ using namespace reactphysics3d;
* @param id ID of the body
*/
CollisionBody::CollisionBody(CollisionWorld& world, Entity entity, bodyindex id)
- : Body(entity, id), mType(BodyType::DYNAMIC),
- mContactManifoldsList(nullptr), mWorld(world) {
+ : Body(entity, id), mType(BodyType::DYNAMIC), mWorld(world) {
#ifdef IS_PROFILING_ACTIVE
mProfiler = nullptr;
@@ -51,7 +50,7 @@ CollisionBody::CollisionBody(CollisionWorld& world, Entity entity, bodyindex id)
// Destructor
CollisionBody::~CollisionBody() {
- assert(mContactManifoldsList == nullptr);
+
}
// Add a collision shape to the body. Note that you can share a collision
@@ -198,23 +197,6 @@ void CollisionBody::removeAllCollisionShapes() {
}
}
-// Reset the contact manifold lists
-void CollisionBody::resetContactManifoldsList() {
-
- // Delete the linked list of contact manifolds of that body
- ContactManifoldListElement* currentElement = mContactManifoldsList;
- while (currentElement != nullptr) {
- ContactManifoldListElement* nextElement = currentElement->getNext();
-
- // Delete the current element
- currentElement->~ContactManifoldListElement();
- mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, currentElement, sizeof(ContactManifoldListElement));
-
- currentElement = nextElement;
- }
- mContactManifoldsList = nullptr;
-}
-
// Return the current position and orientation
/**
* @return The current transformation of the body that transforms the local-space
@@ -283,9 +265,6 @@ void CollisionBody::setIsActive(bool isActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(proxyShape);
}
}
-
- // Reset the contact manifold list of the body
- resetContactManifoldsList();
}
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
@@ -307,26 +286,6 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const {
}
}
-// Reset the mIsAlreadyInIsland variable of the body and contact manifolds.
-/// This method also returns the number of contact manifolds of the body.
-int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
-
- mIsAlreadyInIsland = false;
-
- int nbManifolds = 0;
-
- // Reset the mIsAlreadyInIsland variable of the contact manifolds for
- // this body
- ContactManifoldListElement* currentElement = mContactManifoldsList;
- while (currentElement != nullptr) {
- currentElement->getContactManifold()->mIsAlreadyInIsland = false;
- currentElement = currentElement->getNext();
- nbManifolds++;
- }
-
- return nbManifolds;
-}
-
// Return true if a point is inside the collision body
/// This method returns true if a point is inside any collision shape of the body
/**
diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h
index 018eb87e..152b3ec9 100644
--- a/src/body/CollisionBody.h
+++ b/src/body/CollisionBody.h
@@ -67,12 +67,10 @@ class CollisionBody : public Body {
// -------------------- Attributes -------------------- //
+ // TODO : Move this into the dynamics components
/// Type of body (static, kinematic or dynamic)
BodyType mType;
- /// First element of the linked list of contact manifolds involving this body
- ContactManifoldListElement* mContactManifoldsList;
-
/// Reference to the world the body belongs to
CollisionWorld& mWorld;
@@ -85,9 +83,6 @@ class CollisionBody : public Body {
// -------------------- Methods -------------------- //
- /// Reset the contact manifold lists
- void resetContactManifoldsList();
-
/// Remove all the collision shapes
void removeAllCollisionShapes();
@@ -98,9 +93,6 @@ class CollisionBody : public Body {
/// (as if the body has moved).
void askForBroadPhaseCollisionCheck() const;
- /// Reset the mIsAlreadyInIsland variable of the body and contact manifolds
- int resetIsAlreadyInIslandAndCountManifolds();
-
/// Set the variable to know whether or not the body is sleeping
virtual void setIsSleeping(bool isSleeping) override;
@@ -142,9 +134,6 @@ class CollisionBody : public Body {
/// Remove a collision shape from the body
virtual void removeCollisionShape(ProxyShape *proxyShape);
- /// Return the first element of the linked list of contact manifolds involving this body
- const ContactManifoldListElement* getContactManifoldsList() const;
-
/// Return true if a point is inside the collision body
bool testPointInside(const Vector3& worldPoint) const;
@@ -203,15 +192,6 @@ inline BodyType CollisionBody::getType() const {
return mType;
}
-// Return the first element of the linked list of contact manifolds involving this body
-/**
- * @return A pointer to the first element of the linked-list with the contact
- * manifolds of this body
- */
-inline const ContactManifoldListElement* CollisionBody::getContactManifoldsList() const {
- return mContactManifoldsList;
-}
-
/// 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
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index 393481d0..aed6977f 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -40,13 +40,11 @@ using namespace reactphysics3d;
* @param id The ID of the body
*/
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id)
- : CollisionBody(world, entity, id), mArrayIndex(0), mInitMass(decimal(1.0)),
- mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
- mIsGravityEnabled(true), mMaterial(world.mConfig), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
- mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
+ : CollisionBody(world, entity, id), mMaterial(world.mConfig), mJointsList(nullptr),
+ mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
// Compute the inverse mass
- mMassInverse = decimal(1.0) / mInitMass;
+ mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
@@ -91,15 +89,15 @@ void RigidBody::setType(BodyType type) {
if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) {
// Reset the inverse mass and inverse inertia tensor to zero
- mMassInverse = decimal(0.0);
- mInertiaTensorLocalInverse.setToZero();
- mInertiaTensorInverseWorld.setToZero();
+ mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0));
+ mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
+ mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
}
else { // If it is a dynamic body
- mMassInverse = decimal(1.0) / mInitMass;
+ mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
if (mIsInertiaTensorSetByUser) {
- mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
+ mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
}
}
@@ -109,16 +107,73 @@ void RigidBody::setType(BodyType type) {
// Awake the body
setIsSleeping(false);
- // Remove all the contacts with this body
- resetContactManifoldsList();
-
// Ask the broad-phase to test again the collision shapes of the body for collision
// detection (as if the body has moved)
askForBroadPhaseCollisionCheck();
// Reset the force and torque on the body
- mExternalForce.setToZero();
- mExternalTorque.setToZero();
+ mWorld.mDynamicsComponents.setExternalForce(mEntity, Vector3::zero());
+ mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero());
+}
+
+// Get the inverse local inertia tensor of the body (in body coordinates)
+const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
+ return mWorld.mDynamicsComponents.getInertiaTensorLocalInverse(mEntity);
+}
+
+// Return the inverse of the inertia tensor in world coordinates.
+/// 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
+/**
+ * @return The 3x3 inverse inertia tensor matrix of the body in world-space
+ * coordinates
+ */
+Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
+
+ // Compute and return the inertia tensor in world coordinates
+ return mWorld.mDynamicsComponents.getInertiaTensorWorldInverse(mEntity);
+}
+
+// Method that return the mass of the body
+/**
+ * @return The mass (in kilograms) of the body
+ */
+decimal RigidBody::getMass() const {
+ return mWorld.mDynamicsComponents.getInitMass(mEntity);
+}
+
+// Apply an external force to the body at a given point (in world-space coordinates).
+/// If the point is not at the center of mass of the body, it will also
+/// generate some torque and therefore, change the angular velocity of the body.
+/// If the body is sleeping, calling this method will wake it up. Note that the
+/// force will we added to the sum of the applied forces and that this sum will be
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param force The force to apply on the body
+ * @param point The point where the force is applied (in world-space coordinates)
+ */
+void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
+
+ // If it is not a dynamic body, we do nothing
+ if (mType != BodyType::DYNAMIC) return;
+
+ // Awake the body if it was sleeping
+ if (mIsSleeping) {
+ setIsSleeping(false);
+ }
+
+ // Add the force
+ const Vector3& externalForce = mWorld.mDynamicsComponents.getExternalForce(mEntity);
+ mWorld.mDynamicsComponents.setExternalForce(mEntity, externalForce + force);
+
+ // Add the torque
+ const Vector3& externalTorque = mWorld.mDynamicsComponents.getExternalTorque(mEntity);
+ const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
+ mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + (point - centerOfMassWorld).cross(force));
}
// Set the local inertia tensor of the body (in local-space coordinates)
@@ -136,7 +191,7 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
if (mType != BodyType::DYNAMIC) return;
// Compute the inverse local inertia tensor
- mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
+ mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
@@ -145,6 +200,45 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
"Body " + std::to_string(mID) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
}
+// Apply an external force to the body at its center of mass.
+/// If the body is sleeping, calling this method will wake it up. Note that the
+/// force will we added to the sum of the applied forces and that this sum will be
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param force The external force to apply on the center of mass of the body
+ */
+void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
+
+ // If it is not a dynamic body, we do nothing
+ if (mType != BodyType::DYNAMIC) return;
+
+ // Awake the body if it was sleeping
+ if (mIsSleeping) {
+ setIsSleeping(false);
+ }
+
+ // Add the force
+ const Vector3& externalForce = mWorld.mDynamicsComponents.getExternalForce(mEntity);
+ mWorld.mDynamicsComponents.setExternalForce(mEntity, externalForce + force);
+}
+
+// Return the linear velocity damping factor
+/**
+ * @return The linear damping factor of this body
+ */
+decimal RigidBody::getLinearDamping() const {
+ return mWorld.mDynamicsComponents.getLinearDamping(mEntity);
+}
+
+// Return the angular velocity damping factor
+/**
+ * @return The angular damping factor of this body
+ */
+decimal RigidBody::getAngularDamping() const {
+ return mWorld.mDynamicsComponents.getAngularDamping(mEntity);
+}
+
// Set the inverse local inertia tensor of the body (in local-space coordinates)
/// If the inverse inertia tensor is set with this method, it will not be computed
/// using the collision shapes of the body.
@@ -160,7 +254,7 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens
if (mType != BodyType::DYNAMIC) return;
// Compute the inverse local inertia tensor
- mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
+ mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
@@ -178,20 +272,24 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens
*/
void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
+ // TODO : Check if we need to update the postion of the body here at the end (transform of the body)
+
if (mType != BodyType::DYNAMIC) return;
mIsCenterOfMassSetByUser = true;
- const Vector3 oldCenterOfMass = mCenterOfMassWorld;
- mCenterOfMassLocal = centerOfMassLocal;
+ const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
+ mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, centerOfMassLocal);
// Compute the center of mass in world-space coordinates
- mCenterOfMassWorld = mWorld.mTransformComponents.getTransform(mEntity) * mCenterOfMassLocal;
+ const Vector3& updatedCenterOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
+ mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, mWorld.mTransformComponents.getTransform(mEntity) * updatedCenterOfMassLocal);
// Update the linear velocity of the center of mass
Vector3 linearVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
- linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+ const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
+ linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass);
mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
@@ -206,14 +304,14 @@ void RigidBody::setMass(decimal mass) {
if (mType != BodyType::DYNAMIC) return;
- mInitMass = mass;
+ mWorld.mDynamicsComponents.setInitMass(mEntity, mass);
- if (mInitMass > decimal(0.0)) {
- mMassInverse = decimal(1.0) / mInitMass;
+ if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) {
+ mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
}
else {
- mInitMass = decimal(1.0);
- mMassInverse = decimal(1.0);
+ mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(1.0));
+ mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0));
}
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
@@ -261,7 +359,8 @@ void RigidBody::updateInertiaTensorInverseWorld() {
// TODO : Make sure we do this in a system
Matrix3x3 orientation = mWorld.mTransformComponents.getTransform(mEntity).getOrientation().getMatrix();
- mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose();
+ const Matrix3x3& inverseInertiaLocalTensor = mWorld.mDynamicsComponents.getInertiaTensorLocalInverse(mEntity);
+ mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, orientation * inverseInertiaLocalTensor * orientation.getTranspose());
}
// Add a collision shape to the body.
@@ -361,11 +460,11 @@ void RigidBody::removeCollisionShape(ProxyShape* proxyShape) {
* @param isEnabled True if you want the gravity to be applied to this body
*/
void RigidBody::enableGravity(bool isEnabled) {
- mIsGravityEnabled = isEnabled;
+ mWorld.mDynamicsComponents.setIsGravityEnabled(mEntity, isEnabled);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set isGravityEnabled=" +
- (mIsGravityEnabled ? "true" : "false"));
+ (isEnabled ? "true" : "false"));
}
// Set the linear damping factor. This is the ratio of the linear velocity
@@ -375,10 +474,10 @@ void RigidBody::enableGravity(bool isEnabled) {
*/
void RigidBody::setLinearDamping(decimal linearDamping) {
assert(linearDamping >= decimal(0.0));
- mLinearDamping = linearDamping;
+ mWorld.mDynamicsComponents.setLinearDamping(mEntity, linearDamping);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
- "Body " + std::to_string(mID) + ": Set linearDamping=" + std::to_string(mLinearDamping));
+ "Body " + std::to_string(mID) + ": Set linearDamping=" + std::to_string(linearDamping));
}
// Set the angular damping factor. This is the ratio of the angular velocity
@@ -388,10 +487,10 @@ void RigidBody::setLinearDamping(decimal linearDamping) {
*/
void RigidBody::setAngularDamping(decimal angularDamping) {
assert(angularDamping >= decimal(0.0));
- mAngularDamping = angularDamping;
+ mWorld.mDynamicsComponents.setAngularDamping(mEntity, angularDamping);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
- "Body " + std::to_string(mID) + ": Set angularDamping=" + std::to_string(mAngularDamping));
+ "Body " + std::to_string(mID) + ": Set angularDamping=" + std::to_string(angularDamping));
}
/// Update the transform of the body after a change of the center of mass
@@ -401,7 +500,9 @@ void RigidBody::updateTransformWithCenterOfMass() {
// Translate the body according to the translation of the center of mass position
Transform& transform = mWorld.mTransformComponents.getTransform(mEntity);
- transform.setPosition(mCenterOfMassWorld - transform.getOrientation() * mCenterOfMassLocal);
+ const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
+ const Vector3& centerOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
+ transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
}
// Set a new material for this rigid body
@@ -466,15 +567,17 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
*/
void RigidBody::setTransform(const Transform& transform) {
- const Vector3 oldCenterOfMass = mCenterOfMassWorld;
+ const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
// Compute the new center of mass in world-space coordinates
- mCenterOfMassWorld = transform * mCenterOfMassLocal;
+ const Vector3& centerOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
+ mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform * centerOfMassLocal);
// Update the linear velocity of the center of mass
Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity);
const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
- linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+ const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
+ linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass);
mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
CollisionBody::setTransform(transform);
@@ -490,11 +593,11 @@ void RigidBody::setTransform(const Transform& transform) {
// the collision shapes attached to the body.
void RigidBody::recomputeMassInformation() {
- mInitMass = decimal(0.0);
- mMassInverse = decimal(0.0);
- if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero();
- if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero();
- if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero();
+ mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(0.0));
+ mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0.0));
+ if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
+ if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
+ if (!mIsCenterOfMassSetByUser) mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, Vector3::zero());
Matrix3x3 inertiaTensorLocal;
inertiaTensorLocal.setToZero();
@@ -502,7 +605,7 @@ void RigidBody::recomputeMassInformation() {
// If it is a STATIC or a KINEMATIC body
if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) {
- mCenterOfMassWorld = transform.getPosition();
+ mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform.getPosition());
return;
}
@@ -512,29 +615,30 @@ void RigidBody::recomputeMassInformation() {
const List<Entity>& proxyShapesEntities = mWorld.mBodyComponents.getProxyShapes(mEntity);
for (uint i=0; i < proxyShapesEntities.size(); i++) {
ProxyShape* proxyShape = mWorld.mProxyShapesComponents.getProxyShape(proxyShapesEntities[i]);
- mInitMass += proxyShape->getMass();
+ mWorld.mDynamicsComponents.setInitMass(mEntity, mWorld.mDynamicsComponents.getInitMass(mEntity) + proxyShape->getMass());
if (!mIsCenterOfMassSetByUser) {
- mCenterOfMassLocal += proxyShape->getLocalToBodyTransform().getPosition() * proxyShape->getMass();
+ mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity) +
+ proxyShape->getLocalToBodyTransform().getPosition() * proxyShape->getMass());
}
}
- if (mInitMass > decimal(0.0)) {
- mMassInverse = decimal(1.0) / mInitMass;
+ if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) {
+ mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
}
else {
- mCenterOfMassWorld = transform.getPosition();
+ mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform.getPosition());
return;
}
// Compute the center of mass
- const Vector3 oldCenterOfMass = mCenterOfMassWorld;
+ const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity);
if (!mIsCenterOfMassSetByUser) {
- mCenterOfMassLocal *= mMassInverse;
+ mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity) * mWorld.mDynamicsComponents.getMassInverse(mEntity));
}
- mCenterOfMassWorld = transform * mCenterOfMassLocal;
+ mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform * mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity));
if (!mIsInertiaTensorSetByUser) {
@@ -555,7 +659,7 @@ void RigidBody::recomputeMassInformation() {
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
// center into a inertia tensor w.r.t to the body origin.
- Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal;
+ Vector3 offset = shapeTransform.getPosition() - mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity);
decimal offsetSquare = offset.lengthSquare();
Matrix3x3 offsetMatrix;
offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0));
@@ -570,7 +674,7 @@ void RigidBody::recomputeMassInformation() {
}
// Compute the local inverse inertia tensor
- mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
+ mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse());
}
// Update the world inverse inertia tensor
@@ -579,7 +683,7 @@ void RigidBody::recomputeMassInformation() {
// Update the linear velocity of the center of mass
Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity);
Vector3 angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
- linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+ linearVelocity += angularVelocity.cross(mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity) - oldCenterOfMass);
mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
}
@@ -599,16 +703,48 @@ Vector3 RigidBody::getAngularVelocity() const {
return mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
}
+// Return true if the gravity needs to be applied to this rigid body
+/**
+ * @return True if the gravity is applied to the body
+ */
+bool RigidBody::isGravityEnabled() const {
+ return mWorld.mDynamicsComponents.getIsGravityEnabled(mEntity);
+}
+
+// Apply an external torque to the body.
+/// If the body is sleeping, calling this method will wake it up. Note that the
+/// force will we added to the sum of the applied torques and that this sum will be
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param torque The external torque to apply on the body
+ */
+void RigidBody::applyTorque(const Vector3& torque) {
+
+ // If it is not a dynamic body, we do nothing
+ if (mType != BodyType::DYNAMIC) return;
+
+ // Awake the body if it was sleeping
+ if (mIsSleeping) {
+ setIsSleeping(false);
+ }
+
+ // Add the torque
+ const Vector3& externalTorque = mWorld.mDynamicsComponents.getExternalTorque(mEntity);
+ mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + torque);
+}
+
// Set the variable to know whether or not the body is sleeping
void RigidBody::setIsSleeping(bool isSleeping) {
CollisionBody::setIsSleeping(isSleeping);
if (isSleeping) {
+
mWorld.mDynamicsComponents.setLinearVelocity(mEntity, Vector3::zero());
mWorld.mDynamicsComponents.setAngularVelocity(mEntity, Vector3::zero());
- mExternalForce.setToZero();
- mExternalTorque.setToZero();
+ mWorld.mDynamicsComponents.setExternalForce(mEntity, Vector3::zero());
+ mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero());
}
}
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index 5946e990..1f8e16bb 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -50,62 +50,17 @@ class MemoryManager;
*/
class RigidBody : public CollisionBody {
- private :
-
- /// Index of the body in arrays for contact/constraint solver
- uint mArrayIndex;
-
protected :
// -------------------- Attributes -------------------- //
- /// Intial mass of the body
- decimal mInitMass;
-
- /// Center of mass of the body in local-space coordinates.
- /// The center of mass can therefore be different from the body origin
- Vector3 mCenterOfMassLocal;
-
- /// Center of mass of the body in world-space coordinates
- Vector3 mCenterOfMassWorld;
-
- /// Linear velocity of the body
- //Vector3 mLinearVelocity;
-
- /// Angular velocity of the body
- //Vector3 mAngularVelocity;
-
- /// Current external force on the body
- Vector3 mExternalForce;
-
- /// Current external torque on the body
- Vector3 mExternalTorque;
-
/// Inverse Local inertia tensor of the body (in local-space) set
/// by the user with respect to the center of mass of the body
Matrix3x3 mUserInertiaTensorLocalInverse;
- /// 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;
-
- /// True if the gravity needs to be applied to this rigid body
- bool mIsGravityEnabled;
-
/// Material properties of the rigid body
Material mMaterial;
- /// Linear velocity damping factor
- decimal mLinearDamping;
-
- /// Angular velocity damping factor
- decimal mAngularDamping;
-
/// First element of the linked list of joints involving this body
JointListElement* mJointsList;
@@ -252,43 +207,6 @@ class RigidBody : public CollisionBody {
friend class FixedJoint;
};
-// Method that return the mass of the body
-/**
- * @return The mass (in kilograms) of the body
- */
-inline decimal RigidBody::getMass() const {
- return mInitMass;
-}
-
-// Get the inverse local inertia tensor of the body (in body coordinates)
-inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
- return mInertiaTensorLocalInverse;
-}
-
-// Return the inverse of the inertia tensor in world coordinates.
-/// 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
-/**
- * @return The 3x3 inverse inertia tensor matrix of the body in world-space
- * coordinates
- */
-inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
-
- // Compute and return the inertia tensor in world coordinates
- return mInertiaTensorInverseWorld;
-}
-
-// Return true if the gravity needs to be applied to this rigid body
-/**
- * @return True if the gravity is applied to the body
- */
-inline bool RigidBody::isGravityEnabled() const {
- return mIsGravityEnabled;
-}
-
// Return a reference to the material properties of the rigid body
/**
* @return A reference to the material of the body
@@ -297,22 +215,6 @@ inline Material& RigidBody::getMaterial() {
return mMaterial;
}
-// Return the linear velocity damping factor
-/**
- * @return The linear damping factor of this body
- */
-inline decimal RigidBody::getLinearDamping() const {
- return mLinearDamping;
-}
-
-// Return the angular velocity damping factor
-/**
- * @return The angular damping factor of this body
- */
-inline decimal RigidBody::getAngularDamping() const {
- return mAngularDamping;
-}
-
// Return the first element of the linked list of joints involving this body
/**
* @return The first element of the linked-list of all the joints involving this body
@@ -329,76 +231,6 @@ inline JointListElement* RigidBody::getJointsList() {
return mJointsList;
}
-// Apply an external force to the body at its center of mass.
-/// If the body is sleeping, calling this method will wake it up. Note that the
-/// force will we added to the sum of the applied forces and that this sum will be
-/// reset to zero at the end of each call of the DynamicsWorld::update() method.
-/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
-/**
- * @param force The external force to apply on the center of mass of the body
- */
-inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
-
- // If it is not a dynamic body, we do nothing
- if (mType != BodyType::DYNAMIC) return;
-
- // Awake the body if it was sleeping
- if (mIsSleeping) {
- setIsSleeping(false);
- }
-
- // Add the force
- mExternalForce += force;
-}
-
-// Apply an external force to the body at a given point (in world-space coordinates).
-/// If the point is not at the center of mass of the body, it will also
-/// generate some torque and therefore, change the angular velocity of the body.
-/// If the body is sleeping, calling this method will wake it up. Note that the
-/// force will we added to the sum of the applied forces and that this sum will be
-/// reset to zero at the end of each call of the DynamicsWorld::update() method.
-/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
-/**
- * @param force The force to apply on the body
- * @param point The point where the force is applied (in world-space coordinates)
- */
-inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
-
- // If it is not a dynamic body, we do nothing
- if (mType != BodyType::DYNAMIC) return;
-
- // Awake the body if it was sleeping
- if (mIsSleeping) {
- setIsSleeping(false);
- }
-
- // Add the force and torque
- mExternalForce += force;
- mExternalTorque += (point - mCenterOfMassWorld).cross(force);
-}
-
-// Apply an external torque to the body.
-/// If the body is sleeping, calling this method will wake it up. Note that the
-/// force will we added to the sum of the applied torques and that this sum will be
-/// reset to zero at the end of each call of the DynamicsWorld::update() method.
-/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
-/**
- * @param torque The external torque to apply on the body
- */
-inline void RigidBody::applyTorque(const Vector3& torque) {
-
- // If it is not a dynamic body, we do nothing
- if (mType != BodyType::DYNAMIC) return;
-
- // Awake the body if it was sleeping
- if (mIsSleeping) {
- setIsSleeping(false);
- }
-
- // Add the torque
- mExternalTorque += torque;
-}
-
}
#endif
diff --git a/src/collision/CollisionCallback.cpp b/src/collision/CollisionCallback.cpp
index e6ee5ba3..0162edfa 100644
--- a/src/collision/CollisionCallback.cpp
+++ b/src/collision/CollisionCallback.cpp
@@ -25,59 +25,71 @@
// Libraries
#include "collision/CollisionCallback.h"
-#include "engine/OverlappingPair.h"
-#include "memory/MemoryAllocator.h"
-#include "collision/ContactManifold.h"
-#include "memory/MemoryManager.h"
+#include "collision/ContactPair.h"
+#include "constraint/ContactPoint.h"
+#include "engine/CollisionWorld.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
-CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager) :
- contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()),
- body2(pair->getShape2()->getBody()),
- proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()),
- mMemoryManager(memoryManager) {
+CollisionCallback::ContactPoint::ContactPoint(const reactphysics3d::ContactPoint& contactPoint) : mContactPoint(contactPoint) {
- assert(pair != nullptr);
-
- const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
-
- // 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);
-
- // Add the contact manifold at the beginning of the linked
- // list of contact manifolds of the first body
- ContactManifoldListElement* element = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
- sizeof(ContactManifoldListElement)))
- ContactManifoldListElement(contactManifold,
- contactManifoldElements);
- contactManifoldElements = element;
-
- contactManifold = contactManifold->getNext();
- }
}
-// Destructor
-CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() {
+// Contact Pair Constructor
+CollisionCallback::ContactPair::ContactPair(const reactphysics3d::ContactPair& contactPair,
+ List<reactphysics3d::ContactPoint>* contactPoints, CollisionWorld& world)
+ :mContactPair(contactPair), mContactPoints(contactPoints),
+ mWorld(world) {
- // 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();
- mMemoryManager.release(MemoryManager::AllocationType::Pool, element,
- sizeof(ContactManifoldListElement));
-
- element = nextElement;
- }
}
+// Return a pointer to the first body in contact
+CollisionBody* CollisionCallback::ContactPair::getBody1() const {
+ return static_cast<CollisionBody*>(mWorld.mBodyComponents.getBody(mContactPair.body1Entity));
+}
+
+// Return a pointer to the second body in contact
+CollisionBody* CollisionCallback::ContactPair::getBody2() const {
+ return static_cast<CollisionBody*>(mWorld.mBodyComponents.getBody(mContactPair.body2Entity));
+}
+
+// Return a pointer to the first proxy-shape in contact (in body 1)
+ProxyShape* CollisionCallback::ContactPair::getProxyShape1() const {
+ return mWorld.mProxyShapesComponents.getProxyShape(mContactPair.proxyShape1Entity);
+}
+
+// Return a pointer to the second proxy-shape in contact (in body 1)
+ProxyShape* CollisionCallback::ContactPair::getProxyShape2() const {
+ return mWorld.mProxyShapesComponents.getProxyShape(mContactPair.proxyShape2Entity);
+}
+
+// CollisionCallbackInfo Constructor
+CollisionCallback::CallbackData::CallbackData(List<reactphysics3d::ContactPair>* contactPairs, List<ContactManifold>* manifolds,
+ List<reactphysics3d::ContactPoint>* contactPoints, CollisionWorld& world)
+ :mContactPairs(contactPairs), mContactManifolds(manifolds), mContactPoints(contactPoints), mWorld(world) {
+
+}
+
+// Return a given contact point of the contact pair
+/// Note that the returned ContactPoint object is only valid during the call of the CollisionCallback::onContact()
+/// method. Therefore, you need to get contact data from it and make a copy. Do not make a copy of the ContactPoint
+/// object itself because it won't be valid after the CollisionCallback::onContact() call.
+CollisionCallback::ContactPoint CollisionCallback::ContactPair::getContactPoint(uint index) const {
+
+ assert(index < getNbContactPoints());
+
+ return CollisionCallback::ContactPoint((*mContactPoints)[mContactPair.contactPointsIndex + index]);
+}
+
+// Return a given contact pair
+/// Note that the returned ContactPair object is only valid during the call of the CollisionCallback::onContact()
+/// method. Therefore, you need to get contact data from it and make a copy. Do not make a copy of the ContactPair
+/// object itself because it won't be valid after the CollisionCallback::onContact() call.
+CollisionCallback::ContactPair CollisionCallback::CallbackData::getContactPair(uint index) const {
+
+ assert(index < getNbContactPairs());
+
+ return CollisionCallback::ContactPair((*mContactPairs)[index], mContactPoints, mWorld);
+}
diff --git a/src/collision/CollisionCallback.h b/src/collision/CollisionCallback.h
index bd9e7703..87e416ff 100644
--- a/src/collision/CollisionCallback.h
+++ b/src/collision/CollisionCallback.h
@@ -26,6 +26,11 @@
#ifndef REACTPHYSICS3D_COLLISION_CALLBACK_H
#define REACTPHYSICS3D_COLLISION_CALLBACK_H
+// Libraries
+#include "containers/List.h"
+#include "collision/ContactPair.h"
+#include "constraint/ContactPoint.h"
+
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -36,10 +41,11 @@ struct ContactManifoldListElement;
class CollisionBody;
class ProxyShape;
class MemoryManager;
+class CollisionCallbackInfo;
// Class CollisionCallback
/**
- * This class can be used to register a callback for collision test queries.
+ * This abstract 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.
@@ -48,47 +54,221 @@ class CollisionCallback {
public:
- // Structure CollisionCallbackInfo
+ // Class ContactPoint
/**
- * This structure represents the contact information between two collision
- * shapes of two bodies
+ * This class represents a contact point between two bodies of the physics world.
*/
- struct CollisionCallbackInfo {
+ class ContactPoint {
+
+ private:
+
+ // -------------------- Attributes -------------------- //
+
+ const reactphysics3d::ContactPoint& mContactPoint;
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ ContactPoint(const reactphysics3d::ContactPoint& contactPoint);
public:
- /// Pointer to the first element of the linked-list that contains contact manifolds
- ContactManifoldListElement* contactManifoldElements;
+ // -------------------- Methods -------------------- //
- /// Pointer to the first body of the contact
- CollisionBody* body1;
+ /// Copy constructor
+ ContactPoint(const ContactPoint& contactPoint) = default;
- /// Pointer to the second body of the contact
- CollisionBody* body2;
+ /// Assignment operator
+ ContactPoint& operator=(const ContactPoint& contactPoint) = default;
- /// Pointer to the proxy shape of first body
- const ProxyShape* proxyShape1;
+ /// Destructor
+ ~ContactPoint() = default;
- /// Pointer to the proxy shape of second body
- const ProxyShape* proxyShape2;
+ /// Return the penetration depth
+ decimal getPenetrationDepth() const;
- /// Memory manager
- MemoryManager& mMemoryManager;
+ /// Return the world-space contact normal
+ const Vector3& getWorldNormal() const;
- // Constructor
- CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager);
+ /// Return the contact point on the first proxy shape in the local-space of the first proxy shape
+ const Vector3& getLocalPointOnShape1() const;
- // Destructor
- ~CollisionCallbackInfo();
+ /// Return the contact point on the second proxy shape in the local-space of the second proxy shape
+ const Vector3& getLocalPointOnShape2() const;
+
+ // -------------------- Friendship -------------------- //
+
+ friend class CollisionCallback;
+ };
+
+ // Class ContactPair
+ /**
+ * This class represents the contact between two bodies of the physics world.
+ * A contact pair contains a list of contact points.
+ */
+ class ContactPair {
+
+ private:
+
+ // -------------------- Attributes -------------------- //
+
+ const reactphysics3d::ContactPair& mContactPair;
+
+ /// Pointer to the contact points
+ List<reactphysics3d::ContactPoint>* mContactPoints;
+
+ /// Reference to the physics world
+ CollisionWorld& mWorld;
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ ContactPair(const reactphysics3d::ContactPair& contactPair, List<reactphysics3d::ContactPoint>* contactPoints,
+ CollisionWorld& world);
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Copy constructor
+ ContactPair(const ContactPair& contactPair) = default;
+
+ /// Assignment operator
+ ContactPair& operator=(const ContactPair& contactPair) = default;
+
+ /// Destructor
+ ~ContactPair() = default;
+
+ /// Return the number of contact points in the contact pair
+ uint getNbContactPoints() const;
+
+ /// Return a given contact point
+ ContactPoint getContactPoint(uint index) const;
+
+ /// Return a pointer to the first body in contact
+ CollisionBody* getBody1() const;
+
+ /// Return a pointer to the second body in contact
+ CollisionBody* getBody2() const;
+
+ /// Return a pointer to the first proxy-shape in contact (in body 1)
+ ProxyShape* getProxyShape1() const;
+
+ /// Return a pointer to the second proxy-shape in contact (in body 2)
+ ProxyShape* getProxyShape2() const;
+
+ // -------------------- Friendship -------------------- //
+
+ friend class CollisionCallback;
+ };
+
+ // Class CallbackData
+ /**
+ * This class contains data about contacts between bodies
+ */
+ class CallbackData {
+
+ private:
+
+ // -------------------- Attributes -------------------- //
+
+ /// Pointer to the list of contact pairs
+ List<reactphysics3d::ContactPair>* mContactPairs;
+
+ /// Pointer to the list of contact manifolds
+ List<ContactManifold>* mContactManifolds;
+
+ /// Pointer to the contact points
+ List<reactphysics3d::ContactPoint>* mContactPoints;
+
+ /// Reference to the physics world
+ CollisionWorld& mWorld;
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ CallbackData(List<reactphysics3d::ContactPair>* contactPairs, List<ContactManifold>* manifolds,
+ List<reactphysics3d::ContactPoint>* contactPoints, CollisionWorld& world);
+
+ /// Deleted copy constructor
+ CallbackData(const CallbackData& callbackData) = delete;
+
+ /// Deleted assignment operator
+ CallbackData& operator=(const CallbackData& callbackData) = delete;
+
+ /// Destructor
+ ~CallbackData() = default;
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Return the number of contact pairs
+ uint getNbContactPairs() const;
+
+ /// Return a given contact pair
+ ContactPair getContactPair(uint index) const;
+
+ // -------------------- Friendship -------------------- //
+
+ friend class CollisionDetection;
};
/// Destructor
virtual ~CollisionCallback() = default;
- /// This method will be called for each reported contact point
- virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
+ /// This method is called when some contacts occur
+ virtual void onContact(const CallbackData& callbackData)=0;
};
+// Return the number of contact pairs (there is a single contact pair between two bodies in contact)
+/**
+ * @return The number of contact pairs
+ */
+inline uint CollisionCallback::CallbackData::getNbContactPairs() const {
+ return mContactPairs->size();
+}
+
+// Return the number of contact points in the contact pair
+/**
+ * @return The number of contact points
+ */
+inline uint CollisionCallback::ContactPair::getNbContactPoints() const {
+ return mContactPair.nbToTalContactPoints;
+}
+
+// Return the penetration depth between the two bodies in contact
+/**
+ * @return The penetration depth (larger than zero)
+ */
+inline decimal CollisionCallback::ContactPoint::getPenetrationDepth() const {
+ return mContactPoint.getPenetrationDepth();
+}
+
+// Return the world-space contact normal (vector from first body toward second body)
+/**
+ * @return The contact normal direction at the contact point (in world-space)
+ */
+inline const Vector3& CollisionCallback::ContactPoint::getWorldNormal() const {
+ return mContactPoint.getNormal();
+}
+
+// Return the contact point on the first proxy shape in the local-space of the first proxy shape
+/**
+ * @return The contact point in the local-space of the first proxy-shape (from body1) in contact
+ */
+inline const Vector3& CollisionCallback::ContactPoint::getLocalPointOnShape1() const {
+ return mContactPoint.getLocalPointOnShape1();
+}
+
+// Return the contact point on the second proxy shape in the local-space of the second proxy shape
+/**
+ * @return The contact point in the local-space of the second proxy-shape (from body2) in contact
+ */
+inline const Vector3& CollisionCallback::ContactPoint::getLocalPointOnShape2() const {
+ return mContactPoint.getLocalPointOnShape2();
+}
+
}
#endif
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 6738cae0..a1647ca9 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -1,4 +1,4 @@
-/********************************************************************************
+/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
@@ -30,6 +30,8 @@
#include "body/Body.h"
#include "collision/shapes/BoxShape.h"
#include "collision/shapes/ConcaveShape.h"
+#include "collision/ContactManifoldInfo.h"
+#include "constraint/ContactPoint.h"
#include "body/RigidBody.h"
#include "configuration.h"
#include "collision/CollisionCallback.h"
@@ -40,7 +42,10 @@
#include "utils/Profiler.h"
#include "engine/EventListener.h"
#include "collision/RaycastInfo.h"
+#include "engine/Islands.h"
+#include "containers/Pair.h"
#include <cassert>
+#include <iostream>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@@ -50,12 +55,23 @@ using namespace std;
// Constructor
CollisionDetection::CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, TransformComponents& transformComponents,
DynamicsComponents& dynamicsComponents, MemoryManager& memoryManager)
- : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents), mTransformComponents(transformComponents),
+ : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents),
mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world),
mOverlappingPairs(mMemoryManager.getPoolAllocator()),
- mBroadPhaseSystem(*this, mProxyShapesComponents, mTransformComponents, dynamicsComponents),
+ mBroadPhaseSystem(*this, mProxyShapesComponents, transformComponents, dynamicsComponents),
mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()),
- mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()) {
+ mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()),
+ // TODO : We should probably use single frame allocator for mPotentialContactPoints, mPotentialContactManifolds, mMapPairIdToOverlappingPairContacts
+ mPotentialContactManifolds(mMemoryManager.getSingleFrameAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()),
+ mContactPairs2(mMemoryManager.getPoolAllocator()), mPreviousContactPairs(&mContactPairs1), mCurrentContactPairs(&mContactPairs2),
+ mMapPairIdToContactPairIndex1(mMemoryManager.getPoolAllocator()), mMapPairIdToContactPairIndex2(mMemoryManager.getPoolAllocator()),
+ mPreviousMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex1), mCurrentMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex2),
+ mContactPairsIndicesOrderingForContacts(memoryManager.getSingleFrameAllocator()),
+ mContactManifolds1(mMemoryManager.getPoolAllocator()), mContactManifolds2(mMemoryManager.getPoolAllocator()),
+ mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2),
+ mContactPoints1(mMemoryManager.getPoolAllocator()),
+ mContactPoints2(mMemoryManager.getPoolAllocator()), mPreviousContactPoints(&mContactPoints1),
+ mCurrentContactPoints(&mContactPoints2), mMapBodyToContactPairs(mMemoryManager.getSingleFrameAllocator()) {
#ifdef IS_PROFILING_ACTIVE
@@ -75,7 +91,7 @@ void CollisionDetection::computeCollisionDetection() {
computeBroadPhase();
// Compute the middle-phase collision detection
- computeMiddlePhase();
+ computeMiddlePhase(mOverlappingPairs, mNarrowPhaseInput);
// Compute the narrow-phase collision detection
computeNarrowPhase();
@@ -94,12 +110,40 @@ void CollisionDetection::computeBroadPhase() {
// Create new overlapping pairs if necessary
updateOverlappingPairs(overlappingNodes);
+
+ // Remove non overlapping pairs
+ removeNonOverlappingPairs();
+}
+
+// Remove pairs that are not overlapping anymore
+void CollisionDetection::removeNonOverlappingPairs() {
+
+ // For each possible collision pair of bodies
+ for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+
+ OverlappingPair* pair = it->second;
+
+ ProxyShape* shape1 = pair->getShape1();
+ ProxyShape* shape2 = pair->getShape2();
+
+ // Check if the two shapes are still overlapping. Otherwise, we destroy the overlapping pair
+ if (!mBroadPhaseSystem.testOverlappingShapes(shape1, shape2)) {
+
+ // Destroy the overlapping pair
+ pair->~OverlappingPair();
+
+ mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair));
+ it = mOverlappingPairs.remove(it);
+ continue;
+ }
+ else {
+ ++it;
+ }
+ }
}
// Take a list of overlapping nodes in the broad-phase and create new overlapping pairs if necessary
-void CollisionDetection::updateOverlappingPairs(List<Pair<int, int>>& overlappingNodes) {
-
- List<OverlappingPair*> newOverlappingPairs(mMemoryManager.getPoolAllocator(), overlappingNodes.size());
+void CollisionDetection::updateOverlappingPairs(const List<Pair<int, int>>& overlappingNodes) {
// For each overlapping pair of nodes
for (uint i=0; i < overlappingNodes.size(); i++) {
@@ -151,73 +195,52 @@ void CollisionDetection::updateOverlappingPairs(List<Pair<int, int>>& overlappin
// Add the new overlapping pair
mOverlappingPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(pairID, newPair));
- newOverlappingPairs.add(newPair);
}
}
}
}
}
-
- // For each new overlapping pair
- for (uint i=0; i < newOverlappingPairs.size(); i++) {
-
- // Wake up the two bodies of the new overlapping pair
- mWorld->notifyBodyDisabled(newOverlappingPairs[i]->getShape1()->getBody()->getEntity(), false);
- mWorld->notifyBodyDisabled(newOverlappingPairs[i]->getShape1()->getBody()->getEntity(), false);
- }
}
// Compute the middle-phase collision detection
-void CollisionDetection::computeMiddlePhase() {
+void CollisionDetection::computeMiddlePhase(OverlappingPairMap& overlappingPairs, NarrowPhaseInput& narrowPhaseInput) {
RP3D_PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler);
// Reserve memory for the narrow-phase input using cached capacity from previous frame
- mNarrowPhaseInput.reserveMemory();
+ narrowPhaseInput.reserveMemory();
// For each possible collision pair of bodies
- for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+ for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
OverlappingPair* pair = it->second;
- // Make all the contact manifolds and contact points of the pair obsolete
- pair->makeContactsObsolete();
-
// Make all the last frame collision info obsolete
pair->makeLastFrameCollisionInfosObsolete();
+ const Entity proxyShape1Entity = pair->getShape1()->getEntity();
+ const Entity proxyShape2Entity = pair->getShape2()->getEntity();
+
ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2();
- assert(shape1->getBroadPhaseId() != -1);
- assert(shape2->getBroadPhaseId() != -1);
- assert(shape1->getBroadPhaseId() != shape2->getBroadPhaseId());
-
- // Check if the two shapes are still overlapping. Otherwise, we destroy the
- // overlapping pair
- if (!mBroadPhaseSystem.testOverlappingShapes(shape1, shape2)) {
-
- // Destroy the overlapping pair
- pair->~OverlappingPair();
-
- mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair));
- it = mOverlappingPairs.remove(it);
- continue;
- }
- else {
- ++it;
- }
+ assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1);
+ assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1);
+ assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity));
// Check if the collision filtering allows collision between the two shapes
- if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 &&
- (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0)) {
+ if ((mProxyShapesComponents.getCollideWithMaskBits(proxyShape1Entity) & mProxyShapesComponents.getCollisionCategoryBits(proxyShape2Entity)) != 0 &&
+ (mProxyShapesComponents.getCollisionCategoryBits(proxyShape1Entity) & mProxyShapesComponents.getCollideWithMaskBits(proxyShape2Entity)) != 0) {
CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody();
- // 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;
+ const Entity body1Entity = body1->getEntity();
+ const Entity body2Entity = body2->getEntity();
+
+ // Check that at least one body is enabled (active and awake) and not static
+ bool isBody1Active = !mWorld->mBodyComponents.getIsEntityDisabled(body1Entity) && body1->getType() != BodyType::STATIC;
+ bool isBody2Active = !mWorld->mBodyComponents.getIsEntityDisabled(body2Entity) && body2->getType() != BodyType::STATIC;
if (!isBody1Active && !isBody2Active) continue;
// Check if the bodies are in the set of bodies that cannot collide between each other
@@ -236,7 +259,7 @@ void CollisionDetection::computeMiddlePhase() {
// No middle-phase is necessary, simply create a narrow phase info
// for the narrow-phase collision detection
- mNarrowPhaseInput.addNarrowPhaseTest(pair, shape1->getCollisionShape(), shape2->getCollisionShape(),
+ narrowPhaseInput.addNarrowPhaseTest(pair, shape1->getCollisionShape(), shape2->getCollisionShape(),
shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(),
algorithmType, mMemoryManager.getSingleFrameAllocator());
@@ -244,7 +267,7 @@ void CollisionDetection::computeMiddlePhase() {
// Concave vs Convex algorithm
else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) {
- computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), mNarrowPhaseInput);
+ computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput);
}
// Concave vs Concave shape
else {
@@ -312,8 +335,8 @@ void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair
}
// Execute the narrow-phase collision detection algorithm on batches
-bool CollisionDetection::testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool stopFirstContactFound,
- bool reportContacts, MemoryAllocator& allocator) {
+bool CollisionDetection::testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool reportContacts,
+ bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& allocator) {
bool contactFound = false;
@@ -335,35 +358,37 @@ bool CollisionDetection::testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseI
// Compute the narrow-phase collision detection for each kind of collision shapes
if (sphereVsSphereBatch.getNbObjects() > 0) {
- contactFound |= sphereVsSphereAlgo->testCollision(sphereVsSphereBatch, 0, sphereVsSphereBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator);
- if (stopFirstContactFound && contactFound) return true;
+ contactFound |= sphereVsSphereAlgo->testCollision(sphereVsSphereBatch, 0, sphereVsSphereBatch.getNbObjects(), reportContacts, allocator);
}
if (sphereVsCapsuleBatch.getNbObjects() > 0) {
- contactFound |= sphereVsCapsuleAlgo->testCollision(sphereVsCapsuleBatch, 0, sphereVsCapsuleBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator);
- if (stopFirstContactFound && contactFound) return true;
+ contactFound |= sphereVsCapsuleAlgo->testCollision(sphereVsCapsuleBatch, 0, sphereVsCapsuleBatch.getNbObjects(), reportContacts, allocator);
}
if (capsuleVsCapsuleBatch.getNbObjects() > 0) {
- contactFound |= capsuleVsCapsuleAlgo->testCollision(capsuleVsCapsuleBatch, 0, capsuleVsCapsuleBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator);
- if (stopFirstContactFound && contactFound) return true;
+ contactFound |= capsuleVsCapsuleAlgo->testCollision(capsuleVsCapsuleBatch, 0, capsuleVsCapsuleBatch.getNbObjects(), reportContacts, allocator);
}
if (sphereVsConvexPolyhedronBatch.getNbObjects() > 0) {
- contactFound |= sphereVsConvexPolyAlgo->testCollision(sphereVsConvexPolyhedronBatch, 0, sphereVsConvexPolyhedronBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator);
- if (stopFirstContactFound && contactFound) return true;
+ contactFound |= sphereVsConvexPolyAlgo->testCollision(sphereVsConvexPolyhedronBatch, 0, sphereVsConvexPolyhedronBatch.getNbObjects(), reportContacts, clipWithPreviousAxisIfStillColliding, allocator);
}
if (capsuleVsConvexPolyhedronBatch.getNbObjects() > 0) {
- contactFound |= capsuleVsConvexPolyAlgo->testCollision(capsuleVsConvexPolyhedronBatch, 0, capsuleVsConvexPolyhedronBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator);
- if (stopFirstContactFound && contactFound) return true;
+ contactFound |= capsuleVsConvexPolyAlgo->testCollision(capsuleVsConvexPolyhedronBatch, 0, capsuleVsConvexPolyhedronBatch.getNbObjects(), reportContacts, clipWithPreviousAxisIfStillColliding, allocator);
}
if (convexPolyhedronVsConvexPolyhedronBatch.getNbObjects() > 0) {
- contactFound |= convexPolyVsConvexPolyAlgo->testCollision(convexPolyhedronVsConvexPolyhedronBatch, 0, convexPolyhedronVsConvexPolyhedronBatch.getNbObjects(), reportContacts, stopFirstContactFound, allocator);
- if (stopFirstContactFound && contactFound) return true;
+ contactFound |= convexPolyVsConvexPolyAlgo->testCollision(convexPolyhedronVsConvexPolyhedronBatch, 0, convexPolyhedronVsConvexPolyhedronBatch.getNbObjects(), reportContacts, clipWithPreviousAxisIfStillColliding, allocator);
}
return contactFound;
}
// Process the potential contacts after narrow-phase collision detection
-void CollisionDetection::processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo) {
+void CollisionDetection::processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo,
+ List<ContactPointInfo>& potentialContactPoints,
+ Map<OverlappingPair::OverlappingPairId, uint>* mapPairIdToContactPairIndex,
+ List<ContactManifoldInfo>& potentialContactManifolds,
+ List<ContactPair>* contactPairs,
+ Map<Entity, List<uint>>& mapBodyToContactPairs) {
+
+ assert(contactPairs->size() == 0);
+ assert(mapPairIdToContactPairIndex->size() == 0);
// get the narrow-phase batches to test for collision
NarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch();
@@ -374,12 +399,18 @@ void CollisionDetection::processAllPotentialContacts(NarrowPhaseInput& narrowPha
NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatch = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch();
// Process the potential contacts
- processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo);
- processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo);
- processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo);
- processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo);
- processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo);
- processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo);
+ processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
+ potentialContactManifolds, contactPairs, mapBodyToContactPairs);
+ processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
+ potentialContactManifolds, contactPairs, mapBodyToContactPairs);
+ processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
+ potentialContactManifolds, contactPairs, mapBodyToContactPairs);
+ processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
+ potentialContactManifolds, contactPairs, mapBodyToContactPairs);
+ processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
+ potentialContactManifolds, contactPairs, mapBodyToContactPairs);
+ processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
+ potentialContactManifolds, contactPairs, mapBodyToContactPairs);
}
// Compute the narrow-phase collision detection
@@ -389,23 +420,376 @@ void CollisionDetection::computeNarrowPhase() {
MemoryAllocator& allocator = mMemoryManager.getSingleFrameAllocator();
+ // Swap the previous and current contacts lists
+ swapPreviousAndCurrentContacts();
+
// Test the narrow-phase collision detection on the batches to be tested
- testNarrowPhaseCollision(mNarrowPhaseInput, false, true, allocator);
+ testNarrowPhaseCollision(mNarrowPhaseInput, true, true, allocator);
// Process all the potential contacts after narrow-phase collision
- processAllPotentialContacts(mNarrowPhaseInput, true);
+ processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints, mCurrentMapPairIdToContactPairIndex,
+ mPotentialContactManifolds, mCurrentContactPairs, mMapBodyToContactPairs);
// Reduce the number of contact points in the manifolds
- reduceContactManifolds(mOverlappingPairs);
+ reducePotentialContactManifolds(mCurrentContactPairs, mPotentialContactManifolds, mPotentialContactPoints);
- // Add all the contact manifolds (between colliding bodies) to the bodies
- addAllContactManifoldsToBodies();
+ assert(mCurrentContactManifolds->size() == 0);
+ assert(mCurrentContactPoints->size() == 0);
+
+ mContactPairsIndicesOrderingForContacts.reserve(mCurrentContactPairs->size());
+
+ mNarrowPhaseInput.clear();
+}
+
+// Compute the narrow-phase collision detection for the testOverlap() methods.
+/// This method returns true if contacts are found.
+bool CollisionDetection::computeNarrowPhaseOverlapSnapshot(NarrowPhaseInput& narrowPhaseInput, OverlapCallback* callback) {
+
+ RP3D_PROFILE("CollisionDetection::computeNarrowPhaseOverlapSnapshot()", mProfiler);
+
+ MemoryAllocator& allocator = mMemoryManager.getPoolAllocator();
+
+ // Test the narrow-phase collision detection on the batches to be tested
+ bool collisionFound = testNarrowPhaseCollision(narrowPhaseInput, false, false, allocator);
+ if (collisionFound && callback != nullptr) {
+
+ // Compute the overlapping bodies
+ List<Pair<Entity, Entity>> overlapBodies(allocator);
+ computeSnapshotContactPairs(narrowPhaseInput, overlapBodies);
+
+ // Report overlapping bodies
+ OverlapCallback::CallbackData callbackData(overlapBodies, *mWorld);
+ (*callback).onOverlap(callbackData);
+ }
+
+ return collisionFound;
+}
+
+// Process the potential overlapping bodies for the testOverlap() methods
+void CollisionDetection::computeSnapshotContactPairs(NarrowPhaseInput& narrowPhaseInput, List<Pair<Entity, Entity>>& overlapPairs) const {
+
+ // get the narrow-phase batches to test for collision
+ NarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch();
+ NarrowPhaseInfoBatch& sphereVsCapsuleBatch = narrowPhaseInput.getSphereVsCapsuleBatch();
+ NarrowPhaseInfoBatch& capsuleVsCapsuleBatch = narrowPhaseInput.getCapsuleVsCapsuleBatch();
+ NarrowPhaseInfoBatch& sphereVsConvexPolyhedronBatch = narrowPhaseInput.getSphereVsConvexPolyhedronBatch();
+ NarrowPhaseInfoBatch& capsuleVsConvexPolyhedronBatch = narrowPhaseInput.getCapsuleVsConvexPolyhedronBatch();
+ NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatch = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch();
+
+ // Process the potential contacts
+ computeSnapshotContactPairs(sphereVsSphereBatch, overlapPairs);
+ computeSnapshotContactPairs(sphereVsCapsuleBatch, overlapPairs);
+ computeSnapshotContactPairs(capsuleVsCapsuleBatch, overlapPairs);
+ computeSnapshotContactPairs(sphereVsConvexPolyhedronBatch, overlapPairs);
+ computeSnapshotContactPairs(capsuleVsConvexPolyhedronBatch, overlapPairs);
+ computeSnapshotContactPairs(convexPolyhedronVsConvexPolyhedronBatch, overlapPairs);
+}
+
+// Convert the potential overlapping bodies for the testOverlap() methods
+void CollisionDetection::computeSnapshotContactPairs(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, List<Pair<Entity, Entity>>& overlapPairs) const {
+
+ RP3D_PROFILE("CollisionDetection::computeSnapshotContactPairs()", mProfiler);
+
+ // For each narrow phase info object
+ for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) {
+
+ // If there is a contact
+ if (narrowPhaseInfoBatch.isColliding[i]) {
+
+ // Add the pair of bodies in contact into the list
+ overlapPairs.add(Pair<Entity, Entity>(narrowPhaseInfoBatch.overlappingPairs[i]->getShape1()->getBody()->getEntity(),
+ narrowPhaseInfoBatch.overlappingPairs[i]->getShape2()->getBody()->getEntity()));
+ }
+
+ narrowPhaseInfoBatch.resetContactPoints(i);
+ }
+}
+
+// Compute the narrow-phase collision detection for the testCollision() methods.
+// This method returns true if contacts are found.
+bool CollisionDetection::computeNarrowPhaseCollisionSnapshot(NarrowPhaseInput& narrowPhaseInput, CollisionCallback& callback) {
+
+ RP3D_PROFILE("CollisionDetection::computeNarrowPhaseCollisionSnapshot()", mProfiler);
+
+ MemoryAllocator& allocator = mMemoryManager.getPoolAllocator();
+
+ // Test the narrow-phase collision detection on the batches to be tested
+ bool collisionFound = testNarrowPhaseCollision(narrowPhaseInput, true, false, allocator);
+
+ // If collision has been found, create contacts
+ if (collisionFound) {
+
+ List<ContactPointInfo> potentialContactPoints(allocator);
+ List<ContactManifoldInfo> potentialContactManifolds(allocator);
+ Map<OverlappingPair::OverlappingPairId, uint> mapPairIdToContactPairIndex(allocator);
+ List<ContactPair> contactPairs(allocator);
+ List<ContactManifold> contactManifolds(allocator);
+ List<ContactPoint> contactPoints(allocator);
+ Map<Entity, List<uint>> mapBodyToContactPairs(allocator);
+
+ // Process all the potential contacts after narrow-phase collision
+ processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, &mapPairIdToContactPairIndex, potentialContactManifolds,
+ &contactPairs, mapBodyToContactPairs);
+
+ // Reduce the number of contact points in the manifolds
+ reducePotentialContactManifolds(&contactPairs, potentialContactManifolds, potentialContactPoints);
+
+ // Create the actual contact manifolds and contact points
+ createSnapshotContacts(contactPairs, contactManifolds, contactPoints, potentialContactManifolds,
+ potentialContactPoints);
+
+ // Report the contacts to the user
+ reportContacts(callback, &contactPairs, &contactManifolds, &contactPoints);
+ }
+
+ return collisionFound;
+}
+
+// Swap the previous and current contacts lists
+void CollisionDetection::swapPreviousAndCurrentContacts() {
+
+ if (mPreviousContactPairs == &mContactPairs1) {
+
+ mPreviousContactPairs = &mContactPairs2;
+ mPreviousContactManifolds = &mContactManifolds2;
+ mPreviousContactPoints = &mContactPoints2;
+ mPreviousMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex2;
+
+ mCurrentContactPairs = &mContactPairs1;
+ mCurrentContactManifolds = &mContactManifolds1;
+ mCurrentContactPoints = &mContactPoints1;
+ mCurrentMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex1;
+ }
+ else {
+
+ mPreviousContactPairs = &mContactPairs1;
+ mPreviousContactManifolds = &mContactManifolds1;
+ mPreviousContactPoints = &mContactPoints1;
+ mPreviousMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex1;
+
+ mCurrentContactPairs = &mContactPairs2;
+ mCurrentContactManifolds = &mContactManifolds2;
+ mCurrentContactPoints = &mContactPoints2;
+ mCurrentMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex2;
+ }
+}
+
+// Create the actual contact manifolds and contacts points
+void CollisionDetection::createContacts() {
+
+ RP3D_PROFILE("CollisionDetection::createContacts()", mProfiler);
+
+ assert(mCurrentContactPairs->size() == mContactPairsIndicesOrderingForContacts.size());
+
+ mCurrentContactManifolds->reserve(mCurrentContactPairs->size());
+ mCurrentContactPoints->reserve(mCurrentContactManifolds->size());
+
+ // For each contact pair
+ for (uint p=0; p < mContactPairsIndicesOrderingForContacts.size(); p++) {
+
+ ContactPair& contactPair = (*mCurrentContactPairs)[mContactPairsIndicesOrderingForContacts[p]];
+ assert(contactPair.potentialContactManifoldsIndices.size() > 0);
+
+ contactPair.contactManifoldsIndex = mCurrentContactManifolds->size();
+ contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
+ contactPair.contactPointsIndex = mCurrentContactPoints->size();
+
+ // For each potential contact manifold of the pair
+ for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) {
+
+ ContactManifoldInfo& potentialManifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]];
+
+ // Start index and number of contact points for this manifold
+ const uint contactPointsIndex = mCurrentContactPoints->size();
+ const int8 nbContactPoints = static_cast<int8>(potentialManifold.potentialContactPointsIndices.size());
+ contactPair.nbToTalContactPoints += nbContactPoints;
+
+ // We create a new contact manifold
+ ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.proxyShape1Entity,
+ contactPair.proxyShape2Entity, contactPointsIndex, nbContactPoints);
+
+ // Add the contact manifold
+ mCurrentContactManifolds->add(contactManifold);
+
+ assert(potentialManifold.potentialContactPointsIndices.size() > 0);
+
+ // For each contact point of the manifold
+ for (uint c=0; c < potentialManifold.potentialContactPointsIndices.size(); c++) {
+
+ ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
+
+ // Create a new contact point
+ ContactPoint contactPoint(potentialContactPoint, mWorld->mConfig);
+
+ // Add the contact point
+ mCurrentContactPoints->add(contactPoint);
+ }
+ }
+ }
+
+ // Initialize the current contacts with the contacts from the previous frame (for warmstarting)
+ initContactsWithPreviousOnes();
// Report contacts to the user
- reportAllContacts();
+ if (mWorld->mEventListener != nullptr) {
+ reportContacts(*(mWorld->mEventListener), mCurrentContactPairs, mCurrentContactManifolds, mCurrentContactPoints);
+ }
- // Clear the list of narrow-phase infos
- mNarrowPhaseInput.clear();
+ // Reset the potential contacts
+ mPotentialContactPoints.clear(true);
+ mPotentialContactManifolds.clear(true);
+ mContactPairsIndicesOrderingForContacts.clear(true);
+}
+
+// Create the actual contact manifolds and contacts points for testCollision() methods
+void CollisionDetection::createSnapshotContacts(List<ContactPair>& contactPairs,
+ List<ContactManifold>& contactManifolds,
+ List<ContactPoint>& contactPoints,
+ List<ContactManifoldInfo>& potentialContactManifolds,
+ List<ContactPointInfo>& potentialContactPoints) {
+
+ RP3D_PROFILE("CollisionDetection::createSnapshotContacts()", mProfiler);
+
+ contactManifolds.reserve(contactPairs.size());
+ contactPoints.reserve(contactManifolds.size());
+
+ // For each contact pair
+ for (uint p=0; p < contactPairs.size(); p++) {
+
+ ContactPair& contactPair = contactPairs[p];
+ assert(contactPair.potentialContactManifoldsIndices.size() > 0);
+
+ contactPair.contactManifoldsIndex = contactManifolds.size();
+ contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
+ contactPair.contactPointsIndex = contactPoints.size();
+
+ // For each potential contact manifold of the pair
+ for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) {
+
+ ContactManifoldInfo& potentialManifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]];
+
+ // Start index and number of contact points for this manifold
+ const uint contactPointsIndex = contactPoints.size();
+ const int8 nbContactPoints = static_cast<int8>(potentialManifold.potentialContactPointsIndices.size());
+ contactPair.nbToTalContactPoints += nbContactPoints;
+
+ // We create a new contact manifold
+ ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.proxyShape1Entity,
+ contactPair.proxyShape2Entity, contactPointsIndex, nbContactPoints);
+
+ // Add the contact manifold
+ contactManifolds.add(contactManifold);
+
+ assert(potentialManifold.potentialContactPointsIndices.size() > 0);
+
+ // For each contact point of the manifold
+ for (uint c=0; c < potentialManifold.potentialContactPointsIndices.size(); c++) {
+
+ ContactPointInfo& potentialContactPoint = potentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
+
+ // Create a new contact point
+ ContactPoint contactPoint(potentialContactPoint, mWorld->mConfig);
+
+ // Add the contact point
+ contactPoints.add(contactPoint);
+ }
+ }
+ }
+}
+
+// Initialize the current contacts with the contacts from the previous frame (for warmstarting)
+void CollisionDetection::initContactsWithPreviousOnes() {
+
+ // For each contact pair of the current frame
+ for (uint i=0; i < mCurrentContactPairs->size(); i++) {
+
+ ContactPair& currentContactPair = (*mCurrentContactPairs)[i];
+
+ // Find the corresponding contact pair in the previous frame (if any)
+ auto itPrevContactPair = mPreviousMapPairIdToContactPairIndex->find(currentContactPair.pairId);
+
+ // If we have found a corresponding contact pair in the previous frame
+ if (itPrevContactPair != mPreviousMapPairIdToContactPairIndex->end()) {
+
+ const uint previousContactPairIndex = itPrevContactPair->second;
+ ContactPair& previousContactPair = (*mPreviousContactPairs)[previousContactPairIndex];
+
+ // --------------------- Contact Manifolds --------------------- //
+
+ const uint contactManifoldsIndex = currentContactPair.contactManifoldsIndex;
+ const uint nbContactManifolds = currentContactPair.nbContactManifolds;
+
+ // For each current contact manifold of the contact pair
+ for (uint m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) {
+
+ assert(m < mCurrentContactManifolds->size());
+ ContactManifold& currentContactManifold = (*mCurrentContactManifolds)[m];
+ assert(currentContactManifold.mNbContactPoints > 0);
+ ContactPoint& currentContactPoint = (*mCurrentContactPoints)[currentContactManifold.mContactPointsIndex];
+ const Vector3& currentContactPointNormal = currentContactPoint.getNormal();
+
+ // Find a similar contact manifold among the contact manifolds from the previous frame (for warmstarting)
+ const uint previousContactManifoldIndex = previousContactPair.contactManifoldsIndex;
+ const uint previousNbContactManifolds = previousContactPair.nbContactManifolds;
+ for (uint p=previousContactManifoldIndex; p < previousContactManifoldIndex + previousNbContactManifolds; p++) {
+
+ ContactManifold& previousContactManifold = (*mPreviousContactManifolds)[p];
+ assert(previousContactManifold.mNbContactPoints > 0);
+ ContactPoint& previousContactPoint = (*mPreviousContactPoints)[previousContactManifold.mContactPointsIndex];
+
+ // If the previous contact manifold has a similar contact normal with the current manifold
+ if (previousContactPoint.getNormal().dot(currentContactPointNormal) >= mWorld->mConfig.cosAngleSimilarContactManifold) {
+
+ // Transfer data from the previous contact manifold to the current one
+ currentContactManifold.mFrictionVector1 = previousContactManifold.mFrictionVector1;
+ currentContactManifold.mFrictionVector2 = previousContactManifold.mFrictionVector2;
+ currentContactManifold.mFrictionImpulse1 = previousContactManifold.mFrictionImpulse1;
+ currentContactManifold.mFrictionImpulse2 = previousContactManifold.mFrictionImpulse2;
+ currentContactManifold.mFrictionTwistImpulse = previousContactManifold.mFrictionTwistImpulse;
+ currentContactManifold.mRollingResistanceImpulse = previousContactManifold.mRollingResistanceImpulse;
+
+ break;
+ }
+ }
+ }
+
+ // --------------------- Contact Points --------------------- //
+
+ const uint contactPointsIndex = currentContactPair.contactPointsIndex;
+ const uint nbTotalContactPoints = currentContactPair.nbToTalContactPoints;
+
+ // For each current contact point of the contact pair
+ for (uint c=contactPointsIndex; c < contactPointsIndex + nbTotalContactPoints; c++) {
+
+ assert(c < mCurrentContactPoints->size());
+ ContactPoint& currentContactPoint = (*mCurrentContactPoints)[c];
+
+ // Find a similar contact point among the contact points from the previous frame (for warmstarting)
+ const uint previousContactPointsIndex = previousContactPair.contactPointsIndex;
+ const uint previousNbContactPoints = previousContactPair.nbToTalContactPoints;
+ for (uint p=previousContactPointsIndex; p < previousContactPointsIndex + previousNbContactPoints; p++) {
+
+ ContactPoint& previousContactPoint = (*mPreviousContactPoints)[p];
+
+ // If the previous contact point is very close to th current one
+ const decimal distSquare = (currentContactPoint.getLocalPointOnShape1() - previousContactPoint.getLocalPointOnShape1()).lengthSquare();
+ if (distSquare <= mWorld->mConfig.persistentContactDistanceThreshold * mWorld->mConfig.persistentContactDistanceThreshold) {
+
+ // Transfer data from the previous contact point to the current one
+ currentContactPoint.setPenetrationImpulse(previousContactPoint.getPenetrationImpulse());
+ currentContactPoint.setIsRestingContact(previousContactPoint.getIsRestingContact());
+
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ mPreviousContactPoints->clear();
+ mPreviousContactManifolds->clear();
+ mPreviousContactPairs->clear();
+ mPreviousMapPairIdToContactPairIndex->clear();
}
// Remove a body from the collision detection
@@ -440,19 +824,6 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
mBroadPhaseSystem.removeProxyCollisionShape(proxyShape);
}
-void CollisionDetection::addAllContactManifoldsToBodies() {
-
- RP3D_PROFILE("CollisionDetection::addAllContactManifoldsToBodies()", mProfiler);
-
- // For each overlapping pairs in contact during the narrow-phase
- for (auto 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
- addContactManifoldToBody(it->second);
- }
-}
-
// Ray casting method
void CollisionDetection::raycast(RaycastCallback* raycastCallback,
const Ray& ray,
@@ -467,46 +838,15 @@ void CollisionDetection::raycast(RaycastCallback* raycastCallback,
mBroadPhaseSystem.raycast(ray, rayCastTest, raycastWithCategoryMaskBits);
}
-// Add a contact manifold to the linked list of contact manifolds of the two bodies involved
-// in the corresponding contact
-void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
+// Convert the potential contact into actual contacts
+void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo,
+ List<ContactPointInfo>& potentialContactPoints,
+ Map<OverlappingPair::OverlappingPairId, uint>* mapPairIdToContactPairIndex,
+ List<ContactManifoldInfo>& potentialContactManifolds,
+ List<ContactPair>* contactPairs,
+ Map<Entity, List<uint>>& mapBodyToContactPairs) {
- assert(pair != nullptr);
-
- CollisionBody* body1 = pair->getShape1()->getBody();
- CollisionBody* body2 = pair->getShape2()->getBody();
- 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* 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 (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
- sizeof(ContactManifoldListElement)))
- ContactManifoldListElement(contactManifold,
- body2->mContactManifoldsList);
- body2->mContactManifoldsList = listElement2;
-
- contactManifold = contactManifold->getNext();
- }
-}
-
-/// Convert the potential contact into actual contacts
-void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo) {
-
- RP3D_PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler);
+ RP3D_PROFILE("CollisionDetection::processPotentialContacts()", mProfiler);
// For each narrow phase info object
for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) {
@@ -518,494 +858,564 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
narrowPhaseInfoBatch.lastFrameCollisionInfos[i]->isValid = true;
}
- if (narrowPhaseInfoBatch.isColliding[i]) {
+ // Add the potential contacts
+ for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) {
- assert(narrowPhaseInfoBatch.contactPoints[i].size() > 0);
+ assert(narrowPhaseInfoBatch.isColliding[i]);
- // Transfer the contact points from the narrow phase info to the overlapping pair
- narrowPhaseInfoBatch.overlappingPairs[i]->addPotentialContactPoints(narrowPhaseInfoBatch, i);
+ const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]);
- // Remove the contacts points from the narrow phase info object.
- narrowPhaseInfoBatch.resetContactPoints(i);
+ // Add the contact point to the list of potential contact points
+ const uint contactPointIndex = static_cast<uint>(potentialContactPoints.size());
+
+ // TODO : We should probably use single frame allocator here for potentialContactPoints
+ // If so, do not forget to call potentialContactPoints.clear(true) at the end of frame
+ potentialContactPoints.add(contactPoint);
+
+ bool similarManifoldFound = false;
+
+ // If there is already a contact pair for this overlapping pair
+ OverlappingPair::OverlappingPairId pairId = narrowPhaseInfoBatch.overlappingPairs[i]->getId();
+ auto it = mapPairIdToContactPairIndex->find(pairId);
+ ContactPair* pairContact = nullptr;
+ if (it != mapPairIdToContactPairIndex->end()) {
+
+ assert(it->first == pairId);
+
+ const uint pairContactIndex = it->second;
+ pairContact = &((*contactPairs)[pairContactIndex]);
+
+ assert(pairContact->potentialContactManifoldsIndices.size() > 0);
+
+ // For each contact manifold of the overlapping pair
+ for (uint m=0; m < pairContact->potentialContactManifoldsIndices.size(); m++) {
+
+ uint contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m];
+
+ // Get the first contact point of the current manifold
+ assert(potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.size() > 0);
+ const uint manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0];
+ const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex];
+
+ // If we have found a corresponding manifold for the new contact point
+ // (a manifold with a similar contact normal direction)
+ if (manifoldContactPoint.normal.dot(contactPoint.normal) >= mWorld->mConfig.cosAngleSimilarContactManifold) {
+
+ // Add the contact point to the manifold
+ potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.add(contactPointIndex);
+
+ similarManifoldFound = true;
+
+ break;
+ }
+ }
+ }
+
+ // If we have not found a manifold with a similar contact normal for the contact point
+ if (!similarManifoldFound) {
+
+ // Create a new contact manifold for the overlapping pair
+ // TODO : We should probably use single frame allocator here
+ // If so, do not forget to call potentialContactPoints.clear(true) at the end of frame
+ ContactManifoldInfo contactManifoldInfo(pairId, mMemoryManager.getPoolAllocator());
+
+ // Add the contact point to the manifold
+ contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndex);
+
+ // If the overlapping pair contact does not exists yet
+ if (pairContact == nullptr) {
+
+ Entity body1Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape1()->getBody()->getEntity();
+ Entity body2Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape2()->getBody()->getEntity();
+
+ assert(!mWorld->mBodyComponents.getIsEntityDisabled(body1Entity) || !mWorld->mBodyComponents.getIsEntityDisabled(body2Entity));
+
+ // TODO : We should probably use a single frame allocator here
+ const uint newContactPairIndex = contactPairs->size();
+ ContactPair overlappingPairContact(pairId, body1Entity, body2Entity,
+ narrowPhaseInfoBatch.overlappingPairs[i]->getShape1()->getEntity(),
+ narrowPhaseInfoBatch.overlappingPairs[i]->getShape2()->getEntity(),
+ newContactPairIndex, mMemoryManager.getPoolAllocator());
+ contactPairs->add(overlappingPairContact);
+ pairContact = &((*contactPairs)[newContactPairIndex]);
+ mapPairIdToContactPairIndex->add(Pair<OverlappingPair::OverlappingPairId, uint>(pairId, newContactPairIndex));
+
+ auto itbodyContactPairs = mapBodyToContactPairs.find(body1Entity);
+ if (itbodyContactPairs != mapBodyToContactPairs.end()) {
+ itbodyContactPairs->second.add(newContactPairIndex);
+ }
+ else {
+ List<uint> contactPairs(mMemoryManager.getPoolAllocator(), 1);
+ contactPairs.add(newContactPairIndex);
+ mapBodyToContactPairs.add(Pair<Entity, List<uint>>(body1Entity, contactPairs));
+ }
+ itbodyContactPairs = mapBodyToContactPairs.find(body2Entity);
+ if (itbodyContactPairs != mapBodyToContactPairs.end()) {
+ itbodyContactPairs->second.add(newContactPairIndex);
+ }
+ else {
+ List<uint> contactPairs(mMemoryManager.getPoolAllocator(), 1);
+ contactPairs.add(newContactPairIndex);
+ mapBodyToContactPairs.add(Pair<Entity, List<uint>>(body2Entity, contactPairs));
+ }
+ }
+
+ assert(pairContact != nullptr);
+
+ // Add the potential contact manifold
+ uint contactManifoldIndex = static_cast<uint>(potentialContactManifolds.size());
+ potentialContactManifolds.add(contactManifoldInfo);
+
+ // Add the contact manifold to the overlapping pair contact
+ assert(pairContact->pairId == contactManifoldInfo.pairId);
+ pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex);
+ }
+
+ assert(pairContact->potentialContactManifoldsIndices.size() > 0);
}
+
+ narrowPhaseInfoBatch.resetContactPoints(i);
}
}
// Clear the obsolete manifolds and contact points and reduce the number of contacts points of the remaining manifolds
-void CollisionDetection::reduceContactManifolds(const OverlappingPairMap& overlappingPairs) {
+void CollisionDetection::reducePotentialContactManifolds(List<ContactPair>* contactPairs,
+ List<ContactManifoldInfo>& potentialContactManifolds,
+ const List<ContactPointInfo>& potentialContactPoints) const {
- // For each overlapping pairs in contact during the narrow-phase
- for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
+ RP3D_PROFILE("CollisionDetection::reducePotentialContactManifolds()", mProfiler);
- OverlappingPair* pair = it->second;
+ // Reduce the number of potential contact manifolds in a contact pair
+ for (uint i=0; i < contactPairs->size(); i++) {
- // Clear the obsolete contact manifolds and contact points
- pair->clearObsoleteManifoldsAndContactPoints();
+ ContactPair& contactPair = (*contactPairs)[i];
- // Reduce the contact manifolds and contact points if there are too many of them
- pair->reduceContactManifolds();
+ assert(contactPair.potentialContactManifoldsIndices.size() > 0);
+
+ // While there are too many manifolds in the contact pair
+ while(contactPair.potentialContactManifoldsIndices.size() > mWorld->mConfig.nbMaxContactManifolds) {
+
+ // Look for a manifold with the smallest contact penetration depth.
+ decimal minDepth = DECIMAL_LARGEST;
+ int minDepthManifoldIndex = -1;
+ for (uint j=0; j < contactPair.potentialContactManifoldsIndices.size(); j++) {
+
+ ContactManifoldInfo& manifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[j]];
+
+ // Get the largest contact point penetration depth of the manifold
+ const decimal depth = computePotentialManifoldLargestContactDepth(manifold, potentialContactPoints);
+
+ if (depth < minDepth) {
+ minDepth = depth;
+ minDepthManifoldIndex = static_cast<int>(j);
+ }
+ }
+
+ // Remove the non optimal manifold
+ assert(minDepthManifoldIndex >= 0);
+ contactPair.potentialContactManifoldsIndices.removeAt(minDepthManifoldIndex);
+ }
}
-}
+ // Reduce the number of potential contact points in the manifolds
+ for (uint i=0; i < contactPairs->size(); i++) {
-// Report contacts for all the colliding overlapping pairs
-void CollisionDetection::reportAllContacts() {
+ const ContactPair& pairContact = (*contactPairs)[i];
- RP3D_PROFILE("CollisionDetection::reportAllContacts()", mProfiler);
+ // For each potential contact manifold
+ for (uint j=0; j < pairContact.potentialContactManifoldsIndices.size(); j++) {
- // For each overlapping pairs in contact during the narrow-phase
- for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
+ ContactManifoldInfo& manifold = potentialContactManifolds[pairContact.potentialContactManifoldsIndices[j]];
- OverlappingPair* pair = it->second;
+ // If there are two many contact points in the manifold
+ if (manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD) {
- // If there is a user callback
- if (mWorld->mEventListener != nullptr && pair->hasContacts()) {
+ Transform shape1LocalToWorldTransoform = mOverlappingPairs[manifold.pairId]->getShape1()->getLocalToWorldTransform();
- CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager);
+ // Reduce the number of contact points in the manifold
+ reduceContactPoints(manifold, shape1LocalToWorldTransoform, potentialContactPoints);
+ }
- // Trigger a callback event to report the new contact to the user
- mWorld->mEventListener->newContact(collisionInfo);
+ assert(manifold.potentialContactPointsIndices.size() <= MAX_CONTACT_POINTS_IN_MANIFOLD);
}
}
}
-// Compute the middle-phase collision detection between two proxy shapes
-void CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair, NarrowPhaseInput& outNarrowPhaseInput) {
+// Return the largest depth of all the contact points of a potential manifold
+decimal CollisionDetection::computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold,
+ const List<ContactPointInfo>& potentialContactPoints) const {
- ProxyShape* shape1 = pair->getShape1();
- ProxyShape* shape2 = pair->getShape2();
+ decimal largestDepth = 0.0f;
- // -------------------------------------------------------
+ assert(manifold.potentialContactPointsIndices.size() > 0);
- const bool isShape1Convex = shape1->getCollisionShape()->isConvex();
- const bool isShape2Convex = shape2->getCollisionShape()->isConvex();
+ for (uint i=0; i < manifold.potentialContactPointsIndices.size(); i++) {
+ decimal depth = potentialContactPoints[manifold.potentialContactPointsIndices[i]].penetrationDepth;
- pair->makeLastFrameCollisionInfosObsolete();
-
- // If both shapes are convex
- if ((isShape1Convex && isShape2Convex)) {
-
- // Select the narrow phase algorithm to use according to the two collision shapes
- NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(shape1->getCollisionShape()->getType(),
- shape2->getCollisionShape()->getType());
- // No middle-phase is necessary, simply create a narrow phase info
- // for the narrow-phase collision detection
- outNarrowPhaseInput.addNarrowPhaseTest(pair, shape1->getCollisionShape(), shape2->getCollisionShape(),
- shape1->getLocalToWorldTransform(), shape2->getLocalToWorldTransform(),
- algorithmType, mMemoryManager.getPoolAllocator());
-
- }
- // Concave vs Convex algorithm
- 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
- computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getPoolAllocator(), outNarrowPhaseInput);
+ if (depth > largestDepth) {
+ largestDepth = depth;
+ }
}
- pair->clearObsoleteLastFrameCollisionInfos();
+ return largestDepth;
}
-// 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);
+// Reduce the number of contact points of a potential contact manifold
+// This is based on the technique described by Dirk Gregorius in his
+// "Contacts Creation" GDC presentation. This method will reduce the number of
+// contact points to a maximum of 4 points (but it can be less).
+void CollisionDetection::reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform,
+ const List<ContactPointInfo>& potentialContactPoints) const {
- Set<bodyindex> reportedBodies(mMemoryManager.getPoolAllocator());
+ assert(manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD);
- // Ask the broad-phase to get all the overlapping shapes
- List<int> overlappingNodes(mMemoryManager.getPoolAllocator());
- mBroadPhaseSystem.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes);
+ // The following algorithm only works to reduce to a maximum of 4 contact points
+ assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
- // For each overlaping proxy shape
- for (uint i=0; i < overlappingNodes.size(); i++) {
+ // List of the candidate contact points indices in the manifold. Every time that we have found a
+ // point we want to keep, we will remove it from this list
+ List<uint> candidatePointsIndices(manifold.potentialContactPointsIndices);
- // Get the overlapping proxy shape
- const int broadPhaseId = overlappingNodes[i];
- ProxyShape* proxyShape = mBroadPhaseSystem.getProxyShapeForBroadPhaseId(broadPhaseId);
+ int8 nbReducedPoints = 0;
- CollisionBody* overlapBody = proxyShape->getBody();
+ uint pointsToKeepIndices[MAX_CONTACT_POINTS_IN_MANIFOLD];
+ for (int8 i=0; i<MAX_CONTACT_POINTS_IN_MANIFOLD; i++) {
+ pointsToKeepIndices[i] = 0;
+ }
- // If the proxy shape is from a body that we have not already reported collision
- if (reportedBodies.find(overlapBody->getId()) == reportedBodies.end()) {
+ // 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)
- // Check if the collision filtering allows collision between the two shapes
- if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) {
+ const Transform worldToShape1Transform = shape1ToWorldTransform.getInverse();
- // Add the body into the set of reported bodies
- reportedBodies.add(overlapBody->getId());
+ // 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() * potentialContactPoints[candidatePointsIndices[0]].normal;
- // Notify the overlap to the user
- overlapCallback->notifyOverlap(overlapBody);
+ // Compute a search direction
+ const Vector3 searchDirection(1, 1, 1);
+ decimal maxDotProduct = DECIMAL_SMALLEST;
+ uint elementIndexToKeep = 0;
+ for (uint i=0; i < candidatePointsIndices.size(); i++) {
+
+ const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
+ decimal dotProduct = searchDirection.dot(element.localPoint1);
+ if (dotProduct > maxDotProduct) {
+ maxDotProduct = dotProduct;
+ elementIndexToKeep = i;
+ nbReducedPoints = 1;
+ }
+ }
+ pointsToKeepIndices[0] = candidatePointsIndices[elementIndexToKeep];
+ candidatePointsIndices.removeAt(elementIndexToKeep);
+ assert(nbReducedPoints == 1);
+
+ // 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);
+ elementIndexToKeep = 0;
+ for (uint i=0; i < candidatePointsIndices.size(); i++) {
+
+ const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
+ const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]];
+
+ assert(candidatePointsIndices[i] != pointsToKeepIndices[0]);
+
+ const decimal distance = (pointToKeep0.localPoint1 - element.localPoint1).lengthSquare();
+ if (distance >= maxDistance) {
+ maxDistance = distance;
+ elementIndexToKeep = i;
+ nbReducedPoints = 2;
+ }
+
+ }
+ pointsToKeepIndices[1] = candidatePointsIndices[elementIndexToKeep];
+ candidatePointsIndices.removeAt(elementIndexToKeep);
+ assert(nbReducedPoints == 2);
+
+ // Compute the third contact point we need to keep.
+ // The third 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)
+ uint thirdPointMaxAreaIndex = 0;
+ uint thirdPointMinAreaIndex = 0;
+ decimal minArea = decimal(0.0);
+ decimal maxArea = decimal(0.0);
+ bool isPreviousAreaPositive = true;
+ for (uint i=0; i < candidatePointsIndices.size(); i++) {
+
+ const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
+ const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]];
+ const ContactPointInfo& pointToKeep1 = potentialContactPoints[pointsToKeepIndices[1]];
+
+ assert(candidatePointsIndices[i] != pointsToKeepIndices[0]);
+ assert(candidatePointsIndices[i] != pointsToKeepIndices[1]);
+
+ const Vector3 newToFirst = pointToKeep0.localPoint1 - element.localPoint1;
+ const Vector3 newToSecond = pointToKeep1.localPoint1 - element.localPoint1;
+
+ // Compute the triangle area
+ decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
+
+ if (area >= maxArea) {
+ maxArea = area;
+ thirdPointMaxAreaIndex = i;
+ }
+ if (area <= minArea) {
+ minArea = area;
+ thirdPointMinAreaIndex = i;
+ }
+ }
+ assert(minArea <= decimal(0.0));
+ assert(maxArea >= decimal(0.0));
+ if (maxArea > (-minArea)) {
+ isPreviousAreaPositive = true;
+ pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMaxAreaIndex];
+ candidatePointsIndices.removeAt(thirdPointMaxAreaIndex);
+ }
+ else {
+ isPreviousAreaPositive = false;
+ pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMinAreaIndex];
+ candidatePointsIndices.removeAt(thirdPointMinAreaIndex);
+ }
+ nbReducedPoints = 3;
+
+ // 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)
+ elementIndexToKeep = 0;
+ nbReducedPoints = 4;
+ decimal area;
+
+ // For each remaining candidate points
+ for (uint i=0; i < candidatePointsIndices.size(); i++) {
+
+ const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
+
+ assert(candidatePointsIndices[i] != pointsToKeepIndices[0]);
+ assert(candidatePointsIndices[i] != pointsToKeepIndices[1]);
+ assert(candidatePointsIndices[i] != pointsToKeepIndices[2]);
+
+ // For each edge of the triangle made by the first three points
+ for (uint j=0; j<3; j++) {
+
+ uint edgeVertex1Index = j;
+ uint edgeVertex2Index = j < 2 ? j + 1 : 0;
+
+ const ContactPointInfo& pointToKeepEdgeV1 = potentialContactPoints[pointsToKeepIndices[edgeVertex1Index]];
+ const ContactPointInfo& pointToKeepEdgeV2 = potentialContactPoints[pointsToKeepIndices[edgeVertex2Index]];
+
+ const Vector3 newToFirst = pointToKeepEdgeV1.localPoint1 - element.localPoint1;
+ const Vector3 newToSecond = pointToKeepEdgeV2.localPoint1 - element.localPoint1;
+
+ // Compute the triangle area
+ 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;
+ elementIndexToKeep = i;
+ }
+ else if (!isPreviousAreaPositive && area >= largestArea) {
+ largestArea = area;
+ elementIndexToKeep = i;
}
}
}
+ pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep];
+ candidatePointsIndices.removeAt(elementIndexToKeep);
+
+ // Only keep the four selected contact points in the manifold
+ manifold.potentialContactPointsIndices.clear();
+ manifold.potentialContactPointsIndices.add(pointsToKeepIndices[0]);
+ manifold.potentialContactPointsIndices.add(pointsToKeepIndices[1]);
+ manifold.potentialContactPointsIndices.add(pointsToKeepIndices[2]);
+ manifold.potentialContactPointsIndices.add(pointsToKeepIndices[3]);
}
-// Return true if two bodies overlap
+// Report contacts
+void CollisionDetection::reportContacts() {
+
+ if (mWorld->mEventListener != nullptr) {
+ reportContacts(*(mWorld->mEventListener), mCurrentContactPairs, mCurrentContactManifolds,
+ mCurrentContactPoints);
+ }
+}
+
+// Report all contacts
+void CollisionDetection::reportContacts(CollisionCallback& callback, List<ContactPair>* contactPairs,
+ List<ContactManifold>* manifolds, List<ContactPoint>* contactPoints) {
+
+ RP3D_PROFILE("CollisionDetection::reportContacts()", mProfiler);
+
+ // If there are contacts
+ if (contactPairs->size() > 0) {
+
+ CollisionCallback::CallbackData callbackData(contactPairs, manifolds, contactPoints, *mWorld);
+
+ // Call the callback method to report the contacts
+ callback.onContact(callbackData);
+ }
+}
+
+// Return true if two bodies overlap (collide)
bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) {
NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
- // For each proxy shape proxy shape of the first body
- const List<Entity>& body1ProxyShapesEntities = mWorld->mBodyComponents.getProxyShapes(body1->getEntity());
- const List<Entity>& body2ProxyShapesEntities = mWorld->mBodyComponents.getProxyShapes(body2->getEntity());
- for (uint i=0; i < body1ProxyShapesEntities.size(); i++) {
+ // Compute the broad-phase collision detection
+ computeBroadPhase();
- ProxyShape* body1ProxyShape = mWorld->mProxyShapesComponents.getProxyShape(body1ProxyShapesEntities[i]);
-
- AABB aabb1 = body1ProxyShape->getWorldAABB();
-
- // For each proxy shape of the second body
- for (uint j=0; j < body2ProxyShapesEntities.size(); j++) {
-
- ProxyShape* body2ProxyShape = mWorld->mProxyShapesComponents.getProxyShape(body2ProxyShapesEntities[j]);
-
- AABB aabb2 = body2ProxyShape->getWorldAABB();
-
- // Test if the AABBs of the two proxy shapes overlap
- if (aabb1.testCollision(aabb2)) {
-
- // Create a temporary overlapping pair
- OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(),
- mMemoryManager.getPoolAllocator(), mWorld->mConfig);
-
- // Compute the middle-phase collision detection between the two shapes
- computeMiddlePhaseForProxyShapes(&pair, narrowPhaseInput);
-
- }
- }
- }
-
- // Test narrow-phase collision
- bool isCollisionFound = testNarrowPhaseCollision(narrowPhaseInput, true, false, mMemoryManager.getPoolAllocator());
-
- // No overlap has been found
- return isCollisionFound;
-}
-
-// Report all the bodies that overlap with the body in parameter
-void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overlapCallback,
- unsigned short categoryMaskBits) {
-
- assert(overlapCallback != nullptr);
-
- Set<bodyindex> reportedBodies(mMemoryManager.getPoolAllocator());
- NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
-
- // For each proxy shape proxy shape of the body
- const List<Entity>& proxyShapesEntities = mWorld->mBodyComponents.getProxyShapes(body->getEntity());
- for (uint i=0; i < proxyShapesEntities.size(); i++) {
-
- ProxyShape* bodyProxyShape = mWorld->mProxyShapesComponents.getProxyShape(proxyShapesEntities[i]);
-
- if (bodyProxyShape->getBroadPhaseId() != -1) {
-
- // Get the AABB of the shape
- const AABB& shapeAABB = mBroadPhaseSystem.getFatAABB(bodyProxyShape->getBroadPhaseId());
-
- // Ask the broad-phase to get all the overlapping shapes
- List<int> overlappingNodes(mMemoryManager.getPoolAllocator());
- mBroadPhaseSystem.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
-
- const bodyindex bodyId = body->getId();
-
- // For each overlaping proxy shape
- for (uint i=0; i < overlappingNodes.size(); i++) {
-
- // Get the overlapping proxy shape
- const int broadPhaseId = overlappingNodes[i];
- ProxyShape* proxyShape = mBroadPhaseSystem.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) {
-
- // Create a temporary overlapping pair
- OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(),
- mMemoryManager.getPoolAllocator(), mWorld->mConfig);
-
- // Compute the middle-phase collision detection between the two shapes
- computeMiddlePhaseForProxyShapes(&pair, narrowPhaseInput);
-
- // Test narrow-phase collision
- if (testNarrowPhaseCollision(narrowPhaseInput, true, false, mMemoryManager.getPoolAllocator())) {
-
- CollisionBody* overlapBody = proxyShape->getBody();
-
- // Add the body into the set of reported bodies
- reportedBodies.add(overlapBody->getId());
-
- // Notify the overlap to the user
- overlapCallback->notifyOverlap(overlapBody);
- }
-
- narrowPhaseInput.clear();
- }
- }
- }
- }
- }
-}
-
-// Test and report collisions between two bodies
-void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* collisionCallback) {
-
- assert(collisionCallback != nullptr);
-
- NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
+ // Filter the overlapping pairs to get only the ones with the selected body involved
OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
+ filterOverlappingPairs(body1->getEntity(), body2->getEntity(), overlappingPairs);
- // For each proxy shape proxy shape of the first body
- const List<Entity>& body1ProxyShapesEntities = mWorld->mBodyComponents.getProxyShapes(body1->getEntity());
- const List<Entity>& body2ProxyShapesEntities = mWorld->mBodyComponents.getProxyShapes(body2->getEntity());
- for (uint i=0; i < body1ProxyShapesEntities.size(); i++) {
+ if (overlappingPairs.size() > 0) {
- ProxyShape* body1ProxyShape = mWorld->mProxyShapesComponents.getProxyShape(body1ProxyShapesEntities[i]);
+ // Compute the middle-phase collision detection
+ computeMiddlePhase(overlappingPairs, narrowPhaseInput);
- AABB aabb1 = body1ProxyShape->getWorldAABB();
-
- // For each proxy shape of the second body
- for (uint j=0; j < body2ProxyShapesEntities.size(); j++) {
-
- ProxyShape* body2ProxyShape = mWorld->mProxyShapesComponents.getProxyShape(body2ProxyShapesEntities[i]);
-
- AABB aabb2 = body2ProxyShape->getWorldAABB();
-
- // Test if the AABBs of the two proxy shapes overlap
- if (aabb1.testCollision(aabb2)) {
-
- OverlappingPair* pair;
- const Pair<uint, uint> pairID = OverlappingPair::computeID(body1ProxyShape->getBroadPhaseId(), body2ProxyShape->getBroadPhaseId());
-
- // Try to retrieve a corresponding copy of the overlapping pair (if it exists)
- auto itPair = overlappingPairs.find(pairID);
-
- // If a copy of the overlapping pair does not exist yet
- if (itPair == overlappingPairs.end()) {
-
- // Create a temporary copy of the overlapping pair
- pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair)))
- OverlappingPair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(),
- mMemoryManager.getPoolAllocator(), mWorld->mConfig);
-
- overlappingPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(pairID, pair));
- }
- else { // If a temporary copy of this overlapping pair already exists
-
- // Retrieve the existing copy of the overlapping pair
- pair = itPair->second;
- }
-
- // Compute the middle-phase collision detection between the two shapes
- computeMiddlePhaseForProxyShapes(pair, narrowPhaseInput);
- }
- }
+ // Compute the narrow-phase collision detection
+ return computeNarrowPhaseOverlapSnapshot(narrowPhaseInput, nullptr);
}
- // Test narrow-phase collision
- testNarrowPhaseCollision(narrowPhaseInput, false, true, mMemoryManager.getPoolAllocator());
-
- // Process the potential contacts
- processAllPotentialContacts(narrowPhaseInput, false);
-
- // Reduce the number of contact points in the manifolds
- reduceContactManifolds(overlappingPairs);
-
- // For each overlapping pair
- for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
-
- OverlappingPair* pair = it->second;
-
- if (pair->hasContacts()) {
-
- // Report the contacts to the user
- CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager);
- collisionCallback->notifyContact(collisionInfo);
- }
-
- // Destroy the temporary overlapping pair
- pair->~OverlappingPair();
- mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair));
- }
+ return false;
}
-// 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);
+// Report all the bodies that overlap (collide) in the world
+void CollisionDetection::testOverlap(OverlapCallback& callback) {
NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
- OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
-
- // For each proxy shape proxy shape of the body
- const List<Entity>& proxyShapesEntities = mWorld->mBodyComponents.getProxyShapes(body->getEntity());
- for (uint i=0; i < proxyShapesEntities.size(); i++) {
-
- ProxyShape* bodyProxyShape = mWorld->mProxyShapesComponents.getProxyShape(proxyShapesEntities[i]);
-
- if (bodyProxyShape->getBroadPhaseId() != -1) {
-
- // Get the AABB of the shape
- const AABB& shapeAABB = mBroadPhaseSystem.getFatAABB(bodyProxyShape->getBroadPhaseId());
-
- // Ask the broad-phase to get all the overlapping shapes
- List<int> overlappingNodes(mMemoryManager.getPoolAllocator());
- mBroadPhaseSystem.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
-
- const bodyindex bodyId = body->getId();
-
- // For each overlaping proxy shape
- for (uint i=0; i < overlappingNodes.size(); i++) {
-
- // Get the overlapping proxy shape
- const int broadPhaseId = overlappingNodes[i];
- ProxyShape* proxyShape = mBroadPhaseSystem.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) {
-
- OverlappingPair* pair;
- const Pair<uint, uint> pairID = OverlappingPair::computeID(bodyProxyShape->getBroadPhaseId(), proxyShape->getBroadPhaseId());
-
- // Try to retrieve a corresponding copy of the overlapping pair (if it exists)
- auto itPair = overlappingPairs.find(pairID);
-
- // If a copy of the overlapping pair does not exist yet
- if (itPair == overlappingPairs.end()) {
-
- // Create a temporary overlapping pair
- pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair)))
- OverlappingPair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(),
- mMemoryManager.getPoolAllocator(), mWorld->mConfig);
-
- overlappingPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(pairID, pair));
- }
- else { // If a temporary copy of this overlapping pair already exists
-
- // Retrieve the existing copy of the overlapping pair
- pair = itPair->second;
- }
-
- // Compute the middle-phase collision detection between the two shapes
- computeMiddlePhaseForProxyShapes(pair, narrowPhaseInput);
- }
- }
- }
- }
- }
-
- // Test narrow-phase collision
- testNarrowPhaseCollision(narrowPhaseInput, false, true, mMemoryManager.getPoolAllocator());
-
- // Process the potential contacts
- processAllPotentialContacts(narrowPhaseInput, false);
-
- // Reduce the number of contact points in the manifolds
- reduceContactManifolds(overlappingPairs);
-
- // For each overlapping pair
- for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
-
- OverlappingPair* pair = it->second;
-
- if (pair->hasContacts()) {
-
- // Report the contacts to the user
- CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager);
- callback->notifyContact(collisionInfo);
- }
-
- // Destroy the temporary overlapping pair
- pair->~OverlappingPair();
- mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair));
- }
-}
-
-// 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();
+ // Compute the middle-phase collision detection
+ computeMiddlePhase(mOverlappingPairs, narrowPhaseInput);
+
+ // Compute the narrow-phase collision detection and report overlapping shapes
+ computeNarrowPhaseOverlapSnapshot(narrowPhaseInput, &callback);
+}
+
+// Report all the bodies that overlap (collide) with the body in parameter
+void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback& callback) {
+
NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
+
+ // Compute the broad-phase collision detection
+ computeBroadPhase();
+
+ // Filter the overlapping pairs to get only the ones with the selected body involved
OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
+ filterOverlappingPairs(body->getEntity(), overlappingPairs);
+
+ if (overlappingPairs.size() > 0) {
+
+ // Compute the middle-phase collision detection
+ computeMiddlePhase(overlappingPairs, narrowPhaseInput);
+
+ // Compute the narrow-phase collision detection
+ computeNarrowPhaseOverlapSnapshot(narrowPhaseInput, &callback);
+ }
+}
+
+// Test collision and report contacts between two bodies.
+void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback& callback) {
+
+ NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
+
+ // Compute the broad-phase collision detection
+ computeBroadPhase();
+
+ // Filter the overlapping pairs to get only the ones with the selected body involved
+ OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
+ filterOverlappingPairs(body1->getEntity(), body2->getEntity(), overlappingPairs);
+
+ if (overlappingPairs.size() > 0) {
+
+ // Compute the middle-phase collision detection
+ computeMiddlePhase(overlappingPairs, narrowPhaseInput);
+
+ // Compute the narrow-phase collision detection and report contacts
+ computeNarrowPhaseCollisionSnapshot(narrowPhaseInput, callback);
+ }
+}
+
+// Test collision and report all the contacts involving the body in parameter
+void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback& callback) {
+
+ NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
+
+ // Compute the broad-phase collision detection
+ computeBroadPhase();
+
+ // Filter the overlapping pairs to get only the ones with the selected body involved
+ OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
+ filterOverlappingPairs(body->getEntity(), overlappingPairs);
+
+ if (overlappingPairs.size() > 0) {
+
+ // Compute the middle-phase collision detection
+ computeMiddlePhase(overlappingPairs, narrowPhaseInput);
+
+ // Compute the narrow-phase collision detection and report contacts
+ computeNarrowPhaseCollisionSnapshot(narrowPhaseInput, callback);
+ }
+}
+
+// Test collision and report contacts between each colliding bodies in the world
+void CollisionDetection::testCollision(CollisionCallback& callback) {
+
+ NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
+
+ // Compute the broad-phase collision detection
+ computeBroadPhase();
+
+ // Compute the middle-phase collision detection
+ computeMiddlePhase(mOverlappingPairs, narrowPhaseInput);
+
+ // Compute the narrow-phase collision detection and report contacts
+ computeNarrowPhaseCollisionSnapshot(narrowPhaseInput, callback);
+}
+
+// Filter the overlapping pairs to keep only the pairs where a given body is involved
+void CollisionDetection::filterOverlappingPairs(Entity bodyEntity, OverlappingPairMap& outFilteredPairs) const {
// For each possible collision pair of bodies
for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
- OverlappingPair* originalPair = it->second;
+ OverlappingPair* pair = it->second;
- OverlappingPair* pair;
- const Pair<uint, uint> pairID = OverlappingPair::computeID(originalPair->getShape1()->getBroadPhaseId(), originalPair->getShape2()->getBroadPhaseId());
+ if (pair->getShape1()->getBody()->getEntity() == bodyEntity || pair->getShape2()->getBody()->getEntity() == bodyEntity) {
- // Try to retrieve a corresponding copy of the overlapping pair (if it exists)
- auto itPair = overlappingPairs.find(pairID);
-
- // If a copy of the overlapping pair does not exist yet
- if (itPair == overlappingPairs.end()) {
-
- // Create a temporary overlapping pair
- pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair)))
- OverlappingPair(originalPair->getShape1(), originalPair->getShape2(), mMemoryManager.getPoolAllocator(),
- mMemoryManager.getPoolAllocator(), mWorld->mConfig);
-
- overlappingPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(pairID, pair));
- }
- else { // If a temporary copy of this overlapping pair already exists
-
- // Retrieve the existing copy of the overlapping pair
- pair = itPair->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) &&
- mBroadPhaseSystem.testOverlappingShapes(shape1, shape2)) {
-
- // Compute the middle-phase collision detection between the two shapes
- computeMiddlePhaseForProxyShapes(pair, narrowPhaseInput);
+ outFilteredPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(it->first, pair));
}
}
- // Test narrow-phase collision
- testNarrowPhaseCollision(narrowPhaseInput, false, true, mMemoryManager.getPoolAllocator());
+}
- // Process the potential contacts
- processAllPotentialContacts(narrowPhaseInput, false);
+// Filter the overlapping pairs to keep only the pairs where two given bodies are involved
+void CollisionDetection::filterOverlappingPairs(Entity body1Entity, Entity body2Entity, OverlappingPairMap& outFilteredPairs) const {
- // Reduce the number of contact points in the manifolds
- reduceContactManifolds(overlappingPairs);
-
- // For each overlapping pair
- for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
+ // For each possible collision pair of bodies
+ for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
OverlappingPair* pair = it->second;
- if (pair->hasContacts()) {
+ if ((pair->getShape1()->getBody()->getEntity() == body1Entity && pair->getShape2()->getBody()->getEntity() == body2Entity) ||
+ (pair->getShape1()->getBody()->getEntity() == body2Entity && pair->getShape2()->getBody()->getEntity() == body1Entity)) {
- // Report the contacts to the user
- CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager);
- callback->notifyContact(collisionInfo);
+ outFilteredPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(it->first, pair));
}
-
- // Destroy the temporary overlapping pair
- pair->~OverlappingPair();
- mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair));
}
+
}
// Return the world event listener
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index 8c1104ef..e4b16417 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -30,6 +30,11 @@
#include "body/CollisionBody.h"
#include "systems/BroadPhaseSystem.h"
#include "collision/shapes/CollisionShape.h"
+#include "collision/ContactPointInfo.h"
+#include "constraint/ContactPoint.h"
+#include "collision/ContactManifoldInfo.h"
+#include "collision/ContactManifold.h"
+#include "collision/ContactPair.h"
#include "engine/OverlappingPair.h"
#include "collision/narrowphase/NarrowPhaseInput.h"
#include "collision/narrowphase/CollisionDispatch.h"
@@ -64,6 +69,11 @@ class CollisionDetection {
using OverlappingPairMap = Map<Pair<uint, uint>, OverlappingPair*>;
+ // -------------------- Constants -------------------- //
+
+ /// Maximum number of contact points in a reduced contact manifold
+ const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4;
+
// -------------------- Attributes -------------------- //
/// Memory manager
@@ -72,9 +82,6 @@ class CollisionDetection {
/// Reference the proxy-shape components
ProxyShapeComponents& mProxyShapesComponents;
- /// Reference the transform components
- TransformComponents& mTransformComponents;
-
/// Collision Detection Dispatch configuration
CollisionDispatch mCollisionDispatch;
@@ -96,6 +103,71 @@ class CollisionDetection {
/// Narrow-phase collision detection input
NarrowPhaseInput mNarrowPhaseInput;
+ /// List of the potential contact points
+ List<ContactPointInfo> mPotentialContactPoints;
+
+ /// List of the potential contact manifolds
+ List<ContactManifoldInfo> mPotentialContactManifolds;
+
+ /// First list of narrow-phase pair contacts
+ List<ContactPair> mContactPairs1;
+
+ /// Second list of narrow-phase pair contacts
+ List<ContactPair> mContactPairs2;
+
+ /// Pointer to the list of contact pairs of the previous frame (either mContactPairs1 or mContactPairs2)
+ List<ContactPair>* mPreviousContactPairs;
+
+ /// Pointer to the list of contact pairs of the current frame (either mContactPairs1 or mContactPairs2)
+ List<ContactPair>* mCurrentContactPairs;
+
+ /// First map of overlapping pair id to the index of the corresponding pair contact
+ Map<OverlappingPair::OverlappingPairId, uint> mMapPairIdToContactPairIndex1;
+
+ /// Second map of overlapping pair id to the index of the corresponding pair contact
+ Map<OverlappingPair::OverlappingPairId, uint> mMapPairIdToContactPairIndex2;
+
+ /// Pointer to the map of overlappingPairId to the index of contact pair of the previous frame
+ /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2)
+ Map<OverlappingPair::OverlappingPairId, uint>* mPreviousMapPairIdToContactPairIndex;
+
+ /// Pointer to the map of overlappingPairId to the index of contact pair of the current frame
+ /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2)
+ Map<OverlappingPair::OverlappingPairId, uint>* mCurrentMapPairIdToContactPairIndex;
+
+ /// List of the indices of the contact pairs (in mCurrentContacPairs array) with contact pairs of
+ /// same islands packed together linearly and contact pairs that are not part of islands at the end.
+ /// This is used when we create contact manifolds and contact points so that there are also packed
+ /// together linearly if they are part of the same island.
+ List<uint> mContactPairsIndicesOrderingForContacts;
+
+ /// First list with the contact manifolds
+ List<ContactManifold> mContactManifolds1;
+
+ /// Second list with the contact manifolds
+ List<ContactManifold> mContactManifolds2;
+
+ /// Pointer to the list of contact manifolds from the previous frame (either mContactManifolds1 or mContactManifolds2)
+ List<ContactManifold>* mPreviousContactManifolds;
+
+ /// Pointer to the list of contact manifolds from the current frame (either mContactManifolds1 or mContactManifolds2)
+ List<ContactManifold>* mCurrentContactManifolds;
+
+ /// Second list of contact points (contact points from either the current frame of the previous frame)
+ List<ContactPoint> mContactPoints1;
+
+ /// Second list of contact points (contact points from either the current frame of the previous frame)
+ List<ContactPoint> mContactPoints2;
+
+ /// Pointer to the contact points of the previous frame (either mContactPoints1 or mContactPoints2)
+ List<ContactPoint>* mPreviousContactPoints;
+
+ /// Pointer to the contact points of the current frame (either mContactPoints1 or mContactPoints2)
+ List<ContactPoint>* mCurrentContactPoints;
+
+ /// Map a body entity to the list of contact pairs in which it is involved
+ Map<Entity, List<uint>> mMapBodyToContactPairs;
+
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
@@ -109,48 +181,89 @@ class CollisionDetection {
void computeBroadPhase();
/// Compute the middle-phase collision detection
- void computeMiddlePhase();
+ void computeMiddlePhase(OverlappingPairMap& overlappingPairs, NarrowPhaseInput& narrowPhaseInput);
/// Compute the narrow-phase collision detection
void computeNarrowPhase();
+ /// Compute the narrow-phase collision detection for the testOverlap() methods.
+ bool computeNarrowPhaseOverlapSnapshot(NarrowPhaseInput& narrowPhaseInput, OverlapCallback* callback);
+
+ /// Compute the narrow-phase collision detection for the testCollision() methods
+ bool computeNarrowPhaseCollisionSnapshot(NarrowPhaseInput& narrowPhaseInput, CollisionCallback& callback);
+
+ /// Process the potential contacts after narrow-phase collision detection
+ void computeSnapshotContactPairs(NarrowPhaseInput& narrowPhaseInput, List<Pair<Entity, Entity>>& overlapPairs) const;
+
+ /// Convert the potential contact into actual contacts
+ void computeSnapshotContactPairs(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, List<Pair<Entity, Entity>>& overlapPairs) const;
+
/// Take a list of overlapping nodes in the broad-phase and create new overlapping pairs if necessary
- void updateOverlappingPairs(List<Pair<int, int> >& overlappingNodes);
+ void updateOverlappingPairs(const List<Pair<int, int>>& overlappingNodes);
+
+ /// Remove pairs that are not overlapping anymore
+ void removeNonOverlappingPairs();
/// Execute the narrow-phase collision detection algorithm on batches
- bool testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool stopFirstContactFound,
- bool reportContacts, MemoryAllocator& allocator);
-
- /// Add a contact manifold to the linked list of contact manifolds of the two bodies
- /// involved in the corresponding contact.
- void addContactManifoldToBody(OverlappingPair* pair);
-
- /// Add all the contact manifold of colliding pairs to their bodies
- void addAllContactManifoldsToBodies();
+ bool testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool reportContacts, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& allocator);
/// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator,
NarrowPhaseInput& narrowPhaseInput);
- /// Compute the middle-phase collision detection between two proxy shapes
- void computeMiddlePhaseForProxyShapes(OverlappingPair* pair, NarrowPhaseInput& outNarrowPhaseInput);
+ /// Swap the previous and current contacts lists
+ void swapPreviousAndCurrentContacts();
/// Convert the potential contact into actual contacts
void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
- bool updateLastFrameInfo);
+ bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
+ Map<OverlappingPair::OverlappingPairId, uint>* mapPairIdToContactPairIndex,
+ List<ContactManifoldInfo>& potentialContactManifolds, List<ContactPair>* contactPairs,
+ Map<Entity, List<uint>>& mapBodyToContactPairs);
/// Process the potential contacts after narrow-phase collision detection
- void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo);
+ void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
+ Map<OverlappingPair::OverlappingPairId, uint>* mapPairIdToContactPairIndex,
+ List<ContactManifoldInfo> &potentialContactManifolds, List<ContactPair>* contactPairs,
+ Map<Entity, List<uint>>& mapBodyToContactPairs);
- /// Clear the obsolete manifolds and contact points and reduce the number of contacts points of the remaining manifolds
- void reduceContactManifolds(const OverlappingPairMap& overlappingPairs);
+ /// Reduce the potential contact manifolds and contact points of the overlapping pair contacts
+ void reducePotentialContactManifolds(List<ContactPair>* contactPairs, List<ContactManifoldInfo>& potentialContactManifolds,
+ const List<ContactPointInfo>& potentialContactPoints) const;
- /// Report contacts for all the colliding overlapping pairs
- void reportAllContacts();
+ /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair
+ void createContacts();
+
+ /// Create the actual contact manifolds and contacts points for testCollision() methods
+ void createSnapshotContacts(List<ContactPair>& contactPairs, List<ContactManifold> &contactManifolds,
+ List<ContactPoint>& contactPoints,
+ List<ContactManifoldInfo>& potentialContactManifolds,
+ List<ContactPointInfo>& potentialContactPoints);
+
+ /// Initialize the current contacts with the contacts from the previous frame (for warmstarting)
+ void initContactsWithPreviousOnes();
+
+ /// Reduce the number of contact points of a potential contact manifold
+ void reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform,
+ const List<ContactPointInfo>& potentialContactPoints) const;
+
+ /// Report contacts
+ void reportContacts(CollisionCallback& callback, List<ContactPair>* contactPairs,
+ List<ContactManifold>* manifolds, List<ContactPoint>* contactPoints);
+
+ /// Return the largest depth of all the contact points of a potential manifold
+ decimal computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold,
+ const List<ContactPointInfo>& potentialContactPoints) const;
/// Process the potential contacts where one collion is a concave shape
void processSmoothMeshContacts(OverlappingPair* pair);
+ /// Filter the overlapping pairs to keep only the pairs where a given body is involved
+ void filterOverlappingPairs(Entity bodyEntity, OverlappingPairMap& outFilteredPairs) const;
+
+ /// Filter the overlapping pairs to keep only the pairs where two given bodies are involved
+ void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, OverlappingPairMap& outFilteredPairs) const;
+
public :
// -------------------- Methods -------------------- //
@@ -193,6 +306,9 @@ class CollisionDetection {
/// Ask for a collision shape to be tested again during broad-phase.
void askForBroadPhaseCollisionCheck(ProxyShape* shape);
+ /// Report contacts
+ void reportContacts();
+
/// Compute the collision detection
void computeCollisionDetection();
@@ -200,23 +316,23 @@ class CollisionDetection {
void raycast(RaycastCallback* raycastCallback, const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const;
- /// Report all the bodies that overlap with the aabb in parameter
- void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
-
- /// Return true if two bodies overlap
+ /// Return true if two bodies (collide) 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);
+ /// Report all the bodies that overlap (collide) with the body in parameter
+ void testOverlap(CollisionBody* body, OverlapCallback& callback);
- /// Test and report collisions between two bodies
- void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback);
+ /// Report all the bodies that overlap (collide) in the world
+ void testOverlap(OverlapCallback& overlapCallback);
- /// 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 collision and report contacts between two bodies.
+ void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback& callback);
- /// Test and report collisions between all shapes of the world
- void testCollision(CollisionCallback* callback);
+ /// Test collision and report all the contacts involving the body in parameter
+ void testCollision(CollisionBody* body, CollisionCallback& callback);
+
+ /// Test collision and report contacts between each colliding bodies in the world
+ void testCollision(CollisionCallback& callback);
/// Return a reference to the memory manager
MemoryManager& getMemoryManager() const;
diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp
index 4e80a625..05232626 100644
--- a/src/collision/ContactManifold.cpp
+++ b/src/collision/ContactManifold.cpp
@@ -26,364 +26,23 @@
// Libraries
#include "ContactManifold.h"
#include "constraint/ContactPoint.h"
+#include "collision/ContactManifoldInfo.h"
using namespace reactphysics3d;
// Constructor
-ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
- MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings)
- : 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),
- mWorldSettings(worldSettings) {
-
+ContactManifold::ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity proxyShapeEntity1, Entity proxyShapeEntity2,
+ uint contactPointsIndex, int8 nbContactPoints)
+ :mContactPointsIndex(0), bodyEntity1(bodyEntity1), bodyEntity2(bodyEntity2),
+ proxyShapeEntity1(proxyShapeEntity1), proxyShapeEntity2(proxyShapeEntity2), mFrictionImpulse1(0), mFrictionImpulse2(0),
+ mFrictionTwistImpulse(0), mIsAlreadyInIsland(false) {
+
+
+ mContactPointsIndex = contactPointsIndex;
+ mNbContactPoints = nbContactPoints;
}
// Destructor
ContactManifold::~ContactManifold() {
- // Delete all the contact points
- ContactPoint* contactPoint = mContactPoints;
- while(contactPoint != nullptr) {
-
- ContactPoint* nextContactPoint = contactPoint->getNext();
-
- // Delete the contact point
- contactPoint->~ContactPoint();
- mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
-
- contactPoint = nextContactPoint;
- }
-}
-
-// 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);
-}
-
-// Return the largest depth of all the contact points
-decimal ContactManifold::getLargestContactDepth() const {
- decimal largestDepth = 0.0f;
-
- assert(mNbContactPoints > 0);
-
- ContactPoint* contactPoint = mContactPoints;
- while(contactPoint != nullptr){
- decimal depth = contactPoint->getPenetrationDepth();
- if (depth > largestDepth) {
- largestDepth = depth;
- }
-
- contactPoint = contactPoint->getNext();
- }
-
- return largestDepth;
-}
-
-// Add a contact point
-void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
-
- // For each contact point in the manifold
- bool isSimilarPointFound = false;
- ContactPoint* oldContactPoint = mContactPoints;
- while (oldContactPoint != nullptr) {
-
- assert(oldContactPoint != nullptr);
-
- // If the new contact point is similar (very close) to the old contact point
- if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) {
-
- // Replace (update) the old contact point with the new one
- oldContactPoint->update(contactPointInfo);
- isSimilarPointFound = true;
- break;
- }
-
- oldContactPoint = oldContactPoint->getNext();
- }
-
- // If we have not found a similar contact point
- if (!isSimilarPointFound) {
-
- // Create the new contact point
- ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo, mWorldSettings);
-
- // Add the new contact point into the manifold
- contactPoint->setNext(mContactPoints);
- contactPoint->setPrevious(nullptr);
- if (mContactPoints != nullptr) {
- mContactPoints->setPrevious(contactPoint);
- }
-
- mContactPoints = contactPoint;
-
- mNbContactPoints++;
- }
-
- // The old manifold is no longer obsolete
- mIsObsolete = false;
-}
-
-// Set to true to make the manifold obsolete
-void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoints) {
- mIsObsolete = isObsolete;
-
- if (setContactPoints) {
- ContactPoint* contactPoint = mContactPoints;
- while (contactPoint != nullptr) {
- contactPoint->setIsObsolete(isObsolete);
-
- contactPoint = contactPoint->getNext();
- }
- }
-}
-
-// Clear the obsolete contact points
-void ContactManifold::clearObsoleteContactPoints() {
-
- assert(mContactPoints != nullptr);
-
- // For each contact point of the manifold
- ContactPoint* contactPoint = mContactPoints;
- while (contactPoint != nullptr) {
-
- ContactPoint* nextContactPoint = contactPoint->getNext();
-
- // If the contact point is obsolete
- if (contactPoint->getIsObsolete()) {
-
- // Remove the contact point
- removeContactPoint(contactPoint);
- }
-
- contactPoint = nextContactPoint;
- }
-
- assert(mNbContactPoints > 0);
- assert(mContactPoints != nullptr);
-}
-
-// 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. This method will reduce the number of
-// contact points to a maximum of 4 points (but it can be less).
-void ContactManifold::reduce(const Transform& shape1ToWorldTransform) {
-
- assert(mContactPoints != nullptr);
-
- // The following algorithm only works to reduce to a maximum of 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) {
-
- uint nbReducedPoints = 0;
-
- ContactPoint* pointsToKeep[MAX_CONTACT_POINTS_IN_MANIFOLD];
- for (int i=0; i<MAX_CONTACT_POINTS_IN_MANIFOLD; i++) {
- pointsToKeep[i] = nullptr;
- }
-
- // 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() * mContactPoints->getNormal();
-
- // Compute a search direction
- const Vector3 searchDirection(1, 1, 1);
- ContactPoint* element = mContactPoints;
- pointsToKeep[0] = element;
- decimal maxDotProduct = searchDirection.dot(element->getLocalPointOnShape1());
- element = element->getNext();
- nbReducedPoints = 1;
- while(element != nullptr) {
-
- decimal dotProduct = searchDirection.dot(element->getLocalPointOnShape1());
- if (dotProduct > maxDotProduct) {
- maxDotProduct = dotProduct;
- pointsToKeep[0] = element;
- }
- element = element->getNext();
- }
- assert(pointsToKeep[0] != nullptr);
- assert(nbReducedPoints == 1);
-
- // 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 = mContactPoints;
- while(element != nullptr) {
-
- if (element == pointsToKeep[0]) {
- element = element->getNext();
- continue;
- }
-
- decimal distance = (pointsToKeep[0]->getLocalPointOnShape1() - element->getLocalPointOnShape1()).lengthSquare();
- if (distance >= maxDistance) {
- maxDistance = distance;
- pointsToKeep[1] = element;
- nbReducedPoints = 2;
- }
- element = element->getNext();
- }
- assert(pointsToKeep[1] != nullptr);
- assert(nbReducedPoints == 2);
-
- // 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)
- ContactPoint* thirdPointMaxArea = nullptr;
- ContactPoint* thirdPointMinArea = nullptr;
- decimal minArea = decimal(0.0);
- decimal maxArea = decimal(0.0);
- bool isPreviousAreaPositive = true;
- element = mContactPoints;
- while(element != nullptr) {
-
- if (element == pointsToKeep[0] || element == pointsToKeep[1]) {
- element = element->getNext();
- continue;
- }
-
- const Vector3 newToFirst = pointsToKeep[0]->getLocalPointOnShape1() - element->getLocalPointOnShape1();
- const Vector3 newToSecond = pointsToKeep[1]->getLocalPointOnShape1() - element->getLocalPointOnShape1();
-
- // 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->getNext();
- }
- assert(minArea <= decimal(0.0));
- assert(maxArea >= decimal(0.0));
- if (maxArea > (-minArea)) {
- isPreviousAreaPositive = true;
- pointsToKeep[2] = thirdPointMaxArea;
- nbReducedPoints = 3;
- }
- else {
- isPreviousAreaPositive = false;
- pointsToKeep[2] = thirdPointMinArea;
- nbReducedPoints = 3;
- }
-
- // 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 = mContactPoints;
-
- if (nbReducedPoints == 3) {
-
- // For each remaining point
- while(element != nullptr) {
-
- if (element == pointsToKeep[0] || element == pointsToKeep[1] || element == pointsToKeep[2]) {
- element = element->getNext();
- 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]->getLocalPointOnShape1() - element->getLocalPointOnShape1();
- const Vector3 newToSecond = pointsToKeep[edgeVertex2Index]->getLocalPointOnShape1() - element->getLocalPointOnShape1();
-
- // 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;
- nbReducedPoints = 4;
- }
- else if (!isPreviousAreaPositive && area >= largestArea) {
- largestArea = area;
- pointsToKeep[3] = element;
- nbReducedPoints = 4;
- }
- }
-
- element = element->getNext();
- }
- }
-
- // Delete the contact points we do not want to keep from the linked list
- element = mContactPoints;
- ContactPoint* previousElement = nullptr;
- while(element != nullptr) {
-
- bool deletePoint = true;
-
- // Skip the points we want to keep
- for (uint i=0; i<nbReducedPoints; i++) {
-
- if (element == pointsToKeep[i]) {
-
- previousElement = element;
- element = element->getNext();
- deletePoint = false;
- }
- }
-
- if (deletePoint) {
-
- ContactPoint* contactPointToDelete = element;
- element = element->getNext();
-
- removeContactPoint(contactPointToDelete);
- }
- }
-
- assert(nbReducedPoints > 0 && nbReducedPoints <= 4);
- mNbContactPoints = nbReducedPoints;
- }
}
diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h
index 4b368660..e0c86f19 100644
--- a/src/collision/ContactManifold.h
+++ b/src/collision/ContactManifold.h
@@ -40,44 +40,6 @@ class CollisionBody;
class ContactPoint;
class PoolAllocator;
-// Structure ContactManifoldListElement
-/**
- * This structure represents a single element of a linked list of contact manifolds
- */
-struct ContactManifoldListElement {
-
- private:
-
- // -------------------- Attributes -------------------- //
-
- /// Pointer to a contact manifold with contact points
- ContactManifold* mContactManifold;
-
- /// Next element of the list
- ContactManifoldListElement* mNext;
-
- public:
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- 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 a set of contact points between two bodies that
@@ -91,6 +53,7 @@ struct ContactManifoldListElement {
*/
class ContactManifold {
+ // TODO : Check if we can use public fields in this class (maybe this class is used by users directly)
private:
// -------------------- Constants -------------------- //
@@ -100,14 +63,22 @@ class ContactManifold {
// -------------------- Attributes -------------------- //
- /// Pointer to the first proxy shape of the contact
- ProxyShape* mShape1;
+ // TODO : For each of the attributes, check if we need to keep it or not
- /// Pointer to the second proxy shape of the contact
- ProxyShape* mShape2;
+ /// Index of the first contact point of the manifold in the list of contact points
+ uint mContactPointsIndex;
- /// Contact points in the manifold
- ContactPoint* mContactPoints;
+ /// Entity of the first body in contact
+ Entity bodyEntity1;
+
+ /// Entity of the second body in contact
+ Entity bodyEntity2;
+
+ /// Entity of the first proxy-shape in contact
+ Entity proxyShapeEntity1;
+
+ /// Entity of the second proxy-shape in contact
+ Entity proxyShapeEntity2;
/// Number of contacts in the cache
int8 mNbContactPoints;
@@ -133,44 +104,11 @@ class ContactManifold {
/// True if the contact manifold has already been added into an island
bool mIsAlreadyInIsland;
- /// Reference to the memory allocator
- MemoryAllocator& 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 obsolete
- bool mIsObsolete;
-
- /// World settings
- const WorldSettings& mWorldSettings;
-
// -------------------- 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 obsolete
- bool getIsObsolete() const;
-
- /// Set to true to make the manifold obsolete
- void setIsObsolete(bool isObselete, bool setContactPoints);
-
- /// Clear the obsolete contact points
- void clearObsoleteContactPoints();
-
- /// 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);
@@ -183,24 +121,12 @@ class ContactManifold {
/// Set the second friction accumulated impulse
void setFrictionImpulse2(decimal frictionImpulse2);
- /// Add a contact point
- void addContactPoint(const ContactPointInfo* contactPointInfo);
-
- /// Reduce the number of contact points of the currently computed manifold
- void reduce(const Transform& shape1ToWorldTransform);
-
- /// Remove a contact point
- void removeContactPoint(ContactPoint* contactPoint);
-
/// 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;
@@ -221,42 +147,21 @@ class ContactManifold {
// -------------------- Methods -------------------- //
/// Constructor
- ContactManifold(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator,
- const WorldSettings& worldSettings);
+ ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity proxyShapeEntity1, Entity proxyShapeEntity2,
+ uint contactPointsIndex, int8 nbContactPoints);
/// Destructor
~ContactManifold();
- /// Deleted copy-constructor
- ContactManifold(const ContactManifold& contactManifold) = delete;
+ /// Copy-constructor
+ ContactManifold(const ContactManifold& contactManifold) = default;
- /// Deleted assignment operator
- ContactManifold& operator=(const ContactManifold& contactManifold) = delete;
-
- /// Return a pointer to the first proxy shape of the contact
- ProxyShape* getShape1() const;
-
- /// Return a pointer to the second proxy shape of the contact
- ProxyShape* getShape2() const;
-
- /// Return a pointer to the first body of the contact manifold
- CollisionBody* getBody1() const;
-
- /// Return a pointer to the second body of the contact manifold
- CollisionBody* getBody2() const;
+ /// Assignment operator
+ ContactManifold& operator=(const ContactManifold& contactManifold) = default;
/// Return the number of contact points in the manifold
int8 getNbContactPoints() const;
- /// Return a pointer to the first contact point of the manifold
- ContactPoint* getContactPoints() const;
-
- /// Return a pointer to the previous element in the linked-list
- ContactManifold* getPrevious() const;
-
- /// Return a pointer to the next element in the linked-list
- ContactManifold* getNext() const;
-
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
@@ -264,28 +169,9 @@ class ContactManifold {
friend class CollisionBody;
friend class ContactManifoldSet;
friend class ContactSolver;
+ friend class CollisionDetection;
};
-// Return a pointer to the first proxy shape of the contact
-inline ProxyShape* ContactManifold::getShape1() const {
- return mShape1;
-}
-
-// Return a pointer to the second proxy shape of the contact
-inline ProxyShape* ContactManifold::getShape2() const {
- return mShape2;
-}
-
-// Return a pointer to the first body of the contact manifold
-inline CollisionBody* ContactManifold::getBody1() const {
- return mShape1->getBody();
-}
-
-// Return a pointer to the second body of the contact manifold
-inline CollisionBody* ContactManifold::getBody2() const {
- return mShape2->getBody();
-}
-
// Return the number of contact points in the manifold
inline int8 ContactManifold::getNbContactPoints() const {
return mNbContactPoints;
@@ -346,41 +232,11 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR
mRollingResistanceImpulse = rollingResistanceImpulse;
}
-// 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
inline bool ContactManifold::isAlreadyInIsland() const {
return mIsAlreadyInIsland;
}
-// 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 obsolete
-inline bool ContactManifold::getIsObsolete() const {
- return mIsObsolete;
-}
-
}
#endif
diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h
new file mode 100644
index 00000000..9d3fed31
--- /dev/null
+++ b/src/collision/ContactManifoldInfo.h
@@ -0,0 +1,67 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2018 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 "mathematics/mathematics.h"
+#include "configuration.h"
+#include "engine/OverlappingPair.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Structure ContactManifoldInfo
+/**
+ * This structure contains informations about a collision contact
+ * manifold computed during the narrow-phase collision detection.
+ */
+struct ContactManifoldInfo {
+
+ public:
+
+ // -------------------- Attributes -------------------- //
+
+ /// Indices of the contact points in the mPotentialContactPoints array
+ List<uint> potentialContactPointsIndices;
+
+ /// Overlapping pair id
+ OverlappingPair::OverlappingPairId pairId;
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ ContactManifoldInfo(OverlappingPair::OverlappingPairId pairId, MemoryAllocator& allocator)
+ : potentialContactPointsIndices(allocator), pairId(pairId) {
+
+ }
+
+
+};
+
+}
+
+#endif
diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp
deleted file mode 100644
index a842fe64..00000000
--- a/src/collision/ContactManifoldSet.cpp
+++ /dev/null
@@ -1,295 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2018 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 "ContactManifoldSet.h"
-#include "narrowphase/NarrowPhaseInfoBatch.h"
-#include "constraint/ContactPoint.h"
-#include "ProxyShape.h"
-#include "collision/ContactManifold.h"
-
-using namespace reactphysics3d;
-
-// Constructor
-ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
- MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings)
- : mNbManifolds(0), mShape1(shape1),
- mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr), mWorldSettings(worldSettings) {
-
- // Compute the maximum number of manifolds allowed between the two shapes
- mNbMaxManifolds = computeNbMaxContactManifolds(shape1->getCollisionShape(), shape2->getCollisionShape());
-}
-
-// Destructor
-ContactManifoldSet::~ContactManifoldSet() {
-
- // Clear all the contact manifolds
- clear();
-}
-
-void ContactManifoldSet::addContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex) {
-
- assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() > 0);
-
- // For each potential contact point to add
- for (uint i=0; i < narrowPhaseInfoBatch.contactPoints[batchIndex].size(); i++) {
-
- ContactPointInfo* contactPoint = narrowPhaseInfoBatch.contactPoints[batchIndex][i];
-
- // 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)
- ContactManifold* manifold = mManifolds;
- bool similarManifoldFound = false;
- while(manifold != nullptr) {
-
- // Get the first contact point
- const ContactPoint* point = manifold->getContactPoints();
- assert(point != nullptr);
-
- // If we have found a corresponding manifold for the new contact point
- // (a manifold with a similar contact normal direction)
- if (point->getNormal().dot(contactPoint->normal) >= mWorldSettings.cosAngleSimilarContactManifold) {
-
- // Add the contact point to the manifold
- manifold->addContactPoint(contactPoint);
-
- similarManifoldFound = true;
-
- break;
- }
-
- manifold = manifold->getNext();
- }
-
- // If we have not found an existing manifold with a similar contact normal
- if (!similarManifoldFound) {
-
- // Create a new contact manifold
- ContactManifold* manifold = createManifold();
-
- // Add the contact point to the manifold
- manifold->addContactPoint(contactPoint);
- }
- }
-}
-
-// Return the total number of contact points in the set of manifolds
-int ContactManifoldSet::getTotalNbContactPoints() const {
- int nbPoints = 0;
-
- 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
-int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2) {
-
- // If both shapes are convex
- if (shape1->isConvex() && shape2->isConvex()) {
- return mWorldSettings.nbMaxContactManifoldsConvexShape;
-
- } // If there is at least one concave shape
- else {
- return mWorldSettings.nbMaxContactManifoldsConcaveShape;
- }
-}
-
-// Remove a contact manifold that is the least optimal (smaller penetration depth)
-void ContactManifoldSet::removeNonOptimalManifold() {
-
- 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* 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 (depth < minDepth) {
- minDepth = depth;
- minDepthManifold = manifold;
- }
-
- manifold = manifold->getNext();
- }
-
- // Remove the non optimal manifold
- assert(minDepthManifold != nullptr);
- removeManifold(minDepthManifold);
-}
-
-// Create a new contact manifold and add it to the set
-ContactManifold* ContactManifoldSet::createManifold() {
-
- ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
- ContactManifold(mShape1, mShape2, mMemoryAllocator, mWorldSettings);
- manifold->setPrevious(nullptr);
- manifold->setNext(mManifolds);
- if (mManifolds != nullptr) {
- mManifolds->setPrevious(manifold);
- }
- mManifolds = manifold;
-
- mNbManifolds++;
-
- return manifold;
-}
-
-// Return the contact manifold with a similar contact normal.
-// If no manifold has close enough contact normal, it returns nullptr
-ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(const Vector3& contactNormal) const {
-
- ContactManifold* manifold = mManifolds;
-
- // Return the Id of the manifold with the same normal direction id (if exists)
- while (manifold != nullptr) {
-
- // 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 (contactNormal.dot(point->getNormal()) >= mWorldSettings.cosAngleSimilarContactManifold) {
- return manifold;
- }
-
- manifold = manifold->getNext();
- }
-
- return nullptr;
-}
-
-// Clear the contact manifold set
-void ContactManifoldSet::clear() {
-
- ContactManifold* manifold = mManifolds;
- while(manifold != nullptr) {
-
- ContactManifold* nextManifold = manifold->getNext();
-
- // Delete the contact manifold
- manifold->~ContactManifold();
- mMemoryAllocator.release(manifold, sizeof(ContactManifold));
-
- manifold = nextManifold;
-
- mNbManifolds--;
- }
-
- mManifolds = nullptr;
-
- assert(mNbManifolds == 0);
-}
-
-// Remove a contact manifold from the set
-void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
-
- assert(mNbManifolds > 0);
- assert(manifold != nullptr);
-
- ContactManifold* previous = manifold->getPrevious();
- ContactManifold* next = manifold->getNext();
-
- if (previous != nullptr) {
- previous->setNext(next);
- }
- else {
- mManifolds = next;
- }
-
- if (next != nullptr) {
- next->setPrevious(previous);
- }
-
- // Delete the contact manifold
- manifold->~ContactManifold();
- mMemoryAllocator.release(manifold, sizeof(ContactManifold));
- mNbManifolds--;
-}
-
-// Make all the contact manifolds and contact points obsolete
-void ContactManifoldSet::makeContactsObsolete() {
-
- ContactManifold* manifold = mManifolds;
- while (manifold != nullptr) {
-
- manifold->setIsObsolete(true, true);
-
- manifold = manifold->getNext();
- }
-}
-
-// Clear the obsolete contact manifolds and contact points
-void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() {
-
- ContactManifold* manifold = mManifolds;
- while (manifold != nullptr) {
-
- // Get the next manifold in the linked-list
- ContactManifold* nextManifold = manifold->getNext();
-
- // If the manifold is obsolete
- if (manifold->getIsObsolete()) {
-
- // Delete the contact manifold
- removeManifold(manifold);
- }
- else {
-
- // Clear the obsolete contact points of the manifold
- 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(mShape1->getLocalToWorldTransform());
- manifold = manifold->getNext();
- }
-}
diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h
deleted file mode 100644
index 8ba5017d..00000000
--- a/src/collision/ContactManifoldSet.h
+++ /dev/null
@@ -1,166 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://www.reactphysics3d.com *
-* Copyright (c) 2010-2018 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_SET_H
-#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
-
-// Libraries
-#include "configuration.h"
-
-namespace reactphysics3d {
-
-// Declarations
-class ContactManifold;
-class ContactManifoldInfo;
-class ProxyShape;
-class MemoryAllocator;
-struct WorldSettings;
-struct NarrowPhaseInfoBatch;
-struct Vector3;
-class CollisionShape;
-class Transform;
-
-
-// Constants
-const int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set
-const int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap
-
-// Class ContactManifoldSet
-/**
- * This class represents a set of one or several contact manifolds. Typically a
- * convex/convex collision will have a set with a single manifold and a convex-concave
- * collision can have more than one manifolds. Note that a contact manifold can
- * contains several contact points.
- */
-class ContactManifoldSet {
-
- private:
-
- // -------------------- Attributes -------------------- //
-
- /// Maximum number of contact manifolds in the set
- int mNbMaxManifolds;
-
- /// Current number of contact manifolds in the set
- int mNbManifolds;
-
- /// Pointer to the first proxy shape of the contact
- ProxyShape* mShape1;
-
- /// Pointer to the second proxy shape of the contact
- ProxyShape* mShape2;
-
- /// Reference to the memory allocator for the contact manifolds
- MemoryAllocator& mMemoryAllocator;
-
- /// Contact manifolds of the set
- ContactManifold* mManifolds;
-
- /// World settings
- const WorldSettings& mWorldSettings;
-
- // -------------------- Methods -------------------- //
-
- /// Create a new contact manifold and add it to the set
- ContactManifold* createManifold();
-
- // Return the contact manifold with a similar contact normal.
- ContactManifold* selectManifoldWithSimilarNormal(const Vector3& contactNormal) const;
-
- /// Remove a contact manifold that is the least optimal (smaller penetration depth)
- void removeNonOptimalManifold();
-
- /// 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:
-
- // -------------------- Methods -------------------- //
-
- /// Constructor
- ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
- MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings);
-
- /// Destructor
- ~ContactManifoldSet();
-
- /// Add the contact points from the narrow phase
- void addContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex);
-
- /// Return the first proxy shape
- ProxyShape* getShape1() const;
-
- /// Return the second proxy shape
- ProxyShape* getShape2() const;
-
- /// Return the number of manifolds in the set
- int getNbContactManifolds() 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 obsolete
- void makeContactsObsolete();
-
- /// Return the total number of contact points in the set of manifolds
- int getTotalNbContactPoints() const;
-
- /// Clear the obsolete contact manifolds and contact points
- void clearObsoleteManifoldsAndContactPoints();
-
- // Remove some contact manifolds and contact points if there are too many of them
- void reduce();
-};
-
-// Return the first proxy shape
-inline ProxyShape* ContactManifoldSet::getShape1() const {
- return mShape1;
-}
-
-// Return the second proxy shape
-inline ProxyShape* ContactManifoldSet::getShape2() const {
- return mShape2;
-}
-
-// Return the number of manifolds in the set
-inline int ContactManifoldSet::getNbContactManifolds() const {
- return mNbManifolds;
-}
-
-// Return a pointer to the first element of the linked-list of contact manifolds
-inline ContactManifold* ContactManifoldSet::getContactManifolds() const {
- return mManifolds;
-}
-
-}
-
-#endif
-
diff --git a/src/collision/ContactPair.h b/src/collision/ContactPair.h
new file mode 100644
index 00000000..d8fbf46c
--- /dev/null
+++ b/src/collision/ContactPair.h
@@ -0,0 +1,98 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2018 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_OVERLAPPING_PAIR_CONTACT_H
+#define REACTPHYSICS3D_OVERLAPPING_PAIR_CONTACT_H
+
+// Libraries
+#include "mathematics/mathematics.h"
+#include "configuration.h"
+#include "engine/OverlappingPair.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Structure ContactPair
+/**
+ * This structure represents a pair of shapes that are in contact during narrow-phase.
+ */
+struct ContactPair {
+
+ public:
+
+ // -------------------- Attributes -------------------- //
+
+ /// Overlapping pair Id
+ OverlappingPair::OverlappingPairId pairId;
+
+ /// Indices of the potential contact manifolds
+ List<uint> potentialContactManifoldsIndices;
+
+ /// Entity of the first body of the contact
+ Entity body1Entity;
+
+ /// Entity of the second body of the contact
+ Entity body2Entity;
+
+ /// Entity of the first proxy-shape of the contact
+ Entity proxyShape1Entity;
+
+ /// Entity of the second proxy-shape of the contact
+ Entity proxyShape2Entity;
+
+ /// True if the manifold is already in an island
+ bool isAlreadyInIsland;
+
+ /// Index of the contact pair in the array of pairs
+ uint contactPairIndex;
+
+ /// Index of the first contact manifold in the array
+ uint contactManifoldsIndex;
+
+ /// Number of contact manifolds
+ int8 nbContactManifolds;
+
+ /// Index of the first contact point in the array of contact points
+ uint contactPointsIndex;
+
+ /// Total number of contact points in all the manifolds of the contact pair
+ uint nbToTalContactPoints;
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ ContactPair(OverlappingPair::OverlappingPairId pairId, Entity body1Entity, Entity body2Entity, Entity proxyShape1Entity,
+ Entity proxyShape2Entity, uint contactPairIndex, MemoryAllocator& allocator)
+ : pairId(pairId), potentialContactManifoldsIndices(allocator), body1Entity(body1Entity), body2Entity(body2Entity),
+ proxyShape1Entity(proxyShape1Entity), proxyShape2Entity(proxyShape2Entity),
+ isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0),
+ contactPointsIndex(0), nbToTalContactPoints(0) {
+
+ }
+};
+
+}
+
+#endif
diff --git a/src/collision/ContactPointInfo.h b/src/collision/ContactPointInfo.h
index c672e208..6e81783c 100644
--- a/src/collision/ContactPointInfo.h
+++ b/src/collision/ContactPointInfo.h
@@ -65,9 +65,11 @@ struct ContactPointInfo {
/// Contact point of body 2 in local space of body 2
Vector3 localPoint2;
+ // TODO : Remove this field
/// Pointer to the next contact point info
ContactPointInfo* next;
+ // TODO : Remove this field
/// True if the contact point has already been inserted into a manifold
bool isUsed;
diff --git a/src/collision/OverlapCallback.cpp b/src/collision/OverlapCallback.cpp
new file mode 100644
index 00000000..096f9421
--- /dev/null
+++ b/src/collision/OverlapCallback.cpp
@@ -0,0 +1,53 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2018 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/OverlapCallback.h"
+#include "engine/CollisionWorld.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Contact Pair Constructor
+OverlapCallback::OverlapPair::OverlapPair(Pair<Entity, Entity>& overlapPair, CollisionWorld& world)
+ : mOverlapPair(overlapPair), mWorld(world) {
+
+}
+
+// Return a pointer to the first body in contact
+CollisionBody* OverlapCallback::OverlapPair::getBody1() const {
+ return static_cast<CollisionBody*>(mWorld.mBodyComponents.getBody(mOverlapPair.first));
+}
+
+// Return a pointer to the second body in contact
+CollisionBody* OverlapCallback::OverlapPair::getBody2() const {
+ return static_cast<CollisionBody*>(mWorld.mBodyComponents.getBody(mOverlapPair.second));
+}
+
+// CollisionCallbackData Constructor
+OverlapCallback::CallbackData::CallbackData(List<Pair<Entity, Entity>>& overlapBodies, CollisionWorld& world)
+ :mOverlapBodies(overlapBodies), mWorld(world) {
+
+}
diff --git a/src/collision/OverlapCallback.h b/src/collision/OverlapCallback.h
index dd128394..c9e83250 100644
--- a/src/collision/OverlapCallback.h
+++ b/src/collision/OverlapCallback.h
@@ -26,32 +26,141 @@
#ifndef REACTPHYSICS3D_OVERLAP_CALLBACK_H
#define REACTPHYSICS3D_OVERLAP_CALLBACK_H
+// Libraries
+#include "containers/List.h"
+
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class CollisionBody;
+class CollisionWorld;
+class ProxyShape;
+struct Entity;
// 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.
+ * This class can be used to register a callback for collision overlap queries between bodies.
+ * You should implement your own class inherited from this one and implement the onOverlap() method.
*/
class OverlapCallback {
public:
+ // Class OverlapPair
+ /**
+ * This class represents the contact between two proxy-shapes of the physics world.
+ */
+ class OverlapPair {
+
+ private:
+
+ // -------------------- Attributes -------------------- //
+
+ /// Pair of overlapping body entities
+ Pair<Entity, Entity>& mOverlapPair;
+
+ /// Reference to the physics world
+ CollisionWorld& mWorld;
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ OverlapPair(Pair<Entity, Entity>& overlapPair, CollisionWorld& world);
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Copy constructor
+ OverlapPair(const OverlapPair& contactPair) = default;
+
+ /// Assignment operator
+ OverlapPair& operator=(const OverlapPair& contactPair) = default;
+
+ /// Destructor
+ ~OverlapPair() = default;
+
+ /// Return a pointer to the first body in contact
+ CollisionBody* getBody1() const;
+
+ /// Return a pointer to the second body in contact
+ CollisionBody* getBody2() const;
+
+ // -------------------- Friendship -------------------- //
+
+ friend class OverlapCallback;
+ };
+
+ // Class CallbackData
+ /**
+ * This class contains data about overlap between bodies
+ */
+ class CallbackData {
+
+ private:
+
+ // -------------------- Attributes -------------------- //
+
+ List<Pair<Entity, Entity>>& mOverlapBodies;
+
+ /// Reference to the physics world
+ CollisionWorld& mWorld;
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ CallbackData(List<Pair<Entity, Entity>>& overlapProxyShapes, CollisionWorld& world);
+
+ /// Deleted copy constructor
+ CallbackData(const CallbackData& callbackData) = delete;
+
+ /// Deleted assignment operator
+ CallbackData& operator=(const CallbackData& callbackData) = delete;
+
+ /// Destructor
+ ~CallbackData() = default;
+
+ public:
+
+ // -------------------- Methods -------------------- //
+
+ /// Return the number of overlapping pairs of bodies
+ uint getNbOverlappingPairs() const;
+
+ /// Return a given overlapping pair of bodies
+ OverlapPair getOverlappingPair(uint index) const;
+
+ // -------------------- Friendship -------------------- //
+
+ friend class CollisionDetection;
+ };
+
/// Destructor
virtual ~OverlapCallback() {
}
- /// This method will be called for each reported overlapping bodies
- virtual void notifyOverlap(CollisionBody* collisionBody)=0;
+ /// This method will be called to report bodies that overlap
+ virtual void onOverlap(CallbackData& callbackData)=0;
};
+// Return the number of overlapping pairs of bodies
+inline uint OverlapCallback::CallbackData::getNbOverlappingPairs() const {
+ return mOverlapBodies.size();
+}
+
+// Return a given overlapping pair of bodies
+/// Note that the returned OverlapPair object is only valid during the call of the CollisionCallback::onOverlap()
+/// method. Therefore, you need to get contact data from it and make a copy. Do not make a copy of the OverlapPair
+/// object itself because it won't be valid after the CollisionCallback::onOverlap() call.
+inline OverlapCallback::OverlapPair OverlapCallback::CallbackData::getOverlappingPair(uint index) const {
+
+ assert(index < getNbOverlappingPairs());
+
+ return OverlapPair(mOverlapBodies[index], mWorld);
+}
+
}
#endif
diff --git a/src/collision/ProxyShape.cpp b/src/collision/ProxyShape.cpp
index d8dbeacd..cbb34788 100644
--- a/src/collision/ProxyShape.cpp
+++ b/src/collision/ProxyShape.cpp
@@ -78,6 +78,10 @@ bool ProxyShape::testPointInside(const Vector3& worldPoint) {
*/
void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) {
+ // TODO : Here we should probably remove all overlapping pairs with this shape in the
+ // broad-phase and add the shape in the "has moved" shape list so it is reevaluated
+ // with the new mask bits
+
mBody->mWorld.mProxyShapesComponents.setCollisionCategoryBits(mEntity, collisionCategoryBits);
int broadPhaseId = mBody->mWorld.mProxyShapesComponents.getBroadPhaseId(mEntity);
@@ -93,6 +97,10 @@ void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits)
*/
void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) {
+ // TODO : Here we should probably remove all overlapping pairs with this shape in the
+ // broad-phase and add the shape in the "has moved" shape list so it is reevaluated
+ // with the new mask bits
+
mBody->mWorld.mProxyShapesComponents.setCollideWithMaskBits(mEntity, collideWithMaskBits);
int broadPhaseId = mBody->mWorld.mProxyShapesComponents.getBroadPhaseId(mEntity);
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
index ecf69ee1..7b4831fc 100755
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.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 CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
- bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) {
+ bool reportContacts, MemoryAllocator& memoryAllocator) {
bool isCollisionFound = false;
@@ -154,9 +154,6 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
continue;
}
}
@@ -234,9 +231,6 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBat
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
}
}
diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
index 32f76abe..d8b69bca 100644
--- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
@@ -68,8 +68,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
/// Compute the narrow-phase collision detection between two capsules
bool testCollision(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
- uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
- MemoryAllocator& memoryAllocator);
+ uint batchNbItems, bool reportContacts, MemoryAllocator& memoryAllocator);
};
}
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
index d425b80e..f42e8934 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
@@ -41,14 +41,14 @@ using namespace reactphysics3d;
// by Dirk Gregorius.
bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
uint batchStartIndex, uint batchNbItems,
- bool reportContacts, bool stopFirstContactFound,
+ bool reportContacts, bool clipWithPreviousAxisIfStillColliding,
MemoryAllocator& memoryAllocator) {
bool isCollisionFound = false;
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
- SATAlgorithm satAlgorithm(memoryAllocator);
+ SATAlgorithm satAlgorithm(clipWithPreviousAxisIfStillColliding, memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
@@ -166,9 +166,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
// Colision found
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
continue;
}
@@ -183,9 +180,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& nar
if (narrowPhaseInfoBatch.isColliding[batchIndex]) {
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
}
}
}
diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
index 70f223b5..53b5ca56 100644
--- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
@@ -71,8 +71,8 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
/// Compute the narrow-phase collision detection between a capsule and a polyhedron
bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
- uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
- MemoryAllocator& memoryAllocator);
+ uint batchNbItems, bool reportContacts, bool clipWithPreviousAxisIfStillColliding,
+ MemoryAllocator& memoryAllocator);
};
}
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
index 3ed15c74..570345fe 100644
--- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
@@ -37,11 +37,10 @@ using namespace reactphysics3d;
// by Dirk Gregorius.
bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
uint batchStartIndex, uint batchNbItems,
- bool reportContacts, bool stopFirstContactFound,
- MemoryAllocator& memoryAllocator) {
+ bool reportContacts, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator) {
// Run the SAT algorithm to find the separating axis and compute contact point
- SATAlgorithm satAlgorithm(memoryAllocator);
+ SATAlgorithm satAlgorithm(clipWithPreviousAxisIfStillColliding, memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
@@ -50,7 +49,7 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoB
#endif
bool isCollisionFound = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfoBatch, batchStartIndex,
- batchNbItems, reportContacts, stopFirstContactFound);
+ batchNbItems, reportContacts);
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
@@ -59,10 +58,6 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoB
lastFrameCollisionInfo->wasUsingSAT = true;
lastFrameCollisionInfo->wasUsingGJK = false;
-
- if (isCollisionFound && stopFirstContactFound) {
- return isCollisionFound;
- }
}
return isCollisionFound;
diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
index 7f9fccd2..6e43e76b 100644
--- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
@@ -65,9 +65,8 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm
ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two convex polyhedra
- bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
- uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
- MemoryAllocator& memoryAllocator);
+ bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts,
+ bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator);
};
}
diff --git a/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp b/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp
index 61b2fb5b..ed989870 100644
--- a/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp
+++ b/src/collision/narrowphase/NarrowPhaseInfoBatch.cpp
@@ -28,6 +28,7 @@
#include "collision/ContactPointInfo.h"
#include "collision/shapes/TriangleShape.h"
#include "engine/OverlappingPair.h"
+#include <iostream>
using namespace reactphysics3d;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
index 3c467ea3..d8d18876 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp
@@ -43,7 +43,8 @@ using namespace reactphysics3d;
const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
// Constructor
-SATAlgorithm::SATAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) {
+SATAlgorithm::SATAlgorithm(bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator)
+ : mClipWithPreviousAxisIfStillColliding(clipWithPreviousAxisIfStillColliding), mMemoryAllocator(memoryAllocator) {
#ifdef IS_PROFILING_ACTIVE
mProfiler = nullptr;
@@ -54,7 +55,7 @@ SATAlgorithm::SATAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator(
// Test collision between a sphere and a convex mesh
bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
uint batchStartIndex, uint batchNbItems,
- bool reportContacts, bool stopFirstContactFound) const {
+ bool reportContacts) const {
bool isCollisionFound = false;
@@ -136,9 +137,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& n
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
}
return isCollisionFound;
@@ -476,7 +474,7 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
}
// Test collision between two convex polyhedrons
-bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts, bool stopFirstContactFound) const {
+bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts) const {
RP3D_PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()", mProfiler);
@@ -528,7 +526,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
}
// The two shapes were overlapping in the previous frame and still seem to overlap in this one
- if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
+ if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0)) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
@@ -548,9 +546,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// Therefore, we can return without running the whole SAT algorithm
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
continue;
}
@@ -571,7 +566,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
}
// The two shapes were overlapping in the previous frame and still seem to overlap in this one
- if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
+ if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0)) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
@@ -591,9 +586,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// Therefore, we can return without running the whole SAT algorithm
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
continue;
}
@@ -632,7 +624,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
}
// If the shapes were overlapping on the previous axis and still seem to overlap in this frame
- if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
+ if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0)) {
// Compute the closest points between the two edges (in the local-space of poylhedron 2)
Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
@@ -683,9 +675,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// we return without running the whole SAT algorithm
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
continue;
}
@@ -878,9 +867,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
}
return isCollisionFound;
diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h
index 1eb1b923..2d7afd4a 100644
--- a/src/collision/narrowphase/SAT/SATAlgorithm.h
+++ b/src/collision/narrowphase/SAT/SATAlgorithm.h
@@ -60,6 +60,20 @@ class SATAlgorithm {
/// make sure the contact manifold does not change too much between frames.
static const decimal SAME_SEPARATING_AXIS_BIAS;
+ /// True means that if two shapes were colliding last time (previous frame) and are still colliding
+ /// we use the previous (minimum penetration depth) axis to clip the colliding features and we don't
+ /// recompute a new (minimum penetration depth) axis. This value must be true for a dynamic simulation
+ /// because it uses temporal coherence and clip the colliding features with the previous
+ /// axis (this is good for stability). However, when we use the testCollision() methods, the penetration
+ /// depths might be very large and we always want the current true axis with minimum penetration depth.
+ /// In this case, this value must be set to false. Consider the following situation. Two shapes start overlaping
+ /// with "x" being the axis of minimum penetration depth. Then, if the shapes move but are still penetrating,
+ /// it is possible that the axis of minimum penetration depth changes for axis "y" for instance. If this value
+ /// if true, we will always use the axis of the previous collision test and therefore always report that the
+ /// penetrating axis is "x" even if it has changed to axis "y" during the collision. This is not what we want
+ /// when we call the testCollision() methods.
+ bool mClipWithPreviousAxisIfStillColliding;
+
/// Memory allocator
MemoryAllocator& mMemoryAllocator;
@@ -126,7 +140,7 @@ class SATAlgorithm {
// -------------------- Methods -------------------- //
/// Constructor
- SATAlgorithm(MemoryAllocator& memoryAllocator);
+ SATAlgorithm(bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator);
/// Destructor
~SATAlgorithm() = default;
@@ -140,7 +154,7 @@ class SATAlgorithm {
/// Test collision between a sphere and a convex mesh
bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
uint batchStartIndex, uint batchNbItems,
- bool reportContacts, bool stopFirstContactFound) const;
+ bool reportContacts) const;
/// Test collision between a capsule and a convex mesh
bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex, bool reportContacts) const;
@@ -158,7 +172,7 @@ class SATAlgorithm {
/// Test collision between two convex meshes
bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
- uint batchNbItems, bool reportContacts, bool stopFirstContactFound) const;
+ uint batchNbItems, bool reportContacts) const;
#ifdef IS_PROFILING_ACTIVE
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
index 68a02509..3abb1f09 100755
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
@@ -36,7 +36,7 @@ using namespace reactphysics3d;
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
- bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) {
+ bool reportContacts, MemoryAllocator& memoryAllocator) {
bool isCollisionFound = false;
@@ -137,9 +137,6 @@ bool SphereVsCapsuleAlgorithm::testCollision(SphereVsCapsuleNarrowPhaseInfoBatch
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
continue;
}
}
diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
index 7394bfb2..25926fce 100644
--- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
@@ -68,8 +68,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
/// Compute the narrow-phase collision detection between a sphere and a capsule
bool testCollision(SphereVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
- uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
- MemoryAllocator& memoryAllocator);
+ uint batchNbItems, bool reportContacts, MemoryAllocator& memoryAllocator);
};
}
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
index a8e638be..b8fb779f 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
@@ -36,7 +36,7 @@ using namespace reactphysics3d;
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
- bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) {
+ bool reportContacts, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator) {
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
@@ -73,9 +73,6 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr
// Return true
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
continue;
}
@@ -83,7 +80,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr
if (gjkResults[batchIndex] == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
- SATAlgorithm satAlgorithm(memoryAllocator);
+ SATAlgorithm satAlgorithm(clipWithPreviousAxisIfStillColliding, memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
@@ -91,15 +88,11 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfoBatch& narr
#endif
- isCollisionFound |= satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, 1, reportContacts, stopFirstContactFound);
+ isCollisionFound |= satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfoBatch, batchIndex, 1, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true;
- if (isCollisionFound && stopFirstContactFound) {
- return isCollisionFound;
- }
-
continue;
}
}
diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
index 040caec6..793dc48a 100644
--- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
@@ -71,9 +71,8 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
- bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
- uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
- MemoryAllocator& memoryAllocator);
+ bool testCollision(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems, bool reportContacts,
+ bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator);
};
}
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
index 52bf5d96..a6d5a61d 100755
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
@@ -32,7 +32,7 @@
using namespace reactphysics3d;
bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
- bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) {
+ bool reportContacts, MemoryAllocator& memoryAllocator) {
bool isCollisionFound = false;
@@ -94,9 +94,6 @@ bool SphereVsSphereAlgorithm::testCollision(SphereVsSphereNarrowPhaseInfoBatch&
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
isCollisionFound = true;
- if (stopFirstContactFound) {
- return isCollisionFound;
- }
}
}
diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
index e0f4c5be..5e9554b4 100644
--- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h
+++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h
@@ -67,8 +67,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
/// Compute a contact info if the two bounding volume collide
bool testCollision(SphereVsSphereNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex,
- uint batchNbItems, bool reportContacts, bool stopFirstContactFound,
- MemoryAllocator& memoryAllocator);
+ uint batchNbItems, bool reportContacts, MemoryAllocator& memoryAllocator);
};
}
diff --git a/src/components/Components.h b/src/components/Components.h
index e4dc213e..320104a3 100644
--- a/src/components/Components.h
+++ b/src/components/Components.h
@@ -121,6 +121,9 @@ class Components {
/// Return the number of enabled components
uint32 getNbEnabledComponents() const;
+
+ /// Return the index in the arrays for a given entity
+ uint32 getEntityIndex(Entity entity) const;
};
// Return true if an entity is sleeping
@@ -144,6 +147,11 @@ inline uint32 Components::getNbEnabledComponents() const {
return mDisabledStartIndex;
}
+// Return the index in the arrays for a given entity
+inline uint32 Components::getEntityIndex(Entity entity) const {
+ assert(hasComponent(entity));
+ return mMapEntityToComponentIndex[entity];
+}
}
#endif
diff --git a/src/components/DynamicsComponents.cpp b/src/components/DynamicsComponents.cpp
index e23b3cfb..8350ae2f 100644
--- a/src/components/DynamicsComponents.cpp
+++ b/src/components/DynamicsComponents.cpp
@@ -35,7 +35,11 @@ using namespace reactphysics3d;
// Constructor
DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator)
- :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3)) {
+ :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
+ sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) +
+ sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) + sizeof(Matrix3x3) +
+ sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + sizeof(bool) +
+ sizeof(bool)) {
// Allocate memory for the components data
allocate(INIT_NB_ALLOCATED_COMPONENTS);
@@ -57,6 +61,24 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
Entity* newBodies = static_cast<Entity*>(newBuffer);
Vector3* newLinearVelocities = reinterpret_cast<Vector3*>(newBodies + nbComponentsToAllocate);
Vector3* newAngularVelocities = reinterpret_cast<Vector3*>(newLinearVelocities + nbComponentsToAllocate);
+ Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newAngularVelocities + nbComponentsToAllocate);
+ Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
+ Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
+ Vector3* newSplitAngularVelocities = reinterpret_cast<Vector3*>(newSplitLinearVelocities + nbComponentsToAllocate);
+ Vector3* newExternalForces = reinterpret_cast<Vector3*>(newSplitAngularVelocities + nbComponentsToAllocate);
+ Vector3* newExternalTorques = reinterpret_cast<Vector3*>(newExternalForces + nbComponentsToAllocate);
+ decimal* newLinearDampings = reinterpret_cast<decimal*>(newExternalTorques + nbComponentsToAllocate);
+ decimal* newAngularDampings = reinterpret_cast<decimal*>(newLinearDampings + nbComponentsToAllocate);
+ decimal* newInitMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate);
+ decimal* newInverseMasses = reinterpret_cast<decimal*>(newInitMasses + nbComponentsToAllocate);
+ Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate);
+ Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
+ Vector3* newConstrainedPositions = reinterpret_cast<Vector3*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
+ Quaternion* newConstrainedOrientations = reinterpret_cast<Quaternion*>(newConstrainedPositions + nbComponentsToAllocate);
+ Vector3* newCentersOfMassLocal = reinterpret_cast<Vector3*>(newConstrainedOrientations + nbComponentsToAllocate);
+ Vector3* newCentersOfMassWorld = reinterpret_cast<Vector3*>(newCentersOfMassLocal + nbComponentsToAllocate);
+ bool* newIsGravityEnabled = reinterpret_cast<bool*>(newCentersOfMassWorld + nbComponentsToAllocate);
+ bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newIsGravityEnabled + nbComponentsToAllocate);
// If there was already components before
if (mNbComponents > 0) {
@@ -65,6 +87,24 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
memcpy(newBodies, mBodies, mNbComponents * sizeof(Entity));
memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Vector3));
memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Vector3));
+ memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3));
+ memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3));
+ memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3));
+ memcpy(newSplitAngularVelocities, mSplitAngularVelocities, mNbComponents * sizeof(Vector3));
+ memcpy(newExternalForces, mExternalForces, mNbComponents * sizeof(Vector3));
+ memcpy(newExternalTorques, mExternalTorques, mNbComponents * sizeof(Vector3));
+ memcpy(newLinearDampings, mLinearDampings, mNbComponents * sizeof(decimal));
+ memcpy(newAngularDampings, mAngularDampings, mNbComponents * sizeof(decimal));
+ memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal));
+ memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
+ memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3));
+ memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3));
+ memcpy(newConstrainedPositions, mConstrainedPositions, mNbComponents * sizeof(Vector3));
+ memcpy(newConstrainedOrientations, mConstrainedOrientations, mNbComponents * sizeof(Quaternion));
+ memcpy(newCentersOfMassLocal, mCentersOfMassLocal, mNbComponents * sizeof(Vector3));
+ memcpy(newCentersOfMassWorld, mCentersOfMassWorld, mNbComponents * sizeof(Vector3));
+ memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool));
+ memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
// Deallocate previous memory
mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
@@ -74,6 +114,24 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
mBodies = newBodies;
mLinearVelocities = newLinearVelocities;
mAngularVelocities = newAngularVelocities;
+ mConstrainedLinearVelocities = newConstrainedLinearVelocities;
+ mConstrainedAngularVelocities = newConstrainedAngularVelocities;
+ mSplitLinearVelocities = newSplitLinearVelocities;
+ mSplitAngularVelocities = newSplitAngularVelocities;
+ mExternalForces = newExternalForces;
+ mExternalTorques = newExternalTorques;
+ mLinearDampings = newLinearDampings;
+ mAngularDampings = newAngularDampings;
+ mInitMasses = newInitMasses;
+ mInverseMasses = newInverseMasses;
+ mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses;
+ mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses;
+ mConstrainedPositions = newConstrainedPositions;
+ mConstrainedOrientations = newConstrainedOrientations;
+ mCentersOfMassLocal = newCentersOfMassLocal;
+ mCentersOfMassWorld = newCentersOfMassWorld;
+ mIsGravityEnabled = newIsGravityEnabled;
+ mIsAlreadyInIsland = newIsAlreadyInIsland;
mNbAllocatedComponents = nbComponentsToAllocate;
}
@@ -85,8 +143,26 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
// Insert the new component data
new (mBodies + index) Entity(bodyEntity);
- new (mLinearVelocities + index) Vector3(component.linearVelocity);
- new (mAngularVelocities + index) Vector3(component.angularVelocity);
+ new (mLinearVelocities + index) Vector3(0, 0, 0);
+ new (mAngularVelocities + index) Vector3(0, 0, 0);
+ new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0);
+ new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0);
+ new (mSplitLinearVelocities + index) Vector3(0, 0, 0);
+ new (mSplitAngularVelocities + index) Vector3(0, 0, 0);
+ new (mExternalForces + index) Vector3(0, 0, 0);
+ new (mExternalTorques + index) Vector3(0, 0, 0);
+ mLinearDampings[index] = decimal(0.0);
+ mAngularDampings[index] = decimal(0.0);
+ mInitMasses[index] = decimal(1.0);
+ mInverseMasses[index] = decimal(1.0);
+ new (mInverseInertiaTensorsLocal + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
+ new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
+ new (mConstrainedPositions + index) Vector3(0, 0, 0);
+ new (mConstrainedOrientations + index) Quaternion(0, 0, 0, 1);
+ new (mCentersOfMassLocal + index) Vector3(0, 0, 0);
+ new (mCentersOfMassWorld + index) Vector3(component.worldPosition);
+ mIsGravityEnabled[index] = true;
+ mIsAlreadyInIsland[index] = false;
// Map the entity with the new component lookup index
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(bodyEntity, index));
@@ -107,6 +183,24 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
new (mBodies + destIndex) Entity(mBodies[srcIndex]);
new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]);
new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]);
+ new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]);
+ new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]);
+ new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]);
+ new (mSplitAngularVelocities + destIndex) Vector3(mSplitAngularVelocities[srcIndex]);
+ new (mExternalForces + destIndex) Vector3(mExternalForces[srcIndex]);
+ new (mExternalTorques + destIndex) Vector3(mExternalTorques[srcIndex]);
+ mLinearDampings[destIndex] = mLinearDampings[srcIndex];
+ mAngularDampings[destIndex] = mAngularDampings[srcIndex];
+ mInitMasses[destIndex] = mInitMasses[srcIndex];
+ mInverseMasses[destIndex] = mInverseMasses[srcIndex];
+ new (mInverseInertiaTensorsLocal + destIndex) Matrix3x3(mInverseInertiaTensorsLocal[srcIndex]);
+ new (mInverseInertiaTensorsWorld + destIndex) Matrix3x3(mInverseInertiaTensorsWorld[srcIndex]);
+ new (mConstrainedPositions + destIndex) Vector3(mConstrainedPositions[srcIndex]);
+ new (mConstrainedOrientations + destIndex) Quaternion(mConstrainedOrientations[srcIndex]);
+ new (mCentersOfMassLocal + destIndex) Vector3(mCentersOfMassLocal[srcIndex]);
+ new (mCentersOfMassWorld + destIndex) Vector3(mCentersOfMassWorld[srcIndex]);
+ mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex];
+ mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
// Destroy the source component
destroyComponent(srcIndex);
@@ -129,6 +223,24 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
Entity entity1(mBodies[index1]);
Vector3 linearVelocity1(mLinearVelocities[index1]);
Vector3 angularVelocity1(mAngularVelocities[index1]);
+ Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]);
+ Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]);
+ Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]);
+ Vector3 splitAngularVelocity1(mSplitAngularVelocities[index1]);
+ Vector3 externalForce1(mExternalForces[index1]);
+ Vector3 externalTorque1(mExternalTorques[index1]);
+ decimal linearDamping1 = mLinearDampings[index1];
+ decimal angularDamping1 = mAngularDampings[index1];
+ decimal initMass1 = mInitMasses[index1];
+ decimal inverseMass1 = mInverseMasses[index1];
+ Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
+ Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1];
+ Vector3 constrainedPosition1 = mConstrainedPositions[index1];
+ Quaternion constrainedOrientation1 = mConstrainedOrientations[index1];
+ Vector3 centerOfMassLocal1 = mCentersOfMassLocal[index1];
+ Vector3 centerOfMassWorld1 = mCentersOfMassWorld[index1];
+ bool isGravityEnabled1 = mIsGravityEnabled[index1];
+ bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
// Destroy component 1
destroyComponent(index1);
@@ -139,6 +251,24 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
new (mBodies + index2) Entity(entity1);
new (mLinearVelocities + index2) Vector3(linearVelocity1);
new (mAngularVelocities + index2) Vector3(angularVelocity1);
+ new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1);
+ new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1);
+ new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1);
+ new (mSplitAngularVelocities + index2) Vector3(splitAngularVelocity1);
+ new (mExternalForces + index2) Vector3(externalForce1);
+ new (mExternalTorques + index2) Vector3(externalTorque1);
+ mLinearDampings[index2] = linearDamping1;
+ mAngularDampings[index2] = angularDamping1;
+ mInitMasses[index2] = initMass1;
+ mInverseMasses[index2] = inverseMass1;
+ mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1;
+ mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1;
+ mConstrainedPositions[index2] = constrainedPosition1;
+ mConstrainedOrientations[index2] = constrainedOrientation1;
+ mCentersOfMassLocal[index2] = centerOfMassLocal1;
+ mCentersOfMassWorld[index2] = centerOfMassWorld1;
+ mIsGravityEnabled[index2] = isGravityEnabled1;
+ mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
// Update the entity to component index mapping
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity1, index2));
@@ -160,4 +290,16 @@ void DynamicsComponents::destroyComponent(uint32 index) {
mBodies[index].~Entity();
mLinearVelocities[index].~Vector3();
mAngularVelocities[index].~Vector3();
+ mConstrainedLinearVelocities[index].~Vector3();
+ mConstrainedAngularVelocities[index].~Vector3();
+ mSplitLinearVelocities[index].~Vector3();
+ mSplitAngularVelocities[index].~Vector3();
+ mExternalForces[index].~Vector3();
+ mExternalTorques[index].~Vector3();
+ mInverseInertiaTensorsLocal[index].~Matrix3x3();
+ mInverseInertiaTensorsWorld[index].~Matrix3x3();
+ mConstrainedPositions[index].~Vector3();
+ mConstrainedOrientations[index].~Quaternion();
+ mCentersOfMassLocal[index].~Vector3();
+ mCentersOfMassWorld[index].~Vector3();
}
diff --git a/src/components/DynamicsComponents.h b/src/components/DynamicsComponents.h
index 7c81e61b..8cbe2135 100644
--- a/src/components/DynamicsComponents.h
+++ b/src/components/DynamicsComponents.h
@@ -30,6 +30,7 @@
#include "mathematics/Transform.h"
#include "engine/Entity.h"
#include "components/Components.h"
+#include "mathematics/Matrix3x3.h"
#include "containers/Map.h"
// ReactPhysics3D namespace
@@ -59,6 +60,60 @@ class DynamicsComponents : public Components {
/// Array with the angular velocity of each component
Vector3* mAngularVelocities;
+ /// Array with the constrained linear velocity of each component
+ Vector3* mConstrainedLinearVelocities;
+
+ /// Array with the constrained angular velocity of each component
+ Vector3* mConstrainedAngularVelocities;
+
+ /// Array with the split linear velocity of each component
+ Vector3* mSplitLinearVelocities;
+
+ /// Array with the split angular velocity of each component
+ Vector3* mSplitAngularVelocities;
+
+ /// Array with the external force of each component
+ Vector3* mExternalForces;
+
+ /// Array with the external torque of each component
+ Vector3* mExternalTorques;
+
+ /// Array with the linear damping factor of each component
+ decimal* mLinearDampings;
+
+ /// Array with the angular damping factor of each component
+ decimal* mAngularDampings;
+
+ /// Array with the initial mass of each component
+ decimal* mInitMasses;
+
+ /// Array with the inverse mass of each component
+ decimal* mInverseMasses;
+
+ /// Array with the inverse of the inertia tensor of each component
+ Matrix3x3* mInverseInertiaTensorsLocal;
+
+ /// Array with the inverse of the world inertia tensor of each component
+ Matrix3x3* mInverseInertiaTensorsWorld;
+
+ /// Array with the constrained position of each component (for position error correction)
+ Vector3* mConstrainedPositions;
+
+ /// Array of constrained orientation for each component (for position error correction)
+ Quaternion* mConstrainedOrientations;
+
+ /// Array of center of mass of each component (in local-space coordinates)
+ Vector3* mCentersOfMassLocal;
+
+ /// Array of center of mass of each component (in world-space coordinates)
+ Vector3* mCentersOfMassWorld;
+
+ /// True if the gravity needs to be applied to this component
+ bool* mIsGravityEnabled;
+
+ /// Array with the boolean value to know if the body has already been added into an island
+ bool* mIsAlreadyInIsland;
+
// -------------------- Methods -------------------- //
/// Allocate memory for a given number of components
@@ -78,12 +133,11 @@ class DynamicsComponents : public Components {
/// Structure for the data of a transform component
struct DynamicsComponent {
- const Vector3& linearVelocity;
- const Vector3& angularVelocity;
+ const Vector3& worldPosition;
/// Constructor
- DynamicsComponent(const Vector3& linearVelocity, const Vector3& angularVelocity)
- : linearVelocity(linearVelocity), angularVelocity(angularVelocity) {
+ DynamicsComponent(const Vector3& worldPosition)
+ : worldPosition(worldPosition) {
}
};
@@ -100,10 +154,64 @@ class DynamicsComponents : public Components {
void addComponent(Entity bodyEntity, bool isSleeping, const DynamicsComponent& component);
/// Return the linear velocity of an entity
- Vector3& getLinearVelocity(Entity bodyEntity) const;
+ const Vector3& getLinearVelocity(Entity bodyEntity) const;
/// Return the angular velocity of an entity
- Vector3& getAngularVelocity(Entity bodyEntity) const;
+ const Vector3& getAngularVelocity(Entity bodyEntity) const;
+
+ /// Return the constrained linear velocity of an entity
+ const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const;
+
+ /// Return the constrained angular velocity of an entity
+ const Vector3& getConstrainedAngularVelocity(Entity bodyEntity) const;
+
+ /// Return the split linear velocity of an entity
+ const Vector3& getSplitLinearVelocity(Entity bodyEntity) const;
+
+ /// Return the split angular velocity of an entity
+ const Vector3& getSplitAngularVelocity(Entity bodyEntity) const;
+
+ /// Return the external force of an entity
+ const Vector3& getExternalForce(Entity bodyEntity) const;
+
+ /// Return the external torque of an entity
+ const Vector3& getExternalTorque(Entity bodyEntity) const;
+
+ /// Return the linear damping factor of an entity
+ decimal getLinearDamping(Entity bodyEntity) const;
+
+ /// Return the angular damping factor of an entity
+ decimal getAngularDamping(Entity bodyEntity) const;
+
+ /// Return the initial mass of an entity
+ decimal getInitMass(Entity bodyEntity) const;
+
+ /// Return the mass inverse of an entity
+ decimal getMassInverse(Entity bodyEntity) const;
+
+ /// Return the inverse local inertia tensor of an entity
+ const Matrix3x3& getInertiaTensorLocalInverse(Entity bodyEntity);
+
+ /// Return the inverse world inertia tensor of an entity
+ const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity);
+
+ /// Return the constrained position of an entity
+ const Vector3& getConstrainedPosition(Entity bodyEntity);
+
+ /// Return the constrained orientation of an entity
+ const Quaternion& getConstrainedOrientation(Entity bodyEntity);
+
+ /// Return the local center of mass of an entity
+ const Vector3& getCenterOfMassLocal(Entity bodyEntity);
+
+ /// Return the world center of mass of an entity
+ const Vector3& getCenterOfMassWorld(Entity bodyEntity);
+
+ /// Return true if gravity is enabled for this entity
+ bool getIsGravityEnabled(Entity bodyEntity) const;
+
+ /// Return true if the entity is already in an island
+ bool getIsAlreadyInIsland(Entity bodyEntity) const;
/// Set the linear velocity of an entity
void setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity);
@@ -111,13 +219,74 @@ class DynamicsComponents : public Components {
/// Set the angular velocity of an entity
void setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity);
+ /// Set the constrained linear velocity of an entity
+ void setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity);
+
+ /// Set the constrained angular velocity of an entity
+ void setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity);
+
+ /// Set the split linear velocity of an entity
+ void setSplitLinearVelocity(Entity bodyEntity, const Vector3& splitLinearVelocity);
+
+ /// Set the split angular velocity of an entity
+ void setSplitAngularVelocity(Entity bodyEntity, const Vector3& splitAngularVelocity);
+
+ /// Set the external force of an entity
+ void setExternalForce(Entity bodyEntity, const Vector3& externalForce);
+
+ /// Set the external force of an entity
+ void setExternalTorque(Entity bodyEntity, const Vector3& externalTorque);
+
+ /// Set the linear damping factor of an entity
+ void setLinearDamping(Entity bodyEntity, decimal linearDamping);
+
+ /// Set the angular damping factor of an entity
+ void setAngularDamping(Entity bodyEntity, decimal angularDamping);
+
+ /// Set the initial mass of an entity
+ void setInitMass(Entity bodyEntity, decimal initMass);
+
+ /// Set the inverse mass of an entity
+ void setMassInverse(Entity bodyEntity, decimal inverseMass);
+
+ /// Set the inverse local inertia tensor of an entity
+ void setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse);
+
+ /// Set the inverse world inertia tensor of an entity
+ void setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse);
+
+ /// Set the constrained position of an entity
+ void setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition);
+
+ /// Set the constrained orientation of an entity
+ void setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation);
+
+ /// Set the local center of mass of an entity
+ void setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal);
+
+ /// Set the world center of mass of an entity
+ void setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld);
+
+ /// Set the value to know if the gravity is enabled for this entity
+ bool setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled);
+
+ /// Set the value to know if the entity is already in an island
+ bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland);
+
// -------------------- Friendship -------------------- //
friend class BroadPhaseSystem;
+ friend class DynamicsWorld;
+ friend class ContactSolver;
+ friend class BallAndSocketJoint;
+ friend class FixedJoint;
+ friend class HingeJoint;
+ friend class SliderJoint;
+
};
// Return the linear velocity of an entity
-inline Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const {
+inline const Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
@@ -125,7 +294,7 @@ inline Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const {
}
// Return the angular velocity of an entity
-inline Vector3& DynamicsComponents::getAngularVelocity(Entity bodyEntity) const {
+inline const Vector3 &DynamicsComponents::getAngularVelocity(Entity bodyEntity) const {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
@@ -148,6 +317,294 @@ inline void DynamicsComponents::setAngularVelocity(Entity bodyEntity, const Vect
mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = angularVelocity;
}
+// Return the constrained linear velocity of an entity
+inline const Vector3& DynamicsComponents::getConstrainedLinearVelocity(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the constrained angular velocity of an entity
+inline const Vector3& DynamicsComponents::getConstrainedAngularVelocity(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the split linear velocity of an entity
+inline const Vector3& DynamicsComponents::getSplitLinearVelocity(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mSplitLinearVelocities[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the split angular velocity of an entity
+inline const Vector3& DynamicsComponents::getSplitAngularVelocity(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mSplitAngularVelocities[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the external force of an entity
+inline const Vector3& DynamicsComponents::getExternalForce(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mExternalForces[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the external torque of an entity
+inline const Vector3& DynamicsComponents::getExternalTorque(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mExternalTorques[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the linear damping factor of an entity
+inline decimal DynamicsComponents::getLinearDamping(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mLinearDampings[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the angular damping factor of an entity
+inline decimal DynamicsComponents::getAngularDamping(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mAngularDampings[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the initial mass of an entity
+inline decimal DynamicsComponents::getInitMass(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mInitMasses[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the inverse mass of an entity
+inline decimal DynamicsComponents::getMassInverse(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mInverseMasses[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the inverse local inertia tensor of an entity
+inline const Matrix3x3& DynamicsComponents::getInertiaTensorLocalInverse(Entity bodyEntity) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the inverse world inertia tensor of an entity
+inline const Matrix3x3& DynamicsComponents::getInertiaTensorWorldInverse(Entity bodyEntity) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the constrained position of an entity
+inline const Vector3& DynamicsComponents::getConstrainedPosition(Entity bodyEntity) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the constrained orientation of an entity
+inline const Quaternion& DynamicsComponents::getConstrainedOrientation(Entity bodyEntity) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the local center of mass of an entity
+inline const Vector3& DynamicsComponents::getCenterOfMassLocal(Entity bodyEntity) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mCentersOfMassLocal[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the world center of mass of an entity
+inline const Vector3& DynamicsComponents::getCenterOfMassWorld(Entity bodyEntity) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mCentersOfMassWorld[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Set the constrained linear velocity of an entity
+inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedLinearVelocity;
+}
+
+// Set the constrained angular velocity of an entity
+inline void DynamicsComponents::setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedAngularVelocity;
+}
+
+// Set the split linear velocity of an entity
+inline void DynamicsComponents::setSplitLinearVelocity(Entity bodyEntity, const Vector3& splitLinearVelocity) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mSplitLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = splitLinearVelocity;
+}
+
+// Set the split angular velocity of an entity
+inline void DynamicsComponents::setSplitAngularVelocity(Entity bodyEntity, const Vector3& splitAngularVelocity) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mSplitAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = splitAngularVelocity;
+}
+
+// Set the external force of an entity
+inline void DynamicsComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mExternalForces[mMapEntityToComponentIndex[bodyEntity]] = externalForce;
+}
+
+// Set the external force of an entity
+inline void DynamicsComponents::setExternalTorque(Entity bodyEntity, const Vector3& externalTorque) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mExternalTorques[mMapEntityToComponentIndex[bodyEntity]] = externalTorque;
+}
+
+// Set the linear damping factor of an entity
+inline void DynamicsComponents::setLinearDamping(Entity bodyEntity, decimal linearDamping) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mLinearDampings[mMapEntityToComponentIndex[bodyEntity]] = linearDamping;
+}
+
+// Set the angular damping factor of an entity
+inline void DynamicsComponents::setAngularDamping(Entity bodyEntity, decimal angularDamping) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mAngularDampings[mMapEntityToComponentIndex[bodyEntity]] = angularDamping;
+}
+
+// Set the initial mass of an entity
+inline void DynamicsComponents::setInitMass(Entity bodyEntity, decimal initMass) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mInitMasses[mMapEntityToComponentIndex[bodyEntity]] = initMass;
+}
+
+// Set the mass inverse of an entity
+inline void DynamicsComponents::setMassInverse(Entity bodyEntity, decimal inverseMass) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mInverseMasses[mMapEntityToComponentIndex[bodyEntity]] = inverseMass;
+}
+
+// Set the inverse local inertia tensor of an entity
+inline void DynamicsComponents::setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorLocalInverse;
+}
+
+// Set the inverse world inertia tensor of an entity
+inline void DynamicsComponents::setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorWorldInverse;
+}
+
+// Set the constrained position of an entity
+inline void DynamicsComponents::setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]] = constrainedPosition;
+}
+
+// Set the constrained orientation of an entity
+inline void DynamicsComponents::setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]] = constrainedOrientation;
+}
+
+// Set the local center of mass of an entity
+inline void DynamicsComponents::setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mCentersOfMassLocal[mMapEntityToComponentIndex[bodyEntity]] = centerOfMassLocal;
+}
+
+// Set the world center of mass of an entity
+inline void DynamicsComponents::setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mCentersOfMassWorld[mMapEntityToComponentIndex[bodyEntity]] = centerOfMassWorld;
+}
+
+// Return true if gravity is enabled for this entity
+inline bool DynamicsComponents::getIsGravityEnabled(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return true if the entity is already in an island
+inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ return mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Set the value to know if the gravity is enabled for this entity
+inline bool DynamicsComponents::setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]] = isGravityEnabled;
+}
+
+/// Set the value to know if the entity is already in an island
+inline bool DynamicsComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) {
+
+ assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+ mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]] = isAlreadyInIsland;
+}
+
}
#endif
diff --git a/src/configuration.h b/src/configuration.h
index 2686635a..3dae1e18 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -129,7 +129,7 @@ struct WorldSettings {
decimal defaultBounciness = decimal(0.5);
/// Velocity threshold for contact velocity restitution
- decimal restitutionVelocityThreshold = decimal(1.0);
+ decimal restitutionVelocityThreshold = decimal(0.5);
/// Default rolling resistance
decimal defaultRollingRestistance = decimal(0.0);
@@ -154,13 +154,8 @@ struct WorldSettings {
/// might enter sleeping mode
decimal defaultSleepAngularVelocity = decimal(3.0) * (PI / decimal(180.0));
- /// Maximum number of contact manifolds in an overlapping pair that involves two
- /// convex collision shapes.
- int nbMaxContactManifoldsConvexShape = 1;
-
- /// Maximum number of contact manifolds in an overlapping pair that involves at
- /// least one concave collision shape.
- int nbMaxContactManifoldsConcaveShape = 3;
+ /// Maximum number of contact manifolds in an overlapping pair
+ uint nbMaxContactManifolds = 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
@@ -184,8 +179,7 @@ struct WorldSettings {
ss << "defaultTimeBeforeSleep=" << defaultTimeBeforeSleep << std::endl;
ss << "defaultSleepLinearVelocity=" << defaultSleepLinearVelocity << std::endl;
ss << "defaultSleepAngularVelocity=" << defaultSleepAngularVelocity << std::endl;
- ss << "nbMaxContactManifoldsConvexShape=" << nbMaxContactManifoldsConvexShape << std::endl;
- ss << "nbMaxContactManifoldsConcaveShape=" << nbMaxContactManifoldsConcaveShape << std::endl;
+ ss << "nbMaxContactManifolds=" << nbMaxContactManifolds << std::endl;
ss << "cosAngleSimilarContactManifold=" << cosAngleSimilarContactManifold << std::endl;
return ss.str();
diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp
index 0da68185..b795560d 100644
--- a/src/constraint/BallAndSocketJoint.cpp
+++ b/src/constraint/BallAndSocketJoint.cpp
@@ -26,6 +26,7 @@
// Libraries
#include "BallAndSocketJoint.h"
#include "engine/ConstraintSolver.h"
+#include "components/DynamicsComponents.h"
using namespace reactphysics3d;
@@ -44,13 +45,9 @@ BallAndSocketJoint::BallAndSocketJoint(uint id, const BallAndSocketJointInfo& jo
// Initialize before solving the constraint
void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
- // Initialize the bodies index in the velocity array
- mIndexBody1 = mBody1->mArrayIndex;
- mIndexBody2 = mBody2->mArrayIndex;
-
// Get the bodies center of mass and orientations
- const Vector3& x1 = mBody1->mCenterOfMassWorld;
- const Vector3& x2 = mBody2->mCenterOfMassWorld;
+ const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
+ const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
@@ -67,7 +64,9 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix)
- decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+ decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
+ decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
+ decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@@ -98,36 +97,42 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
+ uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+ uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
// Get the velocities
- Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
- Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
- Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
- Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+ Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
+ Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
+ Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
+ Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Compute the impulse P=J^T * lambda for the body 1
const Vector3 linearImpulseBody1 = -mImpulse;
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
// Apply the impulse to the body 1
- v1 += mBody1->mMassInverse * linearImpulseBody1;
+ v1 += constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity) * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
// Apply the impulse to the body to the body 2
- v2 += mBody2->mMassInverse * mImpulse;
+ v2 += constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity) * mImpulse;
w2 += mI2 * angularImpulseBody2;
}
// Solve the velocity constraint
void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
+ uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+ uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
// Get the velocities
- Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
- Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
- Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
- Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+ Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
+ Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
+ Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
+ Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Compute J*v
const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
@@ -141,14 +146,14 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Apply the impulse to the body 1
- v1 += mBody1->mMassInverse * linearImpulseBody1;
+ v1 += constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity) * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the body 2
- v2 += mBody2->mMassInverse * deltaLambda;
+ v2 += constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity) * deltaLambda;
w2 += mI2 * angularImpulseBody2;
}
@@ -160,14 +165,14 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies center of mass and orientations
- Vector3& x1 = constraintSolverData.positions[mIndexBody1];
- Vector3& x2 = constraintSolverData.positions[mIndexBody2];
- Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
- Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+ Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
+ Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
+ Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
+ Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
- decimal inverseMassBody1 = mBody1->mMassInverse;
- decimal inverseMassBody2 = mBody2->mMassInverse;
+ const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
@@ -225,5 +230,10 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
+
+ constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
+ constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
+ constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
+ constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
}
diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp
index 9be0f1d0..e3290b4b 100644
--- a/src/constraint/ContactPoint.cpp
+++ b/src/constraint/ContactPoint.cpp
@@ -45,6 +45,21 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const WorldSetti
mIsObsolete = false;
}
+// Constructor
+ContactPoint::ContactPoint(const ContactPointInfo& contactInfo, const WorldSettings& worldSettings)
+ : mNormal(contactInfo.normal),
+ mPenetrationDepth(contactInfo.penetrationDepth),
+ mLocalPointOnShape1(contactInfo.localPoint1),
+ mLocalPointOnShape2(contactInfo.localPoint2),
+ mIsRestingContact(false), mPenetrationImpulse(0), mIsObsolete(false),
+ mWorldSettings(worldSettings) {
+
+ assert(mPenetrationDepth > decimal(0.0));
+ 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) {
diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h
index e705c7bf..c681396d 100644
--- a/src/constraint/ContactPoint.h
+++ b/src/constraint/ContactPoint.h
@@ -93,18 +93,6 @@ class ContactPoint {
/// Set the mIsRestingContact variable
void setIsRestingContact(bool isRestingContact);
- /// 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);
-
- /// Set the previous contact point in the linked list
- void setPrevious(ContactPoint* previous);
-
- /// Return true if the contact point is obsolete
- bool getIsObsolete() const;
-
public :
// -------------------- Methods -------------------- //
@@ -112,17 +100,20 @@ class ContactPoint {
/// Constructor
ContactPoint(const ContactPointInfo* contactInfo, const WorldSettings& worldSettings);
+ /// Constructor
+ ContactPoint(const ContactPointInfo& contactInfo, const WorldSettings& worldSettings);
+
/// Destructor
~ContactPoint() = default;
- /// Deleted copy-constructor
- ContactPoint(const ContactPoint& contact) = delete;
+ /// Copy-constructor
+ ContactPoint(const ContactPoint& contact) = default;
- /// Deleted assignment operator
- ContactPoint& operator=(const ContactPoint& contact) = delete;
+ /// Assignment operator
+ ContactPoint& operator=(const ContactPoint& contact) = default;
/// Return the normal vector of the contact
- Vector3 getNormal() const;
+ const Vector3& getNormal() const;
/// Return the contact point on the first proxy shape in the local-space of the proxy shape
const Vector3& getLocalPointOnShape1() const;
@@ -136,12 +127,6 @@ class ContactPoint {
/// 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;
-
/// Return the penetration depth
decimal getPenetrationDepth() const;
@@ -152,13 +137,14 @@ class ContactPoint {
friend class ContactManifold;
friend class ContactManifoldSet;
friend class ContactSolver;
+ friend class CollisionDetection;
};
// Return the normal vector of the contact
/**
* @return The contact normal
*/
-inline Vector3 ContactPoint::getNormal() const {
+inline const Vector3& ContactPoint::getNormal() const {
return mNormal;
}
@@ -216,54 +202,6 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
mIsRestingContact = isRestingContact;
}
-// Return true if the contact point is obsolete
-/**
- * @return True if the contact is obsolete
- */
-inline bool ContactPoint::getIsObsolete() const {
- return mIsObsolete;
-}
-
-// Set to true to make the contact point obsolete
-/**
- * @param isObsolete True if the contact is obsolete
- */
-inline void ContactPoint::setIsObsolete(bool isObsolete) {
- mIsObsolete = isObsolete;
-}
-
-// Return the next contact point in the linked list
-/**
- * @return A pointer to the next contact point in the linked-list of points
- */
-inline ContactPoint* ContactPoint::getNext() const {
- return mNext;
-}
-
-// Set the next contact point in the linked list
-/**
- * @param next Pointer to the next contact point in the linked-list of points
- */
-inline void ContactPoint::setNext(ContactPoint* next) {
- mNext = next;
-}
-
-// Return the previous contact point in the linked list
-/**
- * @return A pointer to the previous contact point in the linked-list of points
- */
-inline ContactPoint* ContactPoint::getPrevious() const {
- return mPrevious;
-}
-
-// Set the previous contact point in the linked list
-/**
- * @param previous Pointer to the previous contact point in the linked-list of points
- */
-inline void ContactPoint::setPrevious(ContactPoint* previous) {
- mPrevious = previous;
-}
-
// Return the penetration depth of the contact
/**
* @return the penetration depth (in meters)
diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp
index 99fca6c2..a195e974 100644
--- a/src/constraint/FixedJoint.cpp
+++ b/src/constraint/FixedJoint.cpp
@@ -26,6 +26,7 @@
// Libraries
#include "FixedJoint.h"
#include "engine/ConstraintSolver.h"
+#include "components/DynamicsComponents.h"
using namespace reactphysics3d;
@@ -59,13 +60,9 @@ FixedJoint::FixedJoint(uint id, const FixedJointInfo& jointInfo)
// Initialize before solving the constraint
void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
- // Initialize the bodies index in the velocity array
- mIndexBody1 = mBody1->mArrayIndex;
- mIndexBody2 = mBody2->mArrayIndex;
-
// Get the bodies positions and orientations
- const Vector3& x1 = mBody1->mCenterOfMassWorld;
- const Vector3& x2 = mBody2->mCenterOfMassWorld;
+ const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
+ const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
@@ -82,7 +79,9 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
- decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+ const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
+ const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
+ const decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@@ -129,15 +128,18 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
+ uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+ uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
// Get the velocities
- Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
- Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
- Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
- Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+ Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
+ Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
+ Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
+ Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass of the bodies
- const decimal inverseMassBody1 = mBody1->mMassInverse;
- const decimal inverseMassBody2 = mBody2->mMassInverse;
+ const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
Vector3 linearImpulseBody1 = -mImpulseTranslation;
@@ -164,15 +166,18 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Solve the velocity constraint
void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
+ uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+ uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
// Get the velocities
- Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
- Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
- Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
- Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+ Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
+ Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
+ Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
+ Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass of the bodies
- decimal inverseMassBody1 = mBody1->mMassInverse;
- decimal inverseMassBody2 = mBody2->mMassInverse;
+ decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// --------------- Translation Constraints --------------- //
@@ -226,14 +231,14 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
- Vector3& x1 = constraintSolverData.positions[mIndexBody1];
- Vector3& x2 = constraintSolverData.positions[mIndexBody2];
- Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
- Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+ Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
+ Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
+ Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
+ Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
- decimal inverseMassBody1 = mBody1->mMassInverse;
- decimal inverseMassBody2 = mBody2->mMassInverse;
+ decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
@@ -250,7 +255,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
- decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+ decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@@ -348,5 +353,10 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
+
+ constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
+ constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
+ constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
+ constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
}
diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp
index 50106d13..a1651027 100644
--- a/src/constraint/HingeJoint.cpp
+++ b/src/constraint/HingeJoint.cpp
@@ -26,6 +26,7 @@
// Libraries
#include "HingeJoint.h"
#include "engine/ConstraintSolver.h"
+#include "components/DynamicsComponents.h"
using namespace reactphysics3d;
@@ -66,13 +67,9 @@ HingeJoint::HingeJoint(uint id, const HingeJointInfo& jointInfo)
// Initialize before solving the constraint
void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
- // Initialize the bodies index in the velocity array
- mIndexBody1 = mBody1->mArrayIndex;
- mIndexBody2 = mBody2->mArrayIndex;
-
// Get the bodies positions and orientations
- const Vector3& x1 = mBody1->mCenterOfMassWorld;
- const Vector3& x2 = mBody2->mCenterOfMassWorld;
+ const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
+ const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
@@ -116,7 +113,9 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
- decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+ decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
+ decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
+ decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@@ -198,15 +197,18 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
+ uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+ uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
// Get the velocities
- Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
- Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
- Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
- Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+ Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
+ Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
+ Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
+ Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
- const decimal inverseMassBody1 = mBody1->mMassInverse;
- const decimal inverseMassBody2 = mBody2->mMassInverse;
+ const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y;
@@ -254,15 +256,18 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Solve the velocity constraint
void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
+ uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+ uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
// Get the velocities
- Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
- Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
- Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
- Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+ Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
+ Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
+ Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
+ Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
- decimal inverseMassBody1 = mBody1->mMassInverse;
- decimal inverseMassBody2 = mBody2->mMassInverse;
+ decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// --------------- Translation Constraints --------------- //
@@ -405,14 +410,14 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
- Vector3& x1 = constraintSolverData.positions[mIndexBody1];
- Vector3& x2 = constraintSolverData.positions[mIndexBody2];
- Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
- Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+ Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
+ Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
+ Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
+ Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
- decimal inverseMassBody1 = mBody1->mMassInverse;
- decimal inverseMassBody2 = mBody2->mMassInverse;
+ decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
@@ -448,7 +453,9 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
- decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
+ const decimal body1InverseMass = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ const decimal body2InverseMass = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
+ decimal inverseMassBodies = body1InverseMass + body2InverseMass;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@@ -603,6 +610,11 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
q2.normalize();
}
}
+
+ constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
+ constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
+ constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
+ constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
}
diff --git a/src/constraint/Joint.cpp b/src/constraint/Joint.cpp
index a24b1f2e..f1ca34eb 100644
--- a/src/constraint/Joint.cpp
+++ b/src/constraint/Joint.cpp
@@ -30,7 +30,8 @@ using namespace reactphysics3d;
// Constructor
Joint::Joint(uint id, const JointInfo& jointInfo)
- :mId(id), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mType(jointInfo.type),
+ :mId(id), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mBody1Entity(jointInfo.body1->getEntity()),
+ mBody2Entity(jointInfo.body2->getEntity()), mType(jointInfo.type),
mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique),
mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) {
diff --git a/src/constraint/Joint.h b/src/constraint/Joint.h
index 1c89d464..38e26b5d 100644
--- a/src/constraint/Joint.h
+++ b/src/constraint/Joint.h
@@ -124,20 +124,22 @@ class Joint {
uint mId;
/// Pointer to the first body of the joint
+ // TODO : Use Entities instead
RigidBody* const mBody1;
/// Pointer to the second body of the joint
+ // TODO : Use Entities instead
RigidBody* const mBody2;
+ /// Entity of the first body of the joint
+ Entity mBody1Entity;
+
+ /// Entity of the second body of the joint
+ Entity mBody2Entity;
+
/// Type of the joint
const JointType mType;
- /// Body 1 index in the velocity array to solve the constraint
- uint mIndexBody1;
-
- /// Body 2 index in the velocity array to solve the constraint
- uint mIndexBody2;
-
/// Position correction technique used for the constraint (used for joints)
JointsPositionCorrectionTechnique mPositionCorrectionTechnique;
diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp
index b01fb410..48be0755 100644
--- a/src/constraint/SliderJoint.cpp
+++ b/src/constraint/SliderJoint.cpp
@@ -26,6 +26,7 @@
// Libraries
#include "SliderJoint.h"
#include "engine/ConstraintSolver.h"
+#include "components/DynamicsComponents.h"
using namespace reactphysics3d;
@@ -74,13 +75,9 @@ SliderJoint::SliderJoint(uint id, const SliderJointInfo& jointInfo)
// Initialize before solving the constraint
void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
- // Initialize the bodies index in the veloc ity array
- mIndexBody1 = mBody1->mArrayIndex;
- mIndexBody2 = mBody2->mArrayIndex;
-
// Get the bodies positions and orientations
- const Vector3& x1 = mBody1->mCenterOfMassWorld;
- const Vector3& x2 = mBody2->mCenterOfMassWorld;
+ const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity);
+ const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity);
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
@@ -127,7 +124,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
- decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
+ const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
+ const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
+ const decimal sumInverseMass = body1MassInverse + body2MassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
@@ -173,7 +172,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
- mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse +
+ mInverseMassMatrixLimit = sumInverseMass +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
@@ -196,7 +195,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
if (mIsMotorEnabled) {
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
- mInverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse;
+ mInverseMassMatrixMotor = sumInverseMass;
mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0);
}
@@ -216,15 +215,18 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
+ uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+ uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
// Get the velocities
- Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
- Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
- Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
- Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+ Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
+ Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
+ Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
+ Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
- const decimal inverseMassBody1 = mBody1->mMassInverse;
- const decimal inverseMassBody2 = mBody2->mMassInverse;
+ const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit;
@@ -275,15 +277,18 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Solve the velocity constraint
void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
+ uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity);
+ uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity);
+
// Get the velocities
- Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
- Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
- Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
- Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+ Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
+ Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
+ Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
+ Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
- decimal inverseMassBody1 = mBody1->mMassInverse;
- decimal inverseMassBody2 = mBody2->mMassInverse;
+ decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// --------------- Translation Constraints --------------- //
@@ -437,14 +442,14 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
- Vector3& x1 = constraintSolverData.positions[mIndexBody1];
- Vector3& x2 = constraintSolverData.positions[mIndexBody2];
- Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
- Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+ Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
+ Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
+ Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
+ Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
- decimal inverseMassBody1 = mBody1->mMassInverse;
- decimal inverseMassBody2 = mBody2->mMassInverse;
+ const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Recompute the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
@@ -483,7 +488,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
- decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
+ const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
+ decimal sumInverseMass = body1MassInverse + body2MassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
@@ -603,7 +610,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
- mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse +
+ const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
+ const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
+ mInverseMassMatrixLimit = body1MassInverse + body2MassInverse +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
@@ -676,6 +685,11 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
q2.normalize();
}
}
+
+ constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1);
+ constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2);
+ constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1);
+ constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2);
}
// Enable/Disable the limits of the joint
diff --git a/src/containers/Set.h b/src/containers/Set.h
index 93cba1ca..c6162c44 100755
--- a/src/containers/Set.h
+++ b/src/containers/Set.h
@@ -543,7 +543,7 @@ class Set {
}
/// Return a list with all the values of the set
- List<V> toList(MemoryAllocator& listAllocator) {
+ List<V> toList(MemoryAllocator& listAllocator) const {
List<V> list(listAllocator);
diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp
index 09809a17..c63d017a 100644
--- a/src/engine/CollisionWorld.cpp
+++ b/src/engine/CollisionWorld.cpp
@@ -202,9 +202,6 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
// Add the body ID to the list of free IDs
mFreeBodiesIds.add(collisionBody->getId());
- // Reset the contact manifold list of the body
- collisionBody->resetContactManifoldsList();
-
mBodyComponents.removeComponent(collisionBody->getEntity());
mTransformComponents.removeComponent(collisionBody->getEntity());
mEntityManager.destroyEntity(collisionBody->getEntity());
@@ -236,17 +233,6 @@ bodyindex CollisionWorld::computeNextAvailableBodyId() {
return bodyID;
}
-// Reset all the contact manifolds linked list of each body
-void CollisionWorld::resetContactManifoldListsOfBodies() {
-
- // For each rigid body of the world
- for (List<CollisionBody*>::Iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
-
- // Reset the contact manifold list of the body
- (*it)->resetContactManifoldsList();
- }
-}
-
// Notify the world if a body is disabled (sleeping or inactive) or not
void CollisionWorld::notifyBodyDisabled(Entity bodyEntity, bool isDisabled) {
@@ -270,37 +256,10 @@ void CollisionWorld::notifyBodyDisabled(Entity bodyEntity, bool isDisabled) {
}
}
-// Test if the AABBs of two bodies overlap
-/**
- * @param body1 Pointer to the first body to test
- * @param body2 Pointer to the second body to test
- * @return True if the AABBs of the two bodies overlap and false otherwise
- */
-bool CollisionWorld::testAABBOverlap(const CollisionBody* body1,
- const CollisionBody* body2) const {
-
- // If one of the body is not active, we return no overlap
- if (!body1->isActive() || !body2->isActive()) return false;
-
- // Compute the AABBs of both bodies
- AABB body1AABB = body1->getAABB();
- AABB body2AABB = body2->getAABB();
-
- // Return true if the two AABBs overlap
- return body1AABB.testCollision(body2AABB);
-}
-
-// Report all the bodies which have an AABB that overlaps with the AABB in parameter
-/**
- * @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::testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits) {
- mCollisionDetection.testAABBOverlap(aabb, overlapCallback, categoryMaskBits);
-}
-
// Return true if two bodies overlap
+/// Use this method if you are not interested in contacts but if you simply want to know
+/// if the two bodies overlap. If you want to get the contacts, you need to use the
+/// testCollision() method instead.
/**
* @param body1 Pointer to the first body
* @param body2 Pointer to a second body
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index 3bf33372..f9d0b5d8 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -37,6 +37,8 @@
#include "components/TransformComponents.h"
#include "components/ProxyShapeComponents.h"
#include "components/DynamicsComponents.h"
+#include "collision/CollisionCallback.h"
+#include "collision/OverlapCallback.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
@@ -130,9 +132,6 @@ class CollisionWorld {
/// Return the next available body id
bodyindex computeNextAvailableBodyId();
- /// Reset all the contact manifolds linked list of each body
- void resetContactManifoldListsOfBodies();
-
/// Notify the world if a body is disabled (slepping or inactive) or not
void notifyBodyDisabled(Entity entity, bool isDisabled);
@@ -163,30 +162,25 @@ class CollisionWorld {
CollisionDispatch& getCollisionDispatch();
/// Ray cast method
- void raycast(const Ray& ray, RaycastCallback* raycastCallback,
- unsigned short raycastWithCategoryMaskBits = 0xFFFF) const;
+ void raycast(const Ray& ray, RaycastCallback* raycastCallback, unsigned short raycastWithCategoryMaskBits = 0xFFFF) const;
- /// Test if the AABBs of two bodies overlap
- bool testAABBOverlap(const CollisionBody* body1,
- const CollisionBody* body2) const;
-
- /// Report all the bodies which have an AABB that overlaps with the AABB in parameter
- void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
-
- /// Return true if two bodies overlap
+ /// Return true if two bodies overlap (collide)
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);
+ /// Report all the bodies that overlap (collide) with the body in parameter
+ void testOverlap(CollisionBody* body, OverlapCallback& overlapCallback);
- /// Test and report collisions between two bodies
- void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback);
+ /// Report all the bodies that overlap (collide) in the world
+ void testOverlap(OverlapCallback& overlapCallback);
- /// 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 collision and report contacts between two bodies.
+ void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback& callback);
- /// Test and report collisions between all shapes of the world
- void testCollision(CollisionCallback* callback);
+ /// Test collision and report all the contacts involving the body in parameter
+ void testCollision(CollisionBody* body, CollisionCallback& callback);
+
+ /// Test collision and report contacts between each colliding bodies in the world
+ void testCollision(CollisionCallback& callback);
#ifdef IS_PROFILING_ACTIVE
@@ -215,6 +209,8 @@ class CollisionWorld {
friend class RigidBody;
friend class ProxyShape;
friend class ConvexMeshShape;
+ friend class CollisionCallback::ContactPair;
+ friend class OverlapCallback::OverlapPair;
};
// Set the collision dispatch configuration
@@ -241,42 +237,63 @@ inline void CollisionWorld::raycast(const Ray& ray,
mCollisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits);
}
-// Test and report collisions between two bodies
+// Test collision and report contacts between two bodies.
+/// Use this method if you only want to get all the contacts between two bodies.
+/// All the contacts will be reported using the callback object in paramater.
+/// If you are not interested in the contacts but you only want to know if the bodies collide,
+/// you can use the testOverlap() method instead.
/**
* @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 void CollisionWorld::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback) {
+inline void CollisionWorld::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback& callback) {
mCollisionDetection.testCollision(body1, body2, callback);
}
-// Test and report collisions between a body and all the others bodies of the world
+// Test collision and report all the contacts involving the body in parameter
+/// Use this method if you only want to get all the contacts involving a given body.
+/// All the contacts will be reported using the callback object in paramater.
+/// If you are not interested in the contacts but you only want to know if the bodies collide,
+/// you can use the testOverlap() method instead.
/**
* @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);
+inline void CollisionWorld::testCollision(CollisionBody* body, CollisionCallback& callback) {
+ mCollisionDetection.testCollision(body, callback);
}
-// Test and report collisions between all bodies of the world
+// Test collision and report contacts between each colliding bodies in the world
+/// Use this method if you want to get all the contacts between colliding bodies in the world.
+/// All the contacts will be reported using the callback object in paramater.
+/// If you are not interested in the contacts but you only want to know if the bodies collide,
+/// you can use the testOverlap() method instead.
/**
* @param callback Pointer to the object with the callback method to report contacts
*/
-inline void CollisionWorld::testCollision(CollisionCallback* callback) {
+inline void CollisionWorld::testCollision(CollisionCallback& callback) {
mCollisionDetection.testCollision(callback);
}
-// Report all the bodies that overlap with the body in parameter
+// Report all the bodies that overlap (collide) with the body in parameter
+/// Use this method if you are not interested in contacts but if you simply want to know
+/// which bodies overlap with the body in parameter. If you want to get the contacts, you need to use the
+/// testCollision() method instead.
/**
* @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);
+inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback& overlapCallback) {
+ mCollisionDetection.testOverlap(body, overlapCallback);
+}
+
+// Report all the bodies that overlap (collide) in the world
+/// Use this method if you are not interested in contacts but if you simply want to know
+/// which bodies overlap. If you want to get the contacts, you need to use the
+/// testCollision() method instead.
+inline void CollisionWorld::testOverlap(OverlapCallback& overlapCallback) {
+ mCollisionDetection.testOverlap(overlapCallback);
}
// Return the name of the world
diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp
index 58844faa..2d95e041 100644
--- a/src/engine/ConstraintSolver.cpp
+++ b/src/engine/ConstraintSolver.cpp
@@ -31,7 +31,8 @@
using namespace reactphysics3d;
// Constructor
-ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) {
+ConstraintSolver::ConstraintSolver(Islands& islands, DynamicsComponents& dynamicsComponents)
+ : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(dynamicsComponents) {
#ifdef IS_PROFILING_ACTIVE
@@ -42,13 +43,12 @@ ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) {
}
// Initialize the constraint solver for a given island
-void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
+void ConstraintSolver::initializeForIsland(decimal dt, uint islandIndex) {
RP3D_PROFILE("ConstraintSolver::initializeForIsland()", mProfiler);
- assert(island != nullptr);
- assert(island->getNbBodies() > 0);
- assert(island->getNbJoints() > 0);
+ assert(mIslands.bodyEntities[islandIndex].size() > 0);
+ assert(mIslands.joints[islandIndex].size() > 0);
// Set the current time step
mTimeStep = dt;
@@ -58,49 +58,44 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
// For each joint of the island
- Joint** joints = island->getJoints();
- for (uint i=0; i<island->getNbJoints(); i++) {
+ for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
// Initialize the constraint before solving it
- joints[i]->initBeforeSolve(mConstraintSolverData);
+ mIslands.joints[islandIndex][i]->initBeforeSolve(mConstraintSolverData);
// Warm-start the constraint if warm-starting is enabled
if (mIsWarmStartingActive) {
- joints[i]->warmstart(mConstraintSolverData);
+ mIslands.joints[islandIndex][i]->warmstart(mConstraintSolverData);
}
}
}
// Solve the velocity constraints
-void ConstraintSolver::solveVelocityConstraints(Island* island) {
+void ConstraintSolver::solveVelocityConstraints(uint islandIndex) {
RP3D_PROFILE("ConstraintSolver::solveVelocityConstraints()", mProfiler);
- assert(island != nullptr);
- assert(island->getNbJoints() > 0);
+ assert(mIslands.joints[islandIndex].size() > 0);
// For each joint of the island
- Joint** joints = island->getJoints();
- for (uint i=0; i<island->getNbJoints(); i++) {
+ for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
// Solve the constraint
- joints[i]->solveVelocityConstraint(mConstraintSolverData);
+ mIslands.joints[islandIndex][i]->solveVelocityConstraint(mConstraintSolverData);
}
}
// Solve the position constraints
-void ConstraintSolver::solvePositionConstraints(Island* island) {
+void ConstraintSolver::solvePositionConstraints(uint islandIndex) {
RP3D_PROFILE("ConstraintSolver::solvePositionConstraints()", mProfiler);
- assert(island != nullptr);
- assert(island->getNbJoints() > 0);
+ assert(mIslands.joints[islandIndex].size() > 0);
// For each joint of the island
- Joint** joints = island->getJoints();
- for (uint i=0; i < island->getNbJoints(); i++) {
+ for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
// Solve the constraint
- joints[i]->solvePositionConstraint(mConstraintSolverData);
+ mIslands.joints[islandIndex][i]->solvePositionConstraint(mConstraintSolverData);
}
}
diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h
index 829d6e32..0a4ae48c 100644
--- a/src/engine/ConstraintSolver.h
+++ b/src/engine/ConstraintSolver.h
@@ -29,6 +29,7 @@
// Libraries
#include "configuration.h"
#include "mathematics/mathematics.h"
+#include "engine/Islands.h"
namespace reactphysics3d {
@@ -36,6 +37,7 @@ namespace reactphysics3d {
class Joint;
class Island;
class Profiler;
+class DynamicsComponents;
// Structure ConstraintSolverData
/**
@@ -49,24 +51,15 @@ struct ConstraintSolverData {
/// Current time step of the simulation
decimal timeStep;
- /// Array with the bodies linear velocities
- Vector3* linearVelocities;
-
- /// Array with the bodies angular velocities
- Vector3* angularVelocities;
-
- /// Reference to the bodies positions
- Vector3* positions;
-
- /// Reference to the bodies orientations
- Quaternion* orientations;
+ /// Reference to the dynamics components
+ DynamicsComponents& dynamicsComponents;
/// True if warm starting of the solver is active
bool isWarmStartingActive;
/// Constructor
- ConstraintSolverData() :linearVelocities(nullptr), angularVelocities(nullptr),
- positions(nullptr), orientations(nullptr) {
+ ConstraintSolverData(DynamicsComponents& dynamicsComponents)
+ :dynamicsComponents(dynamicsComponents) {
}
@@ -153,6 +146,9 @@ class ConstraintSolver {
/// True if the warm starting of the solver is active
bool mIsWarmStartingActive;
+ /// Reference to the islands
+ Islands& mIslands;
+
/// Constraint solver data used to initialize and solve the constraints
ConstraintSolverData mConstraintSolverData;
@@ -167,19 +163,19 @@ class ConstraintSolver {
// -------------------- Methods -------------------- //
/// Constructor
- ConstraintSolver();
+ ConstraintSolver(Islands& islands, DynamicsComponents& dynamicsComponents);
/// Destructor
~ConstraintSolver() = default;
/// Initialize the constraint solver for a given island
- void initializeForIsland(decimal dt, Island* island);
+ void initializeForIsland(decimal dt, uint islandIndex);
/// Solve the constraints
- void solveVelocityConstraints(Island* island);
+ void solveVelocityConstraints(uint islandIndex);
/// Solve the position constraints
- void solvePositionConstraints(Island* island);
+ void solvePositionConstraints(uint islandIndex);
/// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
@@ -187,14 +183,6 @@ class ConstraintSolver {
/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
- /// Set the constrained velocities arrays
- void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
- Vector3* constrainedAngularVelocities);
-
- /// Set the constrained positions/orientations arrays
- void setConstrainedPositionsArrays(Vector3* constrainedPositions,
- Quaternion* constrainedOrientations);
-
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
@@ -204,28 +192,6 @@ class ConstraintSolver {
};
-// Set the constrained velocities arrays
-inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
- Vector3* constrainedAngularVelocities) {
-
- assert(constrainedLinearVelocities != nullptr);
- assert(constrainedAngularVelocities != nullptr);
-
- mConstraintSolverData.linearVelocities = constrainedLinearVelocities;
- mConstraintSolverData.angularVelocities = constrainedAngularVelocities;
-}
-
-// Set the constrained positions/orientations arrays
-inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions,
- Quaternion* constrainedOrientations) {
-
- assert(constrainedPositions != nullptr);
- assert(constrainedOrientations != nullptr);
-
- mConstraintSolverData.positions = constrainedPositions;
- mConstraintSolverData.orientations = constrainedOrientations;
-}
-
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp
index 385cbe5e..a290a9d0 100644
--- a/src/engine/ContactSolver.cpp
+++ b/src/engine/ContactSolver.cpp
@@ -30,6 +30,9 @@
#include "constraint/ContactPoint.h"
#include "utils/Profiler.h"
#include "engine/Island.h"
+#include "components/BodyComponents.h"
+#include "components/DynamicsComponents.h"
+#include "components/ProxyShapeComponents.h"
#include "collision/ContactManifold.h"
using namespace reactphysics3d;
@@ -41,11 +44,12 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor
-ContactSolver::ContactSolver(MemoryManager& memoryManager, const WorldSettings& worldSettings)
- :mMemoryManager(memoryManager), mSplitLinearVelocities(nullptr),
- mSplitAngularVelocities(nullptr), mContactConstraints(nullptr),
- mContactPoints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
- mIsSplitImpulseActive(true), mWorldSettings(worldSettings) {
+ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents, DynamicsComponents& dynamicsComponents,
+ ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings)
+ :mMemoryManager(memoryManager), mContactConstraints(nullptr), mContactPoints(nullptr),
+ mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr), mBodyComponents(bodyComponents),
+ mDynamicsComponents(dynamicsComponents), mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true),
+ mWorldSettings(worldSettings) {
#ifdef IS_PROFILING_ACTIVE
mProfiler = nullptr;
@@ -54,23 +58,18 @@ ContactSolver::ContactSolver(MemoryManager& memoryManager, const WorldSettings&
}
// Initialize the contact constraints
-void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
+void ContactSolver::init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep) {
+
+ mAllContactManifolds = contactManifolds;
+ mAllContactPoints = contactPoints;
RP3D_PROFILE("ContactSolver::init()", mProfiler);
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();
- }
- }
+ uint nbContactManifolds = mAllContactManifolds->size();
+ uint nbContactPoints = mAllContactPoints->size();
mNbContactManifolds = 0;
mNbContactPoints = 0;
@@ -90,10 +89,10 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
assert(mContactConstraints != nullptr);
// For each island of the world
- for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) {
+ for (uint i = 0; i < mIslands.getNbIslands(); i++) {
- if (islands[islandIndex]->getNbContactManifolds() > 0) {
- initializeForIsland(islands[islandIndex]);
+ if (mIslands.nbContactManifolds[i] > 0) {
+ initializeForIsland(i);
}
}
@@ -102,83 +101,87 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
}
// Initialize the constraint solver for a given island
-void ContactSolver::initializeForIsland(Island* island) {
+void ContactSolver::initializeForIsland(uint islandIndex) {
RP3D_PROFILE("ContactSolver::initializeForIsland()", mProfiler);
- assert(island != nullptr);
- assert(island->getNbBodies() > 0);
- assert(island->getNbContactManifolds() > 0);
- assert(mSplitLinearVelocities != nullptr);
- assert(mSplitAngularVelocities != nullptr);
+ assert(mIslands.bodyEntities[islandIndex].size() > 0);
+ assert(mIslands.nbContactManifolds[islandIndex] > 0);
// For each contact manifold of the island
- ContactManifold** contactManifolds = island->getContactManifolds();
- for (uint i=0; i<island->getNbContactManifolds(); i++) {
+ uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex];
+ uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex];
+ for (uint m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) {
- ContactManifold* externalManifold = contactManifolds[i];
+ ContactManifold& externalManifold = (*mAllContactManifolds)[m];
- assert(externalManifold->getNbContactPoints() > 0);
+ assert(externalManifold.getNbContactPoints() > 0);
// Get the two bodies of the contact
- RigidBody* body1 = static_cast<RigidBody*>(externalManifold->getBody1());
- RigidBody* body2 = static_cast<RigidBody*>(externalManifold->getBody2());
+ RigidBody* body1 = static_cast<RigidBody*>(mBodyComponents.getBody(externalManifold.bodyEntity1));
+ RigidBody* body2 = static_cast<RigidBody*>(mBodyComponents.getBody(externalManifold.bodyEntity2));
assert(body1 != nullptr);
assert(body2 != nullptr);
+ assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1));
+ assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2));
// Get the two contact shapes
- const ProxyShape* shape1 = externalManifold->getShape1();
- const ProxyShape* shape2 = externalManifold->getShape2();
+ // TODO : Do we really need to get the proxy-shape here
+ const ProxyShape* shape1 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity1);
+ const ProxyShape* shape2 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity2);
// Get the position of the two bodies
- const Vector3& x1 = body1->mCenterOfMassWorld;
- const Vector3& x2 = body2->mCenterOfMassWorld;
+ const Vector3& x1 = mDynamicsComponents.getCenterOfMassWorld(externalManifold.bodyEntity1);
+ const Vector3& x2 = mDynamicsComponents.getCenterOfMassWorld(externalManifold.bodyEntity2);
// Initialize the internal contact manifold structure using the external
// contact manifold
new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
- mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex;
- mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex;
+ mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mDynamicsComponents.getEntityIndex(body1->getEntity());
+ mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mDynamicsComponents.getEntityIndex(body2->getEntity());
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].massInverseBody1 = mDynamicsComponents.getMassInverse(body1->getEntity());
+ mContactConstraints[mNbContactManifolds].massInverseBody2 = mDynamicsComponents.getMassInverse(body2->getEntity());
+ mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.getNbContactPoints();
mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
- mContactConstraints[mNbContactManifolds].externalContactManifold = externalManifold;
+ mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold;
mContactConstraints[mNbContactManifolds].normal.setToZero();
mContactConstraints[mNbContactManifolds].frictionPointBody1.setToZero();
mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero();
// Get the velocities of the bodies
- 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];
+ const Vector3& v1 = mDynamicsComponents.getLinearVelocity(externalManifold.bodyEntity1);
+ const Vector3& w1 = mDynamicsComponents.getAngularVelocity(externalManifold.bodyEntity1);
+ const Vector3& v2 = mDynamicsComponents.getLinearVelocity(externalManifold.bodyEntity2);
+ const Vector3& w2 = mDynamicsComponents.getAngularVelocity(externalManifold.bodyEntity2);
// For each contact point of the contact manifold
- ContactPoint* externalContact = externalManifold->getContactPoints();
- assert(externalContact != nullptr);
- while (externalContact != nullptr) {
+ assert(externalManifold.getNbContactPoints() > 0);
+ uint contactPointsStartIndex = externalManifold.mContactPointsIndex;
+ uint nbContactPoints = externalManifold.mNbContactPoints;
+ for (uint c=contactPointsStartIndex; c < contactPointsStartIndex + nbContactPoints; c++) {
+
+ ContactPoint& externalContact = (*mAllContactPoints)[c];
// Get the contact point on the two bodies
- Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnShape1();
- Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnShape2();
+ Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact.getLocalPointOnShape1();
+ Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact.getLocalPointOnShape2();
new (mContactPoints + mNbContactPoints) ContactPointSolver();
- mContactPoints[mNbContactPoints].externalContact = externalContact;
- mContactPoints[mNbContactPoints].normal = externalContact->getNormal();
+ mContactPoints[mNbContactPoints].externalContact = &externalContact;
+ mContactPoints[mNbContactPoints].normal = externalContact.getNormal();
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].penetrationDepth = externalContact.getPenetrationDepth();
+ mContactPoints[mNbContactPoints].isRestingContact = externalContact.getIsRestingContact();
+ externalContact.setIsRestingContact(true);
+ mContactPoints[mNbContactPoints].penetrationImpulse = externalContact.getPenetrationImpulse();
mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0;
mContactConstraints[mNbContactManifolds].frictionPointBody1.x += p1.x;
@@ -240,8 +243,6 @@ void ContactSolver::initializeForIsland(Island* island) {
mContactConstraints[mNbContactManifolds].normal.z += mContactPoints[mNbContactPoints].normal.z;
mNbContactPoints++;
-
- externalContact = externalContact->getNext();
}
mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
@@ -252,13 +253,13 @@ void ContactSolver::initializeForIsland(Island* island) {
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();
+ mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold.getFrictionVector1();
+ mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold.getFrictionVector2();
// Initialize the accumulated impulses with the previous step accumulated impulses
- mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold->getFrictionImpulse1();
- mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold->getFrictionImpulse2();
- mContactConstraints[mNbContactManifolds].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
bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;
@@ -343,6 +344,7 @@ void ContactSolver::warmStart() {
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 (mContactPoints[contactPointIndex].isRestingContact) {
@@ -354,22 +356,22 @@ void ContactSolver::warmStart() {
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;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].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;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
// Update the velocities of the body 2 by applying the impulse P
- 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;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].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;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse;
}
else { // If it is a new contact point
@@ -409,12 +411,12 @@ void ContactSolver::warmStart() {
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;
- mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 1 by applying the impulse P
- mLinearVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
- mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifold ----- //
@@ -430,18 +432,18 @@ void ContactSolver::warmStart() {
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].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;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
- mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
- 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;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
- mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifold ------ //
@@ -455,10 +457,10 @@ void ContactSolver::warmStart() {
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;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
- mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Rolling resistance at the center of the contact manifold ------ //
@@ -466,10 +468,10 @@ 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;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
// Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
}
else { // If it is a new contact manifold
@@ -497,10 +499,10 @@ void ContactSolver::solve() {
decimal sumPenetrationImpulse = 0.0;
// Get the constrained velocities
- 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];
+ const Vector3& v1 = mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
+ const Vector3& w1 = mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
+ const Vector3& v2 = mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
+ const Vector3& w2 = mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
for (short int i=0; i<mContactConstraints[c].nbContacts; i++) {
@@ -543,22 +545,22 @@ void ContactSolver::solve() {
mContactPoints[contactPointIndex].normal.z * deltaLambda);
// Update the velocities of the body 1 by applying the impulse P
- 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;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].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;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda;
// Update the velocities of the body 2 by applying the impulse P
- 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;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].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;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda;
sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse;
@@ -566,10 +568,10 @@ void ContactSolver::solve() {
if (mIsSplitImpulseActive) {
// Split impulse (position correction)
- 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];
+ const Vector3& v1Split = mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
+ const Vector3& w1Split = mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1];
+ const Vector3& v2Split = mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
+ const Vector3& w2Split = mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2];
//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 -
@@ -594,22 +596,22 @@ void ContactSolver::solve() {
mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit);
// Update the velocities of the body 1 by applying the impulse P
- 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;
+ mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x;
+ mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y;
+ mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].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;
+ mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit;
+ mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit;
+ mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit;
// Update the velocities of the body 1 by applying the impulse P
- 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;
+ mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x;
+ mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y;
+ mDynamicsComponents.mSplitLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].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;
+ mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit;
+ mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit;
+ mDynamicsComponents.mSplitAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit;
}
contactPointIndex++;
@@ -650,18 +652,18 @@ void ContactSolver::solve() {
mContactConstraints[c].r2CrossT1.z * deltaLambda);
// Update the velocities of the body 1 by applying the impulse P
- 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;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
- mAngularVelocities[mContactConstraints[c].indexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
- 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;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
- mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Second friction constraint at the center of the contact manifol ----- //
@@ -699,16 +701,16 @@ void ContactSolver::solve() {
angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda;
// Update the velocities of the body 1 by applying the impulse P
- 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;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
// Update the velocities of the body 2 by applying the impulse P
- 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;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
+ mDynamicsComponents.mConstrainedLinearVelocities[mContactConstraints[c].dynamicsComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// ------ Twist friction constraint at the center of the contact manifol ------ //
@@ -731,10 +733,10 @@ void ContactSolver::solve() {
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;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
// Update the velocities of the body 1 by applying the impulse P
- mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
// --------- Rolling resistance constraint at the center of the contact manifold --------- //
@@ -752,10 +754,10 @@ 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;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
// Update the velocities of the body 2 by applying the impulse P
- mAngularVelocities[mContactConstraints[c].indexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
+ mDynamicsComponents.mConstrainedAngularVelocities[mContactConstraints[c].dynamicsComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
}
}
}
diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h
index 26260c0c..5edfc38c 100644
--- a/src/engine/ContactSolver.h
+++ b/src/engine/ContactSolver.h
@@ -30,6 +30,7 @@
#include "configuration.h"
#include "mathematics/Vector3.h"
#include "mathematics/Matrix3x3.h"
+#include "engine/Islands.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -42,6 +43,9 @@ class MemoryManager;
class Profiler;
class Island;
class RigidBody;
+class BodyComponents;
+class DynamicsComponents;
+class ProxyShapeComponents;
// Class Contact Solver
/**
@@ -170,11 +174,11 @@ class ContactSolver {
/// Pointer to the external contact manifold
ContactManifold* externalContactManifold;
- /// Index of body 1 in the constraint solver
- int32 indexBody1;
+ /// Index of body 1 in the dynamics components arrays
+ uint32 dynamicsComponentIndexBody1;
- /// Index of body 2 in the constraint solver
- int32 indexBody2;
+ /// Index of body 2 in the dynamics components arrays
+ uint32 dynamicsComponentIndexBody2;
/// Inverse of the mass of body 1
decimal massInverseBody1;
@@ -279,19 +283,15 @@ class ContactSolver {
/// Memory manager
MemoryManager& mMemoryManager;
- /// Split linear velocities for the position contact solver (split impulse)
- Vector3* mSplitLinearVelocities;
-
- /// Split angular velocities for the position contact solver (split impulse)
- Vector3* mSplitAngularVelocities;
-
/// Current time step
decimal mTimeStep;
/// Contact constraints
+ // TODO : Use List<> here
ContactManifoldSolver* mContactConstraints;
/// Contact points
+ // TODO : Use List<> here
ContactPointSolver* mContactPoints;
/// Number of contact point constraints
@@ -300,11 +300,24 @@ class ContactSolver {
/// Number of contact constraints
uint mNbContactManifolds;
- /// Array of linear velocities
- Vector3* mLinearVelocities;
+ /// Reference to the islands
+ Islands& mIslands;
- /// Array of angular velocities
- Vector3* mAngularVelocities;
+ /// Pointer to the list of contact manifolds from narrow-phase
+ List<ContactManifold>* mAllContactManifolds;
+
+ /// Pointer to the list of contact points from narrow-phase
+ List<ContactPoint>* mAllContactPoints;
+
+ /// Reference to the body components
+ BodyComponents& mBodyComponents;
+
+ /// Reference to the dynamics components
+ DynamicsComponents& mDynamicsComponents;
+
+ /// Reference to the proxy-shapes components
+ // TODO : Do we really need to use this ?
+ ProxyShapeComponents& mProxyShapeComponents;
/// True if the split impulse position correction is active
bool mIsSplitImpulseActive;
@@ -346,24 +359,18 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Constructor
- ContactSolver(MemoryManager& memoryManager, const WorldSettings& worldSettings);
+ ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents,
+ DynamicsComponents& dynamicsComponents, ProxyShapeComponents& proxyShapeComponents,
+ const WorldSettings& worldSettings);
/// Destructor
~ContactSolver() = default;
/// Initialize the contact constraints
- void init(Island** islands, uint nbIslands, decimal timeStep);
+ void init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep);
/// Initialize the constraint solver for a given island
- void initializeForIsland(Island* island);
-
- /// Set the split velocities arrays
- void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
- Vector3* splitAngularVelocities);
-
- /// Set the constrained velocities arrays
- void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
- Vector3* constrainedAngularVelocities);
+ void initializeForIsland(uint islandIndex);
/// Store the computed impulses to use them to
/// warm start the solver at the next iteration
@@ -386,28 +393,6 @@ class ContactSolver {
#endif
};
-// Set the split velocities arrays
-inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
- Vector3* splitAngularVelocities) {
-
- assert(splitLinearVelocities != nullptr);
- assert(splitAngularVelocities != nullptr);
-
- mSplitLinearVelocities = splitLinearVelocities;
- mSplitAngularVelocities = splitAngularVelocities;
-}
-
-// Set the constrained velocities arrays
-inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
- Vector3* constrainedAngularVelocities) {
-
- assert(constrainedLinearVelocities != nullptr);
- assert(constrainedAngularVelocities != nullptr);
-
- mLinearVelocities = constrainedLinearVelocities;
- mAngularVelocities = constrainedAngularVelocities;
-}
-
// Return true if the split impulses position correction technique is used for contacts
inline bool ContactSolver::isSplitImpulseActive() const {
return mIsSplitImpulseActive;
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index 15524353..d3d18d06 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -32,7 +32,9 @@
#include "utils/Profiler.h"
#include "engine/EventListener.h"
#include "engine/Island.h"
+#include "engine/Islands.h"
#include "collision/ContactManifold.h"
+#include "containers/Stack.h"
// Namespaces
using namespace reactphysics3d;
@@ -45,21 +47,17 @@ using namespace std;
* @param logger Pointer to the logger
* @param profiler Pointer to the profiler
*/
-DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings,
- Logger* logger, Profiler* profiler)
+DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler)
: CollisionWorld(worldSettings, logger, profiler),
- mContactSolver(mMemoryManager, mConfig),
+ mIslands(mMemoryManager.getSingleFrameAllocator()),
+ mContactSolver(mMemoryManager, mIslands, mBodyComponents, mDynamicsComponents, mProxyShapesComponents, mConfig),
+ mConstraintSolver(mIslands, mDynamicsComponents),
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
- mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr),
- mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr),
- mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr),
- mConstrainedOrientations(nullptr), mNbIslands(0), mIslands(nullptr),
- mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
- mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity),
- mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
+ mIsGravityEnabled(true), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
+ mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) {
#ifdef IS_PROFILING_ACTIVE
@@ -119,14 +117,17 @@ void DynamicsWorld::update(decimal timeStep) {
// Notify the event listener about the beginning of an internal tick
if (mEventListener != nullptr) mEventListener->beginInternalTick();
- // Reset all the contact manifolds lists of each body
- resetContactManifoldListsOfBodies();
-
// Compute the collision detection
mCollisionDetection.computeCollisionDetection();
- // Compute the islands (separate groups of bodies with constraints between each others)
- computeIslands();
+ // Create the islands
+ createIslands();
+
+ // Create the actual narrow-phase contacts
+ mCollisionDetection.createContacts();
+
+ // Report the contacts to the user
+ mCollisionDetection.reportContacts();
// Integrate the velocities
integrateRigidBodiesVelocities();
@@ -151,6 +152,9 @@ void DynamicsWorld::update(decimal timeStep) {
// Reset the external force and torque applied to the bodies
resetBodiesForceAndTorque();
+ // Reset the islands
+ mIslands.clear();
+
// Reset the single frame memory allocator
mMemoryManager.resetFrameAllocator();
}
@@ -162,37 +166,27 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", mProfiler);
- // For each island of the world
- for (uint i=0; i < mNbIslands; i++) {
+ const decimal isSplitImpulseActive = mContactSolver.isSplitImpulseActive() ? decimal(1.0) : decimal(0.0);
- RigidBody** bodies = mIslands[i]->getBodies();
+ for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
- // For each body of the island
- for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+ // Get the constrained velocity
+ Vector3 newLinVelocity = mDynamicsComponents.mConstrainedLinearVelocities[i];
+ Vector3 newAngVelocity = mDynamicsComponents.mConstrainedAngularVelocities[i];
- // Get the constrained velocity
- uint indexArray = bodies[b]->mArrayIndex;
- Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
- Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
+ // Add the split impulse velocity from Contact Solver (only used
+ // to update the position)
+ newLinVelocity += isSplitImpulseActive * mDynamicsComponents.mSplitLinearVelocities[i];
+ newAngVelocity += isSplitImpulseActive * mDynamicsComponents.mSplitAngularVelocities[i];
- // Add the split impulse velocity from Contact Solver (only used
- // to update the position)
- if (mContactSolver.isSplitImpulseActive()) {
+ // Get current position and orientation of the body
+ const Vector3& currentPosition = mDynamicsComponents.mCentersOfMassWorld[i];
+ const Quaternion& currentOrientation = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).getOrientation();
- newLinVelocity += mSplitLinearVelocities[indexArray];
- newAngVelocity += mSplitAngularVelocities[indexArray];
- }
-
- // Get current position and orientation of the body
- const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld;
- const Quaternion& currentOrientation = mTransformComponents.getTransform(bodies[b]->getEntity()).getOrientation();
-
- // Update the new constrained position and orientation of the body
- mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep;
- mConstrainedOrientations[indexArray] = currentOrientation +
- Quaternion(0, newAngVelocity) *
- currentOrientation * decimal(0.5) * mTimeStep;
- }
+ // Update the new constrained position and orientation of the body
+ mDynamicsComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * mTimeStep;
+ mDynamicsComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) *
+ currentOrientation * decimal(0.5) * mTimeStep;
}
}
@@ -203,73 +197,47 @@ void DynamicsWorld::updateBodiesState() {
// TODO : Make sure we compute this in a system
- // For each island of the world
- for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+ for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
- // For each body of the island
- RigidBody** bodies = mIslands[islandIndex]->getBodies();
+ // Update the linear and angular velocity of the body
+ mDynamicsComponents.mLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i];
+ mDynamicsComponents.mAngularVelocities[i] = mDynamicsComponents.mConstrainedAngularVelocities[i];
- for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
+ // Update the position of the center of mass of the body
+ mDynamicsComponents.mCentersOfMassWorld[i] = mDynamicsComponents.mConstrainedPositions[i];
- uint index = bodies[b]->mArrayIndex;
+ // Update the orientation of the body
+ const Quaternion& constrainedOrientation = mDynamicsComponents.mConstrainedOrientations[i];
+ mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).setOrientation(constrainedOrientation.getUnit());
+ }
- // Update the linear and angular velocity of the body
- mDynamicsComponents.setLinearVelocity(bodies[b]->getEntity(), mConstrainedLinearVelocities[index]);
- mDynamicsComponents.setAngularVelocity(bodies[b]->getEntity(), mConstrainedAngularVelocities[index]);
+ // Update the transform of the body (using the new center of mass and new orientation)
+ for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
- // Update the position of the center of mass of the body
- bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
+ Transform& transform = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]);
+ const Vector3& centerOfMassWorld = mDynamicsComponents.mCentersOfMassWorld[i];
+ const Vector3& centerOfMassLocal = mDynamicsComponents.mCentersOfMassLocal[i];
+ transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
+ }
- // Update the orientation of the body
- mTransformComponents.getTransform(bodies[b]->getEntity()).setOrientation(mConstrainedOrientations[index].getUnit());
+ // Update the world inverse inertia tensor of the body
+ for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
- // 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();
- }
+ Matrix3x3 orientation = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).getOrientation().getMatrix();
+ const Matrix3x3& inverseInertiaLocalTensor = mDynamicsComponents.mInverseInertiaTensorsLocal[i];
+ mDynamicsComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose();
}
// Update the proxy-shapes components
mCollisionDetection.updateProxyShapes();
}
-// Initialize the bodies velocities arrays for the next simulation step.
-void DynamicsWorld::initVelocityArrays() {
+// Reset the split velocities of the bodies
+void DynamicsWorld::resetSplitVelocities() {
- RP3D_PROFILE("DynamicsWorld::initVelocityArrays()", mProfiler);
-
- // Allocate memory for the bodies velocity arrays
- uint nbBodies = mRigidBodies.size();
-
- 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);
- assert(mConstrainedAngularVelocities != nullptr);
- assert(mConstrainedPositions != nullptr);
- assert(mConstrainedOrientations != nullptr);
-
- // Initialize the map of body indexes in the velocity arrays
- uint i = 0;
- for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
-
- mSplitLinearVelocities[i].setToZero();
- mSplitAngularVelocities[i].setToZero();
-
- (*it)->mArrayIndex = i++;
+ for(uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
+ mDynamicsComponents.mSplitLinearVelocities[i].setToZero();
+ mDynamicsComponents.mSplitAngularVelocities[i].setToZero();
}
}
@@ -282,61 +250,55 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()", mProfiler);
- // Initialize the bodies velocity arrays
- initVelocityArrays();
+ // Reset the split velocities of the bodies
+ resetSplitVelocities();
- // For each island of the world
- for (uint i=0; i < mNbIslands; i++) {
+ // Integration component velocities using force/torque
+ for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
- RigidBody** bodies = mIslands[i]->getBodies();
+ assert(mDynamicsComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0));
+ assert(mDynamicsComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0));
- // For each body of the island
- for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+ // Integrate the external force to get the new velocity of the body
+ mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mLinearVelocities[i] + mTimeStep *
+ mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mExternalForces[i];
+ mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mAngularVelocities[i] +
+ mTimeStep * mDynamicsComponents.mInverseInertiaTensorsWorld[i] * mDynamicsComponents.mExternalTorques[i];
+ }
- // Insert the body into the map of constrained velocities
- uint indexBody = bodies[b]->mArrayIndex;
+ // Apply gravity force
+ for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
+ // If the gravity has to be applied to this rigid body
+ if (mDynamicsComponents.mIsGravityEnabled[i] && mIsGravityEnabled) {
- assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
- assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
-
- // Integrate the external force to get the new velocity of the body
- mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
- mTimeStep * bodies[b]->mMassInverse * bodies[b]->mExternalForce;
- mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() +
- mTimeStep * bodies[b]->getInertiaTensorInverseWorld() *
- bodies[b]->mExternalTorque;
-
- // If the gravity has to be applied to this rigid body
- if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) {
-
- // Integrate the gravity force
- mConstrainedLinearVelocities[indexBody] += mTimeStep * bodies[b]->mMassInverse *
- bodies[b]->getMass() * mGravity;
- }
-
- // Apply the velocity damping
- // Damping force : F_c = -c' * v (c=damping factor)
- // Equation : m * dv/dt = -c' * v
- // => dv/dt = -c * v (with c=c'/m)
- // => dv/dt + c * v = 0
- // Solution : v(t) = v0 * e^(-c * t)
- // => v(t + dt) = v0 * e^(-c(t + dt))
- // = v0 * e^(-ct) * e^(-c * dt)
- // = v(t) * e^(-c * dt)
- // => v2 = v1 * e^(-c * dt)
- // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
- // => e^(-x) ~ 1 - x
- // => v2 = v1 * (1 - c * dt)
- decimal linDampingFactor = bodies[b]->getLinearDamping();
- decimal angDampingFactor = bodies[b]->getAngularDamping();
- decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
- decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
- mConstrainedLinearVelocities[indexBody] *= linearDamping;
- mConstrainedAngularVelocities[indexBody] *= angularDamping;
-
- indexBody++;
+ // Integrate the gravity force
+ mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] + mTimeStep *
+ mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mInitMasses[i] * mGravity;
}
}
+
+ // Apply the velocity damping
+ // Damping force : F_c = -c' * v (c=damping factor)
+ // Equation : m * dv/dt = -c' * v
+ // => dv/dt = -c * v (with c=c'/m)
+ // => dv/dt + c * v = 0
+ // Solution : v(t) = v0 * e^(-c * t)
+ // => v(t + dt) = v0 * e^(-c(t + dt))
+ // = v0 * e^(-ct) * e^(-c * dt)
+ // = v(t) * e^(-c * dt)
+ // => v2 = v1 * e^(-c * dt)
+ // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
+ // => e^(-x) ~ 1 - x
+ // => v2 = v1 * (1 - c * dt)
+ for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
+
+ const decimal linDampingFactor = mDynamicsComponents.mLinearDampings[i];
+ const decimal angDampingFactor = mDynamicsComponents.mAngularDampings[i];
+ const decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
+ const decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
+ mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] * linearDamping;
+ mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mConstrainedAngularVelocities[i] * angularDamping;
+ }
}
// Solve the contacts and constraints
@@ -344,40 +306,31 @@ void DynamicsWorld::solveContactsAndConstraints() {
RP3D_PROFILE("DynamicsWorld::solveContactsAndConstraints()", mProfiler);
- // Set the velocities arrays
- mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
- mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
- mConstrainedAngularVelocities);
- mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
- mConstrainedAngularVelocities);
- mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
- mConstrainedOrientations);
-
// ---------- Solve velocity constraints for joints and contacts ---------- //
// Initialize the contact solver
- mContactSolver.init(mIslands, mNbIslands, mTimeStep);
+ mContactSolver.init(mCollisionDetection.mCurrentContactManifolds, mCollisionDetection.mCurrentContactPoints, mTimeStep);
// For each island of the world
- for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+ for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
// If there are constraints to solve
- if (mIslands[islandIndex]->getNbJoints() > 0) {
+ if (mIslands.joints[islandIndex].size() > 0) {
// Initialize the constraint solver
- mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
+ mConstraintSolver.initializeForIsland(mTimeStep, islandIndex);
}
}
// For each iteration of the velocity solver
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
- for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+ for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
// Solve the constraints
- if (mIslands[islandIndex]->getNbJoints() > 0) {
+ if (mIslands.joints[islandIndex].size() > 0) {
- mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
+ mConstraintSolver.solveVelocityConstraints(islandIndex);
}
}
@@ -396,17 +349,17 @@ void DynamicsWorld::solvePositionCorrection() {
if (mJoints.size() == 0) return;
// For each island of the world
- for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+ for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
// ---------- Solve the position error correction for the constraints ---------- //
- if (mIslands[islandIndex]->getNbJoints() > 0) {
+ if (mIslands.joints[islandIndex].size() > 0) {
// For each iteration of the position (error correction) solver
for (uint i=0; i<mNbPositionSolverIterations; i++) {
// Solve the position constraints
- mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
+ mConstraintSolver.solvePositionConstraints(islandIndex);
}
}
}
@@ -429,7 +382,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform));
- mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(Vector3::zero(), Vector3::zero()));
+ mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(transform.getPosition()));
// Create the rigid body
RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
@@ -479,9 +432,6 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
destroyJoint(element->joint);
}
- // Reset the contact manifold list of the body
- rigidBody->resetContactManifoldsList();
-
// Destroy the corresponding entity and its components
mBodyComponents.removeComponent(rigidBody->getEntity());
mTransformComponents.removeComponent(rigidBody->getEntity());
@@ -672,123 +622,133 @@ uint DynamicsWorld::computeNextAvailableJointId() {
return jointId;
}
-// Compute the islands of awake bodies.
+// Compute the islands using potential contacts and joints
+/// We compute the islands before creating the actual contacts here because we want all
+/// the contact manifolds and contact points of the same island
+/// to be packed together into linear arrays of manifolds and contacts for better caching.
/// An island is an isolated group of rigid bodies that have constraints (joints or contacts)
/// between each other. This method computes the islands at each time step as follows: For each
/// awake rigid body, we run a Depth First Search (DFS) through the constraint graph of that body
/// (graph where nodes are the bodies and where the edges are the constraints between the bodies) to
/// find all the bodies that are connected with it (the bodies that share joints or contacts with
/// it). Then, we create an island with this group of connected bodies.
-void DynamicsWorld::computeIslands() {
+void DynamicsWorld::createIslands() {
- RP3D_PROFILE("DynamicsWorld::computeIslands()", mProfiler);
+ // TODO : Check if we handle kinematic bodies correctly in islands creation
- uint nbBodies = mRigidBodies.size();
+ // list of contact pairs involving a non-rigid body
+ List<uint> nonRigidBodiesContactPairs(mMemoryManager.getSingleFrameAllocator());
- // Allocate and create the array of islands pointer. This memory is allocated
- // in the single frame allocator
- mIslands = static_cast<Island**>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
- sizeof(Island*) * nbBodies));
- mNbIslands = 0;
+ RP3D_PROFILE("DynamicsWorld::createIslands()", mProfiler);
- int nbContactManifolds = 0;
+ // Reset all the isAlreadyInIsland variables of bodies and joints
+ for (uint b=0; b < mDynamicsComponents.getNbComponents(); b++) {
- // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
- for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
- int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds();
- nbContactManifolds += nbBodyManifolds;
+ mDynamicsComponents.mIsAlreadyInIsland[b] = false;
}
for (List<Joint*>::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
(*it)->mIsAlreadyInIsland = false;
}
- // 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**>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
- nbBytesStack));
+ // Create a stack for the bodies to visit during the Depth First Search
+ Stack<Entity> bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator());
- // For each rigid body of the world
- for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+ uint nbTotalManifolds = 0;
- RigidBody* body = *it;
+ // For each dynamic component
+ // TODO : Here we iterate on dynamic component where we can have static, kinematic and dynamic bodies. Maybe we should
+ // not use a dynamic component for a static body.
+ for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) {
// If the body has already been added to an island, we go to the next body
- if (body->mIsAlreadyInIsland) continue;
+ if (mDynamicsComponents.mIsAlreadyInIsland[b]) continue;
// If the body is static, we go to the next body
+ // TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
+ CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(mDynamicsComponents.mBodies[b]));
if (body->getType() == BodyType::STATIC) continue;
- // If the body is sleeping or inactive, we go to the next body
- if (body->isSleeping() || !body->isActive()) continue;
-
// Reset the stack of bodies to visit
- uint stackIndex = 0;
- stackBodiesToVisit[stackIndex] = body;
- stackIndex++;
- body->mIsAlreadyInIsland = true;
+ bodyEntityIndicesToVisit.clear();
+
+ // Add the body into the stack of bodies to visit
+ mDynamicsComponents.mIsAlreadyInIsland[b] = true;
+ bodyEntityIndicesToVisit.push(mDynamicsComponents.mBodies[b]);
// Create the new island
- void* allocatedMemoryIsland = mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
- sizeof(Island));
- mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, mJoints.size(),
- mMemoryManager);
+ uint32 islandIndex = mIslands.addIsland(nbTotalManifolds);
// While there are still some bodies to visit in the stack
- while (stackIndex > 0) {
+ while (bodyEntityIndicesToVisit.size() > 0) {
// Get the next body to visit from the stack
- stackIndex--;
- RigidBody* bodyToVisit = stackBodiesToVisit[stackIndex];
- assert(bodyToVisit->isActive());
-
- // Awake the body if it is sleeping
- bodyToVisit->setIsSleeping(false);
+ const Entity bodyToVisitEntity = bodyEntityIndicesToVisit.pop();
// Add the body into the island
- mIslands[mNbIslands]->addBody(bodyToVisit);
+ mIslands.bodyEntities[islandIndex].add(bodyToVisitEntity);
// If the current body is static, we do not want to perform the DFS
// search across that body
- if (bodyToVisit->getType() == BodyType::STATIC) continue;
+ // TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
+ RigidBody* rigidBodyToVisit = static_cast<RigidBody*>(mBodyComponents.getBody(bodyToVisitEntity));
- // For each contact manifold in which the current body is involded
- ContactManifoldListElement* contactElement;
- for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != nullptr;
- contactElement = contactElement->getNext()) {
+ // Awake the body if it is sleeping
+ rigidBodyToVisit->setIsSleeping(false);
- ContactManifold* contactManifold = contactElement->getContactManifold();
+ if (rigidBodyToVisit->getType() == BodyType::STATIC) continue;
- assert(contactManifold->getNbContactPoints() > 0);
+ // If the body is involved in contacts with other bodies
+ auto itBodyContactPairs = mCollisionDetection.mMapBodyToContactPairs.find(bodyToVisitEntity);
+ if (itBodyContactPairs != mCollisionDetection.mMapBodyToContactPairs.end()) {
- // Check if the current contact manifold has already been added into an island
- if (contactManifold->isAlreadyInIsland()) continue;
+ // For each contact pair in which the current body is involded
+ List<uint>& contactPairs = itBodyContactPairs->second;
+ for (uint p=0; p < contactPairs.size(); p++) {
- // Get the other body of the contact manifold
- RigidBody* body1 = dynamic_cast<RigidBody*>(contactManifold->getBody1());
- RigidBody* body2 = dynamic_cast<RigidBody*>(contactManifold->getBody2());
+ ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[contactPairs[p]];
+ assert(pair.potentialContactManifoldsIndices.size() > 0);
- // If the colliding body is a RigidBody (and not a CollisionBody instead)
- if (body1 != nullptr && body2 != nullptr) {
+ // Check if the current contact pair has already been added into an island
+ if (pair.isAlreadyInIsland) continue;
- // Add the contact manifold into the island
- mIslands[mNbIslands]->addContactManifold(contactManifold);
- contactManifold->mIsAlreadyInIsland = true;
+ // Get the other body of the contact manifold
+ // TODO : Maybe avoid those casts here
+ RigidBody* body1 = dynamic_cast<RigidBody*>(mBodyComponents.getBody(pair.body1Entity));
+ RigidBody* body2 = dynamic_cast<RigidBody*>(mBodyComponents.getBody(pair.body2Entity));
- RigidBody* otherBody = (body1->getId() == bodyToVisit->getId()) ? body2 : body1;
+ // If the colliding body is a RigidBody (and not a CollisionBody instead)
+ if (body1 != nullptr && body2 != nullptr) {
- // Check if the other body has already been added to the island
- if (otherBody->mIsAlreadyInIsland) continue;
+ nbTotalManifolds += pair.potentialContactManifoldsIndices.size();
- // Insert the other body into the stack of bodies to visit
- stackBodiesToVisit[stackIndex] = otherBody;
- stackIndex++;
- otherBody->mIsAlreadyInIsland = true;
+ // Add the pair into the list of pair to process to create contacts
+ mCollisionDetection.mContactPairsIndicesOrderingForContacts.add(pair.contactPairIndex);
+
+ // Add the contact manifold into the island
+ mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size();
+ pair.isAlreadyInIsland = true;
+
+ const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity;
+
+ // Check if the other body has already been added to the island
+ if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue;
+
+ // Insert the other body into the stack of bodies to visit
+ bodyEntityIndicesToVisit.push(otherBodyEntity);
+ mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, true);
+ }
+ else {
+
+ // Add the contact pair index in the list of contact pairs that won't be part of islands
+ nonRigidBodiesContactPairs.add(pair.contactPairIndex);
+ pair.isAlreadyInIsland = true;
+ }
}
}
// For each joint in which the current body is involved
JointListElement* jointElement;
- for (jointElement = bodyToVisit->mJointsList; jointElement != nullptr;
+ for (jointElement = rigidBodyToVisit->mJointsList; jointElement != nullptr;
jointElement = jointElement->next) {
Joint* joint = jointElement->joint;
@@ -797,35 +757,41 @@ void DynamicsWorld::computeIslands() {
if (joint->isAlreadyInIsland()) continue;
// Add the joint into the island
- mIslands[mNbIslands]->addJoint(joint);
+ mIslands.joints[islandIndex].add(joint);
joint->mIsAlreadyInIsland = true;
- // Get the other body of the contact manifold
- RigidBody* body1 = static_cast<RigidBody*>(joint->getBody1());
- RigidBody* body2 = static_cast<RigidBody*>(joint->getBody2());
- RigidBody* otherBody = (body1->getId() == bodyToVisit->getId()) ? body2 : body1;
+ const Entity body1Entity = joint->getBody1()->getEntity();
+ const Entity body2Entity = joint->getBody2()->getEntity();
+ const Entity otherBodyEntity = body1Entity == bodyToVisitEntity ? body2Entity : body1Entity;
// Check if the other body has already been added to the island
- if (otherBody->mIsAlreadyInIsland) continue;
+ if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue;
// Insert the other body into the stack of bodies to visit
- stackBodiesToVisit[stackIndex] = otherBody;
- stackIndex++;
- otherBody->mIsAlreadyInIsland = true;
+ bodyEntityIndicesToVisit.push(otherBodyEntity);
+ mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, true);
}
}
// Reset the isAlreadyIsland variable of the static bodies so that they
// can also be included in the other islands
- for (uint i=0; i < mIslands[mNbIslands]->mNbBodies; i++) {
+ for (uint j=0; j < mDynamicsComponents.getNbEnabledComponents(); j++) {
- if (mIslands[mNbIslands]->mBodies[i]->getType() == BodyType::STATIC) {
- mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false;
+ // If the body is static, we go to the next body
+ // TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
+ CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(mDynamicsComponents.mBodies[j]));
+ if (body->getType() == BodyType::STATIC) {
+ mDynamicsComponents.mIsAlreadyInIsland[j] = false;
}
}
+ }
- mNbIslands++;
- }
+ // Add the contact pairs that are not part of islands at the end of the array of pairs for contacts creations
+ mCollisionDetection.mContactPairsIndicesOrderingForContacts.addRange(nonRigidBodiesContactPairs);
+
+ assert(mCollisionDetection.mCurrentContactPairs->size() == mCollisionDetection.mContactPairsIndicesOrderingForContacts.size());
+
+ mCollisionDetection.mMapBodyToContactPairs.clear(true);
}
// Put bodies to sleep if needed.
@@ -839,32 +805,36 @@ void DynamicsWorld::updateSleepingBodies() {
const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
// For each island of the world
- for (uint i=0; i<mNbIslands; i++) {
+ for (uint i=0; i<mIslands.getNbIslands(); i++) {
decimal minSleepTime = DECIMAL_LARGEST;
// For each body of the island
- RigidBody** bodies = mIslands[i]->getBodies();
- for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+ for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
+
+ const Entity bodyEntity = mIslands.bodyEntities[i][b];
+
+ // TODO : We should not have to do this cast here to get type of body
+ CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(bodyEntity));
// Skip static bodies
- if (bodies[b]->getType() == BodyType::STATIC) continue;
+ if (body->getType() == BodyType::STATIC) continue;
// If the body is velocity is large enough to stay awake
- if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare ||
- bodies[b]->getAngularVelocity().lengthSquare() > sleepAngularVelocitySquare ||
- !bodies[b]->isAllowedToSleep()) {
+ if (mDynamicsComponents.getLinearVelocity(bodyEntity).lengthSquare() > sleepLinearVelocitySquare ||
+ mDynamicsComponents.getAngularVelocity(bodyEntity).lengthSquare() > sleepAngularVelocitySquare ||
+ !body->isAllowedToSleep()) {
// Reset the sleep time of the body
- bodies[b]->mSleepTime = decimal(0.0);
+ body->mSleepTime = decimal(0.0);
minSleepTime = decimal(0.0);
}
- else { // If the body velocity is bellow the sleeping velocity threshold
+ else { // If the body velocity is below the sleeping velocity threshold
// Increase the sleep time
- bodies[b]->mSleepTime += mTimeStep;
- if (bodies[b]->mSleepTime < minSleepTime) {
- minSleepTime = bodies[b]->mSleepTime;
+ body->mSleepTime += mTimeStep;
+ if (body->mSleepTime < minSleepTime) {
+ minSleepTime = body->mSleepTime;
}
}
}
@@ -875,8 +845,11 @@ void DynamicsWorld::updateSleepingBodies() {
if (minSleepTime >= mTimeBeforeSleep) {
// Put all the bodies of the island to sleep
- for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
- bodies[b]->setIsSleeping(true);
+ for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
+
+ const Entity bodyEntity = mIslands.bodyEntities[i][b];
+ CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(bodyEntity));
+ body->setIsSleeping(true);
}
}
}
@@ -906,33 +879,3 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::World,
"Dynamics World: isSleepingEnabled=" + (isSleepingEnabled ? std::string("true") : std::string("false")) );
}
-
-// Return the list of all contacts of the world
-/**
- * @return A pointer to the first contact manifold in the linked-list of manifolds
- */
-List<const ContactManifold*> DynamicsWorld::getContactsList() {
-
- List<const ContactManifold*> contactManifolds(mMemoryManager.getPoolAllocator());
-
- // For each currently overlapping pair of bodies
- for (auto it = mCollisionDetection.mOverlappingPairs.begin();
- it != mCollisionDetection.mOverlappingPairs.end(); ++it) {
-
- OverlappingPair* pair = it->second;
-
- // For each contact manifold of the pair
- const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
- ContactManifold* manifold = manifoldSet.getContactManifolds();
- while (manifold != nullptr) {
-
- // Get the contact manifold
- contactManifolds.add(manifold);
-
- manifold = manifold->getNext();
- }
- }
-
- // Return all the contact manifold
- return contactManifolds;
-}
diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h
index f17aaefd..b6d95ff8 100644
--- a/src/engine/DynamicsWorld.h
+++ b/src/engine/DynamicsWorld.h
@@ -33,6 +33,7 @@
#include "utils/Logger.h"
#include "engine/ContactSolver.h"
#include "components/DynamicsComponents.h"
+#include "engine/Islands.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@@ -54,6 +55,9 @@ class DynamicsWorld : public CollisionWorld {
// -------------------- Attributes -------------------- //
+ /// All the islands of bodies of the current frame
+ Islands mIslands;
+
/// Contact solver
ContactSolver mContactSolver;
@@ -84,32 +88,6 @@ class DynamicsWorld : public CollisionWorld {
/// True if the gravity force is on
bool mIsGravityEnabled;
- /// Array of constrained linear velocities (state of the linear velocities
- /// after solving the constraints)
- Vector3* mConstrainedLinearVelocities;
-
- /// Array of constrained angular velocities (state of the angular velocities
- /// after solving the constraints)
- Vector3* mConstrainedAngularVelocities;
-
- /// Split linear velocities for the position contact solver (split impulse)
- Vector3* mSplitLinearVelocities;
-
- /// Split angular velocities for the position contact solver (split impulse)
- Vector3* mSplitAngularVelocities;
-
- /// Array of constrained rigid bodies position (for position error correction)
- Vector3* mConstrainedPositions;
-
- /// Array of constrained rigid bodies orientation (for position error correction)
- Quaternion* mConstrainedOrientations;
-
- /// Number of islands in the world
- uint mNbIslands;
-
- /// Array with all the islands of awaken bodies
- Island** mIslands;
-
/// Sleep linear velocity threshold
decimal mSleepLinearVelocity;
@@ -134,8 +112,8 @@ class DynamicsWorld : public CollisionWorld {
/// Reset the external force and torque applied to the bodies
void resetBodiesForceAndTorque();
- /// Initialize the bodies velocities arrays for the next simulation step.
- void initVelocityArrays();
+ /// Reset the split velocities of the bodies
+ void resetSplitVelocities();
/// Integrate the velocities of rigid bodies.
void integrateRigidBodiesVelocities();
@@ -149,6 +127,9 @@ class DynamicsWorld : public CollisionWorld {
/// Compute the islands of awake bodies.
void computeIslands();
+ /// Compute the islands using potential contacts and joints and create the actual contacts.
+ void createIslands();
+
/// Update the postion/orientation of the bodies
void updateBodiesState();
@@ -259,9 +240,6 @@ class DynamicsWorld : public CollisionWorld {
/// Set an event listener object to receive events callbacks.
void setEventListener(EventListener* eventListener);
- /// Return the list of all contacts of the world
- List<const ContactManifold*> getContactsList();
-
// -------------------- Friendship -------------------- //
friend class RigidBody;
@@ -271,10 +249,9 @@ class DynamicsWorld : public CollisionWorld {
inline void DynamicsWorld::resetBodiesForceAndTorque() {
// For each body of the world
- List<RigidBody*>::Iterator it;
- for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
- (*it)->mExternalForce.setToZero();
- (*it)->mExternalTorque.setToZero();
+ for (uint32 i=0; i < mDynamicsComponents.getNbComponents(); i++) {
+ mDynamicsComponents.mExternalForces[i].setToZero();
+ mDynamicsComponents.mExternalTorques[i].setToZero();
}
}
diff --git a/src/engine/EventListener.h b/src/engine/EventListener.h
index 16408a1a..e0ee9e80 100644
--- a/src/engine/EventListener.h
+++ b/src/engine/EventListener.h
@@ -39,7 +39,7 @@ namespace reactphysics3d {
* new event listener class to the physics world using the DynamicsWorld::setEventListener()
* method.
*/
-class EventListener {
+class EventListener : public CollisionCallback {
public :
@@ -47,20 +47,22 @@ class EventListener {
EventListener() = default;
/// Destructor
- virtual ~EventListener() = default;
+ virtual ~EventListener() override = default;
- /// Called when a new contact point is found between two bodies
+ /// Called when some contacts occur
/**
- * @param contact Information about the contact
+ * @param collisionInfo Information about the contacts
*/
- virtual void newContact(const CollisionCallback::CollisionCallbackInfo& collisionInfo) {}
+ virtual void onContact(const CollisionCallback::CallbackData& callbackData) override {}
+ // TODO : Remove this method (no internal steps anymore)
/// Called at the beginning of an internal tick of the simulation step.
/// Each time the DynamicsWorld::update() method is called, the physics
/// engine will do several internal simulation steps. This method is
/// called at the beginning of each internal simulation step.
virtual void beginInternalTick() {}
+ // TODO : Remove this method (no internal steps anymore)
/// Called at the end of an internal tick of the simulation step.
/// Each time the DynamicsWorld::update() metho is called, the physics
/// engine will do several internal simulation steps. This method is
diff --git a/src/engine/Islands.h b/src/engine/Islands.h
new file mode 100644
index 00000000..dec4e49e
--- /dev/null
+++ b/src/engine/Islands.h
@@ -0,0 +1,120 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com *
+* Copyright (c) 2010-2018 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_ISLANDS_H
+#define REACTPHYSICS3D_ISLANDS_H
+
+// Libraries
+#include "configuration.h"
+#include "containers/List.h"
+#include "engine/Entity.h"
+#include "constraint/Joint.h"
+
+namespace reactphysics3d {
+
+// Declarations
+
+// Structure Islands
+/**
+ * This class contains all the islands of bodies during a frame.
+ * An island represent an isolated group of awake bodies that are connected with each other by
+ * some contraints (contacts or joints).
+ */
+struct Islands {
+
+ private:
+
+ // -------------------- Attributes -------------------- //
+
+ /// Reference to the memory allocator
+ MemoryAllocator& memoryAllocator;
+
+ public:
+
+ // -------------------- Attributes -------------------- //
+
+
+ /// For each island, index of the first contact manifold of the island in the array of contact manifolds
+ List<uint> contactManifoldsIndices;
+
+ /// For each island, number of contact manifolds in the island
+ List<uint> nbContactManifolds;
+
+ /// For each island, list of all the entities of the bodies in the island
+ List<List<Entity>> bodyEntities;
+
+ // TODO : Use joints entities here and not pointers
+ /// For each island, list of the joints that are part of the island
+ List<List<Joint*>> joints;
+
+ // -------------------- Methods -------------------- //
+
+ /// Constructor
+ Islands(MemoryAllocator& allocator)
+ :memoryAllocator(allocator), contactManifoldsIndices(allocator), nbContactManifolds(allocator), bodyEntities(allocator),
+ joints(allocator) {
+
+ }
+
+ /// Destructor
+ ~Islands() = default;
+
+ /// Assignment operator
+ Islands& operator=(const Islands& island) = default;
+
+ /// Copy-constructor
+ Islands(const Islands& island) = default;
+
+ /// Return the number of islands
+ uint32 getNbIslands() const {
+ return static_cast<uint32>(contactManifoldsIndices.size());
+ }
+
+ /// Add an island and return its index
+ uint32 addIsland(uint32 contactManifoldStartIndex) {
+
+ uint32 islandIndex = contactManifoldsIndices.size();
+
+ contactManifoldsIndices.add(contactManifoldStartIndex);
+ nbContactManifolds.add(0);
+ bodyEntities.add(List<Entity>(memoryAllocator));
+ joints.add(List<Joint*>(memoryAllocator));
+
+ return islandIndex;
+ }
+
+ /// Clear all the islands
+ void clear() {
+
+ contactManifoldsIndices.clear(true);
+ nbContactManifolds.clear(true);
+ bodyEntities.clear(true);
+ joints.clear(true);
+ }
+};
+
+}
+
+#endif
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index 67d0bb18..6dacb6dd 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -35,7 +35,7 @@ using namespace reactphysics3d;
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator,
const WorldSettings& worldSettings)
- : mPairID(computeID(shape1->getBroadPhaseId(), shape2->getBroadPhaseId())), mContactManifoldSet(shape1, shape2, persistentMemoryAllocator, worldSettings),
+ : mPairID(computeID(shape1->getBroadPhaseId(), shape2->getBroadPhaseId())), mProxyShape1(shape1), mProxyShape2(shape2),
mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator),
mLastFrameCollisionInfos(mPersistentAllocator), mWorldSettings(worldSettings) {
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index 1fae4fb1..2062a499 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -27,7 +27,6 @@
#define REACTPHYSICS3D_OVERLAPPING_PAIR_H
// Libraries
-#include "collision/ContactManifoldSet.h"
#include "collision/ProxyShape.h"
#include "containers/Map.h"
#include "containers/Pair.h"
@@ -110,8 +109,9 @@ class OverlappingPair {
/// Pair ID
OverlappingPairId mPairID;
- /// Set of persistent contact manifolds
- ContactManifoldSet mContactManifoldSet;
+ // TODO : Replace this by entities
+ ProxyShape* mProxyShape1;
+ ProxyShape* mProxyShape2;
/// Persistent memory allocator
MemoryAllocator& mPersistentAllocator;
@@ -146,6 +146,9 @@ class OverlappingPair {
/// Deleted assignment operator
OverlappingPair& operator=(const OverlappingPair& pair) = delete;
+ /// Return the Id of the pair
+ OverlappingPairId getId() const;
+
/// Return the pointer to first proxy collision shape
ProxyShape* getShape1() const;
@@ -155,30 +158,12 @@ class OverlappingPair {
/// Return the last frame collision info
LastFrameCollisionInfo* getLastFrameCollisionInfo(ShapeIdPair& shapeIds);
- /// Return the a reference to the contact manifold set
- const ContactManifoldSet& getContactManifoldSet();
-
- /// Add potential contact-points from narrow-phase into potential contact manifolds
- void addPotentialContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex);
-
/// Return a reference to the temporary memory allocator
MemoryAllocator& getTemporaryAllocator();
/// 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;
-
- /// Make the contact manifolds and contact points obsolete
- void makeContactsObsolete();
-
- /// Clear the obsolete contact manifold and contact points
- void clearObsoleteManifoldsAndContactPoints();
-
- /// 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
LastFrameCollisionInfo* addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2);
@@ -202,14 +187,19 @@ class OverlappingPair {
friend class DynamicsWorld;
};
+// Return the Id of the pair
+inline OverlappingPair::OverlappingPairId OverlappingPair::getId() const {
+ return mPairID;
+}
+
// Return the pointer to first body
inline ProxyShape* OverlappingPair::getShape1() const {
- return mContactManifoldSet.getShape1();
-}
+ return mProxyShape1;
+}
// Return the pointer to second body
inline ProxyShape* OverlappingPair::getShape2() const {
- return mContactManifoldSet.getShape2();
+ return mProxyShape2;
}
// Return the last frame collision info for a given shape id or nullptr if none is found
@@ -222,17 +212,6 @@ inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(ShapeI
return nullptr;
}
-// Return the contact manifold
-inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() {
- return mContactManifoldSet;
-}
-
-// Make the contact manifolds and contact points obsolete
-inline void OverlappingPair::makeContactsObsolete() {
-
- mContactManifoldSet.makeContactsObsolete();
-}
-
// Return the pair of bodies index
inline OverlappingPair::OverlappingPairId OverlappingPair::computeID(int shape1BroadPhaseId, int shape2BroadPhaseId) {
assert(shape1BroadPhaseId >= 0 && shape2BroadPhaseId >= 0);
@@ -268,31 +247,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;
-}
-
-// Clear the obsolete contact manifold and contact points
-inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() {
- mContactManifoldSet.clearObsoleteManifoldsAndContactPoints();
-}
-
-// Reduce the contact manifolds that have too many contact points
-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[ShapeIdPair(shapeId1, shapeId2)];
}
-// Create a new potential contact manifold using contact-points from narrow-phase
-inline void OverlappingPair::addPotentialContactPoints(const NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchIndex) {
- mContactManifoldSet.addContactPoints(narrowPhaseInfoBatch, batchIndex);
-}
-
}
#endif
diff --git a/src/memory/DefaultAllocator.h b/src/memory/DefaultAllocator.h
index 5ff94895..ce4d1741 100644
--- a/src/memory/DefaultAllocator.h
+++ b/src/memory/DefaultAllocator.h
@@ -50,6 +50,10 @@ class DefaultAllocator : public MemoryAllocator {
/// Allocate memory of a given size (in bytes) and return a pointer to the
/// allocated memory.
virtual void* allocate(size_t size) override {
+
+ // TODO : Make sure to reduce the calls to default allocator is not called within a frame. For instance by a call
+ // to a pool allocator with size too large
+
return malloc(size);
}
diff --git a/src/systems/BroadPhaseSystem.cpp b/src/systems/BroadPhaseSystem.cpp
index 448df4f1..3b7ec9dc 100644
--- a/src/systems/BroadPhaseSystem.cpp
+++ b/src/systems/BroadPhaseSystem.cpp
@@ -52,8 +52,8 @@ BroadPhaseSystem::BroadPhaseSystem(CollisionDetection& collisionDetection, Proxy
}
// Return true if the two broad-phase collision shapes are overlapping
-bool BroadPhaseSystem::testOverlappingShapes(const ProxyShape* shape1,
- const ProxyShape* shape2) const {
+// TODO : Use proxy-shape entities in parameters
+bool BroadPhaseSystem::testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const {
if (shape1->getBroadPhaseId() == -1 || shape2->getBroadPhaseId() == -1) return false;
diff --git a/src/systems/BroadPhaseSystem.h b/src/systems/BroadPhaseSystem.h
index 140486c3..7931da92 100644
--- a/src/systems/BroadPhaseSystem.h
+++ b/src/systems/BroadPhaseSystem.h
@@ -186,7 +186,7 @@ class BroadPhaseSystem {
void reportAllShapesOverlappingWithAABB(const AABB& aabb, List<int>& overlappingNodes) const;
/// Compute all the overlapping pairs of collision shapes
- void computeOverlappingPairs(MemoryManager& memoryManager, List<Pair<int, int> >& overlappingNodes);
+ void computeOverlappingPairs(MemoryManager& memoryManager, List<Pair<int, int>>& overlappingNodes);
/// Return the proxy shape corresponding to the broad-phase node id in parameter
ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const;
diff --git a/test/tests/collision/TestCollisionWorld.h b/test/tests/collision/TestCollisionWorld.h
index affb678e..c7e78a07 100644
--- a/test/tests/collision/TestCollisionWorld.h
+++ b/test/tests/collision/TestCollisionWorld.h
@@ -32,6 +32,7 @@
#include "constraint/ContactPoint.h"
#include "collision/ContactManifold.h"
#include <map>
+#include <vector>
/// Reactphysics3D namespace
namespace reactphysics3d {
@@ -64,12 +65,12 @@ struct CollisionPointData {
}
};
-// Contact manifold collision data
-struct CollisionManifoldData {
+// Contact pair collision data
+struct ContactPairData {
std::vector<CollisionPointData> contactPoints;
- int getNbContactPoints() const {
+ uint getNbContactPoints() const {
return contactPoints.size();
}
@@ -94,18 +95,18 @@ struct CollisionData {
std::pair<const ProxyShape*, const ProxyShape*> proxyShapes;
std::pair<CollisionBody*, CollisionBody*> bodies;
- std::vector<CollisionManifoldData> contactManifolds;
+ std::vector<ContactPairData> contactPairs;
- int getNbContactManifolds() const {
- return contactManifolds.size();
+ int getNbContactPairs() const {
+ return contactPairs.size();
}
int getTotalNbContactPoints() const {
int nbPoints = 0;
- std::vector<CollisionManifoldData>::const_iterator it;
- for (it = contactManifolds.begin(); it != contactManifolds.end(); ++it) {
+ std::vector<ContactPairData>::const_iterator it;
+ for (it = contactPairs.begin(); it != contactPairs.end(); ++it) {
nbPoints += it->getNbContactPoints();
}
@@ -123,8 +124,8 @@ struct CollisionData {
bool hasContactPointSimilarTo(const Vector3& localPointBody1, const Vector3& localPointBody2, decimal penetrationDepth, decimal epsilon = 0.001) const {
- std::vector<CollisionManifoldData>::const_iterator it;
- for (it = contactManifolds.cbegin(); it != contactManifolds.cend(); ++it) {
+ std::vector<ContactPairData>::const_iterator it;
+ for (it = contactPairs.cbegin(); it != contactPairs.cend(); ++it) {
if (it->hasContactPointSimilarTo(localPointBody1, localPointBody2, penetrationDepth)) {
return true;
@@ -182,34 +183,33 @@ class WorldCollisionCallback : public CollisionCallback
}
}
- // This method will be called for each contact
- virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
+ // This method is called when some contacts occur
+ virtual void onContact(const CallbackData& callbackData) override {
- CollisionData collisionData;
- collisionData.bodies = std::make_pair(collisionCallbackInfo.body1, collisionCallbackInfo.body2);
- collisionData.proxyShapes = std::make_pair(collisionCallbackInfo.proxyShape1, collisionCallbackInfo.proxyShape2);
+ CollisionData collisionData;
- ContactManifoldListElement* element = collisionCallbackInfo.contactManifoldElements;
- while (element != nullptr) {
+ // For each contact pair
+ for (uint p=0; p < callbackData.getNbContactPairs(); p++) {
- ContactManifold* contactManifold = element->getContactManifold();
+ ContactPairData contactPairData;
+ ContactPair contactPair = callbackData.getContactPair(p);
- CollisionManifoldData collisionManifold;
+ collisionData.bodies = std::make_pair(contactPair.getBody1(), contactPair.getBody2());
+ collisionData.proxyShapes = std::make_pair(contactPair.getProxyShape1(), contactPair.getProxyShape2());
- ContactPoint* contactPoint = contactManifold->getContactPoints();
- while (contactPoint != nullptr) {
+ // For each contact point
+ for (uint c=0; c < contactPair.getNbContactPoints(); c++) {
- CollisionPointData collisionPoint(contactPoint->getLocalPointOnShape1(), contactPoint->getLocalPointOnShape2(), contactPoint->getPenetrationDepth());
- collisionManifold.contactPoints.push_back(collisionPoint);
+ ContactPoint contactPoint = contactPair.getContactPoint(c);
- contactPoint = contactPoint->getNext();
- }
+ CollisionPointData collisionPoint(contactPoint.getLocalPointOnShape1(), contactPoint.getLocalPointOnShape2(), contactPoint.getPenetrationDepth());
+ contactPairData.contactPoints.push_back(collisionPoint);
+ }
- collisionData.contactManifolds.push_back(collisionManifold);
- mCollisionDatas.insert(std::make_pair(getCollisionKeyPair(collisionData.proxyShapes), collisionData));
+ collisionData.contactPairs.push_back(contactPairData);
+ }
- element = element->getNext();
- }
+ mCollisionDatas.insert(std::make_pair(getCollisionKeyPair(collisionData.proxyShapes), collisionData));
}
};
@@ -218,30 +218,40 @@ class WorldOverlapCallback : public OverlapCallback {
private:
- std::vector<CollisionBody*> mOverlapBodies;
+ std::vector<std::pair<CollisionBody*, CollisionBody*>> mOverlapBodies;
public:
/// Destructor
- virtual ~WorldOverlapCallback() {
+ virtual ~WorldOverlapCallback() override {
reset();
}
/// This method will be called for each reported overlapping bodies
- virtual void notifyOverlap(CollisionBody* collisionBody) override {
- mOverlapBodies.push_back(collisionBody);
+ virtual void onOverlap(CallbackData& callbackData) override {
+
+ // For each overlapping pair
+ for (uint i=0; i < callbackData.getNbOverlappingPairs(); i++) {
+
+ OverlapPair overlapPair = callbackData.getOverlappingPair(i);
+ mOverlapBodies.push_back(std::make_pair(overlapPair.getBody1(), overlapPair.getBody2()));
+ }
}
void reset() {
mOverlapBodies.clear();
}
- bool hasOverlap() const {
- return !mOverlapBodies.empty();
- }
+ bool hasOverlapWithBody(CollisionBody* collisionBody) const {
- std::vector<CollisionBody*>& getOverlapBodies() {
- return mOverlapBodies;
+ for (uint i=0; i < mOverlapBodies.size(); i++) {
+
+ if (mOverlapBodies[i].first == collisionBody || mOverlapBodies[i].second == collisionBody) {
+ return true;
+ }
+ }
+
+ return false;
}
};
@@ -478,7 +488,6 @@ class TestCollisionWorld : public Test {
testNoCollisions();
testNoOverlap();
- testNoAABBOverlap();
testSphereVsSphereCollision();
testSphereVsBoxCollision();
@@ -507,51 +516,51 @@ class TestCollisionWorld : public Test {
// ---------- Global test ---------- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(!mCollisionCallback.hasContacts());
// ---------- Single body test ---------- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mCollisionCallback);
rp3d_test(!mCollisionCallback.hasContacts());
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody2, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody2, mCollisionCallback);
rp3d_test(!mCollisionCallback.hasContacts());
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCollisionCallback);
rp3d_test(!mCollisionCallback.hasContacts());
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody2, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody2, mCollisionCallback);
rp3d_test(!mCollisionCallback.hasContacts());
// Two bodies test
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, mBoxBody2, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mBoxBody2, mCollisionCallback);
rp3d_test(!mCollisionCallback.hasContacts());
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, mSphereBody2, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mSphereBody2, mCollisionCallback);
rp3d_test(!mCollisionCallback.hasContacts());
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mSphereBody1, mCollisionCallback);
rp3d_test(!mCollisionCallback.hasContacts());
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, mSphereBody2, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mSphereBody2, mCollisionCallback);
rp3d_test(!mCollisionCallback.hasContacts());
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody2, mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody2, mSphereBody1, mCollisionCallback);
rp3d_test(!mCollisionCallback.hasContacts());
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody2, mSphereBody2, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody2, mSphereBody2, mCollisionCallback);
rp3d_test(!mCollisionCallback.hasContacts());
}
@@ -563,20 +572,20 @@ class TestCollisionWorld : public Test {
// ---------- Single body test ---------- //
mOverlapCallback.reset();
- mWorld->testOverlap(mBoxBody1, &mOverlapCallback);
- rp3d_test(!mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mBoxBody1, mOverlapCallback);
+ rp3d_test(!mOverlapCallback.hasOverlapWithBody(mBoxBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mBoxBody2, &mOverlapCallback);
- rp3d_test(!mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mBoxBody2, mOverlapCallback);
+ rp3d_test(!mOverlapCallback.hasOverlapWithBody(mBoxBody2));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
- rp3d_test(!mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mSphereBody1, mOverlapCallback);
+ rp3d_test(!mOverlapCallback.hasOverlapWithBody(mSphereBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody2, &mOverlapCallback);
- rp3d_test(!mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mSphereBody2, mOverlapCallback);
+ rp3d_test(!mOverlapCallback.hasOverlapWithBody(mSphereBody2));
// Two bodies test
@@ -588,21 +597,6 @@ class TestCollisionWorld : public Test {
rp3d_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
-
- rp3d_test(!mWorld->testAABBOverlap(mBoxBody1, mBoxBody2));
- rp3d_test(!mWorld->testAABBOverlap(mSphereBody1, mSphereBody2));
- rp3d_test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody1));
- rp3d_test(!mWorld->testAABBOverlap(mBoxBody1, mSphereBody2));
- rp3d_test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody1));
- rp3d_test(!mWorld->testAABBOverlap(mBoxBody2, mSphereBody2));
- }
-
void testSphereVsSphereCollision() {
Transform initTransform1 = mSphereBody1->getTransform();
@@ -615,29 +609,25 @@ class TestCollisionWorld : public Test {
mSphereBody1->setTransform(transform1);
mSphereBody2->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mSphereBody1, mSphereBody2));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mSphereBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody2, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mSphereBody2, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mSphereBody2));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mSphereProxyShape2));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mSphereProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -654,14 +644,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mSphereProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mSphereProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -675,14 +665,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody2, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mSphereProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mSphereProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -696,14 +686,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, mSphereBody2, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mSphereBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mSphereProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mSphereProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -735,29 +725,25 @@ class TestCollisionWorld : public Test {
mSphereBody1->setTransform(transform1);
mBoxBody1->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mSphereBody1, mBoxBody1));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mSphereBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mBoxBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mBoxBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mBoxBody1));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -774,14 +760,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -795,14 +781,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -816,14 +802,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, mBoxBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mBoxBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -845,29 +831,25 @@ class TestCollisionWorld : public Test {
mSphereBody1->setTransform(transform1);
mBoxBody1->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mSphereBody1, mBoxBody1));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mSphereBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mBoxBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mBoxBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mBoxBody1));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -884,14 +866,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -905,14 +887,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -926,14 +908,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, mBoxBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mBoxBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -955,29 +937,25 @@ class TestCollisionWorld : public Test {
mSphereBody1->setTransform(transform1);
mBoxBody1->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mSphereBody1, mBoxBody1));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mSphereBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mBoxBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mBoxBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mBoxBody1));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -994,14 +972,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1015,14 +993,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1036,14 +1014,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, mBoxBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mBoxBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mBoxProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mBoxProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1075,29 +1053,25 @@ class TestCollisionWorld : public Test {
mSphereBody1->setTransform(transform1);
mCapsuleBody1->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mSphereBody1, mCapsuleBody1));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mSphereBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mCapsuleBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mCapsuleBody1));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1114,14 +1088,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1135,14 +1109,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody1, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1156,14 +1130,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, mCapsuleBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCapsuleBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1185,29 +1159,25 @@ class TestCollisionWorld : public Test {
mSphereBody1->setTransform(transform1);
mCapsuleBody1->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mSphereBody1, mCapsuleBody1));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mSphereBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mCapsuleBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mCapsuleBody1));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1224,14 +1194,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1245,14 +1215,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody1, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1266,14 +1236,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, mCapsuleBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCapsuleBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1305,29 +1275,25 @@ class TestCollisionWorld : public Test {
mSphereBody1->setTransform(transform1);
mConvexMeshBody1->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mSphereBody1, mConvexMeshBody1));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mSphereBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mConvexMeshBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConvexMeshBody1));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1344,14 +1310,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1365,14 +1331,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback);
+ mWorld->testCollision(mConvexMeshBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1386,14 +1352,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, mConvexMeshBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mConvexMeshBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1415,29 +1381,25 @@ class TestCollisionWorld : public Test {
mSphereBody1->setTransform(transform1);
mConvexMeshBody1->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mSphereBody1, mConvexMeshBody1));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mSphereBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mConvexMeshBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConvexMeshBody1));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1454,14 +1416,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1475,14 +1437,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback);
+ mWorld->testCollision(mConvexMeshBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1496,14 +1458,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, mConvexMeshBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mConvexMeshBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1525,29 +1487,25 @@ class TestCollisionWorld : public Test {
mSphereBody1->setTransform(transform1);
mConvexMeshBody1->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mSphereBody1, mConvexMeshBody1));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mSphereBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mConvexMeshBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConvexMeshBody1));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1564,14 +1522,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1585,14 +1543,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback);
+ mWorld->testCollision(mConvexMeshBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1606,14 +1564,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, mConvexMeshBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mConvexMeshBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConvexMeshProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConvexMeshProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1645,29 +1603,25 @@ class TestCollisionWorld : public Test {
mSphereBody1->setTransform(transform1);
mConcaveMeshBody->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mSphereBody1, mConcaveMeshBody));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mSphereBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mSphereBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mSphereBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mConcaveMeshBody, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mConcaveMeshBody, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConcaveMeshBody));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConcaveMeshProxyShape));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1684,14 +1638,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1705,14 +1659,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConcaveMeshBody, &mCollisionCallback);
+ mWorld->testCollision(mConcaveMeshBody, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1726,14 +1680,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mSphereBody1, mConcaveMeshBody, &mCollisionCallback);
+ mWorld->testCollision(mSphereBody1, mConcaveMeshBody, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mSphereProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mSphereProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -1765,29 +1719,25 @@ class TestCollisionWorld : public Test {
mBoxBody1->setTransform(transform1);
mBoxBody2->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mBoxBody1, mBoxBody2));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mBoxBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mBoxBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mBoxBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mBoxBody2, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mBoxBody2, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mBoxBody2));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mBoxProxyShape2));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mBoxProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -1822,14 +1772,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mBoxProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mBoxProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -1851,14 +1801,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody2, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mBoxProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mBoxProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -1881,14 +1831,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, mBoxBody2, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mBoxBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mBoxProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mBoxProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -1929,29 +1879,25 @@ class TestCollisionWorld : public Test {
mBoxBody1->setTransform(transform1);
mConvexMeshBody2->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mBoxBody1, mConvexMeshBody2));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mBoxBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mBoxBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mBoxBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mConvexMeshBody2, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mConvexMeshBody2, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConvexMeshBody2));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConvexMeshProxyShape2));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConvexMeshProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -1986,14 +1932,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConvexMeshProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConvexMeshProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -2015,14 +1961,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConvexMeshBody2, &mCollisionCallback);
+ mWorld->testCollision(mConvexMeshBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConvexMeshProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConvexMeshProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -2045,14 +1991,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, mConvexMeshBody2, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mConvexMeshBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConvexMeshProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConvexMeshProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -2093,29 +2039,25 @@ class TestCollisionWorld : public Test {
mConvexMeshBody1->setTransform(transform1);
mConvexMeshBody2->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mConvexMeshBody1, mConvexMeshBody2));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mConvexMeshBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConvexMeshBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mConvexMeshBody2, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mConvexMeshBody2, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConvexMeshBody2));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConvexMeshProxyShape2));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConvexMeshProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -2150,14 +2092,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback);
+ mWorld->testCollision(mConvexMeshBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConvexMeshProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConvexMeshProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -2179,14 +2121,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConvexMeshBody2, &mCollisionCallback);
+ mWorld->testCollision(mConvexMeshBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConvexMeshProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConvexMeshProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -2209,14 +2151,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConvexMeshBody1, mConvexMeshBody2, &mCollisionCallback);
+ mWorld->testCollision(mConvexMeshBody1, mConvexMeshBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConvexMeshProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConvexMeshProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// True if the bodies are swapped in the collision callback response
@@ -2257,29 +2199,25 @@ class TestCollisionWorld : public Test {
mBoxBody1->setTransform(transform1);
mCapsuleBody1->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mBoxBody1, mCapsuleBody1));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mBoxBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mBoxBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mBoxBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mCapsuleBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mCapsuleBody1));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mCapsuleProxyShape1));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2295,14 +2233,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2316,14 +2254,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody1, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2337,14 +2275,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, mCapsuleBody1, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mCapsuleBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2376,29 +2314,25 @@ class TestCollisionWorld : public Test {
mConvexMeshBody1->setTransform(transform1);
mCapsuleBody1->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mConvexMeshBody1, mCapsuleBody1));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mConvexMeshBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConvexMeshBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mCapsuleBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mCapsuleBody1));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mCapsuleProxyShape1));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2414,14 +2348,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback);
+ mWorld->testCollision(mConvexMeshBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2435,14 +2369,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody1, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2456,14 +2390,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConvexMeshBody1, mCapsuleBody1, &mCollisionCallback);
+ mWorld->testCollision(mConvexMeshBody1, mCapsuleBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mCapsuleProxyShape1));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mCapsuleProxyShape1);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2495,85 +2429,81 @@ class TestCollisionWorld : public Test {
mBoxBody1->setTransform(transform1);
mConcaveMeshBody->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mBoxBody1, mConcaveMeshBody));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mBoxBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mBoxBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mBoxBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mConcaveMeshBody, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mConcaveMeshBody, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConcaveMeshBody));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConcaveMeshProxyShape));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
- for (size_t i=0; i<collisionData->contactManifolds[0].contactPoints.size(); i++) {
- rp3d_test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f));
+ for (size_t i=0; i<collisionData->contactPairs[0].contactPoints.size(); i++) {
+ rp3d_test(approxEqual(collisionData->contactPairs[0].contactPoints[i].penetrationDepth, 1.0f));
}
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
- for (size_t i=0; i<collisionData->contactManifolds[0].contactPoints.size(); i++) {
- rp3d_test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f));
+ for (size_t i=0; i<collisionData->contactPairs[0].contactPoints.size(); i++) {
+ rp3d_test(approxEqual(collisionData->contactPairs[0].contactPoints[i].penetrationDepth, 1.0f));
}
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConcaveMeshBody, &mCollisionCallback);
+ mWorld->testCollision(mConcaveMeshBody, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
- for (size_t i=0; i<collisionData->contactManifolds[0].contactPoints.size(); i++) {
- rp3d_test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f));
+ for (size_t i=0; i<collisionData->contactPairs[0].contactPoints.size(); i++) {
+ rp3d_test(approxEqual(collisionData->contactPairs[0].contactPoints[i].penetrationDepth, 1.0f));
}
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mBoxBody1, mConcaveMeshBody, &mCollisionCallback);
+ mWorld->testCollision(mBoxBody1, mConcaveMeshBody, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mBoxProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mBoxProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// Test contact points
- for (size_t i=0; i<collisionData->contactManifolds[0].contactPoints.size(); i++) {
- rp3d_test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f));
+ for (size_t i=0; i<collisionData->contactPairs[0].contactPoints.size(); i++) {
+ rp3d_test(approxEqual(collisionData->contactPairs[0].contactPoints[i].penetrationDepth, 1.0f));
}
// Reset the init transforms
@@ -2597,85 +2527,81 @@ class TestCollisionWorld : public Test {
mConvexMeshBody1->setTransform(transform1);
mConcaveMeshBody->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mConvexMeshBody1, mConcaveMeshBody));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mConvexMeshBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConvexMeshBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mConvexMeshBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mConcaveMeshBody, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mConcaveMeshBody, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConcaveMeshBody));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConcaveMeshProxyShape));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
- for (size_t i=0; i<collisionData->contactManifolds[0].contactPoints.size(); i++) {
- rp3d_test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f));
+ for (size_t i=0; i<collisionData->contactPairs[0].contactPoints.size(); i++) {
+ rp3d_test(approxEqual(collisionData->contactPairs[0].contactPoints[i].penetrationDepth, 1.0f));
}
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConvexMeshBody1, &mCollisionCallback);
+ mWorld->testCollision(mConvexMeshBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
- for (size_t i=0; i<collisionData->contactManifolds[0].contactPoints.size(); i++) {
- rp3d_test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f));
+ for (size_t i=0; i<collisionData->contactPairs[0].contactPoints.size(); i++) {
+ rp3d_test(approxEqual(collisionData->contactPairs[0].contactPoints[i].penetrationDepth, 1.0f));
}
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConcaveMeshBody, &mCollisionCallback);
+ mWorld->testCollision(mConcaveMeshBody, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
- for (size_t i=0; i<collisionData->contactManifolds[0].contactPoints.size(); i++) {
- rp3d_test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f));
+ for (size_t i=0; i<collisionData->contactPairs[0].contactPoints.size(); i++) {
+ rp3d_test(approxEqual(collisionData->contactPairs[0].contactPoints[i].penetrationDepth, 1.0f));
}
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConvexMeshBody1, mConcaveMeshBody, &mCollisionCallback);
+ mWorld->testCollision(mConvexMeshBody1, mConcaveMeshBody, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mConvexMeshProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mConvexMeshProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 4);
// Test contact points
- for (size_t i=0; i<collisionData->contactManifolds[0].contactPoints.size(); i++) {
- rp3d_test(approxEqual(collisionData->contactManifolds[0].contactPoints[i].penetrationDepth, 1.0f));
+ for (size_t i=0; i<collisionData->contactPairs[0].contactPoints.size(); i++) {
+ rp3d_test(approxEqual(collisionData->contactPairs[0].contactPoints[i].penetrationDepth, 1.0f));
}
// Reset the init transforms
@@ -2699,29 +2625,25 @@ class TestCollisionWorld : public Test {
mCapsuleBody1->setTransform(transform1);
mCapsuleBody2->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mCapsuleBody1, mCapsuleBody2));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mCapsuleBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mCapsuleBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mCapsuleBody2, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mCapsuleBody2, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mCapsuleBody2));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2738,14 +2660,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody1, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2759,14 +2681,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody2, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2780,14 +2702,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody1, mCapsuleBody2, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody1, mCapsuleBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2809,29 +2731,25 @@ class TestCollisionWorld : public Test {
mCapsuleBody1->setTransform(transform1);
mCapsuleBody2->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mCapsuleBody1, mCapsuleBody2));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mCapsuleBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mCapsuleBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mCapsuleBody2, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mCapsuleBody2, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mCapsuleBody2));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2848,14 +2766,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody1, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2869,14 +2787,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody2, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2890,14 +2808,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody1, mCapsuleBody2, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody1, mCapsuleBody2, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mCapsuleProxyShape2));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mCapsuleProxyShape2);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2929,29 +2847,25 @@ class TestCollisionWorld : public Test {
mCapsuleBody1->setTransform(transform1);
mConcaveMeshBody->setTransform(transform2);
- // ----- Test AABB overlap ----- //
-
- rp3d_test(mWorld->testAABBOverlap(mCapsuleBody1, mConcaveMeshBody));
+ mOverlapCallback.reset();
+ mWorld->testOverlap(mCapsuleBody1, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mCapsuleBody1));
mOverlapCallback.reset();
- mWorld->testOverlap(mCapsuleBody1, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
-
- mOverlapCallback.reset();
- mWorld->testOverlap(mConcaveMeshBody, &mOverlapCallback);
- rp3d_test(mOverlapCallback.hasOverlap());
+ mWorld->testOverlap(mConcaveMeshBody, mOverlapCallback);
+ rp3d_test(mOverlapCallback.hasOverlapWithBody(mConcaveMeshBody));
// ----- Test global collision test ----- //
mCollisionCallback.reset();
- mWorld->testCollision(&mCollisionCallback);
+ mWorld->testCollision(mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mConcaveMeshProxyShape));
// Get collision data
const CollisionData* collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2968,14 +2882,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 1 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody1, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody1, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -2989,14 +2903,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against body 2 only ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mConcaveMeshBody, &mCollisionCallback);
+ mWorld->testCollision(mConcaveMeshBody, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
@@ -3010,14 +2924,14 @@ class TestCollisionWorld : public Test {
// ----- Test collision against selected body 1 and 2 ----- //
mCollisionCallback.reset();
- mWorld->testCollision(mCapsuleBody1, mConcaveMeshBody, &mCollisionCallback);
+ mWorld->testCollision(mCapsuleBody1, mConcaveMeshBody, mCollisionCallback);
rp3d_test(mCollisionCallback.areProxyShapesColliding(mCapsuleProxyShape1, mConcaveMeshProxyShape));
// Get collision data
collisionData = mCollisionCallback.getCollisionData(mCapsuleProxyShape1, mConcaveMeshProxyShape);
rp3d_test(collisionData != nullptr);
- rp3d_test(collisionData->getNbContactManifolds() == 1);
+ rp3d_test(collisionData->getNbContactPairs() == 1);
rp3d_test(collisionData->getTotalNbContactPoints() == 1);
// True if the bodies are swapped in the collision callback response
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
index ea57959c..ecff6b94 100755
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
@@ -35,7 +35,7 @@ using namespace collisiondetectionscene;
// Constructor
CollisionDetectionScene::CollisionDetectionScene(const std::string& name, EngineSettings& settings)
: SceneDemo(name, settings, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
- mContactManager(mPhongShader, mMeshFolderPath),
+ mContactManager(mPhongShader, mMeshFolderPath, mContactPoints),
mAreNormalsDisplayed(false) {
mSelectedShapeIndex = 0;
@@ -160,6 +160,8 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name, Engine
// Reset the scene
void CollisionDetectionScene::reset() {
+ SceneDemo::reset();
+
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()));
@@ -206,8 +208,6 @@ CollisionDetectionScene::~CollisionDetectionScene() {
mPhysicsWorld->destroyCollisionBody(mHeightField->getCollisionBody());
delete mHeightField;
- mContactManager.resetPoints();
-
// Destroy the static data for the visual contact points
VisualContactPoint::destroyStaticData();
@@ -218,9 +218,9 @@ CollisionDetectionScene::~CollisionDetectionScene() {
// Take a step for the simulation
void CollisionDetectionScene::update() {
- mContactManager.resetPoints();
+ mContactPoints.clear();
- mPhysicsWorld->testCollision(&mContactManager);
+ mPhysicsWorld->testCollision(mContactManager);
SceneDemo::update();
}
@@ -313,38 +313,33 @@ bool CollisionDetectionScene::keyboardEvent(int key, int scancode, int action, i
return false;
}
-// This method will be called for each reported contact point
-void ContactManager::notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) {
+// This method is called when some contacts occur
+void ContactManager::onContact(const CallbackData& callbackData) {
- // For each contact manifold
- rp3d::ContactManifoldListElement* manifoldElement = collisionCallbackInfo.contactManifoldElements;
- while (manifoldElement != nullptr) {
+ // For each contact pair
+ for (uint p=0; p < callbackData.getNbContactPairs(); p++) {
- // Get the contact manifold
- rp3d::ContactManifold* contactManifold = manifoldElement->getContactManifold();
+ ContactPair contactPair = callbackData.getContactPair(p);
- // For each contact point
- rp3d::ContactPoint* contactPoint = contactManifold->getContactPoints();
- while (contactPoint != nullptr) {
+ // For each contact point of the contact pair
+ for (uint c=0; c < contactPair.getNbContactPoints(); c++) {
- // Contact normal
- rp3d::Vector3 normal = contactPoint->getNormal();
- openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z);
+ ContactPoint contactPoint = contactPair.getContactPoint(c);
- rp3d::Vector3 point1 = contactPoint->getLocalPointOnShape1();
- point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
+ // Contact normal
+ rp3d::Vector3 normal = contactPoint.getWorldNormal();
+ openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z);
- openglframework::Vector3 position1(point1.x, point1.y, point1.z);
- mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red()));
+ rp3d::Vector3 point1 = contactPoint.getLocalPointOnShape1();
+ point1 = contactPair.getProxyShape1()->getLocalToWorldTransform() * point1;
- 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()));
+ openglframework::Vector3 position1(point1.x, point1.y, point1.z);
+ mContactPoints.push_back(SceneContactPoint(position1, contactNormal, openglframework::Color::red()));
- contactPoint = contactPoint->getNext();
- }
-
- manifoldElement = manifoldElement->getNext();
+ rp3d::Vector3 point2 = contactPoint.getLocalPointOnShape2();
+ point2 = contactPair.getProxyShape2()->getLocalToWorldTransform() * point2;
+ openglframework::Vector3 position2(point2.x, point2.y, point2.z);
+ mContactPoints.push_back(SceneContactPoint(position2, contactNormal, openglframework::Color::blue()));
+ }
}
}
diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
index 2e134567..27ab3419 100644
--- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h
+++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h
@@ -63,30 +63,22 @@ class ContactManager : public rp3d::CollisionCallback {
private:
- /// All the visual contact points
- std::vector<ContactPoint> mContactPoints;
-
/// Contact point mesh folder path
std::string mMeshFolderPath;
+ /// Reference to the list of all the visual contact points
+ std::vector<SceneContactPoint>& mContactPoints;
+
public:
- ContactManager(openglframework::Shader& shader, const std::string& meshFolderPath)
- : mMeshFolderPath(meshFolderPath) {
+ ContactManager(openglframework::Shader& shader, const std::string& meshFolderPath,
+ std::vector<SceneContactPoint>& contactPoints)
+ : mMeshFolderPath(meshFolderPath), mContactPoints(contactPoints) {
}
- /// This method will be called for each reported contact point
- virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override;
-
- void resetPoints() {
-
- mContactPoints.clear();
- }
-
- std::vector<ContactPoint> getContactPoints() const {
- return mContactPoints;
- }
+ /// This method is called when some contacts occur
+ virtual void onContact(const CallbackData& callbackData) override;
};
// Class CollisionDetectionScene
@@ -151,9 +143,6 @@ class CollisionDetectionScene : public SceneDemo {
/// Display/Hide the contact points
virtual void setIsContactPointsDisplayed(bool display) override;
-
- /// Return all the contact points of the scene
- virtual std::vector<ContactPoint> getContactPoints() override;
};
// Display or not the surface normals at hit points
@@ -171,11 +160,6 @@ inline void CollisionDetectionScene::setIsContactPointsDisplayed(bool display) {
SceneDemo::setIsContactPointsDisplayed(true);
}
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> CollisionDetectionScene::getContactPoints() {
- return mContactManager.getContactPoints();
-}
-
}
#endif
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
index cffe7b50..12021ca7 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
@@ -49,7 +49,9 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name, EngineSettin
worldSettings.worldName = name;
// Create the dynamics world for the physics simulation
- mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ dynamicsWorld->setEventListener(this);
+ mPhysicsWorld = dynamicsWorld;
for (int i=0; i<NB_COMPOUND_SHAPES; i++) {
@@ -199,6 +201,8 @@ CollisionShapesScene::~CollisionShapesScene() {
/// Reset the scene
void CollisionShapesScene::reset() {
+ SceneDemo::reset();
+
const float radius = 3.0f;
for (uint i = 0; i<NB_COMPOUND_SHAPES; i++) {
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.h b/testbed/scenes/collisionshapes/CollisionShapesScene.h
index dbb49539..d10b3644 100644
--- a/testbed/scenes/collisionshapes/CollisionShapesScene.h
+++ b/testbed/scenes/collisionshapes/CollisionShapesScene.h
@@ -99,16 +99,8 @@ class CollisionShapesScene : public SceneDemo {
/// Reset the scene
virtual void reset() override;
-
- /// Return all the contact points of the scene
- virtual std::vector<ContactPoint> getContactPoints() override;
};
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> CollisionShapesScene::getContactPoints() {
- return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
}
#endif
diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
index 31a06c0f..7b73dec4 100644
--- a/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
+++ b/testbed/scenes/concavemesh/ConcaveMeshScene.cpp
@@ -49,7 +49,9 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name, EngineSettings& sett
worldSettings.worldName = name;
// Create the dynamics world for the physics simulation
- mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ dynamicsWorld->setEventListener(this);
+ mPhysicsWorld = dynamicsWorld;
// ---------- Create the boxes ----------- //
for (int i = 0; i<NB_COMPOUND_SHAPES; i++) {
@@ -206,6 +208,8 @@ ConcaveMeshScene::~ConcaveMeshScene() {
// Reset the scene
void ConcaveMeshScene::reset() {
+ SceneDemo::reset();
+
const float radius = 15.0f;
for (uint i = 0; i<NB_COMPOUND_SHAPES; i++) {
diff --git a/testbed/scenes/concavemesh/ConcaveMeshScene.h b/testbed/scenes/concavemesh/ConcaveMeshScene.h
index 91ede1bb..16dcb753 100644
--- a/testbed/scenes/concavemesh/ConcaveMeshScene.h
+++ b/testbed/scenes/concavemesh/ConcaveMeshScene.h
@@ -96,16 +96,8 @@ class ConcaveMeshScene : public SceneDemo {
/// Reset the scene
virtual void reset() override;
-
- /// Return all the contact points of the scene
- virtual std::vector<ContactPoint> getContactPoints() override;
};
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> ConcaveMeshScene::getContactPoints() {
- return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
}
#endif
diff --git a/testbed/scenes/cubes/CubesScene.cpp b/testbed/scenes/cubes/CubesScene.cpp
index ab363c0b..66a59215 100644
--- a/testbed/scenes/cubes/CubesScene.cpp
+++ b/testbed/scenes/cubes/CubesScene.cpp
@@ -47,7 +47,9 @@ CubesScene::CubesScene(const std::string& name, EngineSettings& settings)
worldSettings.worldName = name;
// Create the dynamics world for the physics simulation
- mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ dynamicsWorld->setEventListener(this);
+ mPhysicsWorld = dynamicsWorld;
// Create all the cubes of the scene
for (int i=0; i<NB_CUBES; i++) {
@@ -117,6 +119,8 @@ CubesScene::~CubesScene() {
// Reset the scene
void CubesScene::reset() {
+ SceneDemo::reset();
+
float radius = 2.0f;
// Create all the cubes of the scene
diff --git a/testbed/scenes/cubes/CubesScene.h b/testbed/scenes/cubes/CubesScene.h
index e284a91d..4e39637a 100755
--- a/testbed/scenes/cubes/CubesScene.h
+++ b/testbed/scenes/cubes/CubesScene.h
@@ -67,16 +67,8 @@ class CubesScene : public SceneDemo {
/// Reset the scene
virtual void reset() override;
-
- /// Return all the contact points of the scene
- virtual std::vector<ContactPoint> getContactPoints() override;
};
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> CubesScene::getContactPoints() {
- return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
}
#endif
diff --git a/testbed/scenes/cubestack/CubeStackScene.cpp b/testbed/scenes/cubestack/CubeStackScene.cpp
index c30674b3..56fb3c35 100644
--- a/testbed/scenes/cubestack/CubeStackScene.cpp
+++ b/testbed/scenes/cubestack/CubeStackScene.cpp
@@ -47,7 +47,9 @@ CubeStackScene::CubeStackScene(const std::string& name, EngineSettings& settings
worldSettings.worldName = name;
// Create the dynamics world for the physics simulation
- mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ dynamicsWorld->setEventListener(this);
+ mPhysicsWorld = dynamicsWorld;
// Create all the cubes of the scene
for (int i=1; i<=NB_FLOORS; i++) {
@@ -120,6 +122,8 @@ CubeStackScene::~CubeStackScene() {
// Reset the scene
void CubeStackScene::reset() {
+ SceneDemo::reset();
+
int index = 0;
for (int i=NB_FLOORS; i > 0; i--) {
diff --git a/testbed/scenes/cubestack/CubeStackScene.h b/testbed/scenes/cubestack/CubeStackScene.h
index d4a4dace..3e3abd2e 100644
--- a/testbed/scenes/cubestack/CubeStackScene.h
+++ b/testbed/scenes/cubestack/CubeStackScene.h
@@ -67,16 +67,8 @@ class CubeStackScene : public SceneDemo {
/// Reset the scene
virtual void reset() override;
-
- /// Return all the contact points of the scene
- virtual std::vector<ContactPoint> getContactPoints() override;
};
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> CubeStackScene::getContactPoints() {
- return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
}
#endif
diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp
index 8b8d489c..d2734bbc 100644
--- a/testbed/scenes/heightfield/HeightFieldScene.cpp
+++ b/testbed/scenes/heightfield/HeightFieldScene.cpp
@@ -49,7 +49,9 @@ HeightFieldScene::HeightFieldScene(const std::string& name, EngineSettings& sett
worldSettings.worldName = name;
// Create the dynamics world for the physics simulation
- mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ dynamicsWorld->setEventListener(this);
+ mPhysicsWorld = dynamicsWorld;
for (int i = 0; i<NB_COMPOUND_SHAPES; i++) {
@@ -205,6 +207,8 @@ HeightFieldScene::~HeightFieldScene() {
// Reset the scene
void HeightFieldScene::reset() {
+ SceneDemo::reset();
+
const float radius = 3.0f;
for (uint i = 0; i<NB_COMPOUND_SHAPES; i++) {
diff --git a/testbed/scenes/heightfield/HeightFieldScene.h b/testbed/scenes/heightfield/HeightFieldScene.h
index 90edda93..5ceee07c 100644
--- a/testbed/scenes/heightfield/HeightFieldScene.h
+++ b/testbed/scenes/heightfield/HeightFieldScene.h
@@ -98,16 +98,8 @@ class HeightFieldScene : public SceneDemo {
/// Reset the scene
virtual void reset() override ;
-
- /// Return all the contact points of the scene
- virtual std::vector<ContactPoint> getContactPoints() override ;
};
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> HeightFieldScene::getContactPoints() {
- return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
}
#endif
diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp
index 736e0252..6c6a2bfc 100644
--- a/testbed/scenes/joints/JointsScene.cpp
+++ b/testbed/scenes/joints/JointsScene.cpp
@@ -48,7 +48,9 @@ JointsScene::JointsScene(const std::string& name, EngineSettings& settings)
worldSettings.worldName = name;
// Create the dynamics world for the physics simulation
- mPhysicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ rp3d::DynamicsWorld* dynamicsWorld = new rp3d::DynamicsWorld(gravity, worldSettings);
+ dynamicsWorld->setEventListener(this);
+ mPhysicsWorld = dynamicsWorld;
// Create the Ball-and-Socket joint
createBallAndSocketJoints();
@@ -129,6 +131,8 @@ void JointsScene::updatePhysics() {
// Reset the scene
void JointsScene::reset() {
+ SceneDemo::reset();
+
openglframework::Vector3 positionBox(0, 15, 5);
openglframework::Vector3 boxDimension(1, 1, 1);
diff --git a/testbed/scenes/joints/JointsScene.h b/testbed/scenes/joints/JointsScene.h
index f101bf32..4a4d1c2d 100644
--- a/testbed/scenes/joints/JointsScene.h
+++ b/testbed/scenes/joints/JointsScene.h
@@ -125,16 +125,8 @@ class JointsScene : public SceneDemo {
/// Reset the scene
virtual void reset() override;
-
- /// Return all the contact points of the scene
- virtual std::vector<ContactPoint> getContactPoints() override;
};
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> JointsScene::getContactPoints() {
- return computeContactPointsOfWorld(getDynamicsWorld());
-}
-
}
#endif
diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp
index 018c28e4..1c5bb9c0 100644
--- a/testbed/scenes/raycast/RaycastScene.cpp
+++ b/testbed/scenes/raycast/RaycastScene.cpp
@@ -33,7 +33,7 @@ using namespace raycastscene;
// Constructor
RaycastScene::RaycastScene(const std::string& name, EngineSettings& settings)
: SceneDemo(name, settings, SCENE_RADIUS, false), mMeshFolderPath("meshes/"),
- mRaycastManager(mPhongShader, mMeshFolderPath), mCurrentBodyIndex(-1),
+ mRaycastManager(mPhongShader, mMeshFolderPath, mContactPoints), mCurrentBodyIndex(-1),
mAreNormalsDisplayed(false), mVBOVertices(GL_ARRAY_BUFFER) {
mIsContactPointsDisplayed = true;
@@ -196,6 +196,8 @@ void RaycastScene::changeBody() {
// Reset the scene
void RaycastScene::reset() {
+ SceneDemo::reset();
+
std::vector<PhysicsObject*>::iterator it;
for (it = mPhysicsObjects.begin(); it != mPhysicsObjects.end(); ++it) {
(*it)->setTransform(rp3d::Transform(rp3d::Vector3::zero(), rp3d::Quaternion::identity()));
diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h
index 1d413995..6de17c45 100644
--- a/testbed/scenes/raycast/RaycastScene.h
+++ b/testbed/scenes/raycast/RaycastScene.h
@@ -64,17 +64,17 @@ class RaycastManager : public rp3d::RaycastCallback {
private:
- /// All the visual contact points
- std::vector<ContactPoint> mHitPoints;
+ /// Reference to the list of contact points of the scene
+ std::vector<SceneContactPoint>& mHitPoints;
/// Contact point mesh folder path
std::string mMeshFolderPath;
public:
- RaycastManager(openglframework::Shader& shader,
- const std::string& meshFolderPath)
- : mMeshFolderPath(meshFolderPath) {
+ RaycastManager(openglframework::Shader& shader, const std::string& meshFolderPath,
+ std::vector<SceneContactPoint>& hitPoints)
+ : mMeshFolderPath(meshFolderPath), mHitPoints(hitPoints) {
}
@@ -85,7 +85,7 @@ class RaycastManager : public rp3d::RaycastCallback {
rp3d::Vector3 hitPos = raycastInfo.worldPoint;
openglframework::Vector3 position(hitPos.x, hitPos.y, hitPos.z);
- mHitPoints.push_back(ContactPoint(position, normal, openglframework::Color::red()));
+ mHitPoints.push_back(SceneContactPoint(position, normal, openglframework::Color::red()));
return raycastInfo.hitFraction;
}
@@ -95,7 +95,7 @@ class RaycastManager : public rp3d::RaycastCallback {
mHitPoints.clear();
}
- std::vector<ContactPoint> getHitPoints() const {
+ std::vector<SceneContactPoint> getHitPoints() const {
return mHitPoints;
}
};
@@ -181,9 +181,6 @@ class RaycastScene : public SceneDemo {
/// Display/Hide the contact points
virtual void setIsContactPointsDisplayed(bool display) override;
-
- /// Return all the contact points of the scene
- virtual std::vector<ContactPoint> getContactPoints() override;
};
// Display or not the surface normals at hit points
@@ -201,11 +198,6 @@ inline void RaycastScene::setIsContactPointsDisplayed(bool display) {
SceneDemo::setIsContactPointsDisplayed(true);
}
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> RaycastScene::getContactPoints() {
- return mRaycastManager.getHitPoints();
-}
-
}
#endif
diff --git a/testbed/src/Scene.cpp b/testbed/src/Scene.cpp
index 2c45b934..4f6e31d9 100644
--- a/testbed/src/Scene.cpp
+++ b/testbed/src/Scene.cpp
@@ -185,3 +185,25 @@ void Scene::rotate(int xMouse, int yMouse) {
}
}
}
+
+// Called when some contacts occur
+void Scene::onContact(const rp3d::CollisionCallback::CallbackData& callbackData) {
+
+ // For each contact pair
+ for (uint p=0; p < callbackData.getNbContactPairs(); p++) {
+
+ rp3d::CollisionCallback::ContactPair contactPair = callbackData.getContactPair(p);
+
+ // For each contact point of the contact pair
+ for (uint c=0; c < contactPair.getNbContactPoints(); c++) {
+
+ rp3d::CollisionCallback::ContactPoint contactPoint = contactPair.getContactPoint(c);
+
+ rp3d::Vector3 point = contactPair.getProxyShape1()->getLocalToWorldTransform() * contactPoint.getLocalPointOnShape1();
+ rp3d::Vector3 normalWorld = contactPoint.getWorldNormal();
+ openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z);
+ SceneContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red());
+ mContactPoints.push_back(contact);
+ }
+ }
+}
diff --git a/testbed/src/Scene.h b/testbed/src/Scene.h
index 8fffff2a..a014a3a1 100644
--- a/testbed/src/Scene.h
+++ b/testbed/src/Scene.h
@@ -31,7 +31,7 @@
#include "reactphysics3d.h"
// Structure ContactPoint
-struct ContactPoint {
+struct SceneContactPoint {
public:
openglframework::Vector3 point;
@@ -39,7 +39,7 @@ struct ContactPoint {
openglframework::Color color;
/// Constructor
- ContactPoint(const openglframework::Vector3& pointWorld, const openglframework::Vector3& normalWorld, const openglframework::Color colorPoint)
+ SceneContactPoint(const openglframework::Vector3& pointWorld, const openglframework::Vector3& normalWorld, const openglframework::Color colorPoint)
: point(pointWorld), normal(normalWorld), color(colorPoint) {
}
@@ -88,7 +88,7 @@ struct EngineSettings {
// Class Scene
// Abstract class that represents a 3D scene.
-class Scene {
+class Scene : public rp3d::EventListener {
protected:
@@ -130,12 +130,15 @@ class Scene {
/// True if contact points are displayed
bool mIsContactPointsDisplayed;
- /// True if the AABBs of the phycis objects are displayed
+ /// True if the AABBs of the physics objects are displayed
bool mIsAABBsDisplayed;
/// True if we render shapes in wireframe mode
bool mIsWireframeEnabled;
+ /// Contact points
+ std::vector<SceneContactPoint> mContactPoints;
+
// -------------------- Methods -------------------- //
/// Set the scene position (where the camera needs to look at)
@@ -165,7 +168,7 @@ class Scene {
Scene(const std::string& name, EngineSettings& engineSettings, bool isShadowMappingEnabled = false);
/// Destructor
- virtual ~Scene();
+ virtual ~Scene() override;
/// Reshape the view
virtual void reshape(int width, int height);
@@ -181,7 +184,7 @@ class Scene {
virtual void render()=0;
/// Reset the scene
- virtual void reset()=0;
+ virtual void reset();
/// Called when a keyboard event occurs
virtual bool keyboardEvent(int key, int scancode, int action, int mods);
@@ -230,11 +233,11 @@ class Scene {
/// Enable/disbale wireframe rendering
void setIsWireframeEnabled(bool isEnabled);
- /// Return all the contact points of the scene
- std::vector<ContactPoint> virtual getContactPoints();
-
/// Update the engine settings
virtual void updateEngineSettings() = 0;
+
+ /// Called when some contacts occur
+ virtual void onContact(const rp3d::CollisionCallback::CallbackData& callbackData) override;
};
// Called when a keyboard event occurs
@@ -247,6 +250,11 @@ inline void Scene::reshape(int width, int height) {
mCamera.setDimensions(width, height);
}
+// Reset the scene
+inline void Scene::reset() {
+ mContactPoints.clear();
+}
+
// Return a reference to the camera
inline const openglframework::Camera& Scene::getCamera() const {
return mCamera;
@@ -306,11 +314,4 @@ inline void Scene::setIsWireframeEnabled(bool isEnabled) {
mIsWireframeEnabled = isEnabled;
}
-// Return all the contact points of the scene
-inline std::vector<ContactPoint> Scene::getContactPoints() {
-
- // Return an empty list of contact points
- return std::vector<ContactPoint>();
-}
-
#endif
diff --git a/testbed/src/SceneDemo.cpp b/testbed/src/SceneDemo.cpp
index 3ecaee92..d4fa3fcc 100644
--- a/testbed/src/SceneDemo.cpp
+++ b/testbed/src/SceneDemo.cpp
@@ -98,7 +98,7 @@ SceneDemo::~SceneDemo() {
mColorShader.destroy();
// Destroy the contact points
- removeAllContactPoints();
+ removeAllVisualContactPoints();
// Destroy rendering data for the AABB
AABB::destroy();
@@ -124,6 +124,9 @@ void SceneDemo::update() {
// Can be called several times per frame
void SceneDemo::updatePhysics() {
+ // Clear contacts points
+ mContactPoints.clear();
+
if (getDynamicsWorld() != nullptr) {
// Take a simulation step
@@ -345,20 +348,17 @@ void SceneDemo::drawTextureQuad() {
void SceneDemo::updateContactPoints() {
// Remove the previous contact points
- removeAllContactPoints();
+ removeAllVisualContactPoints();
if (mIsContactPointsDisplayed) {
- // Get the current contact points of the scene
- std::vector<ContactPoint> contactPoints = getContactPoints();
-
// For each contact point
- std::vector<ContactPoint>::const_iterator it;
- for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
+ std::vector<SceneContactPoint>::const_iterator it;
+ for (it = mContactPoints.begin(); it != mContactPoints.end(); ++it) {
// Create a visual contact point for rendering
VisualContactPoint* point = new VisualContactPoint(it->point, mMeshFolderPath, it->point + it->normal, it->color);
- mContactPoints.push_back(point);
+ mVisualContactPoints.push_back(point);
}
}
}
@@ -367,8 +367,8 @@ void SceneDemo::updateContactPoints() {
void SceneDemo::renderContactPoints(openglframework::Shader& shader, const openglframework::Matrix4& worldToCameraMatrix) {
// Render all the contact points
- for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
- it != mContactPoints.end(); ++it) {
+ for (std::vector<VisualContactPoint*>::iterator it = mVisualContactPoints.begin();
+ it != mVisualContactPoints.end(); ++it) {
(*it)->render(mColorShader, worldToCameraMatrix);
}
}
@@ -397,46 +397,14 @@ void SceneDemo::renderAABBs(const openglframework::Matrix4& worldToCameraMatrix)
}
}
-void SceneDemo::removeAllContactPoints() {
+void SceneDemo::removeAllVisualContactPoints() {
// Destroy all the visual contact points
- for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
- it != mContactPoints.end(); ++it) {
+ for (std::vector<VisualContactPoint*>::iterator it = mVisualContactPoints.begin();
+ it != mVisualContactPoints.end(); ++it) {
delete (*it);
}
- mContactPoints.clear();
-}
-
-// Return all the contact points of the scene
-std::vector<ContactPoint> SceneDemo::computeContactPointsOfWorld(rp3d::DynamicsWorld* world) {
-
- std::vector<ContactPoint> contactPoints;
-
- // Get the list of contact manifolds from the world
- rp3d::List<const rp3d::ContactManifold*> manifolds = world->getContactsList();
-
- // For each contact manifold
- rp3d::List<const rp3d::ContactManifold*>::Iterator it;
- for (it = manifolds.begin(); it != manifolds.end(); ++it) {
-
- const rp3d::ContactManifold* manifold = *it;
-
- // For each contact point of the manifold
- rp3d::ContactPoint* contactPoint = manifold->getContactPoints();
- while (contactPoint != nullptr) {
-
- 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());
- contactPoints.push_back(contact);
-
- contactPoint = contactPoint->getNext();
- }
-
- }
-
- return contactPoints;
+ mVisualContactPoints.clear();
}
// Update the engine settings
diff --git a/testbed/src/SceneDemo.h b/testbed/src/SceneDemo.h
index 95e1ece0..31b832c0 100644
--- a/testbed/src/SceneDemo.h
+++ b/testbed/src/SceneDemo.h
@@ -60,7 +60,7 @@ class SceneDemo : public Scene {
static int shadowMapTextureLevel;
/// All the visual contact points
- std::vector<VisualContactPoint*> mContactPoints;
+ std::vector<VisualContactPoint*> mVisualContactPoints;
/// Shadow map bias matrix
openglframework::Matrix4 mShadowMapBiasMatrix;
@@ -123,7 +123,7 @@ class SceneDemo : public Scene {
void renderAABBs(const openglframework::Matrix4& worldToCameraMatrix);
/// Remove all contact points
- void removeAllContactPoints();
+ void removeAllVisualContactPoints();
/// Return a reference to the dynamics world
rp3d::DynamicsWorld* getDynamicsWorld();
@@ -144,6 +144,9 @@ class SceneDemo : public Scene {
/// Update the scene
virtual void update() override;
+ /// Reset the scene
+ virtual void reset() override;
+
/// Update the physics world (take a simulation step)
/// Can be called several times per frame
virtual void updatePhysics() override;
@@ -159,9 +162,6 @@ class SceneDemo : public Scene {
/// Enabled/Disable the shadow mapping
virtual void setIsShadowMappingEnabled(bool isShadowMappingEnabled) override;
-
- /// Return all the contact points of the scene
- std::vector<ContactPoint> computeContactPointsOfWorld(reactphysics3d::DynamicsWorld *world);
};
// Enabled/Disable the shadow mapping
@@ -184,6 +184,14 @@ inline const rp3d::DynamicsWorld* SceneDemo::getDynamicsWorld() const {
return dynamic_cast<rp3d::DynamicsWorld*>(mPhysicsWorld);
}
+// Reset the scene
+inline void SceneDemo::reset() {
+
+ Scene::reset();
+
+ removeAllVisualContactPoints();
+}
+
#endif