Changes in the Contact class

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

src/collision/CollisionDetection.cpp

@@ -120,13 +120,8 @@ void CollisionDetection::computeAllContacts() {
 void CollisionDetection::computeContactGJK(const ContactInfo* const contactInfo) {
     // TODO : Compute PersisentContact here instead
 
-    // Compute the set of contact points
-    vector<Vector3D> contactPoints;
-    contactPoints.push_back(contactInfo->point1);
-    contactPoints.push_back(contactInfo->point2);
-
     // Create a new contact
-    Contact* contact = new Contact(contactInfo->body1, contactInfo->body2, contactInfo->normal, contactInfo->penetrationDepth, contactPoints);
+    Contact* contact = new Contact(contactInfo);
 
     // Add the contact to the physics world
      world->addConstraint(contact);

src/constants.h

@@ -47,7 +47,7 @@ const double DEFAULT_TIMESTEP = 0.002;
 const double OBJECT_MARGIN = 0.04;          // Object margin for collision detection
 
 // Contact constants
-const double FRICTION_COEFFICIENT = 0.4;    // Friction coefficient
+const double FRICTION_COEFFICIENT = 1.0;    // Friction coefficient
 const double PENETRATION_FACTOR = 0.2;      // Penetration factor (between 0 and 1) which specify the importance of the
                                             // penetration depth in order to calculate the correct impulse for the contact
 

src/constraint/Contact.cpp

@@ -29,8 +29,9 @@ using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const vector<Vector3D>& points)
+Contact::Contact(const ContactInfo* contactInfo)
-                 :Constraint(body1, body2, 3*points.size(), true), normal(normal), penetrationDepth(penetrationDepth), points(points), nbPoints(points.size()) {
+        : Constraint(contactInfo->body1, contactInfo->body2, 3, true), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth),
+          pointOnBody1(contactInfo->point1), pointOnBody2(contactInfo->point2) {
 
     // Compute the auxiliary lower and upper bounds
     // TODO : Now mC is only the mass of the first body but it is probably wrong
@@ -55,124 +56,92 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
     assert(body1);
     assert(body2);
 
-    Vector3D r1;
-    Vector3D r2;
-    Vector3D r1CrossN;
-    Vector3D r2CrossN;
-    Vector3D r1CrossU1;
-    Vector3D r2CrossU1;
-    Vector3D r1CrossU2;
-    Vector3D r2CrossU2;
     Vector3D body1Position = body1->getTransform().getPosition();
     Vector3D body2Position = body2->getTransform().getPosition();
     int currentIndex = noConstraint;                        // Current constraint index
 
-    // For each point in the contact
+    Vector3D r1 = pointOnBody1 - body1Position;
-    for (int i=0; i<nbPoints; i++) {
+    Vector3D r2 = pointOnBody2 - body2Position;
+    Vector3D r1CrossN = r1.cross(normal);
+    Vector3D r2CrossN = r2.cross(normal);
 
-        r1 = points[i] - body1Position;
+    // Compute the jacobian matrix for the body 1 for the contact constraint
-        r2 = points[i] - body2Position;
+    J_sp[currentIndex][0].setValue(0, -normal.getX());
-        r1CrossN = r1.cross(normal);
+    J_sp[currentIndex][0].setValue(1, -normal.getY());
-        r2CrossN = r2.cross(normal);
+    J_sp[currentIndex][0].setValue(2, -normal.getZ());
+    J_sp[currentIndex][0].setValue(3, -r1CrossN.getX());
+    J_sp[currentIndex][0].setValue(4, -r1CrossN.getY());
+    J_sp[currentIndex][0].setValue(5, -r1CrossN.getZ());
 
-        // Compute the jacobian matrix for the body 1 for the contact constraint
+    // Compute the jacobian matrix for the body 2 for the contact constraint
-        //J_sp[currentIndex][0].changeSize(1, 6);
+    J_sp[currentIndex][1].setValue(0, normal.getX());
-        J_sp[currentIndex][0].setValue(0, -normal.getX());
+    J_sp[currentIndex][1].setValue(1, normal.getY());
-        J_sp[currentIndex][0].setValue(1, -normal.getY());
+    J_sp[currentIndex][1].setValue(2, normal.getZ());
-        J_sp[currentIndex][0].setValue(2, -normal.getZ());
+    J_sp[currentIndex][1].setValue(3, r2CrossN.getX());
-        J_sp[currentIndex][0].setValue(3, -r1CrossN.getX());
+    J_sp[currentIndex][1].setValue(4, r2CrossN.getY());
-        J_sp[currentIndex][0].setValue(4, -r1CrossN.getY());
+    J_sp[currentIndex][1].setValue(5, r2CrossN.getZ());
-        J_sp[currentIndex][0].setValue(5, -r1CrossN.getZ());
 
-        // Compute the jacobian matrix for the body 2 for the contact constraint
+    currentIndex++;
-        //J_sp[currentIndex][1].changeSize(1, 6);
-        J_sp[currentIndex][1].setValue(0, normal.getX());
-        J_sp[currentIndex][1].setValue(1, normal.getY());
-        J_sp[currentIndex][1].setValue(2, normal.getZ());
-        J_sp[currentIndex][1].setValue(3, r2CrossN.getX());
-        J_sp[currentIndex][1].setValue(4, r2CrossN.getY());
-        J_sp[currentIndex][1].setValue(5, r2CrossN.getZ());
 
-        currentIndex++;
+    // Compute the jacobian matrix for the body 1 for the first friction constraint
+    Vector3D r1CrossU1 = r1.cross(frictionVectors[0]);
+    Vector3D r2CrossU1 = r2.cross(frictionVectors[0]);
+    Vector3D r1CrossU2 = r1.cross(frictionVectors[1]);
+    Vector3D r2CrossU2 = r2.cross(frictionVectors[1]);
+    J_sp[currentIndex][0].setValue(0, -frictionVectors[0].getX());
+    J_sp[currentIndex][0].setValue(1, -frictionVectors[0].getY());
+    J_sp[currentIndex][0].setValue(2, -frictionVectors[0].getZ());
+    J_sp[currentIndex][0].setValue(3, -r1CrossU1.getX());
+    J_sp[currentIndex][0].setValue(4, -r1CrossU1.getY());
+    J_sp[currentIndex][0].setValue(5, -r1CrossU1.getZ());
 
-        // Compute the jacobian matrix for the body 1 for the first friction constraint
+    // Compute the jacobian matrix for the body 2 for the first friction constraint
-        r1CrossU1 = r1.cross(frictionVectors[0]);
+    J_sp[currentIndex][1].setValue(0, frictionVectors[0].getX());
-        r2CrossU1 = r2.cross(frictionVectors[0]);
+    J_sp[currentIndex][1].setValue(1, frictionVectors[0].getY());
-        r1CrossU2 = r1.cross(frictionVectors[1]);
+    J_sp[currentIndex][1].setValue(2, frictionVectors[0].getZ());
-        r2CrossU2 = r2.cross(frictionVectors[1]);
+    J_sp[currentIndex][1].setValue(3, r2CrossU1.getX());
-        //J_sp[currentIndex][0].changeSize(1, 6);
+    J_sp[currentIndex][1].setValue(4, r2CrossU1.getY());
-        J_sp[currentIndex][0].setValue(0, -frictionVectors[0].getX());
+    J_sp[currentIndex][1].setValue(5, r2CrossU1.getZ());
-        J_sp[currentIndex][0].setValue(1, -frictionVectors[0].getY());
-        J_sp[currentIndex][0].setValue(2, -frictionVectors[0].getZ());
-        J_sp[currentIndex][0].setValue(3, -r1CrossU1.getX());
-        J_sp[currentIndex][0].setValue(4, -r1CrossU1.getY());
-        J_sp[currentIndex][0].setValue(5, -r1CrossU1.getZ());
 
-        // Compute the jacobian matrix for the body 2 for the first friction constraint
+    currentIndex++;
-        //J_sp[currentIndex][1].changeSize(1, 6);
-        J_sp[currentIndex][1].setValue(0, frictionVectors[0].getX());
-        J_sp[currentIndex][1].setValue(1, frictionVectors[0].getY());
-        J_sp[currentIndex][1].setValue(2, frictionVectors[0].getZ());
-        J_sp[currentIndex][1].setValue(3, r2CrossU1.getX());
-        J_sp[currentIndex][1].setValue(4, r2CrossU1.getY());
-        J_sp[currentIndex][1].setValue(5, r2CrossU1.getZ());
 
-        currentIndex++;
+    // Compute the jacobian matrix for the body 1 for the second friction constraint
+    J_sp[currentIndex][0].setValue(0, -frictionVectors[1].getX());
+    J_sp[currentIndex][0].setValue(1, -frictionVectors[1].getY());
+    J_sp[currentIndex][0].setValue(2, -frictionVectors[1].getZ());
+    J_sp[currentIndex][0].setValue(3, -r1CrossU2.getX());
+    J_sp[currentIndex][0].setValue(4, -r1CrossU2.getY());
+    J_sp[currentIndex][0].setValue(5, -r1CrossU2.getZ());
 
-        // Compute the jacobian matrix for the body 1 for the second friction constraint
+    // Compute the jacobian matrix for the body 2 for the second friction constraint
-        //J_sp[currentIndex][0].changeSize(1, 6);
+    J_sp[currentIndex][1].setValue(0, frictionVectors[1].getX());
-        J_sp[currentIndex][0].setValue(0, -frictionVectors[1].getX());
+    J_sp[currentIndex][1].setValue(1, frictionVectors[1].getY());
-        J_sp[currentIndex][0].setValue(1, -frictionVectors[1].getY());
+    J_sp[currentIndex][1].setValue(2, frictionVectors[1].getZ());
-        J_sp[currentIndex][0].setValue(2, -frictionVectors[1].getZ());
+    J_sp[currentIndex][1].setValue(3, r2CrossU2.getX());
-        J_sp[currentIndex][0].setValue(3, -r1CrossU2.getX());
+    J_sp[currentIndex][1].setValue(4, r2CrossU2.getY());
-        J_sp[currentIndex][0].setValue(4, -r1CrossU2.getY());
+    J_sp[currentIndex][1].setValue(5, r2CrossU2.getZ());
-        J_sp[currentIndex][0].setValue(5, -r1CrossU2.getZ());
-        //J_sp[currentIndex][1].changeSize(1, 6);
-
-        // Compute the jacobian matrix for the body 2 for the second friction constraint
-        J_sp[currentIndex][1].setValue(0, frictionVectors[1].getX());
-        J_sp[currentIndex][1].setValue(1, frictionVectors[1].getY());
-        J_sp[currentIndex][1].setValue(2, frictionVectors[1].getZ());
-        J_sp[currentIndex][1].setValue(3, r2CrossU2.getX());
-        J_sp[currentIndex][1].setValue(4, r2CrossU2.getY());
-        J_sp[currentIndex][1].setValue(5, r2CrossU2.getZ());
-
-        currentIndex++;
-    }
 }
 
 // Compute the lowerbounds values for all the mathematical constraints. The
 // argument "lowerBounds" is the lowerbounds values vector of the constraint solver and
 // this methods has to fill in this vector starting from the row "noConstraint"
 void Contact::computeLowerBound(int noConstraint, Vector& lowerBounds) const {
-    int index = noConstraint;
-
     assert(noConstraint >= 0 && noConstraint + nbConstraints <= lowerBounds.getNbComponent());
 
-    // For each constraint
+    lowerBounds.setValue(noConstraint, 0.0);                // Lower bound for the contact constraint
-    for (int i=0; i<nbPoints; i++) {
+    lowerBounds.setValue(noConstraint + 1, -mu_mc_g);       // Lower bound for the first friction constraint
-        lowerBounds.setValue(index, 0.0);           // Lower bound for the contact constraint
+    lowerBounds.setValue(noConstraint + 2, -mu_mc_g);       // Lower bound for the second friction constraint
-        lowerBounds.setValue(index + 1, -mu_mc_g);      // Lower bound for the first friction constraint
-        lowerBounds.setValue(index + 2, -mu_mc_g);      // Lower bound for the second friction constraint
-        index += 3;
-    }
 }
 
 // Compute the upperbounds values for all the mathematical constraints. The
 // argument "upperBounds" is the upperbounds values vector of the constraint solver and
 // this methods has to fill in this vector starting from the row "noConstraint"
 void Contact::computeUpperBound(int noConstraint, Vector& upperBounds) const {
-    int index = noConstraint;
-
     assert(noConstraint >= 0 && noConstraint + nbConstraints <= upperBounds.getNbComponent());
 
-    // For each constraint
+    upperBounds.setValue(noConstraint, INFINITY_CONST);    // Upper bound for the contact constraint
-    for (int i=0; i<nbPoints; i++) {
+    upperBounds.setValue(noConstraint + 1, mu_mc_g);       // Upper bound for the first friction constraint
-        upperBounds.setValue(index, INFINITY_CONST);    // Upper bound for the contact constraint
+    upperBounds.setValue(noConstraint + 2, mu_mc_g);       // Upper bound for the second friction constraint
-        upperBounds.setValue(index + 1, mu_mc_g);       // Upper bound for the first friction constraint
-        upperBounds.setValue(index + 2, mu_mc_g);       // Upper bound for the second friction constraint
-        index += 3;
-    }
 }
 
 // Compute the error values for all the mathematical constraints. The argument
@@ -184,7 +153,6 @@ void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const {
 
     RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
     RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
-    int index = noConstraint;
 
     assert(noConstraint >= 0 && noConstraint + nbConstraints <= errorValues.getNbComponent());
 
@@ -195,10 +163,7 @@ void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const {
     double errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2)) + PENETRATION_FACTOR * penetrationDepth;
 
     // Assign the error value to the vector of error values
-    for (int i=0; i<nbPoints; i++) {
+    errorValues.setValue(noConstraint, errorValue);    // Error value for contact constraint
-        errorValues.setValue(index, errorValue);    // Error value for contact constraint
+    errorValues.setValue(noConstraint + 1, 0.0);       // Error value for friction constraint
-        errorValues.setValue(index + 1, 0.0);       // Error value for friction constraint
+    errorValues.setValue(noConstraint + 2, 0.0);       // Error value for friction constraint
-        errorValues.setValue(index + 2, 0.0);       // Error value for friction constraint
-        index += 3;
-    }
 }

