From f0b8121795bff1f66836f3b1abc34b8ec2168c11 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 27 Sep 2019 07:20:30 +0200 Subject: [PATCH] Working on joints ECS --- CHANGELOG.md | 2 + src/components/JointComponents.h | 2 + src/engine/DynamicsWorld.cpp | 78 +++++++++++--------------- src/engine/DynamicsWorld.h | 29 +--------- src/engine/Island.cpp | 7 +-- src/engine/Island.h | 28 +-------- src/engine/Islands.h | 10 +--- src/systems/ConstraintSolverSystem.cpp | 49 ++++++++-------- src/systems/ConstraintSolverSystem.h | 21 ++++--- 9 files changed, 82 insertions(+), 144 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7ff8f1d9..db4b85ba 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/src/components/JointComponents.h b/src/components/JointComponents.h index 8f7d0eb2..e9986ec2 100644 --- a/src/components/JointComponents.h +++ b/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 diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index ee07c254..7daba967 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/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), - mIsGravityEnabled(true), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity), + mGravity(gravity), 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--) { - destroyJoint(mJoints[i]); + for (uint32 i=0; i < mJointsComponents.getNbComponents(); 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 - for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) { - - // If there are constraints to solve - if (mIslands.joints[islandIndex].size() > 0) { - - // Initialize the constraint solver - mConstraintSolverSystem.initializeForIsland(timeStep, islandIndex); - } - } + // Initialize the constraint solver + mConstraintSolverSystem.initialize(timeStep); // For each iteration of the velocity solver for (uint i=0; i 0) { - - mConstraintSolverSystem.solveVelocityConstraints(islandIndex); - } - } + mConstraintSolverSystem.solveVelocityConstraints(); mContactSolverSystem.solve(); } @@ -207,22 +194,30 @@ void DynamicsWorld::solvePositionCorrection() { RP3D_PROFILE("DynamicsWorld::solvePositionCorrection()", mProfiler); - // Do not continue if there is no constraints - if (mJoints.size() == 0) return; + // ---------- Solve the position error correction for the constraints ---------- // - // For each island of the world - for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) { + // For each iteration of the position (error correction) solver + for (uint i=0; i 0) { +// Disable the joints for pair of sleeping bodies +void DynamicsWorld::disableJointsOfSleepingBodies() { - // For each iteration of the position (error correction) solver - for (uint i=0; igetEntity().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::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) { - mJointsComponents.setIsAlreadyInIsland((*it)->getEntity(), false); + for (uint32 i=0; i < mJointsComponents.getNbComponents(); i++) { + 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& 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]); diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index a8f32617..4e19ab9e 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -79,10 +79,6 @@ class DynamicsWorld : public CollisionWorld { /// All the rigid bodies of the physics world List mRigidBodies; - /// All the joints of the world - // TODO : We probably do not need this list anymore - List 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 diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp index fb75c68b..d69918bc 100644 --- a/src/engine/Island.cpp +++ b/src/engine/Island.cpp @@ -30,17 +30,14 @@ using namespace reactphysics3d; // Constructor -Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, MemoryManager& memoryManager) - : mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0), - mNbContactManifolds(0), mNbJoints(0) { +Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, MemoryManager& memoryManager) + : mBodies(nullptr), mContactManifolds(nullptr), mNbBodies(0), mNbContactManifolds(0) { // Allocate memory for the arrays on the single frame allocator mBodies = static_cast(memoryManager.allocate(MemoryManager::AllocationType::Frame, sizeof(RigidBody*) * nbMaxBodies)); mContactManifolds = static_cast(memoryManager.allocate(MemoryManager::AllocationType::Frame, sizeof(ContactManifold*) * nbMaxContactManifolds)); - mJoints = static_cast(memoryManager.allocate(MemoryManager::AllocationType::Frame, - sizeof(Joint*) * nbMaxJoints)); } // Destructor diff --git a/src/engine/Island.h b/src/engine/Island.h index 9c786e51..6dcee95f 100644 --- a/src/engine/Island.h +++ b/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, - MemoryManager& memoryManager); + Island(uint nbMaxBodies, uint nbMaxContactManifolds, 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 diff --git a/src/engine/Islands.h b/src/engine/Islands.h index dec4e49e..26011fa3 100644 --- a/src/engine/Islands.h +++ b/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> 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) { + :memoryAllocator(allocator), contactManifoldsIndices(allocator), nbContactManifolds(allocator), + bodyEntities(allocator) { } @@ -100,7 +96,6 @@ struct Islands { contactManifoldsIndices.add(contactManifoldStartIndex); nbContactManifolds.add(0); bodyEntities.add(List(memoryAllocator)); - joints.add(List(memoryAllocator)); return islandIndex; } @@ -111,7 +106,6 @@ struct Islands { contactManifoldsIndices.clear(true); nbContactManifolds.clear(true); bodyEntities.clear(true); - joints.clear(true); } }; diff --git a/src/systems/ConstraintSolverSystem.cpp b/src/systems/ConstraintSolverSystem.cpp index 4a8d49eb..55a0e9f1 100644 --- a/src/systems/ConstraintSolverSystem.cpp +++ b/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) - : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(rigidBodyComponents), +ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& 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 -void ConstraintSolverSystem::initializeForIsland(decimal dt, uint islandIndex) { +// Initialize the constraint solver +void ConstraintSolverSystem::initialize(decimal dt) { - RP3D_PROFILE("ConstraintSolverSystem::initializeForIsland()", mProfiler); - - assert(mIslands.bodyEntities[islandIndex].size() > 0); - assert(mIslands.joints[islandIndex].size() > 0); + RP3D_PROFILE("ConstraintSolverSystem::initialize()", mProfiler); // 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 (uint i=0; iinitBeforeSolve(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 of the island - for (uint i=0; isolveVelocityConstraint(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 of the island - for (uint i=0; isolvePositionConstraint(mConstraintSolverData); + mConstraintSolverData.jointComponents.mJoints[i]->solvePositionConstraint(mConstraintSolverData); } } diff --git a/src/systems/ConstraintSolverSystem.h b/src/systems/ConstraintSolverSystem.h index da7ec7ec..0d1dbfd9 100644 --- a/src/systems/ConstraintSolverSystem.h +++ b/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) - :rigidBodyComponents(rigidBodyComponents) { + ConstraintSolverData(RigidBodyComponents& rigidBodyComponents, JointComponents& jointComponents) + :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 - void initializeForIsland(decimal dt, uint islandIndex); + /// Initialize the constraint solver + 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;