Add constrained position/orientation to DynamicsComponents

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<decimal*>(newInitMasses + nbComponentsToAllocate);
     Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate);
     Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
-    bool* newIsGravityEnabled = reinterpret_cast<bool*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
+    Vector3* newConstrainedPositions = reinterpret_cast<Vector3*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
+    Quaternion* newConstrainedOrientations = reinterpret_cast<Quaternion*>(newConstrainedPositions + nbComponentsToAllocate);
+    bool* newIsGravityEnabled = reinterpret_cast<bool*>(newConstrainedOrientations + nbComponentsToAllocate);
     bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(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();
 }

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 {
 

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 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
-    Vector3& x2 = constraintSolverData.positions[mIndexBody2];
+    Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
-    Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
+    Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
-    Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+    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);
 }
 

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 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
-    Vector3& x2 = constraintSolverData.positions[mIndexBody2];
+    Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
-    Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
+    Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
-    Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+    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);
 }
 

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 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
-    Vector3& x2 = constraintSolverData.positions[mIndexBody2];
+    Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
-    Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
+    Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
-    Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+    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);
 }
 
 

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 x1 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody1Entity);
-    Vector3& x2 = constraintSolverData.positions[mIndexBody2];
+    Vector3 x2 = constraintSolverData.dynamicsComponents.getConstrainedPosition(mBody2Entity);
-    Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
+    Quaternion q1 = constraintSolverData.dynamicsComponents.getConstrainedOrientation(mBody1Entity);
-    Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
+    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

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

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),
+                mIsGravityEnabled(true),  mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
-                mConstrainedOrientations(nullptr), 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<RigidBody*>(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;
+            mDynamicsComponents.setConstrainedPosition(bodyEntity, currentPosition + newLinVelocity * mTimeStep);
-            mConstrainedOrientations[indexArray] = currentOrientation +
+            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<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                                          nbBodies * sizeof(Vector3)));
-    mConstrainedOrientations = static_cast<Quaternion*>(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<RigidBody*>::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

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;