src/constraint/Contact.h

@@ -27,6 +27,7 @@
 
 // Libraries
 #include "Constraint.h"
+#include "../collision/ContactInfo.h"
 #include "../body/RigidBody.h"
 #include "../constants.h"
 #include "../mathematics/mathematics.h"
@@ -43,29 +44,29 @@ namespace reactphysics3d {
         This class represents a collision contact between two bodies in
         the physics engine. The contact class inherits from the
         Constraint class. Each Contact represent a contact between two bodies
-        and can have several contact points. The Contact will have 3 mathematical
+        and contains the two contact points on each body. The contact has 3
-        constraints for each contact point (1 for the contact constraint, and 2
+        mathematical constraints (1 for the contact constraint, and 2
         for the friction constraints).
     -------------------------------------------------------------------
 */
 class Contact : public Constraint {
     protected :
-        const Vector3D normal;                          // Normal vector of the contact (From body1 toward body2)
+        const Vector3D normal;                  // Normal vector of the contact (From body1 toward body2)
-        const double penetrationDepth;                  // Penetration depth
+        const double penetrationDepth;          // Penetration depth
-        const std::vector<Vector3D> points;             // Contact points between the two bodies
+        const Vector3D pointOnBody1;            // Contact point on body 1
-        const int nbPoints;                             // Number of points in the contact
+        const Vector3D pointOnBody2;            // Contact point on body 2
-        std::vector<Vector3D> frictionVectors;          // Two orthogonal vectors that span the tangential friction plane
+        std::vector<Vector3D> frictionVectors;  // Two orthogonal vectors that span the tangential friction plane
         double mu_mc_g;
         
         void computeFrictionVectors();                  // Compute the two friction vectors that span the tangential friction plane
 
     public :
-        Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector<Vector3D>& points);      // Constructor
+        Contact(const ContactInfo* contactInfo);        // Constructor
-        virtual ~Contact();                                                                                                                             // Destructor
+        virtual ~Contact();                             // Destructor
 
         Vector3D getNormal() const;                                                     // Return the normal vector of the contact
-        Vector3D getPoint(int index) const;                                             // Return a contact point
+        Vector3D getPointOnBody1() const;                                               // Return the contact point on body 1
-        int getNbPoints() const;                                                        // Return the number of contact points
+        Vector3D getPointOnBody2() const;                                               // Return the contact point on body 2
         virtual void computeJacobian(int noConstraint, Matrix1x6**& J_SP) const;        // Compute the jacobian matrix for all mathematical constraints
         virtual void computeLowerBound(int noConstraint, Vector& lowerBounds) const;    // Compute the lowerbounds values for all the mathematical constraints
         virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const;    // Compute the upperbounds values for all the mathematical constraints
@@ -95,15 +96,14 @@ inline Vector3D Contact::getNormal() const {
     return normal;
 }
 
-// Return a contact points
+// Return the contact point on body 1
-inline Vector3D Contact::getPoint(int index) const {
+inline Vector3D Contact::getPointOnBody1() const {
-    assert(index >= 0 && index < nbPoints);
+    return pointOnBody1;
-    return points[index];
 }
 
-// Return the number of contact points
+// Return the contact point on body 2
-inline int Contact::getNbPoints() const {
+inline Vector3D Contact::getPointOnBody2() const {
-    return nbPoints;
+    return pointOnBody2;
 }
 
 // Return the penetration depth of the contact

src/engine/ConstraintSolver.cpp

@@ -353,9 +353,8 @@ void ConstraintSolver::updateContactCache() {
             // Get all the contact points of the contact
             vector<Vector3D> points;
             vector<double> lambdas;
-            for (int i=0; i<contact->getNbPoints(); i++) {
+            points.push_back(contact->getPointOnBody1());
-                points.push_back(contact->getPoint(i));
+            points.push_back(contact->getPointOnBody2());
-            }
 
             // For each constraint of the contact
             for (int i=0; i<contact->getNbConstraints(); i++) {

src/engine/ContactCache.cpp

@@ -65,30 +65,38 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
         assert((*entry).first.first == contact->getBody1());
         assert((*entry).first.second == contact->getBody2());
 
-        // If the new contact and the contact caching info doesn't have the same number of contact points
+        // Get the position of the current contact
-        if (contact->getNbPoints() != contactInfo->positions.size()) {
+        posX = contact->getPointOnBody1().getX();
-            // We return NULL because, the contact doesn't match
+        posY = contact->getPointOnBody1().getY();
+        posZ = contact->getPointOnBody1().getZ();
+
+        // Get the position of the old contact
+        Vector3D& contactPos1 = contactInfo->positions[0];
+
+        // If the old contact point doesn't match the current one
+        if (posX > contactPos1.getX() + POSITION_TOLERANCE || posX < contactPos1.getX() - POSITION_TOLERANCE ||
+            posY > contactPos1.getY() + POSITION_TOLERANCE || posY < contactPos1.getY() - POSITION_TOLERANCE ||
+            posZ > contactPos1.getZ() + POSITION_TOLERANCE || posZ < contactPos1.getZ() - POSITION_TOLERANCE) {
+
+            // Return NULL
             return NULL;
         }
 
-        for (int i=0; i<contactInfo->positions.size(); i++) {
+        // Get the position of the current contact
+        posX = contact->getPointOnBody2().getX();
+        posY = contact->getPointOnBody2().getY();
+        posZ = contact->getPointOnBody2().getZ();
 
-            // Get the position of the current contact
+        // Get the position of the old contact
-            posX = contact->getPoint(i).getX();
+        Vector3D& contactPos2 = contactInfo->positions[1];
-            posY = contact->getPoint(i).getY();
-            posZ = contact->getPoint(i).getZ();
 
-            // Get the position of the old contact
+        // If the old contact point doesn't match the current one
-            Vector3D& contactPos = contactInfo->positions[i];
+        if (posX > contactPos2.getX() + POSITION_TOLERANCE || posX < contactPos2.getX() - POSITION_TOLERANCE ||
+            posY > contactPos2.getY() + POSITION_TOLERANCE || posY < contactPos2.getY() - POSITION_TOLERANCE ||
+            posZ > contactPos2.getZ() + POSITION_TOLERANCE || posZ < contactPos2.getZ() - POSITION_TOLERANCE) {
 
-            // If the old contact point doesn't match the current one
+            // Return NULL
-            if (posX > contactPos.getX() + POSITION_TOLERANCE || posX < contactPos.getX() - POSITION_TOLERANCE ||
+            return NULL;
-                posY > contactPos.getY() + POSITION_TOLERANCE || posY < contactPos.getY() - POSITION_TOLERANCE ||
-                posZ > contactPos.getZ() + POSITION_TOLERANCE || posZ < contactPos.getZ() - POSITION_TOLERANCE) {
-
-                // Return NULL
-                return NULL;
-            }
         }
 
         // The old contact positions match the current contact, therefore we return the contact caching info