diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index c7f6a878..aa2d6f7f 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -40,13 +40,13 @@ using namespace reactphysics3d; * @param id The ID of the body */ RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id) - : CollisionBody(world, entity, id), mArrayIndex(0), mInitMass(decimal(1.0)), + : CollisionBody(world, entity, id), mArrayIndex(0), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), mIsGravityEnabled(true), mMaterial(world.mConfig), mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) { // Compute the inverse mass - mMassInverse = decimal(1.0) / mInitMass; + mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity)); // Update the world inverse inertia tensor updateInertiaTensorInverseWorld(); @@ -91,12 +91,12 @@ void RigidBody::setType(BodyType type) { if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) { // Reset the inverse mass and inverse inertia tensor to zero - mMassInverse = decimal(0.0); + mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0)); mInertiaTensorLocalInverse.setToZero(); mInertiaTensorInverseWorld.setToZero(); } else { // If it is a dynamic body - mMassInverse = decimal(1.0) / mInitMass; + mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity)); if (mIsInertiaTensorSetByUser) { mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse; @@ -118,6 +118,14 @@ void RigidBody::setType(BodyType type) { mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero()); } +// Method that return the mass of the body +/** + * @return The mass (in kilograms) of the body + */ +decimal RigidBody::getMass() const { + return mWorld.mDynamicsComponents.getInitMass(mEntity); +} + // Apply an external force to the body at a given point (in world-space coordinates). /// If the point is not at the center of mass of the body, it will also /// generate some torque and therefore, change the angular velocity of the body. @@ -274,14 +282,14 @@ void RigidBody::setMass(decimal mass) { if (mType != BodyType::DYNAMIC) return; - mInitMass = mass; + mWorld.mDynamicsComponents.setInitMass(mEntity, mass); - if (mInitMass > decimal(0.0)) { - mMassInverse = decimal(1.0) / mInitMass; + if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) { + mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity)); } else { - mInitMass = decimal(1.0); - mMassInverse = decimal(1.0); + mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(1.0)); + mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0)); } RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, @@ -558,8 +566,8 @@ void RigidBody::setTransform(const Transform& transform) { // the collision shapes attached to the body. void RigidBody::recomputeMassInformation() { - mInitMass = decimal(0.0); - mMassInverse = decimal(0.0); + mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(0.0)); + mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0.0)); if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero(); if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero(); if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero(); @@ -580,15 +588,15 @@ void RigidBody::recomputeMassInformation() { const List& proxyShapesEntities = mWorld.mBodyComponents.getProxyShapes(mEntity); for (uint i=0; i < proxyShapesEntities.size(); i++) { ProxyShape* proxyShape = mWorld.mProxyShapesComponents.getProxyShape(proxyShapesEntities[i]); - mInitMass += proxyShape->getMass(); + mWorld.mDynamicsComponents.setInitMass(mEntity, mWorld.mDynamicsComponents.getInitMass(mEntity) + proxyShape->getMass()); if (!mIsCenterOfMassSetByUser) { mCenterOfMassLocal += proxyShape->getLocalToBodyTransform().getPosition() * proxyShape->getMass(); } } - if (mInitMass > decimal(0.0)) { - mMassInverse = decimal(1.0) / mInitMass; + if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) { + mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity)); } else { mCenterOfMassWorld = transform.getPosition(); @@ -599,7 +607,7 @@ void RigidBody::recomputeMassInformation() { const Vector3 oldCenterOfMass = mCenterOfMassWorld; if (!mIsCenterOfMassSetByUser) { - mCenterOfMassLocal *= mMassInverse; + mCenterOfMassLocal *= mWorld.mDynamicsComponents.getMassInverse(mEntity); } mCenterOfMassWorld = transform * mCenterOfMassLocal; diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index c9a072b5..27789cd2 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -60,9 +60,6 @@ class RigidBody : public CollisionBody { // -------------------- Attributes -------------------- // - /// Intial mass of the body - decimal mInitMass; - /// Center of mass of the body in local-space coordinates. /// The center of mass can therefore be different from the body origin Vector3 mCenterOfMassLocal; @@ -80,9 +77,6 @@ class RigidBody : public CollisionBody { /// Inverse of the world inertia tensor of the body Matrix3x3 mInertiaTensorInverseWorld; - /// Inverse of the mass of the body - decimal mMassInverse; - /// True if the gravity needs to be applied to this rigid body bool mIsGravityEnabled; @@ -235,14 +229,6 @@ class RigidBody : public CollisionBody { friend class FixedJoint; }; -// Method that return the mass of the body -/** - * @return The mass (in kilograms) of the body - */ -inline decimal RigidBody::getMass() const { - return mInitMass; -} - // Get the inverse local inertia tensor of the body (in body coordinates) inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const { return mInertiaTensorLocalInverse; diff --git a/src/components/DynamicsComponents.cpp b/src/components/DynamicsComponents.cpp index 7f3b71d8..7a0b5333 100644 --- a/src/components/DynamicsComponents.cpp +++ b/src/components/DynamicsComponents.cpp @@ -36,8 +36,8 @@ using namespace reactphysics3d; // Constructor DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + - sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + - sizeof(Vector3) + sizeof(bool)) { + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) + + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -67,7 +67,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { Vector3* newExternalTorques = reinterpret_cast(newExternalForces + nbComponentsToAllocate); decimal* newLinearDampings = reinterpret_cast(newExternalTorques + nbComponentsToAllocate); decimal* newAngularDampings = reinterpret_cast(newLinearDampings + nbComponentsToAllocate); - bool* newIsAlreadyInIsland = reinterpret_cast(newAngularDampings + nbComponentsToAllocate); + decimal* newInitMasses = reinterpret_cast(newAngularDampings + nbComponentsToAllocate); + decimal* newInverseMasses = reinterpret_cast(newInitMasses + nbComponentsToAllocate); + bool* newIsAlreadyInIsland = reinterpret_cast(newInverseMasses + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -84,6 +86,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newExternalTorques, mExternalTorques, mNbComponents * sizeof(Vector3)); memcpy(newLinearDampings, mLinearDampings, mNbComponents * sizeof(decimal)); memcpy(newAngularDampings, mAngularDampings, mNbComponents * sizeof(decimal)); + memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal)); + memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal)); memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); // Deallocate previous memory @@ -102,6 +106,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { mExternalTorques = newExternalTorques; mLinearDampings = newLinearDampings; mAngularDampings = newAngularDampings; + mInitMasses = newInitMasses; + mInverseMasses = newInverseMasses; mIsAlreadyInIsland = newIsAlreadyInIsland; mNbAllocatedComponents = nbComponentsToAllocate; } @@ -124,6 +130,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const new (mExternalTorques + index) Vector3(0, 0, 0); mLinearDampings[index] = decimal(0.0); mAngularDampings[index] = decimal(0.0); + mInitMasses[index] = decimal(1.0); + mInverseMasses[index] = decimal(1.0); mIsAlreadyInIsland[index] = false; // Map the entity with the new component lookup index @@ -153,6 +161,8 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) new (mExternalTorques + destIndex) Vector3(mExternalTorques[srcIndex]); mLinearDampings[destIndex] = mLinearDampings[srcIndex]; mAngularDampings[destIndex] = mAngularDampings[srcIndex]; + mInitMasses[destIndex] = mInitMasses[srcIndex]; + mInverseMasses[destIndex] = mInverseMasses[srcIndex]; mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; // Destroy the source component @@ -184,6 +194,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { Vector3 externalTorque1(mExternalTorques[index1]); decimal linearDamping1 = mLinearDampings[index1]; decimal angularDamping1 = mAngularDampings[index1]; + decimal initMass1 = mInitMasses[index1]; + decimal inverseMass1 = mInverseMasses[index1]; bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; // Destroy component 1 @@ -203,6 +215,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { new (mExternalTorques + index2) Vector3(externalTorque1); mLinearDampings[index2] = linearDamping1; mAngularDampings[index2] = angularDamping1; + mInitMasses[index2] = initMass1; + mInverseMasses[index2] = inverseMass1; mIsAlreadyInIsland[index2] = isAlreadyInIsland1; // Update the entity to component index mapping diff --git a/src/components/DynamicsComponents.h b/src/components/DynamicsComponents.h index a655f578..65527710 100644 --- a/src/components/DynamicsComponents.h +++ b/src/components/DynamicsComponents.h @@ -83,6 +83,12 @@ class DynamicsComponents : public Components { /// Array with the angular damping factor of each component decimal* mAngularDampings; + /// Array with the initial mass of each component + decimal* mInitMasses; + + /// Array with the inverse mass of each component + decimal* mInverseMasses; + /// Array with the boolean value to know if the body has already been added into an island bool* mIsAlreadyInIsland; @@ -156,6 +162,12 @@ class DynamicsComponents : public Components { /// Return the angular damping factor of an entity decimal getAngularDamping(Entity bodyEntity) const; + /// Return the initial mass of an entity + decimal getInitMass(Entity bodyEntity) const; + + /// Return the mass inverse of an entity + decimal getMassInverse(Entity bodyEntity) const; + /// Return true if the entity is already in an island bool getIsAlreadyInIsland(Entity bodyEntity) const; @@ -189,6 +201,12 @@ class DynamicsComponents : public Components { /// Set the angular damping factor of an entity void setAngularDamping(Entity bodyEntity, decimal angularDamping); + /// Set the initial mass of an entity + void setInitMass(Entity bodyEntity, decimal initMass); + + /// Set the inverse mass of an entity + void setMassInverse(Entity bodyEntity, decimal inverseMass); + /// Set the value to know if the entity is already in an island bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland); @@ -300,6 +318,22 @@ inline decimal DynamicsComponents::getAngularDamping(Entity bodyEntity) const { return mAngularDampings[mMapEntityToComponentIndex[bodyEntity]]; } +// Return the initial mass of an entity +inline decimal DynamicsComponents::getInitMass(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mInitMasses[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the inverse mass of an entity +inline decimal DynamicsComponents::getMassInverse(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mInverseMasses[mMapEntityToComponentIndex[bodyEntity]]; +} + // Set the constrained linear velocity of an entity inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) { @@ -364,6 +398,22 @@ inline void DynamicsComponents::setAngularDamping(Entity bodyEntity, decimal ang mAngularDampings[mMapEntityToComponentIndex[bodyEntity]] = angularDamping; } +// Set the initial mass of an entity +inline void DynamicsComponents::setInitMass(Entity bodyEntity, decimal initMass) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mInitMasses[mMapEntityToComponentIndex[bodyEntity]] = initMass; +} + +// Set the mass inverse of an entity +inline void DynamicsComponents::setMassInverse(Entity bodyEntity, decimal inverseMass) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mInverseMasses[mMapEntityToComponentIndex[bodyEntity]] = inverseMass; +} + // 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 fbf1c6b6..3304d52a 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -68,7 +68,9 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) - decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; + decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity()); + decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity()); + decimal inverseMassBodies = body1MassInverse + body2MassInverse; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + @@ -113,14 +115,14 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); // Apply the impulse to the body 1 - v1 += mBody1->mMassInverse * linearImpulseBody1; + v1 += constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity) * 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 += mBody2->mMassInverse * mImpulse; + v2 += constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity) * mImpulse; w2 += mI2 * angularImpulseBody2; } @@ -148,14 +150,14 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); // Apply the impulse to the body 1 - v1 += mBody1->mMassInverse * linearImpulseBody1; + v1 += constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity) * 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 += mBody2->mMassInverse * deltaLambda; + v2 += constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity) * deltaLambda; w2 += mI2 * angularImpulseBody2; } @@ -173,8 +175,8 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); // Recompute the inverse inertia tensors mI1 = mBody1->getInertiaTensorInverseWorld(); diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 022984ea..552f77d2 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -83,7 +83,9 @@ 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 - decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; + const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity()); + const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity()); + const decimal inverseMassBodies = body1MassInverse + body2MassInverse; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + @@ -140,8 +142,8 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) { Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass of the bodies - const decimal inverseMassBody1 = mBody1->mMassInverse; - const decimal inverseMassBody2 = mBody2->mMassInverse; + const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 Vector3 linearImpulseBody1 = -mImpulseTranslation; @@ -178,8 +180,8 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); // --------------- Translation Constraints --------------- // @@ -239,8 +241,8 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); // Recompute the inverse inertia tensors mI1 = mBody1->getInertiaTensorInverseWorld(); @@ -257,7 +259,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // --------------- Translation Constraints --------------- // // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints - decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; + decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 41b22a83..ab61f639 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -117,7 +117,9 @@ 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 inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; + decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity()); + decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity()); + decimal inverseMassBodies = body1MassInverse + body2MassInverse; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + @@ -209,8 +211,8 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = mBody1->mMassInverse; - const decimal inverseMassBody2 = mBody2->mMassInverse; + const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); // Compute the impulse P=J^T * lambda for the 2 rotation constraints Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y; @@ -268,8 +270,8 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); // --------------- Translation Constraints --------------- // @@ -418,8 +420,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); // Recompute the inverse inertia tensors mI1 = mBody1->getInertiaTensorInverseWorld(); @@ -455,7 +457,9 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // --------------- Translation Constraints --------------- // // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints - decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse; + const decimal body1InverseMass = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + const decimal body2InverseMass = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + decimal inverseMassBodies = body1InverseMass + body2InverseMass; Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 51501827..a46cf7f6 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -128,7 +128,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) - decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse; + const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity()); + const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity()); + const decimal sumInverseMass = body1MassInverse + body2MassInverse; Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; @@ -174,7 +176,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) { // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) - mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse + + mInverseMassMatrixLimit = sumInverseMass + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? @@ -197,7 +199,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa if (mIsMotorEnabled) { // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) - mInverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse; + mInverseMassMatrixMotor = sumInverseMass; mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ? decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0); } @@ -227,8 +229,8 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - const decimal inverseMassBody1 = mBody1->mMassInverse; - const decimal inverseMassBody2 = mBody2->mMassInverse; + const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1 decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit; @@ -289,8 +291,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); // --------------- Translation Constraints --------------- // @@ -450,8 +452,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - decimal inverseMassBody1 = mBody1->mMassInverse; - decimal inverseMassBody2 = mBody2->mMassInverse; + const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); // Recompute the inertia tensor of bodies mI1 = mBody1->getInertiaTensorInverseWorld(); @@ -490,7 +492,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // constraints (2x2 matrix) - decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse; + const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + decimal sumInverseMass = body1MassInverse + body2MassInverse; Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1; Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2; Vector3 I2R2CrossN1 = mI2 * mR2CrossN1; @@ -610,7 +614,9 @@ 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) - mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse + + const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); + const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity); + mInverseMassMatrixLimit = body1MassInverse + body2MassInverse + mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) + mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis); mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ? diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 20e31f5b..4f510b7f 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -143,8 +143,8 @@ void ContactSolver::initializeForIsland(uint islandIndex) { mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mDynamicsComponents.getEntityIndex(body2->getEntity()); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse; - mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse; + mContactConstraints[mNbContactManifolds].massInverseBody1 = mDynamicsComponents.getMassInverse(body1->getEntity()); + mContactConstraints[mNbContactManifolds].massInverseBody2 = mDynamicsComponents.getMassInverse(body2->getEntity()); mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.getNbContactPoints(); mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 7d09fae3..26e3ddf2 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -314,16 +314,17 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { assert(indexBody < mRigidBodies.size()); // Integrate the external force to get the new velocity of the body - mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, body->getLinearVelocity() + - mTimeStep * body->mMassInverse * mDynamicsComponents.getExternalForce(bodyEntity)); - mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, body->getAngularVelocity() + + mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getLinearVelocity(bodyEntity) + + mTimeStep * mDynamicsComponents.getMassInverse(bodyEntity) * mDynamicsComponents.getExternalForce(bodyEntity)); + mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, mDynamicsComponents.getAngularVelocity(bodyEntity) + mTimeStep * body->getInertiaTensorInverseWorld() * mDynamicsComponents.getExternalTorque(bodyEntity)); // If the gravity has to be applied to this rigid body if (body->isGravityEnabled() && mIsGravityEnabled) { // Integrate the gravity force - mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity) + mTimeStep * body->mMassInverse * + mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity) + + mTimeStep * mDynamicsComponents.getMassInverse(bodyEntity) * body->getMass() * mGravity); }