Working on joints ECS

CHANGELOG.md

@@ -12,6 +12,8 @@
 ### Removed
 
  - DynamicsWorld::getContactsList(). You need to use the EventListener class to retrieve contacts now.
+ - The DynamicsWorld::getNbJoints() method has been removed.
+ - The DynamicsWorld::getNbRigidBodies() method has been removed.
 
 ## Release Candidate
 

src/components/JointComponents.h

@@ -155,6 +155,8 @@ class JointComponents : public Components {
         // -------------------- Friendship -------------------- //
 
         friend class BroadPhaseSystem;
+        friend class ConstraintSolverSystem;
+        friend class DynamicsWorld;
 };
 
 // Return the entity of the first body of a joint

src/engine/DynamicsWorld.cpp

@@ -52,13 +52,12 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
                 mIslands(mMemoryManager.getSingleFrameAllocator()),
                 mContactSolverSystem(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
                                mProxyShapesComponents, mConfig),
-                mConstraintSolverSystem(mIslands, mRigidBodyComponents),
+                mConstraintSolverSystem(mIslands, mRigidBodyComponents, mJointsComponents),
                 mDynamicsSystem(mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity),
                 mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
                 mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), 
                 mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
-                mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity),
+                mGravity(gravity), mIsGravityEnabled(true), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
-                mIsGravityEnabled(true),  mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
                 mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep), mCurrentJointId(0) {
 
 #ifdef IS_PROFILING_ACTIVE
@@ -79,8 +78,8 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
 DynamicsWorld::~DynamicsWorld() {
 
     // Destroy all the joints that have not been removed
-    for (int i=mJoints.size() - 1; i >= 0; i--) {
+    for (uint32 i=0; i < mJointsComponents.getNbComponents(); i++) {
-        destroyJoint(mJoints[i]);
+        destroyJoint(mJointsComponents.mJoints[i]);
     }
 
     // Destroy all the rigid bodies that have not been removed
@@ -88,7 +87,7 @@ DynamicsWorld::~DynamicsWorld() {
         destroyRigidBody(mRigidBodies[i]);
     }
 
-    assert(mJoints.size() == 0);
+    assert(mJointsComponents.getNbComponents() == 0);
     assert(mRigidBodies.size() == 0);
 
 #ifdef IS_PROFILING_ACTIVE
@@ -129,6 +128,9 @@ void DynamicsWorld::update(decimal timeStep) {
     // Report the contacts to the user
     mCollisionDetection.reportContacts();
 
+    // Disable the joints for pair of sleeping bodies
+    disableJointsOfSleepingBodies();
+
     // Integrate the velocities
     mDynamicsSystem.integrateRigidBodiesVelocities(timeStep);
 
@@ -173,28 +175,13 @@ void DynamicsWorld::solveContactsAndConstraints(decimal timeStep) {
     // Initialize the contact solver
     mContactSolverSystem.init(mCollisionDetection.mCurrentContactManifolds, mCollisionDetection.mCurrentContactPoints, timeStep);
 
-    // For each island of the world
+    // Initialize the constraint solver
-    for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
+    mConstraintSolverSystem.initialize(timeStep);
-
-        // If there are constraints to solve
-        if (mIslands.joints[islandIndex].size() > 0) {
-
-            // Initialize the constraint solver
-            mConstraintSolverSystem.initializeForIsland(timeStep, islandIndex);
-        }
-    }
 
     // For each iteration of the velocity solver
     for (uint i=0; i<mNbVelocitySolverIterations; i++) {
 
-        for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
+        mConstraintSolverSystem.solveVelocityConstraints();
-
-            // Solve the constraints
-            if (mIslands.joints[islandIndex].size() > 0) {
-
-                mConstraintSolverSystem.solveVelocityConstraints(islandIndex);
-            }
-        }
 
         mContactSolverSystem.solve();
     }
@@ -207,22 +194,30 @@ void DynamicsWorld::solvePositionCorrection() {
 
     RP3D_PROFILE("DynamicsWorld::solvePositionCorrection()", mProfiler);
 
-    // Do not continue if there is no constraints
+    // ---------- Solve the position error correction for the constraints ---------- //
-    if (mJoints.size() == 0) return;
 
-    // For each island of the world
+    // For each iteration of the position (error correction) solver
-    for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
+    for (uint i=0; i<mNbPositionSolverIterations; i++) {
 
-        // ---------- Solve the position error correction for the constraints ---------- //
+        // Solve the position constraints
+        mConstraintSolverSystem.solvePositionConstraints();
+    }
+}
 
-        if (mIslands.joints[islandIndex].size() > 0) {
+// Disable the joints for pair of sleeping bodies
+void DynamicsWorld::disableJointsOfSleepingBodies() {
 
-            // For each iteration of the position (error correction) solver
+    // For each joint
-            for (uint i=0; i<mNbPositionSolverIterations; i++) {
+    for (uint32 i=0; i < mJointsComponents.getNbEnabledComponents(); i++) {
 
-                // Solve the position constraints
+        Entity body1 = mJointsComponents.mBody1Entities[i];
-                mConstraintSolverSystem.solvePositionConstraints(islandIndex);
+        Entity body2 = mJointsComponents.mBody2Entities[i];
-            }
+
+        // If both bodies of the joint are disabled
+        if (mCollisionBodyComponents.getIsEntityDisabled(body1) && mCollisionBodyComponents.getIsEntityDisabled(body2)) {
+
+            // Disable the joint
+            setJointDisabled(mJointsComponents.mJointEntities[i], true);
         }
     }
 }
@@ -423,9 +418,6 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
         mCollisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2);
     }
 
-    // Add the joint into the world
-    mJoints.add(newJoint);
-
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Joint,
              "Joint " + std::to_string(newJoint->getEntity().id) + ": New joint created");
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Joint,
@@ -463,9 +455,6 @@ void DynamicsWorld::destroyJoint(Joint* joint) {
     body1->setIsSleeping(false);
     body2->setIsSleeping(false);
 
-    // Remove the joint from the world
-    mJoints.remove(joint);
-
     // Remove the joint from the joint list of the bodies involved in the joint
     mRigidBodyComponents.removeJointFromBody(body1->getEntity(), joint->getEntity());
     mRigidBodyComponents.removeJointFromBody(body2->getEntity(), joint->getEntity());
@@ -537,8 +526,8 @@ void DynamicsWorld::createIslands() {
 
         mRigidBodyComponents.mIsAlreadyInIsland[b] = false;
     }
-    for (List<Joint*>::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
+    for (uint32 i=0; i < mJointsComponents.getNbComponents(); i++) {
-        mJointsComponents.setIsAlreadyInIsland((*it)->getEntity(), false);
+        mJointsComponents.mIsAlreadyInIsland[i] = false;
     }
 
     // Create a stack for the bodies to visit during the Depth First Search
@@ -640,13 +629,10 @@ void DynamicsWorld::createIslands() {
             const List<Entity>& joints = mRigidBodyComponents.getJoints(rigidBodyToVisit->getEntity());
             for (uint32 i=0; i < joints.size(); i++) {
 
-                Joint* joint = mJointsComponents.getJoint(joints[i]);
-
                 // Check if the current joint has already been added into an island
                 if (mJointsComponents.getIsAlreadyInIsland(joints[i])) continue;
 
                 // Add the joint into the island
-                mIslands.joints[islandIndex].add(joint);
                 mJointsComponents.setIsAlreadyInIsland(joints[i], true);
 
                 const Entity body1Entity = mJointsComponents.getBody1Entity(joints[i]);

src/engine/DynamicsWorld.h

@@ -79,10 +79,6 @@ class DynamicsWorld : public CollisionWorld {
         /// All the rigid bodies of the physics world
         List<RigidBody*> mRigidBodies;
 
-        /// All the joints of the world
-        // TODO : We probably do not need this list anymore
-        List<Joint*> mJoints;
-
         /// Gravity vector of the world
         Vector3 mGravity;
 
@@ -163,6 +159,9 @@ class DynamicsWorld : public CollisionWorld {
         /// Create a rigid body into the physics world.
         RigidBody* createRigidBody(const Transform& transform);
 
+        /// Disable the joints for pair of sleeping bodies
+        void disableJointsOfSleepingBodies();
+
         /// Destroy a rigid body and all the joints which it belongs
         void destroyRigidBody(RigidBody* rigidBody);
 
@@ -184,12 +183,6 @@ class DynamicsWorld : public CollisionWorld {
         /// Enable/Disable the gravity
         void setIsGratityEnabled(bool isGravityEnabled);
 
-        /// Return the number of rigid bodies in the world
-        uint getNbRigidBodies() const;
-
-        /// Return the number of joints in the world
-        uint getNbJoints() const;
-
         /// Return true if the sleeping technique is enabled
         bool isSleepingEnabled() const;
 
@@ -332,22 +325,6 @@ inline void DynamicsWorld::setIsGratityEnabled(bool isGravityEnabled) {
              "Dynamics World: isGravityEnabled= " + (isGravityEnabled ? std::string("true") : std::string("false")));
 }
 
-// Return the number of rigid bodies in the world
-/**
- * @return Number of rigid bodies in the world
- */
-inline uint DynamicsWorld::getNbRigidBodies() const {
-    return mRigidBodies.size();
-}
-
-/// Return the number of joints in the world
-/**
- * @return Number of joints in the world
- */
-inline uint DynamicsWorld::getNbJoints() const {
-    return mJoints.size();
-}
-
 // Return true if the sleeping technique is enabled
 /**
  * @return True if the sleeping technique is enabled and false otherwise

src/engine/Island.cpp

@@ -30,17 +30,14 @@
 using namespace reactphysics3d;
 
 // Constructor
-Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, MemoryManager& memoryManager)
+Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, MemoryManager& memoryManager)
-       : mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0),
+       : mBodies(nullptr), mContactManifolds(nullptr), mNbBodies(0), mNbContactManifolds(0) {
-         mNbContactManifolds(0), mNbJoints(0) {
 
     // Allocate memory for the arrays on the single frame allocator
     mBodies = static_cast<RigidBody**>(memoryManager.allocate(MemoryManager::AllocationType::Frame,
                                                               sizeof(RigidBody*) * nbMaxBodies));
     mContactManifolds = static_cast<ContactManifold**>(memoryManager.allocate(MemoryManager::AllocationType::Frame,
                                                                               sizeof(ContactManifold*) * nbMaxContactManifolds));
-    mJoints = static_cast<Joint**>(memoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                          sizeof(Joint*) * nbMaxJoints));
 }
 
 // Destructor

src/engine/Island.h

@@ -53,25 +53,18 @@ class Island {
         /// Array with all the contact manifolds between bodies of the island
         ContactManifold** mContactManifolds;
 
-        /// Array with all the joints between bodies of the island
-        Joint** mJoints;
-
         /// Current number of bodies in the island
         uint mNbBodies;
 
         /// Current number of contact manifold in the island
         uint mNbContactManifolds;
 
-        /// Current number of joints in the island
-        uint mNbJoints;
-
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
+        Island(uint nbMaxBodies, uint nbMaxContactManifolds, MemoryManager& memoryManager);
-               MemoryManager& memoryManager);
 
         /// Destructor
         ~Island();
@@ -106,9 +99,6 @@ class Island {
         /// Return a pointer to the array of contact manifolds
         ContactManifold** getContactManifolds();
 
-        /// Return a pointer to the array of joints
-        Joint** getJoints();
-
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
@@ -127,12 +117,6 @@ inline void Island::addContactManifold(ContactManifold* contactManifold) {
     mNbContactManifolds++;
 }
 
-// Add a joint into the island
-inline void Island::addJoint(Joint* joint) {
-    mJoints[mNbJoints] = joint;
-    mNbJoints++;
-}
-
 // Return the number of bodies in the island
 inline uint Island::getNbBodies() const {
     return mNbBodies;
@@ -143,11 +127,6 @@ inline uint Island::getNbContactManifolds() const {
     return mNbContactManifolds;
 }
 
-// Return the number of joints in the island
-inline uint Island::getNbJoints() const {
-    return mNbJoints;
-}
-
 // Return a pointer to the array of bodies
 inline RigidBody** Island::getBodies() {
     return mBodies;
@@ -158,11 +137,6 @@ inline ContactManifold** Island::getContactManifolds() {
     return mContactManifolds;
 }
 
-// Return a pointer to the array of joints
-inline Joint** Island::getJoints() {
-    return mJoints;
-}
-
 }
 
 #endif

src/engine/Islands.h

@@ -65,16 +65,12 @@ struct Islands {
         /// 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),
+            :memoryAllocator(allocator), contactManifoldsIndices(allocator), nbContactManifolds(allocator),
-             joints(allocator) {
+             bodyEntities(allocator) {
 
         }
 
@@ -100,7 +96,6 @@ struct Islands {
             contactManifoldsIndices.add(contactManifoldStartIndex);
             nbContactManifolds.add(0);
             bodyEntities.add(List<Entity>(memoryAllocator));
-            joints.add(List<Joint*>(memoryAllocator));
 
             return islandIndex;
         }
@@ -111,7 +106,6 @@ struct Islands {
             contactManifoldsIndices.clear(true);
             nbContactManifolds.clear(true);
             bodyEntities.clear(true);
-            joints.clear(true);
         }
 };
 

src/systems/ConstraintSolverSystem.cpp

@@ -25,14 +25,17 @@
 
 // Libraries
 #include "systems/ConstraintSolverSystem.h"
+#include "components/JointComponents.h"
 #include "utils/Profiler.h"
 #include "engine/Island.h"
 
 using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents)
+ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents,
-                 : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(rigidBodyComponents),
+                                               JointComponents& jointComponents)
+                 : mIsWarmStartingActive(true), mIslands(islands),
+                   mConstraintSolverData(rigidBodyComponents, jointComponents),
                    mSolveBallAndSocketJointSystem(rigidBodyComponents) {
 
 #ifdef IS_PROFILING_ACTIVE
@@ -43,13 +46,10 @@ ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyCompon
 
 }
 
-// Initialize the constraint solver for a given island
+// Initialize the constraint solver
-void ConstraintSolverSystem::initializeForIsland(decimal dt, uint islandIndex) {
+void ConstraintSolverSystem::initialize(decimal dt) {
 
-    RP3D_PROFILE("ConstraintSolverSystem::initializeForIsland()", mProfiler);
+    RP3D_PROFILE("ConstraintSolverSystem::initialize()", mProfiler);
-
-    assert(mIslands.bodyEntities[islandIndex].size() > 0);
-    assert(mIslands.joints[islandIndex].size() > 0);
 
     // Set the current time step
     mTimeStep = dt;
@@ -58,45 +58,46 @@ void ConstraintSolverSystem::initializeForIsland(decimal dt, uint islandIndex) {
     mConstraintSolverData.timeStep = mTimeStep;
     mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
 
-    // For each joint of the island
+    // For each joint
-    for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
+    for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
+
+        const Entity body1 = mConstraintSolverData.jointComponents.mBody1Entities[i];
+        const Entity body2 = mConstraintSolverData.jointComponents.mBody2Entities[i];
+        assert(!mConstraintSolverData.rigidBodyComponents.getIsEntityDisabled(body1));
+        assert(!mConstraintSolverData.rigidBodyComponents.getIsEntityDisabled(body2));
 
         // Initialize the constraint before solving it
-        mIslands.joints[islandIndex][i]->initBeforeSolve(mConstraintSolverData);
+        mConstraintSolverData.jointComponents.mJoints[i]->initBeforeSolve(mConstraintSolverData);
 
         // Warm-start the constraint if warm-starting is enabled
         if (mIsWarmStartingActive) {
-            mIslands.joints[islandIndex][i]->warmstart(mConstraintSolverData);
+            mConstraintSolverData.jointComponents.mJoints[i]->warmstart(mConstraintSolverData);
         }
     }
 }
 
 // Solve the velocity constraints
-void ConstraintSolverSystem::solveVelocityConstraints(uint islandIndex) {
+void ConstraintSolverSystem::solveVelocityConstraints() {
 
     RP3D_PROFILE("ConstraintSolverSystem::solveVelocityConstraints()", mProfiler);
 
-    assert(mIslands.joints[islandIndex].size() > 0);
+    // For each joint
-
+    for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
-    // For each joint of the island
-    for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
 
         // Solve the constraint
-        mIslands.joints[islandIndex][i]->solveVelocityConstraint(mConstraintSolverData);
+        mConstraintSolverData.jointComponents.mJoints[i]->solveVelocityConstraint(mConstraintSolverData);
     }
 }
 
 // Solve the position constraints
-void ConstraintSolverSystem::solvePositionConstraints(uint islandIndex) {
+void ConstraintSolverSystem::solvePositionConstraints() {
 
     RP3D_PROFILE("ConstraintSolverSystem::solvePositionConstraints()", mProfiler);
 
-    assert(mIslands.joints[islandIndex].size() > 0);
+    // For each joint
-
+    for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
-    // For each joint of the island
-    for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
 
         // Solve the constraint
-        mIslands.joints[islandIndex][i]->solvePositionConstraint(mConstraintSolverData);
+        mConstraintSolverData.jointComponents.mJoints[i]->solvePositionConstraint(mConstraintSolverData);
     }
 }

src/systems/ConstraintSolverSystem.h

@@ -39,6 +39,7 @@ class Joint;
 class Island;
 class Profiler;
 class RigidBodyComponents;
+class JointComponents;
 class DynamicsComponents;
 
 // Structure ConstraintSolverData
@@ -53,15 +54,18 @@ struct ConstraintSolverData {
         /// Current time step of the simulation
         decimal timeStep;
 
-        /// Reference to the rigid body  components
+        /// Reference to the rigid body components
         RigidBodyComponents& rigidBodyComponents;
 
+        /// Reference to the joint components
+        JointComponents& jointComponents;
+
         /// True if warm starting of the solver is active
         bool isWarmStartingActive;
 
         /// Constructor
-        ConstraintSolverData(RigidBodyComponents& rigidBodyComponents)
+        ConstraintSolverData(RigidBodyComponents& rigidBodyComponents, JointComponents& jointComponents)
-            :rigidBodyComponents(rigidBodyComponents) {
+            :rigidBodyComponents(rigidBodyComponents), jointComponents(jointComponents) {
 
         }
 
@@ -168,19 +172,20 @@ class ConstraintSolverSystem {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents);
+        ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents,
+                               JointComponents& jointComponents);
 
         /// Destructor
         ~ConstraintSolverSystem() = default;
 
-        /// Initialize the constraint solver for a given island
+        /// Initialize the constraint solver
-        void initializeForIsland(decimal dt, uint islandIndex);
+        void initialize(decimal dt);
 
         /// Solve the constraints
-        void solveVelocityConstraints(uint islandIndex);
+        void solveVelocityConstraints();
 
         /// Solve the position constraints
-        void solvePositionConstraints(uint islandIndex);
+        void solvePositionConstraints();
 
         /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
         bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;