Start to implement the joints

src/constraint/BallAndSocketJoint.cpp

@@ -0,0 +1,42 @@
+/********************************************************************************
+* 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 "BallAndSocketJoint.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+BallAndSocketJoint::BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2,
+                                       bool active, ConstraintType type)
+                   : Constraint(body1, body2, active, type){
+
+}
+
+// Destructor
+BallAndSocketJoint::~BallAndSocketJoint() {
+
+}
+

src/constraint/BallAndSocketJoint.h

@@ -0,0 +1,59 @@
+/********************************************************************************
+* 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_BALL_AND_SOCKET_JOINT_H
+#define REACTPHYSICS3D_BALL_AND_SOCKET_JOINT_H
+
+// Libraries
+#include "Constraint.h"
+
+namespace reactphysics3d {
+
+// Class BallAndSocketJoint
+/**
+ * This class represents a ball-and-socket joint that allows arbitrary rotation
+ * between two bodies.
+ */
+class BallAndSocketJoint : public Constraint {
+
+    private :
+
+        // -------------------- Attributes -------------------- //
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        BallAndSocketJoint(RigidBody* const body1, RigidBody* const body2,
+                           bool active, ConstraintType type);
+
+        /// Destructor
+        virtual ~BallAndSocketJoint();
+};
+
+}
+
+#endif

src/constraint/Constraint.cpp

