From 9afedae1a7a973a33942dc140fe610675f21013b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 16 May 2019 17:46:26 +0200 Subject: [PATCH] Use DynamicsComponents for constrained linear/angular velocities in solvers --- src/body/RigidBody.h | 1 + src/components/Components.h | 8 ++ src/components/DynamicsComponents.cpp | 20 +++- src/components/DynamicsComponents.h | 69 +++++++++++- src/constraint/BallAndSocketJoint.cpp | 23 ++-- src/constraint/FixedJoint.cpp | 23 ++-- src/constraint/HingeJoint.cpp | 23 ++-- src/constraint/Joint.cpp | 3 +- src/constraint/Joint.h | 8 ++ src/constraint/SliderJoint.cpp | 23 ++-- src/engine/ConstraintSolver.cpp | 3 +- src/engine/ConstraintSolver.h | 29 ++--- src/engine/ContactSolver.cpp | 147 +++++++++++++------------- src/engine/ContactSolver.h | 38 +++---- src/engine/DynamicsWorld.cpp | 49 +++------ src/engine/DynamicsWorld.h | 10 -- 16 files changed, 277 insertions(+), 200 deletions(-) diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 5946e990..49e087a4 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -53,6 +53,7 @@ class RigidBody : public CollisionBody { private : /// Index of the body in arrays for contact/constraint solver + // TODO : REMOVE THIS uint mArrayIndex; protected : diff --git a/src/components/Components.h b/src/components/Components.h index e4dc213e..320104a3 100644 --- a/src/components/Components.h +++ b/src/components/Components.h @@ -121,6 +121,9 @@ class Components { /// Return the number of enabled components uint32 getNbEnabledComponents() const; + + /// Return the index in the arrays for a given entity + uint32 getEntityIndex(Entity entity) const; }; // Return true if an entity is sleeping @@ -144,6 +147,11 @@ inline uint32 Components::getNbEnabledComponents() const { return mDisabledStartIndex; } +// Return the index in the arrays for a given entity +inline uint32 Components::getEntityIndex(Entity entity) const { + assert(hasComponent(entity)); + return mMapEntityToComponentIndex[entity]; +} } #endif diff --git a/src/components/DynamicsComponents.cpp b/src/components/DynamicsComponents.cpp index 838584a4..f02a0728 100644 --- a/src/components/DynamicsComponents.cpp +++ b/src/components/DynamicsComponents.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; // Constructor DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator) - :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3) + sizeof(bool)) { + :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(bool)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -57,7 +57,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { Entity* newBodies = static_cast(newBuffer); Vector3* newLinearVelocities = reinterpret_cast(newBodies + nbComponentsToAllocate); Vector3* newAngularVelocities = reinterpret_cast(newLinearVelocities + nbComponentsToAllocate); - bool* newIsAlreadyInIsland = reinterpret_cast(newAngularVelocities + nbComponentsToAllocate); + Vector3* newConstrainedLinearVelocities = reinterpret_cast(newAngularVelocities + nbComponentsToAllocate); + Vector3* newConstrainedAngularVelocities = reinterpret_cast(newConstrainedLinearVelocities + nbComponentsToAllocate); + bool* newIsAlreadyInIsland = reinterpret_cast(newConstrainedAngularVelocities + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -66,6 +68,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newBodies, mBodies, mNbComponents * sizeof(Entity)); memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Vector3)); memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Vector3)); + memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3)); + memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3)); memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); // Deallocate previous memory @@ -76,6 +80,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { mBodies = newBodies; mLinearVelocities = newLinearVelocities; mAngularVelocities = newAngularVelocities; + mConstrainedLinearVelocities = newConstrainedLinearVelocities; + mConstrainedAngularVelocities = newConstrainedAngularVelocities; mIsAlreadyInIsland = newIsAlreadyInIsland; mNbAllocatedComponents = nbComponentsToAllocate; } @@ -90,6 +96,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const new (mBodies + index) Entity(bodyEntity); new (mLinearVelocities + index) Vector3(component.linearVelocity); new (mAngularVelocities + index) Vector3(component.angularVelocity); + new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0); + new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0); mIsAlreadyInIsland[index] = false; // Map the entity with the new component lookup index @@ -111,6 +119,8 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) new (mBodies + destIndex) Entity(mBodies[srcIndex]); new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]); new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]); + new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]); + new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]); mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; // Destroy the source component @@ -134,6 +144,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { Entity entity1(mBodies[index1]); Vector3 linearVelocity1(mLinearVelocities[index1]); Vector3 angularVelocity1(mAngularVelocities[index1]); + Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]); + Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]); bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; // Destroy component 1 @@ -145,6 +157,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { new (mBodies + index2) Entity(entity1); new (mLinearVelocities + index2) Vector3(linearVelocity1); new (mAngularVelocities + index2) Vector3(angularVelocity1); + new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1); + new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1); mIsAlreadyInIsland[index2] = isAlreadyInIsland1; // Update the entity to component index mapping @@ -167,4 +181,6 @@ void DynamicsComponents::destroyComponent(uint32 index) { mBodies[index].~Entity(); mLinearVelocities[index].~Vector3(); mAngularVelocities[index].~Vector3(); + mConstrainedLinearVelocities[index].~Vector3(); + mConstrainedAngularVelocities[index].~Vector3(); } diff --git a/src/components/DynamicsComponents.h b/src/components/DynamicsComponents.h index 696208bf..715e5607 100644 --- a/src/components/DynamicsComponents.h +++ b/src/components/DynamicsComponents.h @@ -59,6 +59,12 @@ class DynamicsComponents : public Components { /// Array with the angular velocity of each component Vector3* mAngularVelocities; + /// Array with the constrained linear velocity of each component + Vector3* mConstrainedLinearVelocities; + + /// Array with the constrained angular velocity of each component + Vector3* mConstrainedAngularVelocities; + /// Array with the boolean value to know if the body has already been added into an island bool* mIsAlreadyInIsland; @@ -103,10 +109,16 @@ class DynamicsComponents : public Components { void addComponent(Entity bodyEntity, bool isSleeping, const DynamicsComponent& component); /// Return the linear velocity of an entity - Vector3& getLinearVelocity(Entity bodyEntity) const; + const Vector3& getLinearVelocity(Entity bodyEntity) const; /// Return the angular velocity of an entity - Vector3& getAngularVelocity(Entity bodyEntity) const; + const Vector3& getAngularVelocity(Entity bodyEntity) const; + + /// Return the constrained linear velocity of an entity + const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const; + + /// Return the constrained angular velocity of an entity + const Vector3& getConstrainedAngularVelocity(Entity bodyEntity) const; /// Return true if the entity is already in an island bool getIsAlreadyInIsland(Entity bodyEntity) const; @@ -117,17 +129,31 @@ class DynamicsComponents : public Components { /// Set the angular velocity of an entity void setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity); + /// Set the constrained linear velocity of an entity + void setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity); + + /// Set the constrained angular velocity of an entity + void setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity); + /// Set the value to know if the entity is already in an island bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland); + + // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; friend class DynamicsWorld; + friend class ContactSolver; + friend class BallAndSocketJoint; + friend class FixedJoint; + friend class HingeJoint; + friend class SliderJoint; + }; // Return the linear velocity of an entity -inline Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const { +inline const Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -135,7 +161,7 @@ inline Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const { } // Return the angular velocity of an entity -inline Vector3& DynamicsComponents::getAngularVelocity(Entity bodyEntity) const { +inline const Vector3 &DynamicsComponents::getAngularVelocity(Entity bodyEntity) const { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -158,6 +184,41 @@ inline void DynamicsComponents::setAngularVelocity(Entity bodyEntity, const Vect mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = angularVelocity; } +// Return the constrained linear velocity of an entity +inline const Vector3 &DynamicsComponents::getConstrainedLinearVelocity(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + // TODO : DELETE THIS + uint testIndex = mMapEntityToComponentIndex[bodyEntity]; + + return mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the constrained angular velocity of an entity +inline const Vector3 &DynamicsComponents::getConstrainedAngularVelocity(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Set the constrained linear velocity of an entity +inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mConstrainedLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedLinearVelocity; +} + +// Set the constrained angular velocity of an entity +inline void DynamicsComponents::setConstrainedAngularVelocity(Entity bodyEntity, const Vector3& constrainedAngularVelocity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mConstrainedAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = constrainedAngularVelocity; +} + // Return true if the entity is already in an island inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const { diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 0da68185..fbf1c6b6 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -26,6 +26,7 @@ // Libraries #include "BallAndSocketJoint.h" #include "engine/ConstraintSolver.h" +#include "components/DynamicsComponents.h" using namespace reactphysics3d; @@ -98,11 +99,14 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS // Warm start the constraint (apply the previous impulse at the beginning of the step) void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) { + uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Compute the impulse P=J^T * lambda for the body 1 const Vector3 linearImpulseBody1 = -mImpulse; @@ -123,11 +127,14 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD // Solve the velocity constraint void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { + uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Compute J*v const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World); diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 99fca6c2..022984ea 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -26,6 +26,7 @@ // Libraries #include "FixedJoint.h" #include "engine/ConstraintSolver.h" +#include "components/DynamicsComponents.h" using namespace reactphysics3d; @@ -129,11 +130,14 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Warm start the constraint (apply the previous impulse at the beginning of the step) void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { + uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass of the bodies const decimal inverseMassBody1 = mBody1->mMassInverse; @@ -164,11 +168,14 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Solve the velocity constraint void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { + uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass of the bodies decimal inverseMassBody1 = mBody1->mMassInverse; diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 50106d13..41b22a83 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -26,6 +26,7 @@ // Libraries #include "HingeJoint.h" #include "engine/ConstraintSolver.h" +#include "components/DynamicsComponents.h" using namespace reactphysics3d; @@ -198,11 +199,14 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Warm start the constraint (apply the previous impulse at the beginning of the step) void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { + uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies const decimal inverseMassBody1 = mBody1->mMassInverse; @@ -254,11 +258,14 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Solve the velocity constraint void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { + uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = mBody1->mMassInverse; diff --git a/src/constraint/Joint.cpp b/src/constraint/Joint.cpp index a24b1f2e..f1ca34eb 100644 --- a/src/constraint/Joint.cpp +++ b/src/constraint/Joint.cpp @@ -30,7 +30,8 @@ using namespace reactphysics3d; // Constructor Joint::Joint(uint id, const JointInfo& jointInfo) - :mId(id), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mType(jointInfo.type), + :mId(id), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mBody1Entity(jointInfo.body1->getEntity()), + mBody2Entity(jointInfo.body2->getEntity()), mType(jointInfo.type), mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique), mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) { diff --git a/src/constraint/Joint.h b/src/constraint/Joint.h index 1c89d464..880ace90 100644 --- a/src/constraint/Joint.h +++ b/src/constraint/Joint.h @@ -124,11 +124,19 @@ class Joint { uint mId; /// Pointer to the first body of the joint + // TODO : Use Entities instead RigidBody* const mBody1; /// Pointer to the second body of the joint + // TODO : Use Entities instead RigidBody* const mBody2; + /// Entity of the first body of the joint + Entity mBody1Entity; + + /// Entity of the second body of the joint + Entity mBody2Entity; + /// Type of the joint const JointType mType; diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index b01fb410..51501827 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -26,6 +26,7 @@ // Libraries #include "SliderJoint.h" #include "engine/ConstraintSolver.h" +#include "components/DynamicsComponents.h" using namespace reactphysics3d; @@ -216,11 +217,14 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Warm start the constraint (apply the previous impulse at the beginning of the step) void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { + uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies const decimal inverseMassBody1 = mBody1->mMassInverse; @@ -275,11 +279,14 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Solve the velocity constraint void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { + uint32 dynamicsComponentIndexBody1 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.dynamicsComponents.getEntityIndex(mBody2Entity); + // Get the velocities - Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; - Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; - Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; - Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + Vector3& v1 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; + Vector3& v2 = constraintSolverData.dynamicsComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2]; + Vector3& w1 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1]; + Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = mBody1->mMassInverse; diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 5409d318..2d95e041 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -31,7 +31,8 @@ using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver(Islands& islands) : mIsWarmStartingActive(true), mIslands(islands) { +ConstraintSolver::ConstraintSolver(Islands& islands, DynamicsComponents& dynamicsComponents) + : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(dynamicsComponents) { #ifdef IS_PROFILING_ACTIVE diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index d9678323..d051daf2 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -37,6 +37,7 @@ namespace reactphysics3d { class Joint; class Island; class Profiler; +class DynamicsComponents; // Structure ConstraintSolverData /** @@ -50,11 +51,8 @@ struct ConstraintSolverData { /// Current time step of the simulation decimal timeStep; - /// Array with the bodies linear velocities - Vector3* linearVelocities; - - /// Array with the bodies angular velocities - Vector3* angularVelocities; + /// Reference to the dynamics components + DynamicsComponents& dynamicsComponents; /// Reference to the bodies positions Vector3* positions; @@ -66,8 +64,8 @@ struct ConstraintSolverData { bool isWarmStartingActive; /// Constructor - ConstraintSolverData() :linearVelocities(nullptr), angularVelocities(nullptr), - positions(nullptr), orientations(nullptr) { + ConstraintSolverData(DynamicsComponents& dynamicsComponents) + :dynamicsComponents(dynamicsComponents), positions(nullptr), orientations(nullptr) { } @@ -171,7 +169,7 @@ class ConstraintSolver { // -------------------- Methods -------------------- // /// Constructor - ConstraintSolver(Islands& islands); + ConstraintSolver(Islands& islands, DynamicsComponents& dynamicsComponents); /// Destructor ~ConstraintSolver() = default; @@ -191,10 +189,6 @@ class ConstraintSolver { /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); - /// Set the constrained velocities arrays - void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities); - /// Set the constrained positions/orientations arrays void setConstrainedPositionsArrays(Vector3* constrainedPositions, Quaternion* constrainedOrientations); @@ -208,17 +202,6 @@ class ConstraintSolver { }; -// Set the constrained velocities arrays -inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities) { - - assert(constrainedLinearVelocities != nullptr); - assert(constrainedAngularVelocities != nullptr); - - mConstraintSolverData.linearVelocities = constrainedLinearVelocities; - mConstraintSolverData.angularVelocities = constrainedAngularVelocities; -} - // Set the constrained positions/orientations arrays inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions, Quaternion* constrainedOrientations) { diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 133827e2..c95b6ffd 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -31,6 +31,7 @@ #include "utils/Profiler.h" #include "engine/Island.h" #include "components/BodyComponents.h" +#include "components/DynamicsComponents.h" #include "components/ProxyShapeComponents.h" #include "collision/ContactManifold.h" @@ -43,13 +44,14 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents, +ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents, DynamicsComponents& dynamicsComponents, ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings) :mMemoryManager(memoryManager), mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mContactConstraints(nullptr), - mContactPoints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), + mContactPoints(nullptr), mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr), mBodyComponents(bodyComponents), - mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true), mWorldSettings(worldSettings) { + mDynamicsComponents(dynamicsComponents), mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true), + mWorldSettings(worldSettings) { #ifdef IS_PROFILING_ACTIVE mProfiler = nullptr; @@ -141,6 +143,8 @@ void ContactSolver::initializeForIsland(uint islandIndex) { new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex; mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex; + mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mDynamicsComponents.getEntityIndex(body1->getEntity()); + mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mDynamicsComponents.getEntityIndex(body2->getEntity()); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse; @@ -154,10 +158,10 @@ void ContactSolver::initializeForIsland(uint islandIndex) { mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero(); // Get the velocities of the bodies - const Vector3& v1 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody1]; - const Vector3& w1 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody1]; - const Vector3& v2 = mLinearVelocities[mContactConstraints[mNbContactManifolds].indexBody2]; - const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2]; + const Vector3& v1 = mDynamicsComponents.getLinearVelocity(externalManifold.bodyEntity1); + const Vector3& w1 = mDynamicsComponents.getAngularVelocity(externalManifold.bodyEntity1); + const Vector3& v2 = mDynamicsComponents.getLinearVelocity(externalManifold.bodyEntity2); + const Vector3& w2 = mDynamicsComponents.getAngularVelocity(externalManifold.bodyEntity2); // For each contact point of the contact manifold assert(externalManifold.getNbContactPoints() > 0); @@ -346,6 +350,7 @@ void ContactSolver::warmStart() { for (short int i=0; i here - Vector3* mLinearVelocities; - - /// Array of angular velocities - // TODO : Use List<> here - Vector3* mAngularVelocities; - /// Reference to the islands Islands& mIslands; @@ -327,6 +328,9 @@ class ContactSolver { /// Reference to the body components BodyComponents& mBodyComponents; + /// Reference to the dynamics components + DynamicsComponents& mDynamicsComponents; + /// Reference to the proxy-shapes components // TODO : Do we really need to use this ? ProxyShapeComponents& mProxyShapeComponents; @@ -372,7 +376,8 @@ class ContactSolver { /// Constructor ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents, - ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings); + DynamicsComponents& dynamicsComponents, ProxyShapeComponents& proxyShapeComponents, + const WorldSettings& worldSettings); /// Destructor ~ContactSolver() = default; @@ -387,10 +392,6 @@ class ContactSolver { void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, Vector3* splitAngularVelocities); - /// Set the constrained velocities arrays - void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities); - /// Store the computed impulses to use them to /// warm start the solver at the next iteration void storeImpulses(); @@ -423,17 +424,6 @@ inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelociti mSplitAngularVelocities = splitAngularVelocities; } -// Set the constrained velocities arrays -inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, - Vector3* constrainedAngularVelocities) { - - assert(constrainedLinearVelocities != nullptr); - assert(constrainedAngularVelocities != nullptr); - - mLinearVelocities = constrainedLinearVelocities; - mAngularVelocities = constrainedAngularVelocities; -} - // Return true if the split impulses position correction technique is used for contacts inline bool ContactSolver::isSplitImpulseActive() const { return mIsSplitImpulseActive; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 68e64dc6..2f95d942 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -50,19 +50,15 @@ using namespace std; DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler) : CollisionWorld(worldSettings, logger, profiler), mIslands(mMemoryManager.getSingleFrameAllocator()), - mContactSolver(mMemoryManager, mIslands, mBodyComponents, mProxyShapesComponents, mConfig), - mConstraintSolver(mIslands), + mContactSolver(mMemoryManager, mIslands, mBodyComponents, mDynamicsComponents, mProxyShapesComponents, mConfig), + mConstraintSolver(mIslands, mDynamicsComponents), mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations), mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()), mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), - mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr), - mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr), - mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr), - mConstrainedOrientations(nullptr), - mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity), - mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), - mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep), + mIsGravityEnabled(true), mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr), + mConstrainedOrientations(nullptr), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity), + mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep), mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) { #ifdef IS_PROFILING_ACTIVE @@ -181,8 +177,8 @@ void DynamicsWorld::integrateRigidBodiesPositions() { // Get the constrained velocity uint indexArray = body->mArrayIndex; - Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; - Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray]; + Vector3 newLinVelocity = mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity); + Vector3 newAngVelocity = mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity); // TODO : Remove this Vector3 testLinVel = newLinVelocity; @@ -229,8 +225,8 @@ void DynamicsWorld::updateBodiesState() { uint index = body->mArrayIndex; // Update the linear and angular velocity of the body - mDynamicsComponents.setLinearVelocity(bodyEntity, mConstrainedLinearVelocities[index]); - mDynamicsComponents.setAngularVelocity(bodyEntity, mConstrainedAngularVelocities[index]); + mDynamicsComponents.setLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity)); + mDynamicsComponents.setAngularVelocity(bodyEntity, mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity)); // Update the position of the center of mass of the body body->mCenterOfMassWorld = mConstrainedPositions[index]; @@ -267,18 +263,12 @@ void DynamicsWorld::initVelocityArrays() { nbBodies * sizeof(Vector3))); mSplitAngularVelocities = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, nbBodies * sizeof(Vector3))); - mConstrainedLinearVelocities = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, - nbBodies * sizeof(Vector3))); - mConstrainedAngularVelocities = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, - nbBodies * sizeof(Vector3))); mConstrainedPositions = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, nbBodies * sizeof(Vector3))); mConstrainedOrientations = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, nbBodies * sizeof(Quaternion))); assert(mSplitLinearVelocities != nullptr); assert(mSplitAngularVelocities != nullptr); - assert(mConstrainedLinearVelocities != nullptr); - assert(mConstrainedAngularVelocities != nullptr); assert(mConstrainedPositions != nullptr); assert(mConstrainedOrientations != nullptr); @@ -324,18 +314,17 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { assert(indexBody < mRigidBodies.size()); // Integrate the external force to get the new velocity of the body - mConstrainedLinearVelocities[indexBody] = body->getLinearVelocity() + - mTimeStep * body->mMassInverse * body->mExternalForce; - mConstrainedAngularVelocities[indexBody] = body->getAngularVelocity() + - mTimeStep * body->getInertiaTensorInverseWorld() * - body->mExternalTorque; + mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, body->getLinearVelocity() + + mTimeStep * body->mMassInverse * body->mExternalForce); + mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, body->getAngularVelocity() + + mTimeStep * body->getInertiaTensorInverseWorld() * body->mExternalTorque); // If the gravity has to be applied to this rigid body if (body->isGravityEnabled() && mIsGravityEnabled) { // Integrate the gravity force - mConstrainedLinearVelocities[indexBody] += mTimeStep * body->mMassInverse * - body->getMass() * mGravity; + mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity) + mTimeStep * body->mMassInverse * + body->getMass() * mGravity); } // Apply the velocity damping @@ -355,8 +344,8 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { 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; + mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity) * linearDamping); + mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity) * angularDamping); } } } @@ -368,10 +357,6 @@ void DynamicsWorld::solveContactsAndConstraints() { // Set the velocities arrays mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities); - mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities, - mConstrainedAngularVelocities); - mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities, - mConstrainedAngularVelocities); mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions, mConstrainedOrientations); diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 36f3924e..a76c15ca 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -88,16 +88,6 @@ 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;