Modifications in the BallAndSocketJoint to make it work

examples/joints/Scene.cpp

@@ -167,7 +167,7 @@ void Scene::createBallAndSocketJoints() {
     // --------------- Create the second box --------------- //
 
     // Position of the box
-    openglframework::Vector3 positionBox2(0, 10, 0);
+    openglframework::Vector3 positionBox2(5, 10, 0);
 
     // Create a box and a corresponding rigid in the dynamics world
     mBallAndSocketJointBox2 = new Box(BOX_SIZE, positionBox2 , BOX_MASS, mDynamicsWorld);
@@ -184,7 +184,7 @@ void Scene::createBallAndSocketJoints() {
     rp3d::BallAndSocketJointInfo jointInfo;
     jointInfo.body1 = mBallAndSocketJointBox1->getRigidBody();
     jointInfo.body2 = mBallAndSocketJointBox2->getRigidBody();
-    jointInfo.anchorPointWorldSpace = rp3d::Vector3(0, 12.5, 0);
+    jointInfo.anchorPointWorldSpace = rp3d::Vector3(0, 10, 0);
 
     // Create the joint in the dynamics world
     mBallAndSocketJoint = dynamic_cast<rp3d::BallAndSocketJoint*>(

src/body/RigidBody.h

@@ -135,7 +135,7 @@ class RigidBody : public CollisionBody {
         decimal getMassInverse() const;
 
         /// Return the local inertia tensor of the body (in body coordinates)
-        Matrix3x3 getInertiaTensorLocal() const;
+        const Matrix3x3& getInertiaTensorLocal() const;
 
         /// Set the local inertia tensor of the body (in body coordinates)
         void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
@@ -222,7 +222,7 @@ inline decimal RigidBody::getMassInverse() const {
 }
 
 // Return the local inertia tensor of the body (in body coordinates)
-inline Matrix3x3 RigidBody::getInertiaTensorLocal() const {
+inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const {
     return mInertiaTensorLocal;
 }
 

src/constraint/BallAndSocketJoint.cpp

@@ -34,10 +34,8 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo)
                    : Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) {
 
     // Compute the local-space anchor point for each body
-    mLocalAnchorPointBody1 = mBody1->getTransform().getOrientation().getInverse() *
-                             jointInfo.anchorPointWorldSpace;
-    mLocalAnchorPointBody2 = mBody2->getTransform().getOrientation().getInverse() *
-                             jointInfo.anchorPointWorldSpace;
+    mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
+    mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
 }
 
 // Destructor
@@ -53,8 +51,6 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
     mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
 
     // Get the bodies positions and orientations
-    const Vector3& x1 = mBody1->getTransform().getPosition();
-    const Vector3& x2 = mBody2->getTransform().getPosition();
     const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
     const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
 
@@ -62,13 +58,9 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
     Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld();
     Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld();
 
-    // Compute the vector from body center to anchor point in local-space
-    const Vector3 u1Local = mLocalAnchorPointBody1 - x1;
-    const Vector3 u2Local = mLocalAnchorPointBody2 - x2;
-
     // Compute the vector from body center to the anchor point in world-space
-    mU1World = orientationBody1 * u1Local;
-    mU2World = orientationBody2 * u2Local;
+    mU1World = orientationBody1 * mLocalAnchorPointBody1;
+    mU2World = orientationBody2 * mLocalAnchorPointBody2;
 
     // Compute the corresponding skew-symmetric matrices
     Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU1World);
@@ -79,8 +71,10 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
     Matrix3x3 massMatrix= Matrix3x3(inverseMassBodies, 0, 0,
                                     0, inverseMassBodies, 0,
                                     0, 0, inverseMassBodies) +
-                          skewSymmetricMatrixU1 * inverseInertiaTensorBody1 * skewSymmetricMatrixU1+
-                          skewSymmetricMatrixU2 * inverseInertiaTensorBody2 * skewSymmetricMatrixU2;
+                          skewSymmetricMatrixU1 * inverseInertiaTensorBody1 *
+                          skewSymmetricMatrixU1.getTranspose() +
+                          skewSymmetricMatrixU2 * inverseInertiaTensorBody2 *
+                          skewSymmetricMatrixU2.getTranspose();
 
     // Compute the inverse mass matrix K
     mInverseMassMatrix = massMatrix.getInverse();
@@ -109,8 +103,8 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData)
     Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2);
 
     // Compute the bias "b" of the constraint
-    decimal beta = 0.7;     // TODO : Use a constant here
-    decimal biasFactor = -(beta/constraintSolverData.timeStep);
+    decimal beta = decimal(0.2);     // TODO : Use a constant here
+    decimal biasFactor = (beta / constraintSolverData.timeStep);
     Vector3 b = biasFactor * (x2 + mU2World - x1 - mU1World);
 
     // Compute the Lagrange multiplier lambda
@@ -119,9 +113,9 @@ void BallAndSocketJoint::solve(const ConstraintSolverData& constraintSolverData)
 
     // Compute the impulse P=J^T * lambda
     Vector3 linearImpulseBody1 = -deltaLambda;
-    Vector3 angularImpulseBody1 = mU1World.cross(deltaLambda);
+    Vector3 angularImpulseBody1 = deltaLambda.cross(mU1World);
     Vector3 linearImpulseBody2 = deltaLambda;
-    Vector3 angularImpulseBody2 = -mU2World.cross(deltaLambda);
+    Vector3 angularImpulseBody2 = -deltaLambda.cross(mU2World);
 
     // Apply the impulse to the bodies of the joint
     if (mBody1->getIsMotionEnabled()) {

src/engine/DynamicsWorld.cpp

@@ -407,11 +407,14 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
     // Remove the joint from the world
     mJoints.erase(joint);
 
+    // Get the size in bytes of the joint
+    size_t nbBytes = joint->getSizeInBytes();
+
     // Call the destructor of the joint
     joint->Constraint::~Constraint();
 
     // Release the allocated memory
-    mMemoryAllocator.release(joint, joint->getSizeInBytes());
+    mMemoryAllocator.release(joint, nbBytes);
 }
 
 // Notify the world about a new broad-phase overlapping pair