From 1bc7e0710ba5ee9e7a90af9a53919211ae439c5d Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 20 May 2019 07:42:24 +0200 Subject: [PATCH] Add constrained position/orientation to DynamicsComponents --- src/components/DynamicsComponents.cpp | 20 +++++++++-- src/components/DynamicsComponents.h | 50 +++++++++++++++++++++++++++ src/constraint/BallAndSocketJoint.cpp | 13 ++++--- src/constraint/FixedJoint.cpp | 13 ++++--- src/constraint/HingeJoint.cpp | 13 ++++--- src/constraint/SliderJoint.cpp | 13 ++++--- src/engine/ConstraintSolver.h | 23 +----------- src/engine/DynamicsWorld.cpp | 26 ++++---------- src/engine/DynamicsWorld.h | 6 ---- 9 files changed, 112 insertions(+), 65 deletions(-) diff --git a/src/components/DynamicsComponents.cpp b/src/components/DynamicsComponents.cpp index 7dc8764b..372feb1f 100644 --- a/src/components/DynamicsComponents.cpp +++ b/src/components/DynamicsComponents.cpp @@ -38,7 +38,7 @@ 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(bool) + sizeof(bool)) { + sizeof(Vector3) + sizeof(Quaternion) + sizeof(bool) + sizeof(bool)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -72,7 +72,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { decimal* newInverseMasses = reinterpret_cast(newInitMasses + nbComponentsToAllocate); Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast(newInverseMasses + nbComponentsToAllocate); Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast(newInertiaTensorLocalInverses + nbComponentsToAllocate); - bool* newIsGravityEnabled = reinterpret_cast(newInertiaTensorWorldInverses + nbComponentsToAllocate); + Vector3* newConstrainedPositions = reinterpret_cast(newInertiaTensorWorldInverses + nbComponentsToAllocate); + Quaternion* newConstrainedOrientations = reinterpret_cast(newConstrainedPositions + nbComponentsToAllocate); + bool* newIsGravityEnabled = reinterpret_cast(newConstrainedOrientations + nbComponentsToAllocate); bool* newIsAlreadyInIsland = reinterpret_cast(newIsGravityEnabled + nbComponentsToAllocate); // If there was already components before @@ -94,6 +96,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal)); memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3)); memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3)); + memcpy(newConstrainedPositions, mConstrainedPositions, mNbComponents * sizeof(Vector3)); + memcpy(newConstrainedOrientations, mConstrainedOrientations, mNbComponents * sizeof(Quaternion)); memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool)); memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); @@ -117,6 +121,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { mInverseMasses = newInverseMasses; mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses; mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses; + mConstrainedPositions = newConstrainedPositions; + mConstrainedOrientations = newConstrainedOrientations; mIsGravityEnabled = newIsGravityEnabled; mIsAlreadyInIsland = newIsAlreadyInIsland; mNbAllocatedComponents = nbComponentsToAllocate; @@ -144,6 +150,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const mInverseMasses[index] = decimal(1.0); new (mInverseInertiaTensorsLocal + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); 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); mIsGravityEnabled[index] = true; mIsAlreadyInIsland[index] = false; @@ -178,6 +186,8 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) mInverseMasses[destIndex] = mInverseMasses[srcIndex]; mInverseInertiaTensorsLocal[destIndex] = mInverseInertiaTensorsLocal[srcIndex]; mInverseInertiaTensorsWorld[destIndex] = mInverseInertiaTensorsWorld[srcIndex]; + mConstrainedPositions[destIndex] = mConstrainedPositions[srcIndex]; + mConstrainedOrientations[destIndex] = mConstrainedOrientations[srcIndex]; mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex]; mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; @@ -214,6 +224,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { decimal inverseMass1 = mInverseMasses[index1]; Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1]; Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1]; + Vector3 constrainedPosition1 = mConstrainedPositions[index1]; + Quaternion constrainedOrientation1 = mConstrainedOrientations[index1]; bool isGravityEnabled1 = mIsGravityEnabled[index1]; bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; @@ -238,6 +250,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { mInverseMasses[index2] = inverseMass1; mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1; mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1; + mConstrainedPositions[index2] = constrainedPosition1; + mConstrainedOrientations[index2] = constrainedOrientation1; mIsGravityEnabled[index2] = isGravityEnabled1; mIsAlreadyInIsland[index2] = isAlreadyInIsland1; @@ -269,4 +283,6 @@ void DynamicsComponents::destroyComponent(uint32 index) { mExternalTorques[index].~Vector3(); mInverseInertiaTensorsLocal[index].~Matrix3x3(); mInverseInertiaTensorsWorld[index].~Matrix3x3(); + mConstrainedPositions[index].~Vector3(); + mConstrainedOrientations[index].~Quaternion(); } diff --git a/src/components/DynamicsComponents.h b/src/components/DynamicsComponents.h index 96f9bb0e..bcb25018 100644 --- a/src/components/DynamicsComponents.h +++ b/src/components/DynamicsComponents.h @@ -96,6 +96,12 @@ class DynamicsComponents : public Components { /// Array with the inverse of the world inertia tensor of each component Matrix3x3* mInverseInertiaTensorsWorld; + /// Array with the constrained position of each component (for position error correction) + Vector3* mConstrainedPositions; + + /// Array of constrained orientation for each component (for position error correction) + Quaternion* mConstrainedOrientations; + /// True if the gravity needs to be applied to this component bool* mIsGravityEnabled; @@ -184,6 +190,12 @@ class DynamicsComponents : public Components { /// Return the inverse world inertia tensor of an entity const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity); + /// Return the constrained position of an entity + const Vector3& getConstrainedPosition(Entity bodyEntity); + + /// Return the constrained orientation of an entity + const Quaternion& getConstrainedOrientation(Entity bodyEntity); + /// Return true if gravity is enabled for this entity bool getIsGravityEnabled(Entity bodyEntity) const; @@ -232,6 +244,12 @@ class DynamicsComponents : public Components { /// Set the inverse world inertia tensor of an entity void setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse); + /// Set the constrained position of an entity + void setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition); + + /// Set the constrained orientation of an entity + void setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation); + /// Set the value to know if the gravity is enabled for this entity bool setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled); @@ -378,6 +396,22 @@ inline const Matrix3x3& DynamicsComponents::getInertiaTensorWorldInverse(Entity return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]]; } +// Return the constrained position of an entity +inline const Vector3& DynamicsComponents::getConstrainedPosition(Entity bodyEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the constrained orientation of an entity +inline const Quaternion& DynamicsComponents::getConstrainedOrientation(Entity bodyEntity) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]]; +} + // Set the constrained linear velocity of an entity inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) { @@ -474,6 +508,22 @@ inline void DynamicsComponents::setInverseInertiaTensorWorld(Entity bodyEntity, mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorWorldInverse; } +// Set the constrained position of an entity +inline void DynamicsComponents::setConstrainedPosition(Entity bodyEntity, const Vector3& constrainedPosition) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mConstrainedPositions[mMapEntityToComponentIndex[bodyEntity]] = constrainedPosition; +} + +// Set the constrained orientation of an entity +inline void DynamicsComponents::setConstrainedOrientation(Entity bodyEntity, const Quaternion& constrainedOrientation) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mConstrainedOrientations[mMapEntityToComponentIndex[bodyEntity]] = constrainedOrientation; +} + // 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 3304d52a..be218be7 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -169,10 +169,10 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies center of mass and orientations - Vector3& x1 = constraintSolverData.positions[mIndexBody1]; - Vector3& x2 = constraintSolverData.positions[mIndexBody2]; - Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; - Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity); + Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity); + Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity); + Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity); // Get the inverse mass and inverse inertia tensors of the bodies const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); @@ -234,5 +234,10 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con x2 += v2; q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); + + constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1); + constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2); + constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1); + constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2); } diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index 552f77d2..f16ba568 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -235,10 +235,10 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies positions and orientations - Vector3& x1 = constraintSolverData.positions[mIndexBody1]; - Vector3& x2 = constraintSolverData.positions[mIndexBody2]; - Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; - Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity); + Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity); + Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity); + Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity); // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); @@ -357,5 +357,10 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); + + constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1); + constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2); + constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1); + constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2); } diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index ab61f639..af5a540e 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -414,10 +414,10 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies positions and orientations - Vector3& x1 = constraintSolverData.positions[mIndexBody1]; - Vector3& x2 = constraintSolverData.positions[mIndexBody2]; - Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; - Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity); + Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity); + Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity); + Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity); // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); @@ -614,6 +614,11 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS q2.normalize(); } } + + constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1); + constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2); + constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1); + constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2); } diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index a46cf7f6..0d1cf8d1 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -446,10 +446,10 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint if (mPositionCorrectionTechnique != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return; // Get the bodies positions and orientations - Vector3& x1 = constraintSolverData.positions[mIndexBody1]; - Vector3& x2 = constraintSolverData.positions[mIndexBody2]; - Quaternion& q1 = constraintSolverData.orientations[mIndexBody1]; - Quaternion& q2 = constraintSolverData.orientations[mIndexBody2]; + Vector3 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity); + Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity); + Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity); + Quaternion q2 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody2Entity); // Get the inverse mass and inverse inertia tensors of the bodies const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity); @@ -689,6 +689,11 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint q2.normalize(); } } + + constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody1Entity, x1); + constraintSolverData.dynamicsComponents.setConstrainedPosition(mBody2Entity, x2); + constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody1Entity, q1); + constraintSolverData.dynamicsComponents.setConstrainedOrientation(mBody2Entity, q2); } // Enable/Disable the limits of the joint diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index d051daf2..0a4ae48c 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -54,18 +54,12 @@ struct ConstraintSolverData { /// Reference to the dynamics components DynamicsComponents& dynamicsComponents; - /// Reference to the bodies positions - Vector3* positions; - - /// Reference to the bodies orientations - Quaternion* orientations; - /// True if warm starting of the solver is active bool isWarmStartingActive; /// Constructor ConstraintSolverData(DynamicsComponents& dynamicsComponents) - :dynamicsComponents(dynamicsComponents), positions(nullptr), orientations(nullptr) { + :dynamicsComponents(dynamicsComponents) { } @@ -189,10 +183,6 @@ class ConstraintSolver { /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); - /// Set the constrained positions/orientations arrays - void setConstrainedPositionsArrays(Vector3* constrainedPositions, - Quaternion* constrainedOrientations); - #ifdef IS_PROFILING_ACTIVE /// Set the profiler @@ -202,17 +192,6 @@ class ConstraintSolver { }; -// Set the constrained positions/orientations arrays -inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions, - Quaternion* constrainedOrientations) { - - assert(constrainedPositions != nullptr); - assert(constrainedOrientations != nullptr); - - mConstraintSolverData.positions = constrainedPositions; - mConstraintSolverData.orientations = constrainedOrientations; -} - #ifdef IS_PROFILING_ACTIVE // Set the profiler diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 901ca014..a2f7789d 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -56,8 +56,7 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()), mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)), - mIsGravityEnabled(true), mConstrainedPositions(nullptr), - mConstrainedOrientations(nullptr), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity), + mIsGravityEnabled(true), mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity), mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep), mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) { @@ -176,7 +175,6 @@ void DynamicsWorld::integrateRigidBodiesPositions() { RigidBody* body = static_cast(mBodyComponents.getBody(bodyEntity)); // Get the constrained velocity - uint indexArray = body->mArrayIndex; Vector3 newLinVelocity = mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity); Vector3 newAngVelocity = mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity); @@ -196,10 +194,10 @@ void DynamicsWorld::integrateRigidBodiesPositions() { const Quaternion& currentOrientation = mTransformComponents.getTransform(body->getEntity()).getOrientation(); // Update the new constrained position and orientation of the body - mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep; - mConstrainedOrientations[indexArray] = currentOrientation + + mDynamicsComponents.setConstrainedPosition(bodyEntity, currentPosition + newLinVelocity * mTimeStep); + mDynamicsComponents.setConstrainedOrientation(bodyEntity, currentOrientation + Quaternion(0, newAngVelocity) * - currentOrientation * decimal(0.5) * mTimeStep; + currentOrientation * decimal(0.5) * mTimeStep); } } } @@ -229,10 +227,11 @@ void DynamicsWorld::updateBodiesState() { mDynamicsComponents.setAngularVelocity(bodyEntity, mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity)); // Update the position of the center of mass of the body - body->mCenterOfMassWorld = mConstrainedPositions[index]; + body->mCenterOfMassWorld = mDynamicsComponents.getConstrainedPosition(bodyEntity); // Update the orientation of the body - mTransformComponents.getTransform(bodyEntity).setOrientation(mConstrainedOrientations[index].getUnit()); + 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(); @@ -256,13 +255,6 @@ void DynamicsWorld::initVelocityArrays() { assert(mDynamicsComponents.getNbComponents() == nbBodies); - mConstrainedPositions = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, - nbBodies * sizeof(Vector3))); - mConstrainedOrientations = static_cast(mMemoryManager.allocate(MemoryManager::AllocationType::Frame, - nbBodies * sizeof(Quaternion))); - assert(mConstrainedPositions != nullptr); - assert(mConstrainedOrientations != nullptr); - // Initialize the map of body indexes in the velocity arrays uint i = 0; for (List::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { @@ -348,10 +340,6 @@ void DynamicsWorld::solveContactsAndConstraints() { RP3D_PROFILE("DynamicsWorld::solveContactsAndConstraints()", mProfiler); - // Set the velocities arrays - mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions, - mConstrainedOrientations); - // ---------- Solve velocity constraints for joints and contacts ---------- // // Initialize the contact solver diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 45f8f9dd..da5a5129 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -88,12 +88,6 @@ class DynamicsWorld : public CollisionWorld { /// True if the gravity force is on bool mIsGravityEnabled; - /// Array of constrained rigid bodies position (for position error correction) - Vector3* mConstrainedPositions; - - /// Array of constrained rigid bodies orientation (for position error correction) - Quaternion* mConstrainedOrientations; - /// Sleep linear velocity threshold decimal mSleepLinearVelocity;