Add the fixed joint

src/body/CollisionBody.h

@@ -208,7 +208,7 @@ inline const Transform& CollisionBody::getTransform() const {
 inline void CollisionBody::setTransform(const Transform& transform) {
 
     // Check if the body has moved
-    if (this->mTransform != transform) {
+    if (mTransform != transform) {
         mHasMoved = true;
     }
 

src/constraint/BallAndSocketJoint.cpp

@@ -27,7 +27,6 @@
 #include "BallAndSocketJoint.h"
 #include "../engine/ConstraintSolver.h"
 
-// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution)
 
 using namespace reactphysics3d;
 

src/constraint/BallAndSocketJoint.h

@@ -84,12 +84,6 @@ class BallAndSocketJoint : public Constraint {
         /// Bias vector for the constraint
         Vector3 mBiasVector;
 
-        /// Skew-Symmetric matrix for cross product with vector mR1World
-        Matrix3x3 mSkewSymmetricMatrixR1World;
-
-        /// Skew-Symmetric matrix for cross product with vector mR2World
-        Matrix3x3 mSkewSymmetricMatrixR2World;
-
         /// Inverse mass matrix K=JM^-1J^-t of the constraint
         Matrix3x3 mInverseMassMatrix;
 

src/constraint/Constraint.h

@@ -35,7 +35,7 @@
 namespace reactphysics3d {
 
 // Enumeration for the type of a constraint
-enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT};
+enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT};
 
 // Class declarations
 struct ConstraintSolverData;

src/constraint/FixedJoint.cpp

@@ -0,0 +1,248 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "FixedJoint.h"
+#include "../engine/ConstraintSolver.h"
+
+using namespace reactphysics3d;
+
+// Static variables definition
+const decimal FixedJoint::BETA = decimal(0.2);
+
+// Constructor
+FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
+           : Constraint(jointInfo), mImpulseTranslation(0, 0, 0), mImpulseRotation(0, 0, 0) {
+
+    // Compute the local-space anchor point for each body
+    const Transform& transform1 = mBody1->getTransform();
+    const Transform& transform2 = mBody2->getTransform();
+    mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
+    mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
+
+    // Compute the inverse of the initial orientation difference between the two bodies
+    mInitOrientationDifferenceInv = transform2.getOrientation() *
+                                    transform1.getOrientation().getInverse();
+    mInitOrientationDifferenceInv.normalize();
+    mInitOrientationDifferenceInv.inverse();
+}
+
+// Destructor
+FixedJoint::~FixedJoint() {
+
+}
+
+// Initialize before solving the constraint
+void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
+
+    // Initialize the bodies index in the velocity array
+    mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
+    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();
+
+    // Get the inertia tensor of bodies
+    const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute the vector from body center to the anchor point in world-space
+    mR1World = orientationBody1 * mLocalAnchorPointBody1;
+    mR2World = orientationBody2 * mLocalAnchorPointBody2;
+
+    // Compute the corresponding skew-symmetric matrices
+    Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World);
+    Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
+
+    // Compute the matrix K=JM^-1J^t (3x3 matrix)
+    decimal inverseMassBodies = 0.0;
+    if (mBody1->getIsMotionEnabled()) {
+        inverseMassBodies += mBody1->getMassInverse();
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        inverseMassBodies += mBody2->getMassInverse();
+    }
+    Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
+                                    0, inverseMassBodies, 0,
+                                    0, 0, inverseMassBodies);
+    if (mBody1->getIsMotionEnabled()) {
+        massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose();
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose();
+    }
+
+    // Compute the inverse mass matrix K^-1 for the 3 translation constraints
+    mInverseMassMatrixTranslation.setToZero();
+    if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
+        mInverseMassMatrixTranslation = massMatrix.getInverse();
+    }
+
+    // Compute the bias "b" of the constraint for the 3 translation constraints
+    decimal biasFactor = (BETA / constraintSolverData.timeStep);
+    mBiasTranslation.setToZero();
+    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+        mBiasTranslation = biasFactor * (x2 + mR2World - x1 - mR1World);
+    }
+
+    // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
+    // contraints (3x3 matrix)
+    mInverseMassMatrixRotation.setToZero();
+    if (mBody1->getIsMotionEnabled()) {
+        mInverseMassMatrixRotation += I1;
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        mInverseMassMatrixRotation += I2;
+    }
+    if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
+        mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
+    }
+
+    // Compute the bias "b" for the 3 rotation constraints
+    mBiasRotation.setToZero();
+    if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
+        Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
+        currentOrientationDifference.normalize();
+        const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
+        mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV();
+    }
+
+    // If warm-starting is not enabled
+    if (!constraintSolverData.isWarmStartingActive) {
+
+        // Reset the accumulated impulses
+        mImpulseTranslation.setToZero();
+        mImpulseRotation.setToZero();
+    }
+}
+
+// Warm start the constraint (apply the previous impulse at the beginning of the step)
+void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
+
+    // Get the velocities
+    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+
+    // Get the inverse mass and inverse inertia tensors of the bodies
+    const decimal inverseMassBody1 = mBody1->getMassInverse();
+    const decimal inverseMassBody2 = mBody2->getMassInverse();
+    const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
+
+    // Compute the impulse P=J^T * lambda for the 3 translation constraints
+    Vector3 linearImpulseBody1 = -mImpulseTranslation;
+    Vector3 angularImpulseBody1 = mImpulseTranslation.cross(mR1World);
+    Vector3 linearImpulseBody2 = mImpulseTranslation;
+    Vector3 angularImpulseBody2 = -mImpulseTranslation.cross(mR2World);
+
+    // Compute the impulse P=J^T * lambda for the 3 rotation constraints
+    angularImpulseBody1 += -mImpulseRotation;
+    angularImpulseBody2 += mImpulseRotation;
+
+    // Apply the impulse to the bodies of the joint
+    if (mBody1->getIsMotionEnabled()) {
+        v1 += inverseMassBody1 * linearImpulseBody1;
+        w1 += I1 * angularImpulseBody1;
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        v2 += inverseMassBody2 * linearImpulseBody2;
+        w2 += I2 * angularImpulseBody2;
+    }
+
+}
+
+// Solve the velocity constraint
+void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
+
+    // Get the velocities
+    Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
+    Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
+    Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1];
+    Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2];
+
+    // Get the inverse mass and inverse inertia tensors of the bodies
+    decimal inverseMassBody1 = mBody1->getMassInverse();
+    decimal inverseMassBody2 = mBody2->getMassInverse();
+    Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld();
+    Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
+
+    // --------------- Translation Constraints --------------- //
+
+    // Compute J*v for the 3 translation constraints
+    const Vector3 JvTranslation = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
+
+    // Compute the Lagrange multiplier lambda
+    const Vector3 deltaLambda = mInverseMassMatrixTranslation *
+                               (-JvTranslation - mBiasTranslation);
+    mImpulseTranslation += deltaLambda;
+
+    // Compute the impulse P=J^T * lambda
+    Vector3 linearImpulseBody1 = -deltaLambda;
+    Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
+    Vector3 linearImpulseBody2 = deltaLambda;
+    Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
+
+    // Apply the impulse to the bodies of the joint
+    if (mBody1->getIsMotionEnabled()) {
+        v1 += inverseMassBody1 * linearImpulseBody1;
+        w1 += I1 * angularImpulseBody1;
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        v2 += inverseMassBody2 * linearImpulseBody2;
+        w2 += I2 * angularImpulseBody2;
+    }
+
+    // --------------- Rotation Constraints --------------- //
+
+    // Compute J*v for the 3 rotation constraints
+    const Vector3 JvRotation = w2 - w1;
+
+    // Compute the Lagrange multiplier lambda for the 3 rotation constraints
+    Vector3 deltaLambda2 = mInverseMassMatrixRotation * (-JvRotation - mBiasRotation);
+    mImpulseRotation += deltaLambda2;
+
+    // Compute the impulse P=J^T * lambda for the 3 rotation constraints
+    angularImpulseBody1 = -deltaLambda2;
+    angularImpulseBody2 = deltaLambda2;
+
+    // Apply the impulse to the bodies of the joint
+    if (mBody1->getIsMotionEnabled()) {
+        w1 += I1 * angularImpulseBody1;
+    }
+    if (mBody2->getIsMotionEnabled()) {
+        w2 += I2 * angularImpulseBody2;
+    }
+}
+
+// Solve the position constraint
+void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
+
+}
+

