diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 2948f9be..b02abbf2 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -40,9 +40,8 @@ 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), - mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), - mMaterial(world.mConfig), mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) { + : CollisionBody(world, entity, id), mMaterial(world.mConfig), mJointsList(nullptr), + mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) { // Compute the inverse mass mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity)); @@ -173,7 +172,8 @@ inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) { // Add the torque const Vector3& externalTorque = mWorld.mDynamicsComponents.getExternalTorque(mEntity); - mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + (point - mCenterOfMassWorld).cross(force)); + const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); + mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + (point - centerOfMassWorld).cross(force)); } // Set the local inertia tensor of the body (in local-space coordinates) @@ -278,16 +278,18 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) { mIsCenterOfMassSetByUser = true; - const Vector3 oldCenterOfMass = mCenterOfMassWorld; - mCenterOfMassLocal = centerOfMassLocal; + const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); + mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, centerOfMassLocal); // Compute the center of mass in world-space coordinates - mCenterOfMassWorld = mWorld.mTransformComponents.getTransform(mEntity) * mCenterOfMassLocal; + const Vector3& updatedCenterOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity); + mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, mWorld.mTransformComponents.getTransform(mEntity) * updatedCenterOfMassLocal); // Update the linear velocity of the center of mass Vector3 linearVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); - linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); + linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass); mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, @@ -498,7 +500,9 @@ void RigidBody::updateTransformWithCenterOfMass() { // Translate the body according to the translation of the center of mass position Transform& transform = mWorld.mTransformComponents.getTransform(mEntity); - transform.setPosition(mCenterOfMassWorld - transform.getOrientation() * mCenterOfMassLocal); + const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); + const Vector3& centerOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity); + transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal); } // Set a new material for this rigid body @@ -563,15 +567,17 @@ void RigidBody::setAngularVelocity(const Vector3& angularVelocity) { */ void RigidBody::setTransform(const Transform& transform) { - const Vector3 oldCenterOfMass = mCenterOfMassWorld; + const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); // Compute the new center of mass in world-space coordinates - mCenterOfMassWorld = transform * mCenterOfMassLocal; + const Vector3& centerOfMassLocal = mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity); + mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform * centerOfMassLocal); // Update the linear velocity of the center of mass Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity); const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); - linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + const Vector3& centerOfMassWorld = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); + linearVelocity += angularVelocity.cross(centerOfMassWorld - oldCenterOfMass); mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity); CollisionBody::setTransform(transform); @@ -591,7 +597,7 @@ void RigidBody::recomputeMassInformation() { mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0.0)); if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero()); if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero()); - if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero(); + if (!mIsCenterOfMassSetByUser) mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, Vector3::zero()); Matrix3x3 inertiaTensorLocal; inertiaTensorLocal.setToZero(); @@ -599,7 +605,7 @@ void RigidBody::recomputeMassInformation() { // If it is a STATIC or a KINEMATIC body if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) { - mCenterOfMassWorld = transform.getPosition(); + mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform.getPosition()); return; } @@ -612,7 +618,8 @@ void RigidBody::recomputeMassInformation() { mWorld.mDynamicsComponents.setInitMass(mEntity, mWorld.mDynamicsComponents.getInitMass(mEntity) + proxyShape->getMass()); if (!mIsCenterOfMassSetByUser) { - mCenterOfMassLocal += proxyShape->getLocalToBodyTransform().getPosition() * proxyShape->getMass(); + mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity) + + proxyShape->getLocalToBodyTransform().getPosition() * proxyShape->getMass()); } } @@ -620,18 +627,18 @@ void RigidBody::recomputeMassInformation() { mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity)); } else { - mCenterOfMassWorld = transform.getPosition(); + mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform.getPosition()); return; } // Compute the center of mass - const Vector3 oldCenterOfMass = mCenterOfMassWorld; + const Vector3 oldCenterOfMass = mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity); if (!mIsCenterOfMassSetByUser) { - mCenterOfMassLocal *= mWorld.mDynamicsComponents.getMassInverse(mEntity); + mWorld.mDynamicsComponents.setCenterOfMassLocal(mEntity, mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity) * mWorld.mDynamicsComponents.getMassInverse(mEntity)); } - mCenterOfMassWorld = transform * mCenterOfMassLocal; + mWorld.mDynamicsComponents.setCenterOfMassWorld(mEntity, transform * mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity)); if (!mIsInertiaTensorSetByUser) { @@ -652,7 +659,7 @@ void RigidBody::recomputeMassInformation() { // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape // center into a inertia tensor w.r.t to the body origin. - Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal; + Vector3 offset = shapeTransform.getPosition() - mWorld.mDynamicsComponents.getCenterOfMassLocal(mEntity); decimal offsetSquare = offset.lengthSquare(); Matrix3x3 offsetMatrix; offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0)); @@ -676,7 +683,7 @@ void RigidBody::recomputeMassInformation() { // Update the linear velocity of the center of mass Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity); Vector3 angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity); - linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); + linearVelocity += angularVelocity.cross(mWorld.mDynamicsComponents.getCenterOfMassWorld(mEntity) - oldCenterOfMass); mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity); } diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index d5bd92cc..1f8e16bb 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -50,23 +50,10 @@ class MemoryManager; */ class RigidBody : public CollisionBody { - private : - - /// Index of the body in arrays for contact/constraint solver - // TODO : REMOVE THIS - uint mArrayIndex; - protected : // -------------------- Attributes -------------------- // - /// Center of mass of the body in local-space coordinates. - /// The center of mass can therefore be different from the body origin - Vector3 mCenterOfMassLocal; - - /// Center of mass of the body in world-space coordinates - Vector3 mCenterOfMassWorld; - /// Inverse Local inertia tensor of the body (in local-space) set /// by the user with respect to the center of mass of the body Matrix3x3 mUserInertiaTensorLocalInverse; diff --git a/src/components/DynamicsComponents.cpp b/src/components/DynamicsComponents.cpp index 372feb1f..8350ae2f 100644 --- a/src/components/DynamicsComponents.cpp +++ b/src/components/DynamicsComponents.cpp @@ -38,7 +38,8 @@ 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(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) + sizeof(Matrix3x3) + - sizeof(Vector3) + sizeof(Quaternion) + sizeof(bool) + sizeof(bool)) { + sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + sizeof(bool) + + sizeof(bool)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -74,7 +75,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast(newInertiaTensorLocalInverses + nbComponentsToAllocate); Vector3* newConstrainedPositions = reinterpret_cast(newInertiaTensorWorldInverses + nbComponentsToAllocate); Quaternion* newConstrainedOrientations = reinterpret_cast(newConstrainedPositions + nbComponentsToAllocate); - bool* newIsGravityEnabled = reinterpret_cast(newConstrainedOrientations + nbComponentsToAllocate); + Vector3* newCentersOfMassLocal = reinterpret_cast(newConstrainedOrientations + nbComponentsToAllocate); + Vector3* newCentersOfMassWorld = reinterpret_cast(newCentersOfMassLocal + nbComponentsToAllocate); + bool* newIsGravityEnabled = reinterpret_cast(newCentersOfMassWorld + nbComponentsToAllocate); bool* newIsAlreadyInIsland = reinterpret_cast(newIsGravityEnabled + nbComponentsToAllocate); // If there was already components before @@ -98,6 +101,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3)); memcpy(newConstrainedPositions, mConstrainedPositions, mNbComponents * sizeof(Vector3)); memcpy(newConstrainedOrientations, mConstrainedOrientations, mNbComponents * sizeof(Quaternion)); + memcpy(newCentersOfMassLocal, mCentersOfMassLocal, mNbComponents * sizeof(Vector3)); + memcpy(newCentersOfMassWorld, mCentersOfMassWorld, mNbComponents * sizeof(Vector3)); memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool)); memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); @@ -123,6 +128,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses; mConstrainedPositions = newConstrainedPositions; mConstrainedOrientations = newConstrainedOrientations; + mCentersOfMassLocal = newCentersOfMassLocal; + mCentersOfMassWorld = newCentersOfMassWorld; mIsGravityEnabled = newIsGravityEnabled; mIsAlreadyInIsland = newIsAlreadyInIsland; mNbAllocatedComponents = nbComponentsToAllocate; @@ -136,8 +143,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const // Insert the new component data new (mBodies + index) Entity(bodyEntity); - new (mLinearVelocities + index) Vector3(component.linearVelocity); - new (mAngularVelocities + index) Vector3(component.angularVelocity); + new (mLinearVelocities + index) Vector3(0, 0, 0); + new (mAngularVelocities + index) Vector3(0, 0, 0); new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0); new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0); new (mSplitLinearVelocities + index) Vector3(0, 0, 0); @@ -152,6 +159,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); new (mConstrainedPositions + index) Vector3(0, 0, 0); new (mConstrainedOrientations + index) Quaternion(0, 0, 0, 1); + new (mCentersOfMassLocal + index) Vector3(0, 0, 0); + new (mCentersOfMassWorld + index) Vector3(component.worldPosition); mIsGravityEnabled[index] = true; mIsAlreadyInIsland[index] = false; @@ -184,10 +193,12 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) mAngularDampings[destIndex] = mAngularDampings[srcIndex]; mInitMasses[destIndex] = mInitMasses[srcIndex]; mInverseMasses[destIndex] = mInverseMasses[srcIndex]; - mInverseInertiaTensorsLocal[destIndex] = mInverseInertiaTensorsLocal[srcIndex]; - mInverseInertiaTensorsWorld[destIndex] = mInverseInertiaTensorsWorld[srcIndex]; - mConstrainedPositions[destIndex] = mConstrainedPositions[srcIndex]; - mConstrainedOrientations[destIndex] = mConstrainedOrientations[srcIndex]; + new (mInverseInertiaTensorsLocal + destIndex) Matrix3x3(mInverseInertiaTensorsLocal[srcIndex]); + new (mInverseInertiaTensorsWorld + destIndex) Matrix3x3(mInverseInertiaTensorsWorld[srcIndex]); + new (mConstrainedPositions + destIndex) Vector3(mConstrainedPositions[srcIndex]); + new (mConstrainedOrientations + destIndex) Quaternion(mConstrainedOrientations[srcIndex]); + new (mCentersOfMassLocal + destIndex) Vector3(mCentersOfMassLocal[srcIndex]); + new (mCentersOfMassWorld + destIndex) Vector3(mCentersOfMassWorld[srcIndex]); mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex]; mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; @@ -226,6 +237,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1]; Vector3 constrainedPosition1 = mConstrainedPositions[index1]; Quaternion constrainedOrientation1 = mConstrainedOrientations[index1]; + Vector3 centerOfMassLocal1 = mCentersOfMassLocal[index1]; + Vector3 centerOfMassWorld1 = mCentersOfMassWorld[index1]; bool isGravityEnabled1 = mIsGravityEnabled[index1]; bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; @@ -252,6 +265,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1; mConstrainedPositions[index2] = constrainedPosition1; mConstrainedOrientations[index2] = constrainedOrientation1; + mCentersOfMassLocal[index2] = centerOfMassLocal1; + mCentersOfMassWorld[index2] = centerOfMassWorld1; mIsGravityEnabled[index2] = isGravityEnabled1; mIsAlreadyInIsland[index2] = isAlreadyInIsland1; @@ -285,4 +300,6 @@ void DynamicsComponents::destroyComponent(uint32 index) { mInverseInertiaTensorsWorld[index].~Matrix3x3(); mConstrainedPositions[index].~Vector3(); mConstrainedOrientations[index].~Quaternion(); + mCentersOfMassLocal[index].~Vector3(); + mCentersOfMassWorld[index].~Vector3(); } diff --git a/src/components/DynamicsComponents.h b/src/components/DynamicsComponents.h index bcb25018..8cbe2135 100644 --- a/src/components/DynamicsComponents.h +++ b/src/components/DynamicsComponents.h @@ -102,6 +102,12 @@ class DynamicsComponents : public Components { /// Array of constrained orientation for each component (for position error correction) Quaternion* mConstrainedOrientations; + /// Array of center of mass of each component (in local-space coordinates) + Vector3* mCentersOfMassLocal; + + /// Array of center of mass of each component (in world-space coordinates) + Vector3* mCentersOfMassWorld; + /// True if the gravity needs to be applied to this component bool* mIsGravityEnabled; @@ -127,12 +133,11 @@ class DynamicsComponents : public Components { /// Structure for the data of a transform component struct DynamicsComponent { - const Vector3& linearVelocity; - const Vector3& angularVelocity; + const Vector3& worldPosition; /// Constructor - DynamicsComponent(const Vector3& linearVelocity, const Vector3& angularVelocity) - : linearVelocity(linearVelocity), angularVelocity(angularVelocity) { + DynamicsComponent(const Vector3& worldPosition) + : worldPosition(worldPosition) { } }; @@ -196,6 +201,12 @@ class DynamicsComponents : public Components { /// Return the constrained orientation of an entity const Quaternion& getConstrainedOrientation(Entity bodyEntity); + /// Return the local center of mass of an entity + const Vector3& getCenterOfMassLocal(Entity bodyEntity); + + /// Return the world center of mass of an entity + const Vector3& getCenterOfMassWorld(Entity bodyEntity); + /// Return true if gravity is enabled for this entity bool getIsGravityEnabled(Entity bodyEntity) const; @@ -250,6 +261,12 @@ class DynamicsComponents : public Components { /// Set the constrained orientation of an entity void setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation); + /// Set the local center of mass of an entity + void setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal); + + /// Set the world center of mass of an entity + void setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld); + /// Set the value to know if the gravity is enabled for this entity bool setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled); @@ -412,6 +429,22 @@ inline const Quaternion& DynamicsComponents::getConstrainedOrientation(Entity bo return mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]]; } +// Return the local center of mass of an entity +inline const Vector3& DynamicsComponents::getCenterOfMassLocal(Entity bodyEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mCentersOfMassLocal[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the world center of mass of an entity +inline const Vector3& DynamicsComponents::getCenterOfMassWorld(Entity bodyEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mCentersOfMassWorld[mMapEntityToComponentIndex[bodyEntity]]; +} + // Set the constrained linear velocity of an entity inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) { @@ -524,6 +557,22 @@ inline void DynamicsComponents::setConstrainedOrientation(Entity bodyEntity, con mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]] = constrainedOrientation; } +// Set the local center of mass of an entity +inline void DynamicsComponents::setCenterOfMassLocal(Entity bodyEntity, const Vector3& centerOfMassLocal) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mCentersOfMassLocal[mMapEntityToComponentIndex[bodyEntity]] = centerOfMassLocal; +} + +// Set the world center of mass of an entity +inline void DynamicsComponents::setCenterOfMassWorld(Entity bodyEntity, const Vector3& centerOfMassWorld) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mCentersOfMassWorld[mMapEntityToComponentIndex[bodyEntity]] = centerOfMassWorld; +} + // Return true if gravity is enabled for this entity inline bool DynamicsComponents::getIsGravityEnabled(Entity bodyEntity) const { diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index be218be7..b795560d 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -45,13 +45,9 @@ BallAndSocketJoint::BallAndSocketJoint(uint id, const BallAndSocketJointInfo& jo // Initialize before solving the constraint void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { - // Initialize the bodies index in the velocity array - mIndexBody1 = mBody1->mArrayIndex; - mIndexBody2 = mBody2->mArrayIndex; - // Get the bodies center of mass and orientations - const Vector3& x1 = mBody1->mCenterOfMassWorld; - const Vector3& x2 = mBody2->mCenterOfMassWorld; + const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity); + const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index f16ba568..a195e974 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -60,13 +60,9 @@ FixedJoint::FixedJoint(uint id, const FixedJointInfo& jointInfo) // Initialize before solving the constraint void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { - // Initialize the bodies index in the velocity array - mIndexBody1 = mBody1->mArrayIndex; - mIndexBody2 = mBody2->mArrayIndex; - // Get the bodies positions and orientations - const Vector3& x1 = mBody1->mCenterOfMassWorld; - const Vector3& x2 = mBody2->mCenterOfMassWorld; + const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity); + const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index af5a540e..a1651027 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -67,13 +67,9 @@ HingeJoint::HingeJoint(uint id, const HingeJointInfo& jointInfo) // Initialize before solving the constraint void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { - // Initialize the bodies index in the velocity array - mIndexBody1 = mBody1->mArrayIndex; - mIndexBody2 = mBody2->mArrayIndex; - // Get the bodies positions and orientations - const Vector3& x1 = mBody1->mCenterOfMassWorld; - const Vector3& x2 = mBody2->mCenterOfMassWorld; + const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity); + const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); diff --git a/src/constraint/Joint.h b/src/constraint/Joint.h index 880ace90..38e26b5d 100644 --- a/src/constraint/Joint.h +++ b/src/constraint/Joint.h @@ -140,12 +140,6 @@ class Joint { /// Type of the joint const JointType mType; - /// Body 1 index in the velocity array to solve the constraint - uint mIndexBody1; - - /// Body 2 index in the velocity array to solve the constraint - uint mIndexBody2; - /// Position correction technique used for the constraint (used for joints) JointsPositionCorrectionTechnique mPositionCorrectionTechnique; diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 0d1cf8d1..48be0755 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -75,13 +75,9 @@ SliderJoint::SliderJoint(uint id, const SliderJointInfo& jointInfo) // Initialize before solving the constraint void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { - // Initialize the bodies index in the veloc ity array - mIndexBody1 = mBody1->mArrayIndex; - mIndexBody2 = mBody2->mArrayIndex; - // Get the bodies positions and orientations - const Vector3& x1 = mBody1->mCenterOfMassWorld; - const Vector3& x2 = mBody2->mCenterOfMassWorld; + const Vector3& x1 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody1Entity); + const Vector3& x2 = constraintSolverData.dynamicsComponents.getCenterOfMassWorld(mBody2Entity); const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 4f510b7f..a290a9d0 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -131,14 +131,12 @@ void ContactSolver::initializeForIsland(uint islandIndex) { const ProxyShape* shape2 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity2); // Get the position of the two bodies - const Vector3& x1 = body1->mCenterOfMassWorld; - const Vector3& x2 = body2->mCenterOfMassWorld; + const Vector3& x1 = mDynamicsComponents.getCenterOfMassWorld(externalManifold.bodyEntity1); + const Vector3& x2 = mDynamicsComponents.getCenterOfMassWorld(externalManifold.bodyEntity2); // Initialize the internal contact manifold structure using the external // contact manifold new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); - mContactConstraints[mNbContactManifolds].indexBody1 = body1->mArrayIndex; - mContactConstraints[mNbContactManifolds].indexBody2 = body2->mArrayIndex; mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mDynamicsComponents.getEntityIndex(body1->getEntity()); mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mDynamicsComponents.getEntityIndex(body2->getEntity()); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index e130a4ea..5edfc38c 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -174,14 +174,6 @@ class ContactSolver { /// Pointer to the external contact manifold ContactManifold* externalContactManifold; - /// Index of body 1 in the constraint solver - // TODO : Remove this - int32 indexBody1; - - /// Index of body 2 in the constraint solver - // TODO : Remove this - int32 indexBody2; - /// Index of body 1 in the dynamics components arrays uint32 dynamicsComponentIndexBody1; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index a2f7789d..fc65a54f 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -163,42 +163,27 @@ void DynamicsWorld::integrateRigidBodiesPositions() { RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", mProfiler); - // TODO : We should loop over non-sleeping dynamic components here and not over islands + const decimal isSplitImpulseActive = mContactSolver.isSplitImpulseActive() ? decimal(1.0) : decimal(0.0); - // For each island of the world - for (uint i=0; i < mIslands.getNbIslands(); i++) { + for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) { - // For each body of the island - for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) { + // Get the constrained velocity + Vector3 newLinVelocity = mDynamicsComponents.mConstrainedLinearVelocities[i]; + Vector3 newAngVelocity = mDynamicsComponents.mConstrainedAngularVelocities[i]; - const Entity bodyEntity = mIslands.bodyEntities[i][b]; - RigidBody* body = static_cast(mBodyComponents.getBody(bodyEntity)); + // Add the split impulse velocity from Contact Solver (only used + // to update the position) + newLinVelocity += isSplitImpulseActive * mDynamicsComponents.mSplitLinearVelocities[i]; + newAngVelocity += isSplitImpulseActive * mDynamicsComponents.mSplitAngularVelocities[i]; - // Get the constrained velocity - Vector3 newLinVelocity = mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity); - Vector3 newAngVelocity = mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity); + // Get current position and orientation of the body + const Vector3& currentPosition = mDynamicsComponents.mCentersOfMassWorld[i]; + const Quaternion& currentOrientation = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).getOrientation(); - // TODO : Remove this - Vector3 testLinVel = newLinVelocity; - - // Add the split impulse velocity from Contact Solver (only used - // to update the position) - if (mContactSolver.isSplitImpulseActive()) { - - newLinVelocity += mDynamicsComponents.getSplitLinearVelocity(bodyEntity); - newAngVelocity += mDynamicsComponents.getSplitAngularVelocity(bodyEntity); - } - - // Get current position and orientation of the body - const Vector3& currentPosition = body->mCenterOfMassWorld; - const Quaternion& currentOrientation = mTransformComponents.getTransform(body->getEntity()).getOrientation(); - - // Update the new constrained position and orientation of the body - mDynamicsComponents.setConstrainedPosition(bodyEntity, currentPosition + newLinVelocity * mTimeStep); - mDynamicsComponents.setConstrainedOrientation(bodyEntity, currentOrientation + - Quaternion(0, newAngVelocity) * - currentOrientation * decimal(0.5) * mTimeStep); - } + // Update the new constrained position and orientation of the body + mDynamicsComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * mTimeStep; + mDynamicsComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) * + currentOrientation * decimal(0.5) * mTimeStep; } } @@ -209,60 +194,41 @@ void DynamicsWorld::updateBodiesState() { // TODO : Make sure we compute this in a system - // TODO : We should loop over non-sleeping dynamic components here and not over islands + for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) { - // For each island of the world - for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) { + // Update the linear and angular velocity of the body + mDynamicsComponents.mLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i]; + mDynamicsComponents.mAngularVelocities[i] = mDynamicsComponents.mConstrainedAngularVelocities[i]; - // For each body of the island - for (uint b=0; b < mIslands.bodyEntities[islandIndex].size(); b++) { + // Update the position of the center of mass of the body + mDynamicsComponents.mCentersOfMassWorld[i] = mDynamicsComponents.mConstrainedPositions[i]; - Entity bodyEntity = mIslands.bodyEntities[islandIndex][b]; - RigidBody* body = static_cast(mBodyComponents.getBody(bodyEntity)); + // Update the orientation of the body + const Quaternion& constrainedOrientation = mDynamicsComponents.mConstrainedOrientations[i]; + mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).setOrientation(constrainedOrientation.getUnit()); + } - uint index = body->mArrayIndex; + // Update the transform of the body (using the new center of mass and new orientation) + for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) { - // Update the linear and angular velocity of the body - mDynamicsComponents.setLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity)); - mDynamicsComponents.setAngularVelocity(bodyEntity, mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity)); + Transform& transform = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]); + const Vector3& centerOfMassWorld = mDynamicsComponents.mCentersOfMassWorld[i]; + const Vector3& centerOfMassLocal = mDynamicsComponents.mCentersOfMassLocal[i]; + transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal); + } - // Update the position of the center of mass of the body - body->mCenterOfMassWorld = mDynamicsComponents.getConstrainedPosition(bodyEntity); + // Update the world inverse inertia tensor of the body + for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) { - // Update the orientation of the body - const Quaternion& constrainedOrientation = mDynamicsComponents.getConstrainedOrientation(bodyEntity); - mTransformComponents.getTransform(bodyEntity).setOrientation(constrainedOrientation.getUnit()); - - // Update the transform of the body (using the new center of mass and new orientation) - body->updateTransformWithCenterOfMass(); - - // Update the world inverse inertia tensor of the body - body->updateInertiaTensorInverseWorld(); - } + Matrix3x3 orientation = mTransformComponents.getTransform(mDynamicsComponents.mBodies[i]).getOrientation().getMatrix(); + const Matrix3x3& inverseInertiaLocalTensor = mDynamicsComponents.mInverseInertiaTensorsLocal[i]; + mDynamicsComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose(); } // Update the proxy-shapes components mCollisionDetection.updateProxyShapes(); } -// Initialize the bodies velocities arrays for the next simulation step. -void DynamicsWorld::initVelocityArrays() { - - RP3D_PROFILE("DynamicsWorld::initVelocityArrays()", mProfiler); - - // Allocate memory for the bodies velocity arrays - uint nbBodies = mRigidBodies.size(); - - assert(mDynamicsComponents.getNbComponents() == nbBodies); - - // Initialize the map of body indexes in the velocity arrays - uint i = 0; - for (List::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - - (*it)->mArrayIndex = i++; - } -} - // Reset the split velocities of the bodies void DynamicsWorld::resetSplitVelocities() { @@ -281,9 +247,6 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()", mProfiler); - // Initialize the bodies velocity arrays - initVelocityArrays(); - // Reset the split velocities of the bodies resetSplitVelocities(); @@ -416,7 +379,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform)); - mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(Vector3::zero(), Vector3::zero())); + mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(transform.getPosition())); // Create the rigid body RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index da5a5129..c04c9ccd 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -112,9 +112,6 @@ class DynamicsWorld : public CollisionWorld { /// Reset the external force and torque applied to the bodies void resetBodiesForceAndTorque(); - /// Initialize the bodies velocities arrays for the next simulation step. - void initVelocityArrays(); - /// Reset the split velocities of the bodies void resetSplitVelocities();