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

Refactor contacts and islands

Browse Source
This commit is contained in:
Daniel Chappuis 2019-05-10 17:37:11 +02:00
parent 1c91ef7d48
commit e672c0d617
19 changed files with 408 additions and 409 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

20
src/body/CollisionBody.cpp
View File

@ -307,26 +307,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
/**

4
src/body/CollisionBody.h
View File

@ -67,6 +67,7 @@ class CollisionBody : public Body {
// -------------------- Attributes -------------------- //
// TODO : Move this into the dynamics components
/// Type of body (static, kinematic or dynamic)
BodyType mType;
@ -98,9 +99,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;

2
src/body/RigidBody.cpp
View File

@ -178,6 +178,8 @@ 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;

39
src/collision/CollisionDetection.cpp
View File

@ -172,14 +172,6 @@ void CollisionDetection::updateOverlappingPairs(List<Pair<int, int>>& overlappin
}
}
}
// For each new overlapping pair
for (uint i=0; i < newOverlappingPairs.size(); i++) {
// Wake up the two bodies of the new overlapping pair
mWorld->notifyBodyDisabled(newOverlappingPairs[i]->getShape1()->getBody()->getEntity(), false);
mWorld->notifyBodyDisabled(newOverlappingPairs[i]->getShape1()->getBody()->getEntity(), false);
}
}
// Compute the middle-phase collision detection
@ -201,12 +193,15 @@ void CollisionDetection::computeMiddlePhase() {
// Make all the last frame collision info obsolete
pair->makeLastFrameCollisionInfosObsolete();
const Entity proxyShape1Entity = pair->getShape1()->getEntity();
const Entity proxyShape2Entity = pair->getShape2()->getEntity();
ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2();
assert(shape1->getBroadPhaseId() != -1);
assert(shape2->getBroadPhaseId() != -1);
assert(shape1->getBroadPhaseId() != shape2->getBroadPhaseId());
assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1);
assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1);
assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity));
// Check if the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair
@ -224,15 +219,18 @@ void CollisionDetection::computeMiddlePhase() {
}
// Check if the collision filtering allows collision between the two shapes
if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 &&
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0)) {
if ((mProxyShapesComponents.getCollideWithMaskBits(proxyShape1Entity) & mProxyShapesComponents.getCollisionCategoryBits(proxyShape2Entity)) != 0 &&
(mProxyShapesComponents.getCollisionCategoryBits(proxyShape1Entity) & mProxyShapesComponents.getCollideWithMaskBits(proxyShape2Entity)) != 0) {
CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody();
const Entity body1Entity = body1->getEntity();
const Entity body2Entity = body2->getEntity();
// Check that at least one body is awake and not static
bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC;
bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC;
bool isBody1Active = !mWorld->mBodyComponents.getIsEntityDisabled(body1Entity) && body1->getType() != BodyType::STATIC;
bool isBody2Active = !mWorld->mBodyComponents.getIsEntityDisabled(body2Entity) && body2->getType() != BodyType::STATIC;
if (!isBody1Active && !isBody2Active) continue;
// Check if the bodies are in the set of bodies that cannot collide between each other
@ -500,7 +498,9 @@ void CollisionDetection::createContacts() {
contactPair.nbToTalContactPoints += nbContactPoints;
// We create a new contact manifold
ContactManifold contactManifold(contactPointsIndex, nbContactPoints, mMemoryManager.getPoolAllocator(), mWorld->mConfig);
ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.proxyShape1Entity,
contactPair.proxyShape2Entity, contactPointsIndex, nbContactPoints,
mMemoryManager.getPoolAllocator(), mWorld->mConfig);
// Add the contact manifold
mCurrentContactManifolds->add(contactManifold);
@ -861,9 +861,14 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
Entity body1Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape1()->getBody()->getEntity();
Entity body2Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape2()->getBody()->getEntity();
assert(!mWorld->mBodyComponents.getIsEntityDisabled(body1Entity) || !mWorld->mBodyComponents.getIsEntityDisabled(body2Entity));
// TODO : We should probably use a single frame allocator here
const uint newContactPairIndex = mCurrentContactPairs->size();
ContactPair overlappingPairContact(pairId, body1Entity, body2Entity, newContactPairIndex, mMemoryManager.getPoolAllocator());
ContactPair overlappingPairContact(pairId, body1Entity, body2Entity,
narrowPhaseInfoBatch.overlappingPairs[i]->getShape1()->getEntity(),
narrowPhaseInfoBatch.overlappingPairs[i]->getShape2()->getEntity(),
newContactPairIndex, mMemoryManager.getPoolAllocator());
mCurrentContactPairs->add(overlappingPairContact);
pairContact = &((*mCurrentContactPairs)[newContactPairIndex]);
mCurrentMapPairIdToContactPairIndex->add(Pair<OverlappingPair::OverlappingPairId, uint>(pairId, newContactPairIndex));

9
src/collision/ContactManifold.cpp
View File

@ -38,14 +38,17 @@ ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
mNext(nullptr), mPrevious(nullptr), mIsObsolete(false),
mWorldSettings(worldSettings) {
mWorldSettings(worldSettings), bodyEntity1(0, 0), bodyEntity2(0, 0), proxyShapeEntity1(0, 0), proxyShapeEntity2(0, 0) {
}
// Constructor
// TODO : REMOVE worldSettings from this constructor
ContactManifold::ContactManifold(uint contactPointsIndex, int8 nbContactPoints, MemoryAllocator& allocator, const WorldSettings& worldSettings)
:mMemoryAllocator(allocator), mContactPointsIndex(0), mWorldSettings(worldSettings) {
ContactManifold::ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity proxyShapeEntity1, Entity proxyShapeEntity2,
uint contactPointsIndex, int8 nbContactPoints, MemoryAllocator& allocator, const WorldSettings& worldSettings)
:mMemoryAllocator(allocator), mContactPointsIndex(0), bodyEntity1(bodyEntity1), bodyEntity2(bodyEntity2),
proxyShapeEntity1(proxyShapeEntity1), proxyShapeEntity2(proxyShapeEntity2), mFrictionImpulse1(0), mFrictionImpulse2(0),
mFrictionTwistImpulse(0), mIsAlreadyInIsland(false), mWorldSettings(worldSettings) {
mContactPoints = nullptr;

21
src/collision/ContactManifold.h
View File

@ -91,6 +91,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 -------------------- //
@ -108,6 +109,18 @@ class ContactManifold {
/// Index of the first contact point of the manifold in the list of contact points
uint mContactPointsIndex;
/// 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;
/// Pointer to the first proxy shape of the contact
ProxyShape* mShape1;
@ -230,7 +243,9 @@ class ContactManifold {
const WorldSettings& worldSettings);
/// Constructor
ContactManifold(uint contactPointsIndex, int8 nbContactPoints, MemoryAllocator& allocator, const WorldSettings& worldSettings);
ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity proxyShapeEntity1, Entity proxyShapeEntity2,
uint contactPointsIndex, int8 nbContactPoints,
MemoryAllocator& allocator, const WorldSettings& worldSettings);
/// Destructor
~ContactManifold();
@ -241,6 +256,7 @@ class ContactManifold {
/// Assignment operator
ContactManifold& operator=(const ContactManifold& contactManifold) = default;
/*
/// Return a pointer to the first proxy shape of the contact
ProxyShape* getShape1() const;
@ -252,6 +268,7 @@ class ContactManifold {
/// Return a pointer to the second body of the contact manifold
CollisionBody* getBody2() const;
*/
/// Return the number of contact points in the manifold
int8 getNbContactPoints() const;
@ -275,6 +292,7 @@ class ContactManifold {
friend class CollisionDetection;
};
/*
// Return a pointer to the first proxy shape of the contact
inline ProxyShape* ContactManifold::getShape1() const {
return mShape1;
@ -294,6 +312,7 @@ inline CollisionBody* ContactManifold::getBody1() const {
inline CollisionBody* ContactManifold::getBody2() const {
return mShape2->getBody();
}
*/
// Return the number of contact points in the manifold
inline int8 ContactManifold::getNbContactPoints() const {

14
src/collision/ContactPair.h
View File

@ -50,12 +50,18 @@ struct ContactPair {
/// Indices of the potential contact manifolds
List<uint> potentialContactManifoldsIndices;
/// Entity of the first body of the manifold
/// Entity of the first body of the contact
Entity body1Entity;
/// Entity of the second body of the manifold
/// 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;
@ -77,8 +83,10 @@ struct ContactPair {
// -------------------- Methods -------------------- //
/// Constructor
ContactPair(OverlappingPair::OverlappingPairId pairId, Entity body1Entity, Entity body2Entity, uint contactPairIndex, MemoryAllocator& allocator)
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) {

2
src/constraint/ContactPoint.cpp
View File

@ -51,7 +51,7 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo, const WorldSetti
mPenetrationDepth(contactInfo.penetrationDepth),
mLocalPointOnShape1(contactInfo.localPoint1),
mLocalPointOnShape2(contactInfo.localPoint2),
mIsRestingContact(false), mIsObsolete(false),
mIsRestingContact(false), mPenetrationImpulse(0), mIsObsolete(false),
mWorldSettings(worldSettings) {
assert(mPenetrationDepth > decimal(0.0));

6
src/engine/CollisionWorld.h
View File

@ -163,12 +163,10 @@ 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;
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);

36
src/engine/ConstraintSolver.cpp
View File

@ -31,7 +31,7 @@
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) {
ConstraintSolver::ConstraintSolver(Islands& islands) : mIsWarmStartingActive(true), mIslands(islands) {
#ifdef IS_PROFILING_ACTIVE
@ -42,13 +42,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 +57,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);
}
}

12
src/engine/ConstraintSolver.h
View File

@ -29,6 +29,7 @@
// Libraries
#include "configuration.h"
#include "mathematics/mathematics.h"
#include "engine/Islands.h"
namespace reactphysics3d {
@ -153,6 +154,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 +171,19 @@ class ConstraintSolver {
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver();
ConstraintSolver(Islands& islands);
/// 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;

99
src/engine/ContactSolver.cpp
View File

@ -30,6 +30,8 @@
#include "constraint/ContactPoint.h"
#include "utils/Profiler.h"
#include "engine/Island.h"
#include "components/BodyComponents.h"
#include "components/ProxyShapeComponents.h"
#include "collision/ContactManifold.h"
using namespace reactphysics3d;
@ -41,11 +43,13 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor
ContactSolver::ContactSolver(MemoryManager& memoryManager, const WorldSettings& worldSettings)
ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents,
ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings)
:mMemoryManager(memoryManager), mSplitLinearVelocities(nullptr),
mSplitAngularVelocities(nullptr), mContactConstraints(nullptr),
mContactPoints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
mIsSplitImpulseActive(true), mWorldSettings(worldSettings) {
mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr), mBodyComponents(bodyComponents),
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,33 +101,36 @@ 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(mIslands.bodyEntities[islandIndex].size() > 0);
assert(mIslands.nbContactManifolds[islandIndex] > 0);
assert(mSplitLinearVelocities != nullptr);
assert(mSplitAngularVelocities != nullptr);
// 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;
@ -143,10 +145,10 @@ void ContactSolver::initializeForIsland(Island* island) {
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;
mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse;
mContactConstraints[mNbContactManifolds].nbContacts = externalManifold->getNbContactPoints();
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();
@ -158,27 +160,30 @@ void ContactSolver::initializeForIsland(Island* island) {
const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
// 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 +245,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 +255,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;

32
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,8 @@ class MemoryManager;
class Profiler;
class Island;
class RigidBody;
class BodyComponents;
class ProxyShapeComponents;
// Class Contact Solver
/**
@ -280,18 +283,22 @@ class ContactSolver {
MemoryManager& mMemoryManager;
/// Split linear velocities for the position contact solver (split impulse)
// TODO : Use List<> here
Vector3* mSplitLinearVelocities;
/// Split angular velocities for the position contact solver (split impulse)
// TODO : Use List<> here
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
@ -301,11 +308,29 @@ class ContactSolver {
uint mNbContactManifolds;
/// Array of linear velocities
// TODO : Use List<> here
Vector3* mLinearVelocities;
/// Array of angular velocities
// TODO : Use List<> here
Vector3* mAngularVelocities;
/// Reference to the islands
Islands& mIslands;
/// 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 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,16 +371,17 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Constructor
ContactSolver(MemoryManager& memoryManager, const WorldSettings& worldSettings);
ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents,
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);
void initializeForIsland(uint islandIndex);
/// Set the split velocities arrays
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,

374
src/engine/DynamicsWorld.cpp
View File

@ -47,10 +47,11 @@ 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, mProxyShapesComponents, mConfig),
mConstraintSolver(mIslands),
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
@ -58,12 +59,11 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr),
mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr),
mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr),
mConstrainedOrientations(nullptr), mNbIslands(0), mIslands(nullptr),
mConstrainedOrientations(nullptr),
mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity),
mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0),
mIslands2(mMemoryManager.getSingleFrameAllocator()) {
mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) {
#ifdef IS_PROFILING_ACTIVE
@ -128,32 +128,9 @@ void DynamicsWorld::update(decimal timeStep) {
// Compute the collision detection
mCollisionDetection.computeCollisionDetection();
// Compute the islands (separate groups of bodies with constraints between each others)
computeIslands();
// Create the islands
createIslands();
// TODO : DELETE THIS
/*
std::cout << "--------------------- FRAME ------------------------ " << std::endl;
std::cout << " ---- OLD ISLANDS -----" << std::endl;
for (uint i=0; i < mNbIslands; i++) {
std::cout << "Island " << i << std::endl;
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
std::cout << "Body Id : " << mIslands[i]->getBodies()[b]->getId() << std::endl;
}
}
std::cout << " ---- NEW ISLANDS -----" << std::endl;
for (uint i=0; i < mIslands2.getNbIslands(); i++) {
std::cout << "Island " << i << std::endl;
for (uint b=0; b < mIslands2.bodyEntities[i].size(); b++) {
std::cout << "Body Id : " << mBodyComponents.getBody(mIslands2.bodyEntities[i][b])->getId() << std::endl;
}
}
*/
// Create the actual narrow-phase contacts
mCollisionDetection.createContacts();
@ -181,7 +158,7 @@ void DynamicsWorld::update(decimal timeStep) {
resetBodiesForceAndTorque();
// Reset the islands
mIslands2.clear();
mIslands.clear();
// Reset the single frame memory allocator
mMemoryManager.resetFrameAllocator();
@ -194,19 +171,25 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", mProfiler);
// For each island of the world
for (uint i=0; i < mNbIslands; i++) {
// TODO : We should loop over non-sleeping dynamic components here and not over islands
RigidBody** bodies = mIslands[i]->getBodies();
// For each island of the world
for (uint i=0; i < mIslands.getNbIslands(); i++) {
// For each body of the island
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];
RigidBody* body = static_cast<RigidBody*>(mBodyComponents.getBody(bodyEntity));
// Get the constrained velocity
uint indexArray = bodies[b]->mArrayIndex;
uint indexArray = body->mArrayIndex;
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
// TODO : Remove this
Vector3 testLinVel = newLinVelocity;
// Add the split impulse velocity from Contact Solver (only used
// to update the position)
if (mContactSolver.isSplitImpulseActive()) {
@ -216,8 +199,8 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
}
// Get current position and orientation of the body
const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld;
const Quaternion& currentOrientation = mTransformComponents.getTransform(bodies[b]->getEntity()).getOrientation();
const Vector3& currentPosition = body->mCenterOfMassWorld;
const Quaternion& currentOrientation = mTransformComponents.getTransform(body->getEntity()).getOrientation();
// Update the new constrained position and orientation of the body
mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep;
@ -235,31 +218,37 @@ void DynamicsWorld::updateBodiesState() {
// TODO : Make sure we compute this in a system
// TODO : We should loop over non-sleeping dynamic components here and not over islands
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
// For each body of the island
RigidBody** bodies = mIslands[islandIndex]->getBodies();
for (uint b=0; b < mIslands.bodyEntities[islandIndex].size(); b++) {
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
Entity bodyEntity = mIslands.bodyEntities[islandIndex][b];
RigidBody* body = static_cast<RigidBody*>(mBodyComponents.getBody(bodyEntity));
uint index = bodies[b]->mArrayIndex;
uint index = body->mArrayIndex;
// Update the linear and angular velocity of the body
mDynamicsComponents.setLinearVelocity(bodies[b]->getEntity(), mConstrainedLinearVelocities[index]);
mDynamicsComponents.setAngularVelocity(bodies[b]->getEntity(), mConstrainedAngularVelocities[index]);
mDynamicsComponents.setLinearVelocity(bodyEntity, mConstrainedLinearVelocities[index]);
mDynamicsComponents.setAngularVelocity(bodyEntity, mConstrainedAngularVelocities[index]);
// Update the position of the center of mass of the body
bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
body->mCenterOfMassWorld = mConstrainedPositions[index];
// Update the orientation of the body
mTransformComponents.getTransform(bodies[b]->getEntity()).setOrientation(mConstrainedOrientations[index].getUnit());
mTransformComponents.getTransform(bodyEntity).setOrientation(mConstrainedOrientations[index].getUnit());
// TODO : REMOVE THIS
assert(mConstrainedOrientations[index].lengthSquare() < 1.5 * 1.5);
// Update the transform of the body (using the new center of mass and new orientation)
bodies[b]->updateTransformWithCenterOfMass();
body->updateTransformWithCenterOfMass();
// Update the world inverse inertia tensor of the body
bodies[b]->updateInertiaTensorInverseWorld();
body->updateInertiaTensorInverseWorld();
}
}
@ -275,6 +264,8 @@ void DynamicsWorld::initVelocityArrays() {
// Allocate memory for the bodies velocity arrays
uint nbBodies = mRigidBodies.size();
assert(mDynamicsComponents.getNbComponents() == nbBodies);
mSplitLinearVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
nbBodies * sizeof(Vector3)));
mSplitAngularVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
@ -317,33 +308,37 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
// Initialize the bodies velocity arrays
initVelocityArrays();
// For each island of the world
for (uint i=0; i < mNbIslands; i++) {
// TODO : We should loop over non-sleeping dynamic components here and not over islands
RigidBody** bodies = mIslands[i]->getBodies();
// For each island of the world
for (uint i=0; i < mIslands.getNbIslands(); i++) {
// For each body of the island
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
// Insert the body into the map of constrained velocities
uint indexBody = bodies[b]->mArrayIndex;
const Entity bodyEntity = mIslands.bodyEntities[i][b];
RigidBody* body = static_cast<RigidBody*>(mBodyComponents.getBody(bodyEntity));
const uint indexBody = body->mArrayIndex;
assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
assert(indexBody < mRigidBodies.size());
// 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;
mConstrainedLinearVelocities[indexBody] = body->getLinearVelocity() +
mTimeStep * body->mMassInverse * body->mExternalForce;
mConstrainedAngularVelocities[indexBody] = body->getAngularVelocity() +
mTimeStep * body->getInertiaTensorInverseWorld() *
body->mExternalTorque;
// If the gravity has to be applied to this rigid body
if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) {
if (body->isGravityEnabled() && mIsGravityEnabled) {
// Integrate the gravity force
mConstrainedLinearVelocities[indexBody] += mTimeStep * bodies[b]->mMassInverse *
bodies[b]->getMass() * mGravity;
mConstrainedLinearVelocities[indexBody] += mTimeStep * body->mMassInverse *
body->getMass() * mGravity;
}
// Apply the velocity damping
@ -359,14 +354,12 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
// 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 linDampingFactor = body->getLinearDamping();
decimal angDampingFactor = body->getAngularDamping();
decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
mConstrainedLinearVelocities[indexBody] *= linearDamping;
mConstrainedAngularVelocities[indexBody] *= angularDamping;
indexBody++;
}
}
}
@ -388,28 +381,28 @@ void DynamicsWorld::solveContactsAndConstraints() {
// ---------- 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);
}
}
@ -428,17 +421,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);
}
}
}
@ -704,163 +697,6 @@ uint DynamicsWorld::computeNextAvailableJointId() {
return jointId;
}
// TODO : DELETE THIS
// Compute the islands of awake bodies.
/// 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() {
RP3D_PROFILE("DynamicsWorld::computeIslands()", mProfiler);
uint nbBodies = mRigidBodies.size();
// 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;
int nbContactManifolds = 0;
// 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;
}
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));
// For each rigid body of the world
for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
RigidBody* body = *it;
// If the body has already been added to an island, we go to the next body
if (body->mIsAlreadyInIsland) continue;
// If the body is static, we go to the next body
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;
// Create the new island
void* allocatedMemoryIsland = mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
sizeof(Island));
mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, mJoints.size(),
mMemoryManager);
// While there are still some bodies to visit in the stack
while (stackIndex > 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);
// Add the body into the island
mIslands[mNbIslands]->addBody(bodyToVisit);
// If the current body is static, we do not want to perform the DFS
// search across that body
if (bodyToVisit->getType() == BodyType::STATIC) continue;
// For each contact manifold in which the current body is involded
ContactManifoldListElement* contactElement;
for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != nullptr;
contactElement = contactElement->getNext()) {
ContactManifold* contactManifold = contactElement->getContactManifold();
assert(contactManifold->getNbContactPoints() > 0);
// Check if the current contact manifold has already been added into an island
if (contactManifold->isAlreadyInIsland()) continue;
// Get the other body of the contact manifold
RigidBody* body1 = dynamic_cast<RigidBody*>(contactManifold->getBody1());
RigidBody* body2 = dynamic_cast<RigidBody*>(contactManifold->getBody2());
// If the colliding body is a RigidBody (and not a CollisionBody instead)
if (body1 != nullptr && body2 != nullptr) {
// Add the contact manifold into the island
mIslands[mNbIslands]->addContactManifold(contactManifold);
contactManifold->mIsAlreadyInIsland = true;
RigidBody* otherBody = (body1->getId() == bodyToVisit->getId()) ? body2 : body1;
// Check if the other body has already been added to the island
if (otherBody->mIsAlreadyInIsland) continue;
// Insert the other body into the stack of bodies to visit
stackBodiesToVisit[stackIndex] = otherBody;
stackIndex++;
otherBody->mIsAlreadyInIsland = true;
}
}
// For each joint in which the current body is involved
JointListElement* jointElement;
for (jointElement = bodyToVisit->mJointsList; jointElement != nullptr;
jointElement = jointElement->next) {
Joint* joint = jointElement->joint;
// Check if the current joint has already been added into an island
if (joint->isAlreadyInIsland()) continue;
// Add the joint into the island
mIslands[mNbIslands]->addJoint(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;
// Check if the other body has already been added to the island
if (otherBody->mIsAlreadyInIsland) continue;
// Insert the other body into the stack of bodies to visit
stackBodiesToVisit[stackIndex] = otherBody;
stackIndex++;
otherBody->mIsAlreadyInIsland = 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++) {
if (mIslands[mNbIslands]->mBodies[i]->getType() == BodyType::STATIC) {
mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false;
}
}
mNbIslands++;
}
}
// 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
@ -878,10 +714,11 @@ void DynamicsWorld::createIslands() {
// list of contact pairs involving a non-rigid body
List<uint> nonRigidBodiesContactPairs(mMemoryManager.getSingleFrameAllocator());
RP3D_PROFILE("DynamicsWorld::createIslandsAndContacts()", mProfiler);
RP3D_PROFILE("DynamicsWorld::createIslands()", mProfiler);
// Reset all the isAlreadyInIsland variables of bodies and joints
for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) {
for (uint b=0; b < mDynamicsComponents.getNbComponents(); b++) {
mDynamicsComponents.mIsAlreadyInIsland[b] = false;
}
for (List<Joint*>::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
@ -914,7 +751,7 @@ void DynamicsWorld::createIslands() {
bodyEntityIndicesToVisit.push(mDynamicsComponents.mBodies[b]);
// Create the new island
uint32 islandIndex = mIslands2.addIsland(nbTotalManifolds);
uint32 islandIndex = mIslands.addIsland(nbTotalManifolds);
// While there are still some bodies to visit in the stack
while (bodyEntityIndicesToVisit.size() > 0) {
@ -922,16 +759,17 @@ void DynamicsWorld::createIslands() {
// Get the next body to visit from the stack
const Entity bodyToVisitEntity = bodyEntityIndicesToVisit.pop();
// Awake the body if it is sleeping
notifyBodyDisabled(bodyToVisitEntity, false);
// Add the body into the island
mIslands2.bodyEntities[islandIndex].add(bodyToVisitEntity);
mIslands.bodyEntities[islandIndex].add(bodyToVisitEntity);
// If the current body is static, we do not want to perform the DFS
// search across that body
// TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
RigidBody* rigidBodyToVisit = static_cast<RigidBody*>(mBodyComponents.getBody(bodyToVisitEntity));
// Awake the body if it is sleeping
rigidBodyToVisit->setIsSleeping(false);
if (rigidBodyToVisit->getType() == BodyType::STATIC) continue;
// If the body is involved in contacts with other bodies
@ -962,7 +800,7 @@ void DynamicsWorld::createIslands() {
mCollisionDetection.mContactPairsIndicesOrderingForContacts.add(pair.contactPairIndex);
// Add the contact manifold into the island
mIslands2.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size();
mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size();
pair.isAlreadyInIsland = true;
const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity;
@ -978,6 +816,7 @@ void DynamicsWorld::createIslands() {
// 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;
}
}
}
@ -993,37 +832,31 @@ void DynamicsWorld::createIslands() {
if (joint->isAlreadyInIsland()) continue;
// Add the joint into the island
mIslands2.joints.add(joint);
mIslands.joints[islandIndex].add(joint);
joint->mIsAlreadyInIsland = true;
const Entity body1Entity = joint->getBody1()->getEntity();
const Entity body2Entity = joint->getBody2()->getEntity();
const Entity otherBodyEntity = body1Entity == bodyToVisitEntity ? body2Entity : body1Entity;
// TODO : Maybe avoid those casts here
// 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() == rigidBodyToVisit->getId()) ? body2 : body1;
// 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
bodyEntityIndicesToVisit.push(otherBodyEntity);
otherBody->mIsAlreadyInIsland = true;
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 b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) {
for (uint j=0; j < mDynamicsComponents.getNbEnabledComponents(); j++) {
// 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]));
CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(mDynamicsComponents.mBodies[j]));
if (body->getType() == BodyType::STATIC) {
mDynamicsComponents.mIsAlreadyInIsland[b] = false;
mDynamicsComponents.mIsAlreadyInIsland[j] = false;
}
}
}
@ -1031,6 +864,8 @@ void DynamicsWorld::createIslands() {
// 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);
}
@ -1045,32 +880,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;
}
}
}
@ -1081,8 +920,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);
}
}
}

