diff --git a/sources/reactphysics3d/constraint/Constraint.cpp b/sources/reactphysics3d/constraint/Constraint.cpp index 8bd5dbec..2bd69ab0 100644 --- a/sources/reactphysics3d/constraint/Constraint.cpp +++ b/sources/reactphysics3d/constraint/Constraint.cpp @@ -24,8 +24,9 @@ using namespace reactphysics3d; // Constructor -Constraint::Constraint(Body* const body1, Body* const body2) - :body1(body1), body2(body2), active(true) { +Constraint::Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, bool active) + :body1(body1), body2(body2), active(true), nbAuxConstraints(nbAuxConstraints), + lowerBound(lowerBound), upperBound(upperBound), active(active) { } diff --git a/sources/reactphysics3d/constraint/Constraint.h b/sources/reactphysics3d/constraint/Constraint.h index cfa83523..7c98c9ab 100644 --- a/sources/reactphysics3d/constraint/Constraint.h +++ b/sources/reactphysics3d/constraint/Constraint.h @@ -27,53 +27,52 @@ // ReactPhysics3D namespace namespace reactphysics3d { -// Constants -const double kErp = 0.8; // Value between 0 and 1 used to compute the kCorrection value (kCorrection = kErp * kFps) - /* ------------------------------------------------------------------- Class Constraint : This abstract class represents a constraint in the physics engine. A constraint can be a collision contact or a joint for - instance. + instance. Each constraint have a jacobian matrix associated with + the first body of the constraint and a jacobian matrix associated + with the second body. Each constraint can also have some auxiliary + constraints. Those auxiliary constraint are all represented in the + auxiliary Jacobian matrix. Auxiliary constraints represents some + constraints associated with a main constraint. For instance, a + contact constraint between two bodies can have two associated + auxiliary constraints to represent the frictions force constraints + in two directions. ------------------------------------------------------------------- */ class Constraint { protected : - Body* const body1; // Pointer to the first body of the constraint - Body* const body2; // Pointer to the second body of the constraint - bool active; // True if the constraint is active - Matrix body1LinearJacobian; // Linear jacobian matrix of the constraint for body1 - Matrix body2LinearJacobian; // Linear jacobian matrix of the constraint for body2 - Matrix body1AngularJacobian; // Angular jacobian matrix of the constraint for body1 - Matrix body2AngularJacobian; // Angular jacobian matrix of the constraint for body2 - Vector errorVector; // Error term vector of the constraint - Vector rightHandSideVector; // Right-hand-side vector - Matrix auxiliaryRows; - Matrix auxiliaryColumns; - unsigned int jacobianIndex; // Jacobian index - unsigned int auxiliaryIndex; // Auxiliary index + Body* const body1; // Pointer to the first body of the constraint + Body* const body2; // Pointer to the second body of the constraint + bool active; // True if the constraint is active + 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) + 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 public : - Constraint(Body* const body1, Body* const body2); // 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 - virtual void evaluate(double dt)=0; // Evaluate the constraint - bool isActive() const; // Return true if the constraint is active - virtual unsigned int getNbJacobianRows() const=0; // Return the number of rows of the Jacobian matrix - void setJacobianIndex(unsigned int index); // Set the jacobian index value - unsigned int getJacobianIndex() const; // Return the jacobian index - Matrix getBody1LinearJacobian() const; // Return the linear jacobian matrix of body 1 - Matrix getBody2LinearJacobian() const; // Return the linear jacobian matrix of body 2 - Matrix getBody1AngularJacobian() const; // Return the angular jacobian matrix of body 1 - Matrix getBody2AngularJacobian() const; // Return the angular jacobian matrix of body 2 - Vector getErrorVector() const; // Return the error vector of the constraint - virtual int getNbAuxiliaryVars() const=0; // Return the number of auxiliary variables - void setAuxiliaryIndex(unsigned int index); // Set the auxiliary index - unsigned int getAuxiliaryIndex() const; // Return the auxiliary index - void getAuxiliaryRowsAndCols(Matrix& rows, Matrix& columns) const; // Return the auxiliary rows and columns - Vector getRightHandSideVector() const; // Return the right-hand-side vector + Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, double lowerBound, double upperBound, + bool active); // 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 + virtual void evaluate()=0; // Evaluate the 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 + 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 }; // Return the reference to the body 1 @@ -91,63 +90,46 @@ inline bool Constraint::isActive() const { return active; } -// Set the jacobian index value -inline void Constraint::setJacobianIndex(unsigned int index) { - this->jacobianIndex = index; +// Return the jacobian matrix of body 1 +inline Matrix Constraint::getBody1Jacobian() const { + return body1Jacobian; } -// Return the jacobian index -inline unsigned int Constraint::getJacobianIndex() const { - return jacobianIndex; +// Return the jacobian matrix of body 2 +inline Matrix Constraint::getBody2Jacobian() const { + return body2Jacobian; } -// Return the linear jacobian matrix of body 1 -inline Matrix Constraint::getBody1LinearJacobian() const { - return body1LinearJacobian; +// Return the number auxiliary constraints +inline unsigned int Constraint::getNbAuxConstraints() const { + return nbAuxConstraints; } -// Return the linear jacobian matrix of body 2 -inline Matrix Constraint::getBody2LinearJacobian() const { - return body2LinearJacobian; +// Return the auxiliary jacobian matrix +inline void Constraint::getAuxJacobian(Matrix& auxJacobian) const { + auxJacobian = this->auxJacobian; } -// Return the angular jacobian matrix of body 1 -inline Matrix Constraint::getBody1AngularJacobian() const { - return body1AngularJacobian; +// Return the lower bound value of the constraint +inline double Constraint::getLowerBound() const { + return lowerBound; } -// Return the angular jacobian matrix of body 2 -inline Matrix Constraint::getBody2AngularJacobian() const { - return body2AngularJacobian; + // Return the upper bound value of the constraint +inline double Constraint::getUpperBound() const { + return upperBound; } -// Return the error vector of the constraint -inline Vector Constraint::getErrorVector() const { - return errorVector; +// Return the vector of lower bounds values +inline void Constraint::getAuxLowerBounds(Vector& auxLowerBounds) const { + auxLowerBounds = this->auxLowerBounds; } -// Set the auxiliary index -inline void Constraint::setAuxiliaryIndex(unsigned int index) { - auxiliaryIndex = index; +// Return the vector of the upper bounds values +inline void Constraint::getAuxUpperBounds(Vector& auxUpperBounds) const { + auxUpperBounds = this->auxUpperBounds; } -// Return the auxiliary index -inline unsigned int Constraint::getAuxiliaryIndex() const { - return auxiliaryIndex; -} - -// Return the auxiliary rows and columns -inline void Constraint::getAuxiliaryRowsAndCols(Matrix& rows, Matrix& columns) const { - rows = auxiliaryRows; - columns = auxiliaryColumns; -} - -// Return the right-hand-side vector -inline Vector Constraint::getRightHandSideVector() const { - return rightHandSideVector; -} - - } // End of the ReactPhysics3D namespace #endif diff --git a/sources/reactphysics3d/constraint/Contact.cpp b/sources/reactphysics3d/constraint/Contact.cpp index 65040720..0cc55612 100644 --- a/sources/reactphysics3d/constraint/Contact.cpp +++ b/sources/reactphysics3d/constraint/Contact.cpp @@ -28,8 +28,10 @@ using namespace reactphysics3d; // Constructor Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector& points) - :Constraint(body1, body2), normal(normal), penetrationDepth(penetrationDepth), points(points), nbFrictionVectors(NB_FRICTION_VECTORS) { - active = true; + :Constraint(body1, body2, 2, 0, INFINITY_CONST, true), normal(normal), penetrationDepth(penetrationDepth), points(points) { + body1Jacobian = Matrix(1, 6); + body2Jacobian = Matrix(1, 6); + auxJacobian = Matrix(2, 6); } // Destructor @@ -39,75 +41,15 @@ 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) { +void Contact::evaluate() { RigidBody* rigidBody1 = dynamic_cast(body1); RigidBody* rigidBody2 = dynamic_cast(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()); - } + // Compute the jacobian matrix for the body 1 - // 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()); - } - - // TODO : Compute the auxiliary rows and cols - - // 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) diff --git a/sources/reactphysics3d/constraint/Contact.h b/sources/reactphysics3d/constraint/Contact.h index 12840496..7f23daad 100644 --- a/sources/reactphysics3d/constraint/Contact.h +++ b/sources/reactphysics3d/constraint/Contact.h @@ -27,16 +27,15 @@ // 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 the physics engine. The contact class inherits from the - Constraint class. The collision detection systems compute + Constraint class. The collision detection system computes contact informations that will be used to perform the collision - response. + response. A contact constraint has two auxiliary constraints in + order two represent the two friction forces at the contact + surface. ------------------------------------------------------------------- */ class Contact : public Constraint { @@ -44,11 +43,9 @@ class Contact : public Constraint { Vector3D normal; // Normal vector of the contact (From body1 toward body2) double penetrationDepth; // Penetration depth std::vector 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 frictionVectors; // Set of vectors that span the tangential friction plane + std::vector frictionVectors; // Two orthogonal 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 + 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 @@ -56,26 +53,19 @@ class Contact : public Constraint { Vector3D getNormal() const; // Return the normal vector of the contact std::vector 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 + virtual void evaluate(); // Evaluate the constraint + virtual unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints 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) { +// Compute the two 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(); - 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); - } + // TODO : Implement this method ... } // Return the normal vector of the contact @@ -88,16 +78,6 @@ inline std::vector 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