diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 29c82c11..fbb8d262 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -781,7 +781,6 @@ void RigidBody::setIsSleeping(bool isSleeping) { RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "Body " + std::to_string(mEntity.id) + ": Set isSleeping=" + (isSleeping ? "true" : "false")); - } // Set whether or not the body is allowed to go to sleep diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index ba27a83c..bd427657 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -84,7 +84,7 @@ class RigidBody : public CollisionBody { void updateInertiaTensorInverseWorld(); /// Set the variable to know whether or not the body is sleeping - void setIsSleeping(bool isSleeping); + void setIsSleeping(bool isSleeping); public : @@ -222,6 +222,7 @@ class RigidBody : public CollisionBody { friend class DynamicsWorld; friend class ContactSolverSystem; + friend class Joint; friend class BallAndSocketJoint; friend class SliderJoint; friend class HingeJoint; diff --git a/src/components/JointComponents.cpp b/src/components/JointComponents.cpp index a0939294..41301d70 100644 --- a/src/components/JointComponents.cpp +++ b/src/components/JointComponents.cpp @@ -33,7 +33,8 @@ using namespace reactphysics3d; // Constructor JointComponents::JointComponents(MemoryAllocator& allocator) - :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Entity)) { + :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(Entity) + + sizeof(Joint*)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -55,6 +56,7 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) { Entity* newJointsEntities = static_cast(newBuffer); Entity* newBody1Entities = reinterpret_cast(newJointsEntities + nbComponentsToAllocate); Entity* newBody2Entities = reinterpret_cast(newBody1Entities + nbComponentsToAllocate); + Joint** newJoints = reinterpret_cast(newBody2Entities + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -63,6 +65,7 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newJointsEntities, mJointEntities, mNbComponents * sizeof(Entity)); memcpy(newBody1Entities, mBody1Entities, mNbComponents * sizeof(Entity)); memcpy(newBody2Entities, mBody2Entities, mNbComponents * sizeof(Entity)); + memcpy(newJoints, mJoints, mNbComponents * sizeof(Joint*)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -73,6 +76,7 @@ void JointComponents::allocate(uint32 nbComponentsToAllocate) { mJointEntities = newJointsEntities; mBody1Entities = newBody1Entities; mBody2Entities = newBody2Entities; + mJoints = newJoints; } // Add a component @@ -85,6 +89,7 @@ void JointComponents::addComponent(Entity jointEntity, bool isSleeping, const Jo new (mJointEntities + index) Entity(jointEntity); new (mBody1Entities + index) Entity(component.body1Entity); new (mBody2Entities + index) Entity(component.body2Entity); + mJoints[index] = component.joint; // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(jointEntity, index)); @@ -105,6 +110,7 @@ void JointComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) { new (mJointEntities + destIndex) Entity(mJointEntities[srcIndex]); new (mBody1Entities + destIndex) Entity(mBody1Entities[srcIndex]); new (mBody2Entities + destIndex) Entity(mBody2Entities[srcIndex]); + mJoints[destIndex] = mJoints[srcIndex]; // Destroy the source component destroyComponent(srcIndex); @@ -124,6 +130,7 @@ void JointComponents::swapComponents(uint32 index1, uint32 index2) { Entity jointEntity1(mJointEntities[index1]); Entity body1Entity1(mBody1Entities[index1]); Entity body2Entity1(mBody2Entities[index1]); + Joint* joint1 = mJoints[index1]; // Destroy component 1 destroyComponent(index1); @@ -134,6 +141,7 @@ void JointComponents::swapComponents(uint32 index1, uint32 index2) { new (mJointEntities + index2) Entity(jointEntity1); new (mBody1Entities + index2) Entity(body1Entity1); new (mBody2Entities + index2) Entity(body2Entity1); + mJoints[index2] = joint1; // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(jointEntity1, index2)); @@ -155,4 +163,5 @@ void JointComponents::destroyComponent(uint32 index) { mJointEntities[index].~Entity(); mBody1Entities[index].~Entity(); mBody2Entities[index].~Entity(); + mJoints[index] = nullptr; } diff --git a/src/components/JointComponents.h b/src/components/JointComponents.h index f0deb297..fe25ab34 100644 --- a/src/components/JointComponents.h +++ b/src/components/JointComponents.h @@ -38,6 +38,7 @@ namespace reactphysics3d { // Class declarations class MemoryAllocator; class EntityManager; +class Joint; // Class JointComponents /** @@ -59,6 +60,9 @@ class JointComponents : public Components { /// Array of body entities of the first bodies of the joints Entity* mBody2Entities; + /// Array with pointers to the joints + Joint** mJoints; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components @@ -80,10 +84,11 @@ class JointComponents : public Components { const Entity body1Entity; const Entity body2Entity; + Joint* joint; /// Constructor - JointComponent(Entity body1Entity, Entity body2Entity) - : body1Entity(body1Entity), body2Entity(body2Entity) { + JointComponent(Entity body1Entity, Entity body2Entity, Joint* joint) + : body1Entity(body1Entity), body2Entity(body2Entity), joint(joint) { } }; @@ -105,6 +110,9 @@ class JointComponents : public Components { /// Return the entity of the second body of a joint Entity getBody2Entity(Entity jointEntity) const; + /// Return a pointer to the joint + Joint* getJoint(Entity jointEntity) const; + // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; @@ -122,6 +130,12 @@ inline Entity JointComponents::getBody2Entity(Entity jointEntity) const { return mBody2Entities[mMapEntityToComponentIndex[jointEntity]]; } +// Return a pointer to the joint +inline Joint* JointComponents::getJoint(Entity jointEntity) const { + assert(mMapEntityToComponentIndex.containsKey(jointEntity)); + return mJoints[mMapEntityToComponentIndex[jointEntity]]; +} + } #endif diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 1d544a2c..bf1ff7f9 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -27,6 +27,7 @@ #include "BallAndSocketJoint.h" #include "systems/ConstraintSolverSystem.h" #include "components/RigidBodyComponents.h" +#include "engine/DynamicsWorld.h" using namespace reactphysics3d; @@ -34,26 +35,38 @@ using namespace reactphysics3d; const decimal BallAndSocketJoint::BETA = decimal(0.2); // Constructor -BallAndSocketJoint::BallAndSocketJoint(Entity entity, const BallAndSocketJointInfo& jointInfo) - : Joint(entity, jointInfo), mImpulse(Vector3(0, 0, 0)) { +BallAndSocketJoint::BallAndSocketJoint(Entity entity, DynamicsWorld& world, const BallAndSocketJointInfo& jointInfo) + : Joint(entity, world, jointInfo), mImpulse(Vector3(0, 0, 0)) { + + // Get the transforms of the two bodies + Transform& body1Transform = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); + Transform& body2Transform = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); // Compute the local-space anchor point for each body - mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody1 = body1Transform.getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = body2Transform.getInverse() * jointInfo.anchorPointWorldSpace; } // Initialize before solving the constraint void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { + // Get the bodies entities + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + // TODO : Remove this and use compoents instead of pointers to bodies + RigidBody* body1 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); + RigidBody* body2 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body2Entity)); + // Get the bodies center of mass and orientations - const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity); - const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity); - const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body1Entity); + const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body2Entity); + const Quaternion& orientationBody1 = body1->getTransform().getOrientation(); + const Quaternion& orientationBody2 = body2->getTransform().getOrientation(); // Get the inertia tensor of bodies - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = body1->getInertiaTensorInverseWorld(); + mI2 = body2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space mR1World = orientationBody1 * mLocalAnchorPointBody1; @@ -64,8 +77,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) - decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity()); - decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity()); + decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity()); + decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity()); decimal inverseMassBodies = body1MassInverse + body2MassInverse; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, @@ -75,7 +88,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS // Compute the inverse mass matrix K^-1 mInverseMassMatrix.setToZero(); - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mInverseMassMatrix = massMatrix.getInverse(); } @@ -97,8 +111,11 @@ 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.rigidBodyComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity); // Get the velocities Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; @@ -111,22 +128,25 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); // Apply the impulse to the body 1 - v1 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity) * linearImpulseBody1; + v1 += constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity) * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World); // Apply the impulse to the body to the body 2 - v2 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity) * mImpulse; + v2 += constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity) * mImpulse; w2 += mI2 * angularImpulseBody2; } // Solve the velocity constraint void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity); // Get the velocities Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; @@ -146,37 +166,44 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); // Apply the impulse to the body 1 - v1 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity) * linearImpulseBody1; + v1 += constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity) * linearImpulseBody1; w1 += mI1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); // Apply the impulse to the body 2 - v2 += constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity) * deltaLambda; + v2 += constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity) * deltaLambda; w2 += mI2 * angularImpulseBody2; } // Solve the position constraint (for position error correction) void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + // TODO : Remove this and use compoents instead of pointers to bodies + RigidBody* body1 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); + RigidBody* body2 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body2Entity)); + // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies center of mass and orientations - Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity); - Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity); - Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity); - Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity); + Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity); + Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity); + Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body1Entity); + Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body2Entity); // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // Recompute the inverse inertia tensors - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = body1->getInertiaTensorInverseWorld(); + mI2 = body2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space mR1World = q1 * mLocalAnchorPointBody1; @@ -194,7 +221,8 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); mInverseMassMatrix.setToZero(); - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mInverseMassMatrix = massMatrix.getInverse(); } @@ -231,9 +259,9 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); - constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1); - constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2); - constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1); - constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(body1Entity, x1); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(body2Entity, x2); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body1Entity, q1); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2); } diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index 64e12c36..f61e336b 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -125,7 +125,7 @@ class BallAndSocketJoint : public Joint { // -------------------- Methods -------------------- // /// Constructor - BallAndSocketJoint(Entity entity, const BallAndSocketJointInfo& jointInfo); + BallAndSocketJoint(Entity entity, DynamicsWorld& world, const BallAndSocketJointInfo& jointInfo); /// Destructor virtual ~BallAndSocketJoint() override = default; diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 6ec86b9e..b971cba3 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -27,6 +27,7 @@ #include "FixedJoint.h" #include "systems/ConstraintSolverSystem.h" #include "components/RigidBodyComponents.h" +#include "engine/DynamicsWorld.h" using namespace reactphysics3d; @@ -34,12 +35,13 @@ using namespace reactphysics3d; const decimal FixedJoint::BETA = decimal(0.2); // Constructor -FixedJoint::FixedJoint(Entity entity, const FixedJointInfo& jointInfo) - : Joint(entity, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) { +FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo& jointInfo) + : Joint(entity, world, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) { // Compute the local-space anchor point for each body - const Transform& transform1 = mBody1->getTransform(); - const Transform& transform2 = mBody2->getTransform(); + const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); + const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); + mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; @@ -60,15 +62,23 @@ FixedJoint::FixedJoint(Entity entity, const FixedJointInfo& jointInfo) // Initialize before solving the constraint void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { + // Get the bodies entities + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + // TODO : Remove this and use compoents instead of pointers to bodies + RigidBody* body1 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); + RigidBody* body2 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body2Entity)); + // Get the bodies positions and orientations - const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity); - const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity); - const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body1Entity); + const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body2Entity); + const Quaternion& orientationBody1 = body1->getTransform().getOrientation(); + const Quaternion& orientationBody2 = body2->getTransform().getOrientation(); // Get the inertia tensor of bodies - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = body1->getInertiaTensorInverseWorld(); + mI2 = body2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space mR1World = orientationBody1 * mLocalAnchorPointBody1; @@ -79,8 +89,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints - const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity()); - const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity()); + const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity()); + const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity()); const decimal inverseMassBodies = body1MassInverse + body2MassInverse; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, @@ -90,7 +100,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the inverse mass matrix K^-1 for the 3 translation constraints mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mInverseMassMatrixTranslation = massMatrix.getInverse(); } @@ -104,7 +115,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mInverseMassMatrixRotation = mI1 + mI2; - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); } @@ -128,8 +140,12 @@ 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.rigidBodyComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); + // Get the bodies entities + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity); // Get the velocities Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; @@ -138,8 +154,8 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass of the bodies - const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 Vector3 linearImpulseBody1 = -mImpulseTranslation; @@ -166,8 +182,12 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Solve the velocity constraint void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); + // Get the bodies entities + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity); // Get the velocities Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; @@ -176,8 +196,8 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass of the bodies - decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // --------------- Translation Constraints --------------- // @@ -226,23 +246,31 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS // Solve the position constraint (for position error correction) void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { + // Get the bodies entities + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + // TODO : Remove this and use compoents instead of pointers to bodies + RigidBody* body1 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); + RigidBody* body2 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body2Entity)); + // If the error position correction technique is not the non-linear-gauss-seidel, we do // do not execute this method if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies positions and orientations - Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity); - Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity); - Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity); - Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity); + Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity); + Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity); + Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body1Entity); + Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body2Entity); // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // Recompute the inverse inertia tensors - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = body1->getInertiaTensorInverseWorld(); + mI2 = body2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space mR1World = q1 * mLocalAnchorPointBody1; @@ -262,7 +290,8 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mInverseMassMatrixTranslation = massMatrix.getInverse(); } @@ -302,7 +331,8 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mInverseMassMatrixRotation = mI1 + mI2; - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse(); } @@ -354,9 +384,9 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); - constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1); - constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2); - constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1); - constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(body1Entity, x1); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(body2Entity, x2); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body1Entity, q1); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2); } diff --git a/src/constraint/FixedJoint.h b/src/constraint/FixedJoint.h index ce919eb1..150cb218 100644 --- a/src/constraint/FixedJoint.h +++ b/src/constraint/FixedJoint.h @@ -136,7 +136,7 @@ class FixedJoint : public Joint { // -------------------- Methods -------------------- // /// Constructor - FixedJoint(Entity entity, const FixedJointInfo& jointInfo); + FixedJoint(Entity entity, DynamicsWorld& world, const FixedJointInfo& jointInfo); /// Destructor virtual ~FixedJoint() override = default; diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 4e72b108..22593dc0 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -27,6 +27,7 @@ #include "HingeJoint.h" #include "systems/ConstraintSolverSystem.h" #include "components/RigidBodyComponents.h" +#include "engine/DynamicsWorld.h" using namespace reactphysics3d; @@ -34,8 +35,8 @@ using namespace reactphysics3d; const decimal HingeJoint::BETA = decimal(0.2); // Constructor -HingeJoint::HingeJoint(Entity entity, const HingeJointInfo& jointInfo) - : Joint(entity, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0), +HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo& jointInfo) + : Joint(entity, world, jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0), mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), mLowerLimit(jointInfo.minAngleLimit), mUpperLimit(jointInfo.maxAngleLimit), @@ -46,8 +47,8 @@ HingeJoint::HingeJoint(Entity entity, const HingeJointInfo& jointInfo) assert(mUpperLimit >= decimal(0) && mUpperLimit <= decimal(2.0) * PI); // Compute the local-space anchor point for each body - Transform transform1 = mBody1->getTransform(); - Transform transform2 = mBody2->getTransform(); + Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); + Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; @@ -67,15 +68,23 @@ HingeJoint::HingeJoint(Entity entity, const HingeJointInfo& jointInfo) // Initialize before solving the constraint void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { + // Get the bodies entities + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + // TODO : Remove this and use compoents instead of pointers to bodies + RigidBody* body1 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); + RigidBody* body2 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body2Entity)); + // Get the bodies positions and orientations - const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity); - const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity); - const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body1Entity); + const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body2Entity); + const Quaternion& orientationBody1 = mWorld.mTransformComponents.getTransform(body1Entity).getOrientation(); + const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation(); // Get the inertia tensor of bodies - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = body1->getInertiaTensorInverseWorld(); + mI2 = body2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space mR1World = orientationBody1 * mLocalAnchorPointBody1; @@ -113,8 +122,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix) - decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity()); - decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity()); + decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity()); + decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity()); decimal inverseMassBodies = body1MassInverse + body2MassInverse; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, @@ -122,7 +131,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mInverseMassMatrixTranslation = massMatrix.getInverse(); } @@ -148,7 +158,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat mC2CrossA1.dot(I2C2CrossA1); const Matrix2x2 matrixKRotation(el11, el12, el21, el22); mInverseMassMatrixRotation.setToZero(); - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mInverseMassMatrixRotation = matrixKRotation.getInverse(); } @@ -197,8 +208,12 @@ 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.rigidBodyComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); + // Get the bodies entities + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity); // Get the velocities Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; @@ -207,8 +222,8 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // Compute the impulse P=J^T * lambda for the 2 rotation constraints Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y; @@ -256,8 +271,12 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Solve the velocity constraint void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); + // Get the bodies entities + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity); // Get the velocities Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; @@ -266,8 +285,8 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // --------------- Translation Constraints --------------- // @@ -409,19 +428,27 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // do not execute this method if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; + // Get the bodies entities + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + // TODO : Remove this and use compoents instead of pointers to bodies + RigidBody* body1 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); + RigidBody* body2 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body2Entity)); + // Get the bodies positions and orientations - Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity); - Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity); - Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity); - Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity); + Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity); + Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity); + Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body1Entity); + Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body2Entity); // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // Recompute the inverse inertia tensors - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = body1->getInertiaTensorInverseWorld(); + mI2 = body2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space mR1World = q1 * mLocalAnchorPointBody1; @@ -453,8 +480,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // --------------- Translation Constraints --------------- // // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints - const decimal body1InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - const decimal body2InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + const decimal body1InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + const decimal body2InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); decimal inverseMassBodies = body1InverseMass + body2InverseMass; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, @@ -462,7 +489,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS skewSymmetricMatrixU1 * mI1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * mI2 * skewSymmetricMatrixU2.getTranspose(); mInverseMassMatrixTranslation.setToZero(); - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mInverseMassMatrixTranslation = massMatrix.getInverse(); } @@ -514,7 +542,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS mC2CrossA1.dot(I2C2CrossA1); const Matrix2x2 matrixKRotation(el11, el12, el21, el22); mInverseMassMatrixRotation.setToZero(); - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { mInverseMassMatrixRotation = matrixKRotation.getInverse(); } @@ -611,10 +640,10 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS } } - constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1); - constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2); - constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1); - constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(body1Entity, x1); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(body2Entity, x2); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body1Entity, q1); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2); } @@ -645,8 +674,7 @@ void HingeJoint::enableMotor(bool isMotorEnabled) { mImpulseMotor = 0.0; // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + awakeBodies(); } // Set the minimum angle limit @@ -655,7 +683,7 @@ void HingeJoint::enableMotor(bool isMotorEnabled) { */ void HingeJoint::setMinAngleLimit(decimal lowerLimit) { - assert(mLowerLimit <= 0 && mLowerLimit >= -2.0 * PI); + assert(mLowerLimit <= decimal(0) && mLowerLimit >= decimal(-2.0 * PI)); if (lowerLimit != mLowerLimit) { @@ -672,7 +700,7 @@ void HingeJoint::setMinAngleLimit(decimal lowerLimit) { */ void HingeJoint::setMaxAngleLimit(decimal upperLimit) { - assert(upperLimit >= 0 && upperLimit <= 2.0 * PI); + assert(upperLimit >= decimal(0) && upperLimit <= decimal(2.0 * PI)); if (upperLimit != mUpperLimit) { @@ -691,8 +719,7 @@ void HingeJoint::resetLimits() { mImpulseUpperLimit = 0.0; // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + awakeBodies(); } // Set the motor speed @@ -703,8 +730,7 @@ void HingeJoint::setMotorSpeed(decimal motorSpeed) { mMotorSpeed = motorSpeed; // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + awakeBodies(); } } @@ -716,12 +742,11 @@ void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) { if (maxMotorTorque != mMaxMotorTorque) { - assert(mMaxMotorTorque >= 0.0); + assert(mMaxMotorTorque >= decimal(0.0)); mMaxMotorTorque = maxMotorTorque; // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + awakeBodies(); } } @@ -729,7 +754,7 @@ void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) { decimal HingeJoint::computeNormalizedAngle(decimal angle) const { // Convert it into the range [-2*pi; 2*pi] - angle = fmod(angle, PI_TIMES_2); + angle = std::fmod(angle, PI_TIMES_2); // Convert it into the range [-pi; pi] if (angle < -PI) { @@ -752,13 +777,13 @@ decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, deci return inputAngle; } else if (inputAngle > upperLimitAngle) { - decimal diffToUpperLimit = fabs(computeNormalizedAngle(inputAngle - upperLimitAngle)); - decimal diffToLowerLimit = fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle)); + decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(inputAngle - upperLimitAngle)); + decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle)); return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle; } else if (inputAngle < lowerLimitAngle) { - decimal diffToUpperLimit = fabs(computeNormalizedAngle(upperLimitAngle - inputAngle)); - decimal diffToLowerLimit = fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle)); + decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(upperLimitAngle - inputAngle)); + decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle)); return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2); } else { diff --git a/src/constraint/HingeJoint.h b/src/constraint/HingeJoint.h index 191ff1df..1186c043 100644 --- a/src/constraint/HingeJoint.h +++ b/src/constraint/HingeJoint.h @@ -287,7 +287,7 @@ class HingeJoint : public Joint { // -------------------- Methods -------------------- // /// Constructor - HingeJoint(Entity entity, const HingeJointInfo& jointInfo); + HingeJoint(Entity entity, DynamicsWorld& world, const HingeJointInfo& jointInfo); /// Destructor virtual ~HingeJoint() override = default; diff --git a/src/constraint/Joint.cpp b/src/constraint/Joint.cpp index 1d7c9b6c..85b2df58 100644 --- a/src/constraint/Joint.cpp +++ b/src/constraint/Joint.cpp @@ -25,16 +25,47 @@ // Libraries #include "Joint.h" +#include "engine/DynamicsWorld.h" using namespace reactphysics3d; // Constructor -Joint::Joint(Entity entity, const JointInfo& jointInfo) - :mEntity(entity), mBody1(jointInfo.body1), mBody2(jointInfo.body2), mBody1Entity(jointInfo.body1->getEntity()), - mBody2Entity(jointInfo.body2->getEntity()), mType(jointInfo.type), +Joint::Joint(Entity entity, DynamicsWorld& world, const JointInfo& jointInfo) + :mEntity(entity), mWorld(world), mType(jointInfo.type), mPositionCorrectionTechnique(jointInfo.positionCorrectionTechnique), mIsCollisionEnabled(jointInfo.isCollisionEnabled), mIsAlreadyInIsland(false) { - assert(mBody1 != nullptr); - assert(mBody2 != nullptr); +} + +// Return the reference to the body 1 +/** + * @return The first body involved in the joint + */ +RigidBody* Joint::getBody1() const { + const Entity body1Entiy = mWorld.mJointsComponents.getBody1Entity(mEntity); + return mWorld.mRigidBodyComponents.getRigidBody(body1Entiy); +} + +// Return the reference to the body 2 +/** + * @return The second body involved in the joint + */ +RigidBody* Joint::getBody2() const { + const Entity body2Entiy = mWorld.mJointsComponents.getBody2Entity(mEntity); + return mWorld.mRigidBodyComponents.getRigidBody(body2Entiy); +} + +// Awake the two bodies of the joint +void Joint::awakeBodies() const { + + // Get the bodies entities + Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + RigidBody* body1 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); + RigidBody* body2 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body2Entity)); + + // Wake up the two bodies of the joint + body1->setIsSleeping(false); + body2->setIsSleeping(false); } diff --git a/src/constraint/Joint.h b/src/constraint/Joint.h index 54b7eb8a..e1e7e960 100644 --- a/src/constraint/Joint.h +++ b/src/constraint/Joint.h @@ -123,19 +123,8 @@ class Joint { /// Entity ID of the joint Entity mEntity; - /// 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; + /// Reference to the physics world + DynamicsWorld& mWorld; /// Type of the joint const JointType mType; @@ -172,12 +161,15 @@ class Joint { /// Solve the position constraint virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0; + /// Awake the two bodies of the joint + void awakeBodies() const; + public : // -------------------- Methods -------------------- // /// Constructor - Joint(Entity entity, const JointInfo& jointInfo); + Joint(Entity entity, DynamicsWorld& world, const JointInfo& jointInfo); /// Destructor virtual ~Joint() = default; @@ -213,22 +205,6 @@ class Joint { friend class ConstraintSolverSystem; }; -// Return the reference to the body 1 -/** - * @return The first body involved in the joint - */ -inline RigidBody* Joint::getBody1() const { - return mBody1; -} - -// Return the reference to the body 2 -/** - * @return The second body involved in the joint - */ -inline RigidBody* Joint::getBody2() const { - return mBody2; -} - // Return the type of the joint /** * @return The type of the joint diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index dafb3c48..57a236bd 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -27,6 +27,7 @@ #include "SliderJoint.h" #include "systems/ConstraintSolverSystem.h" #include "components/RigidBodyComponents.h" +#include "engine/DynamicsWorld.h" using namespace reactphysics3d; @@ -34,8 +35,8 @@ using namespace reactphysics3d; const decimal SliderJoint::BETA = decimal(0.2); // Constructor -SliderJoint::SliderJoint(Entity entity, const SliderJointInfo& jointInfo) - : Joint(entity, jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), +SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointInfo& jointInfo) + : Joint(entity, world, jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0), mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled), mLowerLimit(jointInfo.minTranslationLimit), @@ -48,8 +49,8 @@ SliderJoint::SliderJoint(Entity entity, const SliderJointInfo& jointInfo) assert(mMaxMotorForce >= decimal(0.0)); // Compute the local-space anchor point for each body - const Transform& transform1 = mBody1->getTransform(); - const Transform& transform2 = mBody2->getTransform(); + const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); + const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; @@ -64,10 +65,12 @@ SliderJoint::SliderJoint(Entity entity, const SliderJointInfo& jointInfo) // q20 = initial orientation of body 2 // q10 = initial orientation of body 1 // r0 = initial rotation rotation from body 1 to body 2 + // TODO : Do not compute the inverse here, it has already been computed above mInitOrientationDifferenceInv = transform2.getOrientation().getInverse() * transform1.getOrientation(); // Compute the slider axis in local-space of body 1 - mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() * + // TODO : Do not compute the inverse here, it has already been computed above + mSliderAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.sliderAxisWorldSpace; mSliderAxisBody1.normalize(); } @@ -75,15 +78,23 @@ SliderJoint::SliderJoint(Entity entity, const SliderJointInfo& jointInfo) // Initialize before solving the constraint void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { + // Get the bodies entities + const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + // TODO : Remove this and use compoents instead of pointers to bodies + RigidBody* body1 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); + RigidBody* body2 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body2Entity)); + // Get the bodies positions and orientations - const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody1Entity); - const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(mBody2Entity); - const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); + const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body1Entity); + const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body2Entity); + const Quaternion& orientationBody1 = mWorld.mTransformComponents.getTransform(body1Entity).getOrientation(); + const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation(); // Get the inertia tensor of bodies - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = body1->getInertiaTensorInverseWorld(); + mI2 = body2->getInertiaTensorInverseWorld(); // Vector from body center to the anchor point mR1 = orientationBody1 * mLocalAnchorPointBody1; @@ -124,8 +135,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) - const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1->getEntity()); - const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2->getEntity()); + const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); const decimal sumInverseMass = body1MassInverse + body2MassInverse; Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; @@ -141,7 +152,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mR2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); mInverseMassMatrixTranslationConstraint.setToZero(); - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { + mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); } @@ -157,7 +170,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mInverseMassMatrixRotationConstraint = mI1 + mI2; - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { + mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); } @@ -215,8 +230,12 @@ 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.rigidBodyComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); + // Get the bodies entities + const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity); // Get the velocities Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; @@ -225,8 +244,8 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit; @@ -277,8 +296,12 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Solve the velocity constraint void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody1Entity); - uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(mBody2Entity); + // Get the bodies entities + const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity); + uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity); // Get the velocities Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1]; @@ -287,8 +310,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // --------------- Translation Constraints --------------- // @@ -441,19 +464,27 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // do not execute this method if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; + // Get the bodies entities + const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + // TODO : Remove this and use compoents instead of pointers to bodies + RigidBody* body1 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); + RigidBody* body2 = static_cast(mWorld.mRigidBodyComponents.getRigidBody(body2Entity)); + // Get the bodies positions and orientations - Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody1Entity); - Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(mBody2Entity); - Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody1Entity); - Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(mBody2Entity); + Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity); + Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity); + Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body1Entity); + Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body2Entity); // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); // Recompute the inertia tensor of bodies - mI1 = mBody1->getInertiaTensorInverseWorld(); - mI2 = mBody2->getInertiaTensorInverseWorld(); + mI1 = body1->getInertiaTensorInverseWorld(); + mI2 = body2->getInertiaTensorInverseWorld(); // Vector from body center to the anchor point mR1 = q1 * mLocalAnchorPointBody1; @@ -488,8 +519,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) - const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); decimal sumInverseMass = body1MassInverse + body2MassInverse; Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; @@ -505,7 +536,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint mR2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); mInverseMassMatrixTranslationConstraint.setToZero(); - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { + mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); } @@ -548,7 +581,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mInverseMassMatrixRotationConstraint = mI1 + mI2; - if (mBody1->getType() == BodyType::DYNAMIC || mBody2->getType() == BodyType::DYNAMIC) { + if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { + mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); } @@ -610,8 +645,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint if (mIsLowerLimitViolated || mIsUpperLimitViolated) { // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) - const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody1Entity); - const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(mBody2Entity); + const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); + const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); mInverseMassMatrixLimit = body1MassInverse + body2MassInverse + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); @@ -686,10 +721,10 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint } } - constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody1Entity, x1); - constraintSolverData.rigidBodyComponents.setConstrainedPosition(mBody2Entity, x2); - constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody1Entity, q1); - constraintSolverData.rigidBodyComponents.setConstrainedOrientation(mBody2Entity, q2); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(body1Entity, x1); + constraintSolverData.rigidBodyComponents.setConstrainedPosition(body2Entity, x2); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body1Entity, q1); + constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2); } // Enable/Disable the limits of the joint @@ -719,8 +754,7 @@ void SliderJoint::enableMotor(bool isMotorEnabled) { mImpulseMotor = 0.0; // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + awakeBodies(); } // Return the current translation value of the joint @@ -731,11 +765,18 @@ decimal SliderJoint::getTranslation() const { // TODO : Check if we need to compare rigid body position or center of mass here + // Get the bodies entities + const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + // Get the bodies positions and orientations - const Vector3& x1 = mBody1->getTransform().getPosition(); - const Vector3& x2 = mBody2->getTransform().getPosition(); - const Quaternion& q1 = mBody1->getTransform().getOrientation(); - const Quaternion& q2 = mBody2->getTransform().getOrientation(); + const Transform& transform1 = mWorld.mTransformComponents.getTransform(body1Entity); + const Transform& transform2 = mWorld.mTransformComponents.getTransform(body2Entity); + + const Vector3& x1 = transform1.getPosition(); + const Vector3& x2 = transform2.getPosition(); + const Quaternion& q1 = transform1.getOrientation(); + const Quaternion& q2 = transform2.getOrientation(); // Compute the two anchor points in world-space coordinates const Vector3 anchorBody1 = x1 + q1 * mLocalAnchorPointBody1; @@ -794,8 +835,7 @@ void SliderJoint::resetLimits() { mImpulseUpperLimit = 0.0; // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + awakeBodies(); } // Set the motor speed @@ -809,8 +849,7 @@ void SliderJoint::setMotorSpeed(decimal motorSpeed) { mMotorSpeed = motorSpeed; // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + awakeBodies(); } } @@ -822,11 +861,10 @@ void SliderJoint::setMaxMotorForce(decimal maxMotorForce) { if (maxMotorForce != mMaxMotorForce) { - assert(mMaxMotorForce >= 0.0); + assert(mMaxMotorForce >= decimal(0.0)); mMaxMotorForce = maxMotorForce; // Wake up the two bodies of the joint - mBody1->setIsSleeping(false); - mBody2->setIsSleeping(false); + awakeBodies(); } } diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index e0374eb5..501dc4d9 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -289,7 +289,7 @@ class SliderJoint : public Joint { // -------------------- Methods -------------------- // /// Constructor - SliderJoint(Entity entity, const SliderJointInfo& jointInfo); + SliderJoint(Entity entity, DynamicsWorld& world, const SliderJointInfo& jointInfo); /// Destructor virtual ~SliderJoint() override = default; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 06a3896d..bae7d6c2 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -322,11 +322,6 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { Joint* newJoint = nullptr; - bool isJointDisabled = mRigidBodyComponents.getIsEntityDisabled(jointInfo.body1->getEntity()) && - mRigidBodyComponents.getIsEntityDisabled(jointInfo.body2->getEntity()); - JointComponents::JointComponent jointComponent(jointInfo.body1->getEntity(), jointInfo.body2->getEntity()); - mJointsComponents.addComponent(entity, isJointDisabled, jointComponent); - // Allocate memory to create the new joint switch(jointInfo.type) { @@ -337,7 +332,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { sizeof(BallAndSocketJoint)); const BallAndSocketJointInfo& info = static_cast( jointInfo); - newJoint = new (allocatedMemory) BallAndSocketJoint(entity, info); + newJoint = new (allocatedMemory) BallAndSocketJoint(entity, *this, info); break; } @@ -347,7 +342,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(SliderJoint)); const SliderJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) SliderJoint(entity, info); + newJoint = new (allocatedMemory) SliderJoint(entity, *this, info); break; } @@ -357,7 +352,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(HingeJoint)); const HingeJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) HingeJoint(entity, info); + newJoint = new (allocatedMemory) HingeJoint(entity, *this, info); break; } @@ -367,7 +362,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { void* allocatedMemory = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(FixedJoint)); const FixedJointInfo& info = static_cast(jointInfo); - newJoint = new (allocatedMemory) FixedJoint(entity, info); + newJoint = new (allocatedMemory) FixedJoint(entity, *this, info); break; } @@ -378,6 +373,11 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { } } + bool isJointDisabled = mRigidBodyComponents.getIsEntityDisabled(jointInfo.body1->getEntity()) && + mRigidBodyComponents.getIsEntityDisabled(jointInfo.body2->getEntity()); + JointComponents::JointComponent jointComponent(jointInfo.body1->getEntity(), jointInfo.body2->getEntity(), newJoint); + mJointsComponents.addComponent(entity, isJointDisabled, jointComponent); + // If the collision between the two bodies of the constraint is disabled if (!jointInfo.isCollisionEnabled) { @@ -418,16 +418,19 @@ void DynamicsWorld::destroyJoint(Joint* joint) { mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2()); } + RigidBody* body1 = joint->getBody1(); + RigidBody* body2 = joint->getBody2(); + // Wake up the two bodies of the joint - joint->getBody1()->setIsSleeping(false); - joint->getBody2()->setIsSleeping(false); + 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 - joint->mBody1->removeJointFromJointsList(mMemoryManager, joint); - joint->mBody2->removeJointFromJointsList(mMemoryManager, joint); + body1->removeJointFromJointsList(mMemoryManager, joint); + body2->removeJointFromJointsList(mMemoryManager, joint); size_t nbBytes = joint->getSizeInBytes(); @@ -448,26 +451,29 @@ void DynamicsWorld::addJointToBody(Joint* joint) { assert(joint != nullptr); + RigidBody* body1 = joint->getBody1(); + RigidBody* body2 = joint->getBody2(); + // Add the joint at the beginning of the linked list of joints of the first body void* allocatedMemory1 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(JointListElement)); JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint, - joint->mBody1->mJointsList); - joint->mBody1->mJointsList = jointListElement1; + body1->mJointsList); + body1->mJointsList = jointListElement1; RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(joint->mBody1->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) + + "Body " + std::to_string(body1->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) + " added to body"); // Add the joint at the beginning of the linked list of joints of the second body void* allocatedMemory2 = mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(JointListElement)); JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint, - joint->mBody2->mJointsList); - joint->mBody2->mJointsList = jointListElement2; + body2->mJointsList); + body2->mJointsList = jointListElement2; RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(joint->mBody2->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) + + "Body " + std::to_string(body2->getEntity().id) + ": Joint " + std::to_string(joint->getEntity().id) + " added to body"); } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 2c6913da..95296bd5 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -219,6 +219,11 @@ class DynamicsWorld : public CollisionWorld { // -------------------- Friendship -------------------- // friend class RigidBody; + friend class Joint; + friend class BallAndSocketJoint; + friend class FixedJoint; + friend class HingeJoint; + friend class SliderJoint; }; // Get the number of iterations for the velocity constraint solver