git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@281 92aac97c-a6ce-11dd-a772-7fcde58d38e6

sources/reactphysics3d/constraint/Contact.cpp

@@ -19,6 +19,7 @@
 
 // Libraries
 #include "Contact.h"
+#include "../body/RigidBody.h"
 #include <GL/freeglut.h>        // TODO : Remove this in the final version
 #include <GL/gl.h>              // TODO : Remove this in the final version
 
@@ -27,8 +28,8 @@ using namespace reactphysics3d;
 
 // Constructor
 Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector<Vector3D>& points)
-                 :Constraint(body1, body2), normal(normal), penetrationDepth(penetrationDepth), points(points){
+                 :Constraint(body1, body2), normal(normal), penetrationDepth(penetrationDepth), points(points), nbFrictionVectors(NB_FRICTION_VECTORS) {
-
+    active = true;
 }
 
 // Destructor
@@ -36,6 +37,77 @@ Contact::~Contact() {
 
 }
 
+// Evaluate the constraint
+// This method computes the jacobian matrices of the constraint
+// The argument "dt" is the time step of the physics simulation
+void Contact::evaluate(double dt) {
+    RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
+    RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
+
+    assert(frictionVectors.size() == NB_FRICTION_VECTORS);
+    assert(rigidBody1 != 0);
+    assert(rigidBody2 != 0);
+
+    // Computation of the linear jacobian for body 1
+    body1LinearJacobian = Matrix(1 + NB_FRICTION_VECTORS, 3);
+    body1LinearJacobian.setValue(0, 0, -normal.getX());
+    body1LinearJacobian.setValue(0, 1, -normal.getY());
+    body1LinearJacobian.setValue(0, 2, -normal.getZ());
+    for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) {
+        body1LinearJacobian.setValue(i, 0, -frictionVectors.at(i-1).getX());
+        body1LinearJacobian.setValue(i, 1, -frictionVectors.at(i-1).getY());
+        body1LinearJacobian.setValue(i, 2, -frictionVectors.at(i-1).getZ());
+    }
+
+    // Computation of the linear jacobian for body 2
+    body2LinearJacobian = Matrix(1 + NB_FRICTION_VECTORS, 3);
+    body2LinearJacobian.setValue(0, 0, normal.getX());
+    body2LinearJacobian.setValue(0, 1, normal.getY());
+    body2LinearJacobian.setValue(0, 2, normal.getZ());
+    for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) {
+        body2LinearJacobian.setValue(i, 0, frictionVectors.at(i-1).getX());
+        body2LinearJacobian.setValue(i, 1, frictionVectors.at(i-1).getY());
+        body2LinearJacobian.setValue(i, 2, frictionVectors.at(i-1).getZ());
+    }
+
+    // Computation of the angular jacobian for body 1
+    Vector3D r1 = rigidBody1->getCurrentBodyState().getPosition();
+    Vector3D r1CrossNormal = r1.crossProduct(normal);
+    body1AngularJacobian = Matrix(1 + NB_FRICTION_VECTORS, 3);
+    body1AngularJacobian.setValue(0, 0, -r1CrossNormal.getX());
+    body1AngularJacobian.setValue(0, 1, -r1CrossNormal.getY());
+    body1AngularJacobian.setValue(0, 2, -r1CrossNormal.getZ());
+    for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) {
+        Vector3D r1CrossFrictionVector = r1.crossProduct(frictionVectors.at(i-1));
+        body1AngularJacobian.setValue(i, 0, -r1CrossFrictionVector.getX());
+        body1AngularJacobian.setValue(i, 1, -r1CrossFrictionVector.getY());
+        body1AngularJacobian.setValue(i, 2, -r1CrossFrictionVector.getZ());
+    }
+
+    // Computation of the angular jacobian for body 2
+    Vector3D r2 = rigidBody2->getCurrentBodyState().getPosition();
+    Vector3D r2CrossNormal = r2.crossProduct(normal);
+    body2AngularJacobian = Matrix(1 + NB_FRICTION_VECTORS, 3);
+    body2AngularJacobian.setValue(0, 0, r2CrossNormal.getX());
+    body2AngularJacobian.setValue(0, 1, r2CrossNormal.getY());
+    body2AngularJacobian.setValue(0, 2, r2CrossNormal.getZ());
+    for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) {
+        Vector3D r2CrossFrictionVector = r2.crossProduct(frictionVectors.at(i-1));
+        body2AngularJacobian.setValue(i, 0, r2CrossFrictionVector.getX());
+        body2AngularJacobian.setValue(i, 1, r2CrossFrictionVector.getY());
+        body2AngularJacobian.setValue(i, 2, r2CrossFrictionVector.getZ());
+    }
+
+    // Computation of the error vector
+    double kFps = 1.0 / dt;
+    double kCorrection = kErp * kFps;                       // Computation of the error coefficient
+    errorVector = Vector(1+NB_FRICTION_VECTORS);
+    errorVector.setValue(0, kCorrection * penetrationDepth);
+    for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) {
+       errorVector.setValue(i, 0.0);
+    }
+}
+
 // TODO : Delete this (Used to debug collision detection)
 void Contact::draw() const {
     glColor3f(1.0, 0.0, 0.0);

sources/reactphysics3d/constraint/Contact.h

@@ -27,6 +27,9 @@
 // ReactPhysics3D namespace
 namespace reactphysics3d {
 
+// Constants
+const unsigned int NB_FRICTION_VECTORS = 4;         // Number of vectors used to describe the contact friction
+
 /*  -------------------------------------------------------------------
     Class Contact :
         This class represents a collision contact between two bodies in
@@ -37,21 +40,44 @@ namespace reactphysics3d {
     -------------------------------------------------------------------
 */
 class Contact : public Constraint {
-    private :
+    protected :
         Vector3D normal;                                // Normal vector of the contact (From body1 toward body2)
         double penetrationDepth;                        // Penetration depth
         std::vector<Vector3D> points;                   // Contact points
+        unsigned int nbFrictionVectors;                 // Number of vectors used to describe the friction
+        // TODO : Implement the computation of the frictionVectors in the collision detection
+        std::vector<Vector3D> frictionVectors;          // Set of vectors that span the tangential friction plane
+
+        void computeFrictionVectors(const Vector3D& vector1, const Vector3D& vector2);      // Compute all the friction vectors from two vectors that span the friction plane
 
     public :
         Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector<Vector3D>& points);    // Constructor
         virtual ~Contact();                                                                                                                     // Destructor
 
-        Vector3D getNormal() const;                     // Return the normal vector of the contact
+        Vector3D getNormal() const;                         // Return the normal vector of the contact
-        std::vector<Vector3D> getPoints() const;        // Return the contact points
+        std::vector<Vector3D> getPoints() const;            // Return the contact points
+        virtual unsigned int getNbJacobianRows() const;     // Return the number of rows of the Jacobian matrix
+        virtual void evaluate(double dt);                   // Evaluate the constraint
+        virtual int getNbAuxiliaryVars() const;             // Return the number of auxiliary variables
 
         void draw() const;                              // TODO : Delete this (Used to debug collision detection)
 };
 
+// Compute all the friction vectors from two vectors that span the friction cone
+// The vectors "vector1" and "vector2" are two vectors that span the friction cone
+inline void Contact::computeFrictionVectors(const Vector3D& vector1, const Vector3D& vector2) {
+    // Delete the current friction vectors
+    frictionVectors.clear();
+
+    Vector3D vector;
+
+    // Compute each friction vector
+    for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) {
+        vector = cos((2.0 * (i-1) * PI) / NB_FRICTION_VECTORS) * vector1 + sin((2.0 * (i-1) * PI) / NB_FRICTION_VECTORS) * vector2;
+        frictionVectors.push_back(vector);
+    }
+}
+
 // Return the normal vector of the contact
 inline Vector3D Contact::getNormal() const {
     return normal;
@@ -62,6 +88,16 @@ inline std::vector<Vector3D> Contact::getPoints() const {
     return points;
 }
 
+// Return the number of rows of the Jacobian matrix
+inline unsigned int Contact::getNbJacobianRows() const {
+    return (1+nbFrictionVectors);
+}
+
+// Return the number of auxiliary variables
+inline int Contact::getNbAuxiliaryVars() const {
+    return nbFrictionVectors;
+}
+
 
 } // End of the ReactPhysics3D namespace