src/constraint/FixedJoint.h

@@ -0,0 +1,138 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_FIXED_JOINT_H
+#define REACTPHYSICS3D_FIXED_JOINT_H
+
+// Libraries
+#include "Constraint.h"
+#include "../mathematics/mathematics.h"
+
+namespace reactphysics3d {
+
+// Structure FixedJointInfo
+/**
+ * This structure is used to gather the information needed to create a fixed
+ * joint. This structure will be used to create the actual fixed joint.
+ */
+struct FixedJointInfo : public ConstraintInfo {
+
+    public :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Anchor point (in world-space coordinates)
+        Vector3 anchorPointWorldSpace;
+
+        /// Constructor
+        FixedJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
+                       const Vector3& initAnchorPointWorldSpace)
+                       : ConstraintInfo(rigidBody1, rigidBody2, FIXEDJOINT),
+                         anchorPointWorldSpace(initAnchorPointWorldSpace){}
+};
+
+// Class FixedJoint
+/**
+ * This class represents a fixed joint that is used to forbid any translation or rotation
+ * between two bodies.
+ */
+class FixedJoint : public Constraint {
+
+    private :
+
+        // -------------------- Constants -------------------- //
+
+        // Beta value for the bias factor of position correction
+        static const decimal BETA;
+
+        // -------------------- Attributes -------------------- //
+
+        /// Anchor point of body 1 (in local-space coordinates of body 1)
+        Vector3 mLocalAnchorPointBody1;
+
+        /// Anchor point of body 2 (in local-space coordinates of body 2)
+        Vector3 mLocalAnchorPointBody2;
+
+        /// Vector from center of body 2 to anchor point in world-space
+        Vector3 mR1World;
+
+        /// Vector from center of body 2 to anchor point in world-space
+        Vector3 mR2World;
+
+        /// Accumulated impulse for the 3 translation constraints
+        Vector3 mImpulseTranslation;
+
+        /// Accumulate impulse for the 3 rotation constraints
+        Vector3 mImpulseRotation;
+
+        /// Inverse mass matrix K=JM^-1J^-t of the 3 translation constraints (3x3 matrix)
+        Matrix3x3 mInverseMassMatrixTranslation;
+
+        /// Inverse mass matrix K=JM^-1J^-t of the 3 rotation constraints (3x3 matrix)
+        Matrix3x3 mInverseMassMatrixRotation;
+
+        /// Bias vector for the 3 translation constraints
+        Vector3 mBiasTranslation;
+
+        /// Bias vector for the 3 rotation constraints
+        Vector3 mBiasRotation;
+
+        /// Inverse of the initial orientation difference between the two bodies
+        Quaternion mInitOrientationDifferenceInv;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        FixedJoint(const FixedJointInfo& jointInfo);
+
+        /// Destructor
+        virtual ~FixedJoint();
+
+        /// Return the number of bytes used by the joint
+        virtual size_t getSizeInBytes() const;
+
+        /// Initialize before solving the constraint
+        virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData);
+
+        /// Warm start the constraint (apply the previous impulse at the beginning of the step)
+        virtual void warmstart(const ConstraintSolverData& constraintSolverData);
+
+        /// Solve the velocity constraint
+        virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData);
+
+        /// Solve the position constraint
+        virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
+};
+
+// Return the number of bytes used by the joint
+inline size_t FixedJoint::getSizeInBytes() const {
+    return sizeof(FixedJoint);
+}
+
+}
+
+#endif