@@ -30,14 +30,9 @@ using namespace reactphysics3d;
 
 // Constructor
 Constraint::Constraint(RigidBody* const body1, RigidBody* const body2,
-                       uint nbConstraints, bool active, ConstraintType type)
+                       bool active, ConstraintType type)
-           :mBody1(body1), mBody2(body2), mActive(active),
+           :mBody1(body1), mBody2(body2), mActive(active), mType(type) {
-            mNbConstraints(nbConstraints), mType(type) {
 
-    // Initialize the cached lambda values
-    for (uint i=0; i<nbConstraints; i++) {
-        mCachedLambdas.push_back(0.0);
-    }
 }
 
 // Destructor

src/constraint/Constraint.h

@@ -34,7 +34,7 @@
 namespace reactphysics3d {
     
 // Enumeration for the type of a constraint
-enum ConstraintType {CONTACT};
+enum ConstraintType {CONTACT, BALLSOCKETJOINT};
 
 // Class Constraint
 /**
@@ -58,16 +58,9 @@ class Constraint {
         /// True if the constraint is active
         bool mActive;
 
-        /// Number mathematical constraints associated with this Constraint
-        uint mNbConstraints;
-
         /// Type of the constraint
         const ConstraintType mType;
 
-        /// Cached lambda values of each mathematical constraint for
-        /// more precise initializaton of LCP solver
-        std::vector<decimal> mCachedLambdas;
-
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -81,7 +74,7 @@ class Constraint {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        Constraint(RigidBody* const body1, RigidBody* const body2, uint nbConstraints,
+        Constraint(RigidBody* const body1, RigidBody* const body2,
                    bool active, ConstraintType type);
 
         /// Destructor
@@ -98,15 +91,6 @@ class Constraint {
 
         /// Return the type of the constraint
         ConstraintType getType() const;
-
-        /// Return the number of mathematical constraints
-        unsigned int getNbConstraints() const;
-
-        /// Get one cached lambda value
-        decimal getCachedLambda(uint index) const;
-
-        /// Set on cached lambda value
-        void setCachedLambda(uint index, decimal lambda);
 };
 
 // Return the reference to the body 1
@@ -129,24 +113,6 @@ inline ConstraintType Constraint::getType() const {
     return mType;
 }                                                                                                         
 
-
-// Return the number auxiliary constraints
-inline uint Constraint::getNbConstraints() const {
-    return mNbConstraints;
-}
-
-// Get one previous lambda value
-inline decimal Constraint::getCachedLambda(uint index) const {
-    assert(index < mNbConstraints);
-    return mCachedLambdas[index];
-} 
-
-// Set on cached lambda value  
-inline void Constraint::setCachedLambda(uint index, decimal lambda) {
-    assert(index < mNbConstraints);
-    mCachedLambdas[index] = lambda;
-}                                                   
-
 }
 
 #endif

src/constraint/ContactPoint.cpp

@@ -32,13 +32,16 @@ using namespace std;
 // Constructor
 ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2,
                            const ContactInfo* contactInfo)
-             : Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal),
+             : Constraint(body1, body2, true, CONTACT), mNormal(contactInfo->normal),
                mPenetrationDepth(contactInfo->penetrationDepth),
                mLocalPointOnBody1(contactInfo->localPoint1),
                mLocalPointOnBody2(contactInfo->localPoint2),
                mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1),
-              mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2),
+               mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2),
-              mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) {
+               mIsRestingContact(false) {
+
+    mFrictionVectors[0] = Vector3(0, 0, 0);
+    mFrictionVectors[1] = Vector3(0, 0, 0);
 
     assert(mPenetrationDepth > 0.0);
 

src/constraint/ContactPoint.h

@@ -84,7 +84,16 @@ class ContactPoint : public Constraint {
         bool mIsRestingContact;
 
         /// Two orthogonal vectors that span the tangential friction plane
-        std::vector<Vector3> mFrictionVectors;
+        Vector3 mFrictionVectors[2];
+
+        /// Cached penetration impulse
+        decimal mPenetrationImpulse;
+
+        /// Cached first friction impulse
+        decimal mFrictionImpulse1;
+
+        /// Cached second friction impulse
+        decimal mFrictionImpulse2;
         
         // -------------------- Methods -------------------- //
 
@@ -122,6 +131,24 @@ class ContactPoint : public Constraint {
         /// Return the contact world point on body 2
         Vector3 getWorldPointOnBody2() const;
 
+        /// Return the cached penetration impulse
+        decimal getPenetrationImpulse() const;
+
+        /// Return the cached first friction impulse
+        decimal getFrictionImpulse1() const;
+
+        /// Return the cached second friction impulse
+        decimal getFrictionImpulse2() const;
+
+        /// Set the cached penetration impulse
+        void setPenetrationImpulse(decimal impulse);
+
+        /// Set the first cached friction impulse
+        void setFrictionImpulse1(decimal impulse);
+
+        /// Set the second cached friction impulse
+        void setFrictionImpulse2(decimal impulse);
+
         /// Set the contact world point on body 1
         void setWorldPointOnBody1(const Vector3& worldPoint);
 
@@ -185,6 +212,36 @@ inline Vector3 ContactPoint::getWorldPointOnBody2() const {
     return mWorldPointOnBody2;
 }
 
+// Return the cached penetration impulse
+inline decimal ContactPoint::getPenetrationImpulse() const {
+    return mPenetrationImpulse;
+}
+
+// Return the cached first friction impulse
+inline decimal ContactPoint::getFrictionImpulse1() const {
+    return mFrictionImpulse1;
+}
+
+// Return the cached second friction impulse
+inline decimal ContactPoint::getFrictionImpulse2() const {
+    return mFrictionImpulse2;
+}
+
+// Set the cached penetration impulse
+inline void ContactPoint::setPenetrationImpulse(decimal impulse) {
+    mPenetrationImpulse = impulse;
+}
+
+// Set the first cached friction impulse
+inline void ContactPoint::setFrictionImpulse1(decimal impulse) {
+    mFrictionImpulse1 = impulse;
+}
+
+// Set the second cached friction impulse
+inline void ContactPoint::setFrictionImpulse2(decimal impulse) {
+    mFrictionImpulse2 = impulse;
+}
+
 // Set the contact world point on body 1
 inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
     mWorldPointOnBody1 = worldPoint;

src/engine/ConstraintSolver.cpp

@@ -0,0 +1,39 @@
+/********************************************************************************
+* 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 "ConstraintSolver.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+ConstraintSolver::ConstraintSolver() {
+
+}
+
+// Destructor
+ConstraintSolver::~ConstraintSolver() {
+
+}

src/engine/ConstraintSolver.h

@@ -0,0 +1,128 @@
+/********************************************************************************
+* 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_CONSTRAINT_SOLVER_H
+#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H
+
+// Libraries
+
+namespace reactphysics3d {
+
+// Class ConstraintSolver
+/**
+ * This class represents the constraint solver that is used to solve constraints between
+ * the rigid bodies. The constraint solver is based on the "Sequential Impulse" technique
+ * described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
+ *
+ * A constraint between two bodies is represented by a function C(x) which is equal to zero
+ * when the constraint is satisfied. The condition C(x)=0 describes a valid position and the
+ * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
+ * the Jacobian matrix of the constraint, v is a vector that contains the velocity of both
+ * bodies and b is the constraint bias. We are looking for a force F_c that will act on the
+ * bodies to keep the constraint satisfied. Note that from the virtual work principle, we have
+ * F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
+ * Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
+ * multiplier lambda.
+
+ * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
+ * body to change its velocity. The idea of the Sequential Impulse technique is to apply
+ * impulses to bodies of each constraints in order to keep the constraint satisfied.
+ *
+ * --- Step 1 ---
+ *
+ * First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and
+ * we obtain some new velocities v2' that tends to violate the constraints.
+ *
+ * v2' = v1 + dt * M^-1 * F_a
+ *
+ * where M is a matrix that contains mass and inertia tensor information.
+ *
+ * --- Step 2 ---
+ *
+ * During the second step, we iterate over all the constraints for a certain number of
+ * iterations and for each constraint we compute the impulse to apply to the bodies needed
+ * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
+ * M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
+ * P_c is the constraint impulse to apply to the body. Therefore, we have
+ * v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda
+ * using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the
+ * Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
+ * the bodies to satisfy the constraint.
+ *
+ * --- Step 3 ---
+ *
+ * In the third step, we integrate the new position x2 of the bodies using the new velocities
+ * v2 computed in the second step with : x2 = x1 + dt * v2.
+ *
+ * Note that in the following code (as it is also explained in the slides from Erin Catto),
+ * the value lambda is not only the lagrange multiplier but is the multiplication of the
+ * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use
+ * lambda, we mean (lambda * dt).
+ *
+ * We are using the accumulated impulse technique that is also described in the slides from
+ * Erin Catto.
+ *
+ * We are also using warm starting. The idea is to warm start the solver at the beginning of
+ * each step by applying the last impulstes for the constraints that we already existing at the
+ * previous step. This allows the iterative solver to converge faster towards the solution.
+ *
+ * For contact constraints, we are also using split impulses so that the position correction
+ * that uses Baumgarte stabilization does not change the momentum of the bodies.
+ *
+ * There are two ways to apply the friction constraints. Either the friction constraints are
+ * applied at each contact point or they are applied only at the center of the contact manifold
+ * between two bodies. If we solve the friction constraints at each contact point, we need
+ * two constraints (two tangential friction directions) and if we solve the friction
+ * constraints at the center of the contact manifold, we need two constraints for tangential
+ * friction but also another twist friction constraint to prevent spin of the body around the
+ * contact manifold center.
+ */
+class ConstraintSolver {
+
+    private :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Number of iterations of the contact solver
+        uint mNbIterations;
+
+        /// Current time step
+        decimal mTimeStep;
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ConstraintSolver();
+
+        /// Destructor
+        ~ConstraintSolver();
+
+};
+
+}
+
+#endif

src/engine/ContactSolver.cpp

@@ -307,9 +307,9 @@ void ContactSolver::initializeContactConstraints() {
             if (mIsWarmStartingActive) {
 
                 // Get the cached accumulated impulses from the previous step
-                contactPoint.penetrationImpulse = externalContact->getCachedLambda(0);
+                contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse();
-                contactPoint.friction1Impulse = externalContact->getCachedLambda(1);
+                contactPoint.friction1Impulse = externalContact->getFrictionImpulse1();
-                contactPoint.friction2Impulse = externalContact->getCachedLambda(2);
+                contactPoint.friction2Impulse = externalContact->getFrictionImpulse2();
             }
 
             // Initialize the split impulses to zero
@@ -785,9 +785,9 @@ void ContactSolver::storeImpulses() {
 
             ContactPointSolver& contactPoint = manifold.contacts[i];
 
-            contactPoint.externalContact->setCachedLambda(0, contactPoint.penetrationImpulse);
+            contactPoint.externalContact->setPenetrationImpulse(contactPoint.penetrationImpulse);
-            contactPoint.externalContact->setCachedLambda(1, contactPoint.friction1Impulse);
+            contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse);
-            contactPoint.externalContact->setCachedLambda(2, contactPoint.friction2Impulse);
+            contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse);
 
             contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1);
             contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2);

src/engine/ContactSolver.h

@@ -345,7 +345,7 @@ class ContactSolver {
         /// Reference to the world
         DynamicsWorld& mWorld;
 
-        /// Number of iterations of the constraints solver
+        /// Number of iterations of the contact solver
         uint mNbIterations;
 
         /// Split linear velocities for the position contact solver (split impulse)