diff --git a/src/body/Body.cpp b/src/body/Body.cpp index c322c53e..1117572f 100644 --- a/src/body/Body.cpp +++ b/src/body/Body.cpp @@ -36,7 +36,7 @@ using namespace reactphysics3d; * @param id ID of the new body */ Body::Body(Entity entity, bodyindex id) - : mID(id), mEntity(entity), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true), + : mID(id), mEntity(entity), mIsAllowedToSleep(true), mIsActive(true), mIsSleeping(false), mSleepTime(0), mUserData(nullptr) { #ifdef IS_LOGGING_ACTIVE diff --git a/src/body/Body.h b/src/body/Body.h index bdee42c6..3da721a0 100644 --- a/src/body/Body.h +++ b/src/body/Body.h @@ -57,9 +57,6 @@ class Body { /// Identifier of the entity in the ECS Entity mEntity; - /// True if the body has already been added in an island (for sleeping technique) - bool mIsAlreadyInIsland; - /// True if the body is allowed to go to sleep for better efficiency bool mIsAllowedToSleep; @@ -75,8 +72,10 @@ class Body { bool mIsActive; /// True if the body is sleeping (for sleeping technique) + // TODO : DELETE THIS bool mIsSleeping; + // TODO : Move this into the body components /// Elapsed time since the body velocity was bellow the sleep velocity decimal mSleepTime; diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 5ff628e4..7b5e4d1c 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -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 /** diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 018eb87e..44b338f2 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -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; diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 393481d0..0595bd2e 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -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; diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 7c4e8964..9587f6cb 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -172,14 +172,6 @@ void CollisionDetection::updateOverlappingPairs(List>& 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(pairId, newContactPairIndex)); diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index e77a1102..8a69ed7d 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -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; diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index 8b8490c4..e124e34f 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -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 { diff --git a/src/collision/ContactPair.h b/src/collision/ContactPair.h index 816302d7..d8fbf46c 100644 --- a/src/collision/ContactPair.h +++ b/src/collision/ContactPair.h @@ -50,12 +50,18 @@ struct ContactPair { /// Indices of the potential contact manifolds List 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) { diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index 7bf228c6..e3290b4b 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -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)); diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 3bf33372..857bd092 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -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); diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 58844faa..5409d318 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -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; igetNbJoints(); i++) { + for (uint i=0; iinitBeforeSolve(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; igetNbJoints(); i++) { + for (uint i=0; isolveVelocityConstraint(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; isolvePositionConstraint(mConstraintSolverData); + mIslands.joints[islandIndex][i]->solvePositionConstraint(mConstraintSolverData); } } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 829d6e32..d9678323 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -29,6 +29,7 @@ // Libraries #include "configuration.h" #include "mathematics/mathematics.h" +#include "engine/Islands.h" namespace reactphysics3d { @@ -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; diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 385cbe5e..133827e2 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -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* contactManifolds, List* 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; igetNbContactManifolds(); 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(externalManifold->getBody1()); - RigidBody* body2 = static_cast(externalManifold->getBody2()); + RigidBody* body1 = static_cast(mBodyComponents.getBody(externalManifold.bodyEntity1)); + RigidBody* body2 = static_cast(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(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; diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 26260c0c..e4797bb4 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -30,6 +30,7 @@ #include "configuration.h" #include "mathematics/Vector3.h" #include "mathematics/Matrix3x3.h" +#include "engine/Islands.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -42,6 +43,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* mAllContactManifolds; + + /// Pointer to the list of contact points from narrow-phase + List* 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* contactManifolds, List* 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, diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 70488265..2dcfc881 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -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(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(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(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, nbBodies * sizeof(Vector3))); mSplitAngularVelocities = static_cast(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(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; igetNbJoints() > 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(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::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds(); - nbContactManifolds += nbBodyManifolds; - } - for (List::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(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, - nbBytesStack)); - - // For each rigid body of the world - for (List::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(contactManifold->getBody1()); - RigidBody* body2 = dynamic_cast(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(joint->getBody1()); - RigidBody* body2 = static_cast(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 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::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(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(joint->getBody1()); - RigidBody* body2 = static_cast(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(mBodyComponents.getBody(mDynamicsComponents.mBodies[b])); + CollisionBody* body = static_cast(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; igetBodies(); - 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(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(mBodyComponents.getBody(bodyEntity)); + body->setIsSleeping(true); } } } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 5256063a..36f3924e 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -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. diff --git a/src/engine/Islands.h b/src/engine/Islands.h new file mode 100644 index 00000000..dec4e49e --- /dev/null +++ b/src/engine/Islands.h @@ -0,0 +1,120 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2018 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_ISLANDS_H +#define REACTPHYSICS3D_ISLANDS_H + +// Libraries +#include "configuration.h" +#include "containers/List.h" +#include "engine/Entity.h" +#include "constraint/Joint.h" + +namespace reactphysics3d { + +// Declarations + +// Structure Islands +/** + * This class contains all the islands of bodies during a frame. + * An island represent an isolated group of awake bodies that are connected with each other by + * some contraints (contacts or joints). + */ +struct Islands { + + private: + + // -------------------- Attributes -------------------- // + + /// Reference to the memory allocator + MemoryAllocator& memoryAllocator; + + public: + + // -------------------- Attributes -------------------- // + + + /// For each island, index of the first contact manifold of the island in the array of contact manifolds + List contactManifoldsIndices; + + /// For each island, number of contact manifolds in the island + List nbContactManifolds; + + /// For each island, list of all the entities of the bodies in the island + List> bodyEntities; + + // TODO : Use joints entities here and not pointers + /// For each island, list of the joints that are part of the island + List> 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(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(memoryAllocator)); + joints.add(List(memoryAllocator)); + + return islandIndex; + } + + /// Clear all the islands + void clear() { + + contactManifoldsIndices.clear(true); + nbContactManifolds.clear(true); + bodyEntities.clear(true); + joints.clear(true); + } +}; + +} + +#endif diff --git a/src/systems/BroadPhaseSystem.cpp b/src/systems/BroadPhaseSystem.cpp index 448df4f1..3b7ec9dc 100644 --- a/src/systems/BroadPhaseSystem.cpp +++ b/src/systems/BroadPhaseSystem.cpp @@ -52,8 +52,8 @@ BroadPhaseSystem::BroadPhaseSystem(CollisionDetection& collisionDetection, Proxy } // Return true if the two broad-phase collision shapes are overlapping -bool BroadPhaseSystem::testOverlappingShapes(const ProxyShape* shape1, - const ProxyShape* shape2) const { +// TODO : Use proxy-shape entities in parameters +bool BroadPhaseSystem::testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const { if (shape1->getBroadPhaseId() == -1 || shape2->getBroadPhaseId() == -1) return false;