src/constraint/HingeJoint.cpp

@@ -28,8 +28,6 @@
 #include "../engine/ConstraintSolver.h"
 #include <cmath>
 
-// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution)
-
 using namespace reactphysics3d;
 
 // Static variables definition

src/constraint/SliderJoint.cpp

@@ -26,8 +26,6 @@
 // Libraries
 #include "SliderJoint.h"
 
-// TODO : Solve 2x2 or 3x3 linear systems without inverting the A matrix (direct resolution)
-
 using namespace reactphysics3d;
 
 // Static variables definition

src/engine/DynamicsWorld.cpp

@@ -28,6 +28,7 @@
 #include "constraint/BallAndSocketJoint.h"
 #include "constraint/SliderJoint.h"
 #include "constraint/HingeJoint.h"
+#include "constraint/FixedJoint.h"
 
 // Namespaces
 using namespace reactphysics3d;
@@ -423,6 +424,15 @@ Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) {
             break;
         }
 
+        // Fixed joint
+        case FIXEDJOINT:
+        {
+            void* allocatedMemory = mMemoryAllocator.allocate(sizeof(FixedJoint));
+            const FixedJointInfo& info = dynamic_cast<const FixedJointInfo&>(jointInfo);
+            newJoint = new (allocatedMemory) FixedJoint(info);
+            break;
+        }
+
         default:
         {
             assert(false);

src/reactphysics3d.h

@@ -50,6 +50,7 @@
 #include "constraint/BallAndSocketJoint.h"
 #include "constraint/SliderJoint.h"
 #include "constraint/HingeJoint.h"
+#include "constraint/FixedJoint.h"
 
 /// Alias to the ReactPhysics3D namespace
 namespace rp3d = reactphysics3d;