16
src/engine/DynamicsWorld.h
View File

@ -55,6 +55,9 @@ class DynamicsWorld : public CollisionWorld {
// -------------------- Attributes -------------------- //
/// All the islands of bodies of the current frame
Islands mIslands;
/// Contact solver
ContactSolver mContactSolver;
@ -85,17 +88,21 @@ class DynamicsWorld : public CollisionWorld {
/// True if the gravity force is on
bool mIsGravityEnabled;
// TODO : Move this into dynamic components
/// Array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
Vector3* mConstrainedLinearVelocities;
// TODO : Move this into dynamic components
/// Array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
Vector3* mConstrainedAngularVelocities;
// TODO : Move this into dynamic components
/// Split linear velocities for the position contact solver (split impulse)
Vector3* mSplitLinearVelocities;
// TODO : Move this into dynamic components
/// Split angular velocities for the position contact solver (split impulse)
Vector3* mSplitAngularVelocities;
@ -105,12 +112,6 @@ class DynamicsWorld : public CollisionWorld {
/// 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;
@ -127,9 +128,6 @@ class DynamicsWorld : public CollisionWorld {
/// Current joint id
uint mCurrentJointId;
/// All the islands of bodies of the current frame
Islands mIslands2;
// -------------------- Methods -------------------- //
/// Integrate the positions and orientations of rigid bodies.

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

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;