diff --git a/sources/reactphysics3d/constraint/Constraint.h b/sources/reactphysics3d/constraint/Constraint.h index 7c98c9ab..5f61492a 100644 --- a/sources/reactphysics3d/constraint/Constraint.h +++ b/sources/reactphysics3d/constraint/Constraint.h @@ -50,16 +50,17 @@ class Constraint { Matrix body1Jacobian; // Jacobian matrix of the constraint for body1 (dimension 1x6) Matrix body2Jacobian; // Jacobian matrix of the constraint for body2 (dimension 1x6) Matrix auxJacobian; // Jacobian matrix for all the auxiliary constraints jacobian associated with this constraint - // (dimension nx6 where n is the number of auxiliary constraints) + // (dimension nx12 where n is the number of auxiliary constraints) unsigned int nbAuxConstraints; // Number of auxiliary constraints associated with this constraint double lowerBound; // Lower bound of the constraint double upperBound; // Upper bound of the constraint Vector auxLowerBounds; // Vector that contains all the lower bounds of the auxiliary constraints Vector auxUpperBounds; // Vector that contains all the upper bounds of the auxiliary constraints - + double errorValue; // Error value (bias) of the constraint + Vector auxErrorValues; // Error values for the auxiliary constraints + public : - Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, double lowerBound, double upperBound, - bool active); // Constructor + Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, bool active); // Constructor // Constructor virtual ~Constraint(); // Destructor Body* const getBody1() const; // Return the reference to the body 1 Body* const getBody2() const; // Return the reference to the body 2 @@ -67,12 +68,13 @@ class Constraint { bool isActive() const; // Return true if the constraint is active Matrix getBody1Jacobian() const; // Return the jacobian matrix of body 1 Matrix getBody2Jacobian() const; // Return the jacobian matrix of body 2 - virtual unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints + unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints void getAuxJacobian(Matrix& auxJacobian) const; // Return the jacobian matrix of auxiliary constraints double getLowerBound() const; // Return the lower bound value of the constraint double getUpperBound() const; // Return the upper bound value of the constraint void getAuxLowerBounds(Vector& auxLowerBounds) const; // Return the vector of lower bounds values void getAuxUpperBounds(Vector& auxUpperBounds) const; // Return the vector of the upper bounds values + double getErrorValue() const; // Return the error value (bias) of the constraint }; // Return the reference to the body 1 @@ -130,6 +132,11 @@ inline void Constraint::getAuxUpperBounds(Vector& auxUpperBounds) const { auxUpperBounds = this->auxUpperBounds; } +// Return the error value (bias) of the constraint +inline double Constraint::getErrorValue() const { + return errorValue; +} + } // End of the ReactPhysics3D namespace #endif diff --git a/sources/reactphysics3d/constraint/Contact.cpp b/sources/reactphysics3d/constraint/Contact.cpp index 0cc55612..228026da 100644 --- a/sources/reactphysics3d/constraint/Contact.cpp +++ b/sources/reactphysics3d/constraint/Contact.cpp @@ -27,11 +27,14 @@ using namespace reactphysics3d; // Constructor -Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector& points) - :Constraint(body1, body2, 2, 0, INFINITY_CONST, true), normal(normal), penetrationDepth(penetrationDepth), points(points) { +Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point) + :Constraint(body1, body2, 2, true), normal(normal), penetrationDepth(penetrationDepth), point(point) { body1Jacobian = Matrix(1, 6); body2Jacobian = Matrix(1, 6); - auxJacobian = Matrix(2, 6); + auxJacobian = Matrix(nbAuxConstraints, 12); + auxLowerBounds = Vector(nbAuxConstraints); + auxUpperBounds = Vector(nbAuxConstraints); + auxErrorValues = Vector(nbAuxConstraints); } // Destructor @@ -48,9 +51,80 @@ void Contact::evaluate() { assert(rigidBody1 != 0); assert(rigidBody2 != 0); - // Compute the jacobian matrix for the body 1 + // Compute the friction vectors that span the tangential friction plane + computeFrictionVectors(); -} + Vector3D r1 = point - rigidBody1.getCurrentBodyState().getPosition(); + Vector3D r2 = point - rigidBody2.getCurrentBodyState().getPosition(); + Vector3D r1CrossN = r1.crossProduct(normal); + Vector3D r2CrossN = r2.crossProduct(normal); + + // Compute the jacobian matrix for the body 1 + body1Jacobian.setValue(0, 0, -normal.getX()); + body1Jacobian.setValue(0, 1, -normal.getY()); + body1Jacobian.setValue(0, 2, -normal.getZ()); + body1Jacobian.setValue(0, 3, -r1CrossN.getX()); + body1Jacobian.setValue(0, 4, -r1CrossN.getY()); + body1Jacobian.setValue(0, 5, -r1CrossN.getZ()); + + // Compute the jacobian matrix for the body 2 + body2Jacobian.setValue(0, 0, normal.getX()); + body2Jacobian.setValue(0, 1, normal.getY()); + body2Jacobian.setValue(0, 2, normal.getZ()); + body2Jacobian.setValue(0, 3, r2CrossN.getX()); + body2Jacobian.setValue(0, 4, r2CrossN.getY()); + body2Jacobian.setValue(0, 5, r2CrossN.getZ()); + + // Compute the lower and upper bounds values + lowerBound = 0.0; + upperBound = INFINITY_CONST; + + // Compute the error value of the constraint + errorValue = -PENETRATION_FACTOR * penetrationDepth; + + // Compute the auxiliary jacobian matrix (this corresponds to the friction constraint) + Vector3D r1CrossU1 = r1.crossProduct(frictionVectors[0]); + Vector3D r2CrossU1 = r2.crossProduct(frictionVectors[0]); + Vector3D r1CrossU2 = r1.crossProduct(frictionVectors[1]); + Vector3D r2CrossU2 = r2.crossProduct(frictionVectors[1]); + auxJacobian.setValue(0, 0, -frictionVectors[0].getX()); + auxJacobian.setValue(0, 1, -frictionVectors[0].getY()); + auxJacobian.setValue(0, 2, -frictionVectors[0].getZ()); + auxJacobian.setValue(0, 3, -r1CrossU1.getX()); + auxJacobian.setValue(0, 4, -r1CrossU1.getY()); + auxJacobian.setValue(0, 5, -r1CrossU1.getZ()); + auxJacobian.setValue(0, 6, frictionVectors[0].getX()); + auxJacobian.setValue(0, 7, frictionVectors[0].getY()); + auxJacobian.setValue(0, 8, frictionVectors[0].getZ()); + auxJacobian.setValue(0, 9, r2CrossU1.getX()); + auxJacobian.setValue(0, 10, r2CrossU1.getY()); + auxJacobian.setValue(0, 11, r2CrossU1.getZ()); + auxJacobian.setValue(1, 0, -frictionVectors[1].getX()); + auxJacobian.setValue(1, 1, -frictionVectors[1].getY()); + auxJacobian.setValue(1, 2, -frictionVectors[1].getZ()); + auxJacobian.setValue(1, 3, -r1CrossU2.getX()); + auxJacobian.setValue(1, 4, -r1CrossU2.getY()); + auxJacobian.setValue(1, 5, -r1CrossU2.getZ()); + auxJacobian.setValue(1, 6, frictionVectors[1].getX()); + auxJacobian.setValue(1, 7, frictionVectors[1].getY()); + auxJacobian.setValue(1, 8, frictionVectors[1].getZ()); + auxJacobian.setValue(1, 9, r2CrossU2.getX()); + auxJacobian.setValue(1, 10, r2CrossU2.getY()); + auxJacobian.setValue(1, 11, r2CrossU2.getZ()); + + // Compute the auxiliary lower and upper bounds + // TODO : Now mC is only the mass of the first body but it is probably wrong + // TODO : Now g is 9.81 but we should use the true gravity value of the physics world. + double mu_mc_g = FRICTION_COEFFICIENT * rigidBody1.getMass().getValue() * 9.81; + auxLowerBounds.setValue(0, 0, -mu_mc_g); + auxLowerBounds.setValue(1, 0, -mu_mc_g); + auxUpperBounds.setValue(0, 0, mu_mc_g); + auxUpperBounds.setValue(1, 0, mu_mc_g); + + // Compute the error auxiliary values + auxErrorValues.setValue(0, 0, 0.0); + auxErrorValues.setValue(1, 0, 0.0); +;} // TODO : Delete this (Used to debug collision detection) void Contact::draw() const { diff --git a/sources/reactphysics3d/constraint/Contact.h b/sources/reactphysics3d/constraint/Contact.h index 7f23daad..41b536e8 100644 --- a/sources/reactphysics3d/constraint/Contact.h +++ b/sources/reactphysics3d/constraint/Contact.h @@ -27,45 +27,55 @@ // ReactPhysics3D namespace namespace reactphysics3d { +// Constants +const double FRICTION_COEFFICIENT = 0.2; // Friction coefficient +const double PENETRATION_FACTOR = 0.1; // Penetration factor (between 0 and 1) which specify the importance of the + // penetration depth in order to calculate the correct impulse for the contact + /* ------------------------------------------------------------------- Class Contact : This class represents a collision contact between two bodies in the physics engine. The contact class inherits from the Constraint class. The collision detection system computes contact informations that will be used to perform the collision - response. A contact constraint has two auxiliary constraints in - order two represent the two friction forces at the contact - surface. + response. Each contact contains only one contact point. A contact + constraint has two auxiliary constraints in order two represent + the two friction forces at the contact surface. ------------------------------------------------------------------- */ class Contact : public Constraint { protected : - Vector3D normal; // Normal vector of the contact (From body1 toward body2) - double penetrationDepth; // Penetration depth - std::vector points; // Contact points + const Vector3D normal; // Normal vector of the contact (From body1 toward body2) + const double penetrationDepth; // Penetration depth + const Vector3D point; // Contact point std::vector frictionVectors; // Two orthogonal vectors that span the tangential friction plane 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& points); // Constructor - virtual ~Contact(); // Destructor + Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point); // Constructor + virtual ~Contact(); // Destructor Vector3D getNormal() const; // Return the normal vector of the contact - std::vector getPoints() const; // Return the contact points + Vector3D getPoint() const; // Return the contact point virtual void evaluate(); // Evaluate the constraint - virtual unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints + unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints - void draw() const; // TODO : Delete this (Used to debug collision detection) + void draw() const; // TODO : Delete this (Used to debug collision detection) }; -// Compute the two orthogonal vectors "v1" and "v2" that span the tangential friction plane +// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane // The two vectors have to be such that : v1 x v2 = contactNormal inline void Contact::computeFrictionVectors() { // Delete the current friction vectors frictionVectors.clear(); - // TODO : Implement this method ... + // Compute the first orthogonal vector + Vector3D vector1 = normal.getOneOrthogonalVector(); + frictionVectors.push_back(vector1); + + // Compute the second orthogonal vector using the cross product + frictionVectors.push_back(normal.crossProduct(vector1)); } // Return the normal vector of the contact @@ -74,8 +84,8 @@ inline Vector3D Contact::getNormal() const { } // Return the contact points -inline std::vector Contact::getPoints() const { - return points; +inline Vector3D Contact::getPoint() const { + return point; }