mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'contacts' into ecs

Browse Source
This commit is contained in:
Daniel Chappuis 2019-06-27 07:26:06 +02:00
commit 204c9cd50d
86 changed files with 3934 additions and 3541 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
CHANGELOG.md
View File

@ -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)

6
CMakeLists.txt
View File

@ -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"

2
src/body/Body.cpp
View File

@ -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

7
src/body/Body.h
View File

@ -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;

45
src/body/CollisionBody.cpp
View File

@ -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
/**

22
src/body/CollisionBody.h
View File

@ -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

248
src/body/RigidBody.cpp
View File

@ -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());
}
}

168
src/body/RigidBody.h
View File

@ -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

102
src/collision/CollisionCallback.cpp
View File

@ -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);
}

226
src/collision/CollisionCallback.h
View File

@ -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

1474
src/collision/CollisionDetection.cpp
View File

File diff suppressed because it is too large Load Diff

184
src/collision/CollisionDetection.h
View File

@ -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;

361
src/collision/ContactManifold.cpp
View File

@ -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;
}
}

188
src/collision/ContactManifold.h
View File

@ -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

67
src/collision/ContactManifoldInfo.h Normal file
View File

@ -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

295
src/collision/ContactManifoldSet.cpp
View File

@ -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();
}
}

166
src/collision/ContactManifoldSet.h
View File

@ -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

98
src/collision/ContactPair.h Normal file
View File

@ -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

2
src/collision/ContactPointInfo.h
View File

@ -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;

53
src/collision/OverlapCallback.cpp Normal file
View File

@ -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) {
}

121
src/collision/OverlapCallback.h
View File

@ -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

8
src/collision/ProxyShape.cpp
View File

@ -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);

8
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
View File

@ -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;
}
}
}

3
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
View File

@ -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);
};
}

10
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
View File

@ -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;
}
}
}
}

4
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h
View File

@ -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);
};
}

11
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp
View File

@ -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;

5
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h
View File

@ -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);
};
}

1
src/collision/narrowphase/NarrowPhaseInfoBatch.cpp
View File

@ -28,6 +28,7 @@
#include "collision/ContactPointInfo.h"
#include "collision/shapes/TriangleShape.h"
#include "engine/OverlappingPair.h"
#include <iostream>
using namespace reactphysics3d;

28
src/collision/narrowphase/SAT/SATAlgorithm.cpp
View File

@ -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;

20
src/collision/narrowphase/SAT/SATAlgorithm.h
View File

@ -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

5
src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
View File

@ -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;
}
}

3
src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
View File

@ -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);
};
}

13
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
View File

@ -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;
}
}

5
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
View File

@ -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);
};
}

5
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -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;
}
}
}

3
src/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -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);
};
}

8
src/components/Components.h
View File

@ -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

148
src/components/DynamicsComponents.cpp
View File

@ -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();
}

473
src/components/DynamicsComponents.h
View File

@ -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

14
src/configuration.h
View File

@ -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();

60
src/constraint/BallAndSocketJoint.cpp
View File

@ -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);
}

15
src/constraint/ContactPoint.cpp
View File

@ -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) {

82
src/constraint/ContactPoint.h
View File

@ -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)

62
src/constraint/FixedJoint.cpp
View File

@ -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);
}

64
src/constraint/HingeJoint.cpp
View File

@ -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);
}

3
src/constraint/Joint.cpp
View File

@ -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) {

14
src/constraint/Joint.h
View File

@ -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;

72
src/constraint/SliderJoint.cpp
View File

@ -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

2
src/containers/Set.h
View File

@ -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);

47
src/engine/CollisionWorld.cpp
View File

@ -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

83
src/engine/CollisionWorld.h
View File

@ -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

37
src/engine/ConstraintSolver.cpp
View File

@ -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);
}
}

60
src/engine/ConstraintSolver.h
View File

@ -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

288
src/engine/ContactSolver.cpp
View File

@ -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;
}
}
}

79
src/engine/ContactSolver.h
View File

@ -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;

521
src/engine/DynamicsWorld.cpp
View File

@ -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;
}

47
src/engine/DynamicsWorld.h
View File

@ -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();
}
}

12
src/engine/EventListener.h
View File

@ -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

120
src/engine/Islands.h Normal file
View File

@ -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

2
src/engine/OverlappingPair.cpp
View File

@ -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) {

69
src/engine/OverlappingPair.h
View File

@ -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

4
src/memory/DefaultAllocator.h
View File

@ -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);
}

4
src/systems/BroadPhaseSystem.cpp
View File

@ -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;

2
src/systems/BroadPhaseSystem.h
View File

@ -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;

772
test/tests/collision/TestCollisionWorld.h
View File

File diff suppressed because it is too large Load Diff

55
testbed/scenes/collisiondetection/CollisionDetectionScene.cpp
View File

@ -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()));
}
}
}

32
testbed/scenes/collisiondetection/CollisionDetectionScene.h
View File

@ -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

6
testbed/scenes/collisionshapes/CollisionShapesScene.cpp
View File

@ -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++) {

8
testbed/scenes/collisionshapes/CollisionShapesScene.h
View File

@ -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

6
testbed/scenes/concavemesh/ConcaveMeshScene.cpp
View File

@ -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++) {

8
testbed/scenes/concavemesh/ConcaveMeshScene.h
View File

@ -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

6
testbed/scenes/cubes/CubesScene.cpp
View File

@ -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

8
testbed/scenes/cubes/CubesScene.h
View File

@ -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

6
testbed/scenes/cubestack/CubeStackScene.cpp
View File

@ -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--) {

8
testbed/scenes/cubestack/CubeStackScene.h
View File

@ -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

6
testbed/scenes/heightfield/HeightFieldScene.cpp
View File

@ -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++) {

8
testbed/scenes/heightfield/HeightFieldScene.h
View File

@ -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

6
testbed/scenes/joints/JointsScene.cpp
View File

@ -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);

8
testbed/scenes/joints/JointsScene.h
View File

@ -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

4
testbed/scenes/raycast/RaycastScene.cpp
View File

@ -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()));

22
testbed/scenes/raycast/RaycastScene.h
View File

@ -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

22
testbed/src/Scene.cpp
View File

@ -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);
}
}
}

33
testbed/src/Scene.h
View File

@ -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

60
testbed/src/SceneDemo.cpp
View File

@ -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

18
testbed/src/SceneDemo.h
View File

@ -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