From 9fd3d8b5984bf708446a35c1764b383397bb0b59 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Sat, 11 Sep 2010 16:25:43 +0000 Subject: [PATCH] Now we can use a single Contact for several contact points between two bodies git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@404 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- src/collision/CollisionDetection.cpp | 32 ++--- src/constraint/Constraint.cpp | 4 +- src/constraint/Constraint.h | 43 +++---- src/constraint/Contact.cpp | 164 ++++++++++++++++++++++++- src/constraint/Contact.h | 175 ++++----------------------- src/engine/ConstraintSolver.cpp | 105 +++++++--------- src/engine/ConstraintSolver.h | 14 +-- src/engine/ContactCache.cpp | 65 +++++----- src/engine/ContactCache.h | 16 +-- src/engine/ContactCachingInfo.cpp | 5 +- src/engine/ContactCachingInfo.h | 7 +- src/mathematics/Matrix.cpp | 1 + 12 files changed, 320 insertions(+), 311 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index c0fb0e8d..b0c09b0f 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -131,11 +131,11 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) { // If it's a Vertex-Something contact if (nbVerticesExtremeOBB1 == 1) { // Create a new contact and add it to the physics world - world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb1ExtremePoints.at(0))); + world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb1ExtremePoints)); } else if(nbVerticesExtremeOBB2 == 1) { // If its a Vertex-Something contact // Create a new contact and add it to the physics world - world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb2ExtremePoints.at(0))); + world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb2ExtremePoints)); } else if (nbVerticesExtremeOBB1 == 2 && nbVerticesExtremeOBB2 == 2) { // If it's an edge-edge contact // Compute the two vectors of the segment lines @@ -171,11 +171,8 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) { contactSet.push_back(contactPoint); } - // For each point of the set of contact points - for (unsigned int i=0; iaddConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, contactSet.at(i))); - } + // Create a new contact and add it to the physics world + world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, contactSet)); } else if(nbVerticesExtremeOBB1 == 2 && nbVerticesExtremeOBB2 == 4) { // If it's an edge-face contact // Compute the projection of the edge of OBB1 onto the same plane of the face of OBB2 @@ -199,11 +196,8 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) { assert(clippedEdge.size() == 2); - // For each point of the contact set - for (unsigned int i=0; iaddConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge.at(i))); - } + // Create a new contact and add it to the physics world + world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge)); } else if(nbVerticesExtremeOBB1 == 4 && nbVerticesExtremeOBB2 == 2) { // If it's an edge-face contact // Compute the projection of the edge of OBB2 onto the same plane of the face of OBB1 @@ -227,11 +221,8 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) { assert(clippedEdge.size() == 2); - // For each point of the contact set - for (unsigned int i=0; iaddConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge.at(i))); - } + // Create a new contact and add it to the physics world + world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge)); } else { // If it's a face-face contact // Compute the projection of the face vertices of OBB2 onto the plane of the face of OBB1 @@ -244,10 +235,7 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) { clippedFace = movePoints(clippedFace, penetrationDepth/2.0 * normal); assert(clippedFace.size() >= 3); - // For each point of the contact set - for (unsigned int i=0; iaddConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace.at(i))); - } + // Create a new contact and add it to the physics world + world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace)); } } \ No newline at end of file diff --git a/src/constraint/Constraint.cpp b/src/constraint/Constraint.cpp index e5bfd3e0..76c6e03f 100644 --- a/src/constraint/Constraint.cpp +++ b/src/constraint/Constraint.cpp @@ -29,8 +29,8 @@ using namespace reactphysics3d; // Constructor -Constraint::Constraint(Body* const body1, Body* const body2, uint nbAuxConstraints, bool active) - :body1(body1), body2(body2), active(active), nbAuxConstraints(nbAuxConstraints) { +Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active) + :body1(body1), body2(body2), active(active), nbConstraints(nbConstraints) { } diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index 96121f97..bba2033d 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -36,15 +36,8 @@ namespace reactphysics3d { Class Constraint : This abstract class represents a constraint in the physics engine. A constraint can be a collision contact or a joint for - 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. + instance. Each constraint can be made of several "mathematical + constraints" needed to represent the main constraint. ------------------------------------------------------------------- */ class Constraint { @@ -52,23 +45,19 @@ class Constraint { 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 - uint nbAuxConstraints; // Number of auxiliary constraints associated with this constraint - + uint nbConstraints; // Number mathematical constraints associated with this Constraint + public : - Constraint(Body* const body1, Body* const body2, uint 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 // Evaluate the constraint - bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2 - virtual void computeJacobian(int noBody, Matrix& jacobian) const=0; // Compute a part of the jacobian for a given body - virtual void computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const=0; // Compute a part of the jacobian for an auxiliary constraint - virtual double computeLowerBound() const=0; // Compute the lowerbound of the constraint - virtual double computeUpperBound() const=0; // Compute the upperbound of the constraint - virtual void computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const=0; // Compute lowerbounds for the auxiliary constraints - virtual void computeAuxUpperBounds(int beginIndex, Vector& auxUpperBounds) const=0; // Compute upperbounds for the auxiliary constraints - virtual double computeErrorValue() const=0; // Compute the error value for the constraint - virtual void computeAuxErrorValues(int beginIndex, Vector& errorValues) const=0; // Compute the errors values of the auxiliary constraints - unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints // Return the number of auxiliary constraints + Constraint(Body* const body1, Body* const body2, uint nbConstraints, 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 // Evaluate the constraint + bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2 + virtual void computeJacobian(int noConstraint, Matrix**& J_sp) const=0; // Compute the jacobian matrix for all mathematical constraints + virtual void computeLowerBound(int noConstraint, Vector& lowerBounds) const=0; // Compute the lowerbounds values for all the mathematical constraints + virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const=0; // Compute the upperbounds values for all the mathematical constraints + virtual void computeErrorValue(int noConstraint, Vector& errorValues) const=0; // Compute the error values for all the mathematical constraints + unsigned int getNbConstraints() const; // Return the number of mathematical constraints // Return the number of auxiliary constraints }; // Return the reference to the body 1 @@ -87,8 +76,8 @@ inline bool Constraint::isActive() const { } // Return the number auxiliary constraints -inline uint Constraint::getNbAuxConstraints() const { - return nbAuxConstraints; +inline uint Constraint::getNbConstraints() const { + return nbConstraints; } } // End of the ReactPhysics3D namespace diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index 9a37b9b5..30ff9176 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/Contact.cpp @@ -25,12 +25,12 @@ // Libraries #include "Contact.h" -// We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +using namespace std; // Constructor -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) { +Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const vector& points) + :Constraint(body1, body2, 3*points.size(), true), normal(normal), penetrationDepth(penetrationDepth), points(points), nbPoints(points.size()) { // Compute the auxiliary lower and upper bounds // TODO : Now mC is only the mass of the first body but it is probably wrong @@ -45,3 +45,161 @@ Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, d Contact::~Contact() { } + +// This method computes the jacobian matrix for all mathematical constraints +// The argument "J_sp" is the jacobian matrix of the constraint solver. This method +// fill in this matrix with all the jacobian matrix of the mathematical constraint +// of the contact. The argument "noConstraint", is the row were the method have +// to start to fill in the J_sp matrix. +void Contact::computeJacobian(int noConstraint, Matrix**& J_sp) const { + RigidBody* rigidBody1 = dynamic_cast(body1); + RigidBody* rigidBody2 = dynamic_cast(body2); + Vector3D r1; + Vector3D r2; + Vector3D r1CrossN; + Vector3D r2CrossN; + Vector3D r1CrossU1; + Vector3D r2CrossU1; + Vector3D r1CrossU2; + Vector3D r2CrossU2; + Vector3D body1Position = rigidBody1->getPosition(); + Vector3D body2Position = rigidBody2->getPosition(); + int currentIndex = noConstraint; // Current constraint index + + assert(rigidBody1 != 0); + assert(rigidBody2 != 0); + + // For each point in the contact + for (int i=0; i= 0 && noConstraint + nbConstraints <= lowerBounds.getNbComponent()); + + // For each constraint + for (int i=0; i= 0 && noConstraint + nbConstraints <= upperBounds.getNbComponent()); + + // For each constraint + for (int i=0; i(body1); + RigidBody* rigidBody2 = dynamic_cast(body2); + int index = noConstraint; + + assert(rigidBody1); + assert(rigidBody2); + assert(noConstraint >= 0 && noConstraint + nbConstraints <= errorValues.getNbComponent()); + + // Compute the error value for the contact constraint + Vector3D velocity1 = rigidBody1->getLinearVelocity(); + Vector3D velocity2 = rigidBody2->getLinearVelocity(); + double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution(); + double errorValue = restitutionCoeff * (normal.scalarProduct(velocity1) - normal.scalarProduct(velocity2)) + PENETRATION_FACTOR * penetrationDepth; + + // Assign the error value to the vector of error values + for (int i=0; i points; // Contact points between the two bodies + const int nbPoints; // Number of points in the contact std::vector 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 Vector3D& point); // Constructor - virtual ~Contact(); // Destructor + Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector& points); // Constructor + virtual ~Contact(); // Destructor - Vector3D getNormal() const; // Return the normal vector of the contact - Vector3D getPoint() const; // Return the contact point - virtual void computeJacobian(int noBody, Matrix& jacobian) const; // Compute a part of the jacobian for a given body - virtual void computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const; // Compute a part of the jacobian for an auxiliary constraint - virtual double computeLowerBound() const; // Compute the lowerbound of the constraint - virtual double computeUpperBound() const; // Compute the upperbound of the constraint - virtual void computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const; // Compute the lowerbounds for the auxiliary constraints - virtual void computeAuxUpperBounds(int beginIndex, Vector& auxLowerBounds) const; // Compute the upperbounds for the auxiliary constraints - virtual double computeErrorValue() const; // Compute the error value for the constraint - virtual void computeAuxErrorValues(int beginIndex, Vector& errorValues) const; // Compute the errors values of the auxiliary constraints - uint getNbAuxConstraints() const; // Return the number of auxiliary constraints - double getPenetrationDepth() const; // Return the penetration depth - void draw() const; // TODO : Delete this (Used to debug collision detection) + Vector3D getNormal() const; // Return the normal vector of the contact + Vector3D getPoint(int index) const; // Return a contact point + int getNbPoints() const; // Return the number of contact points + virtual void computeJacobian(int noConstraint, Matrix**& 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 + virtual void computeErrorValue(int noConstraint, Vector& errorValues) const; // Compute the error values for all the mathematical constraints + double getPenetrationDepth() const; // Return the penetration depth + void draw() const; // TODO : Delete this (Used to debug collision detection) }; // Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane @@ -100,9 +96,15 @@ inline Vector3D Contact::getNormal() const { return normal; } -// Return the contact points -inline Vector3D Contact::getPoint() const { - return point; +// Return a contact points +inline Vector3D Contact::getPoint(int index) const { + assert(index >= 0 && index < nbPoints); + return points[index]; +} + +// Return the number of contact points +inline int Contact::getNbPoints() const { + return nbPoints; } // Return the penetration depth of the contact @@ -110,131 +112,6 @@ inline double Contact::getPenetrationDepth() const { return penetrationDepth; } -// This method computes a part of the jacobian matrix for a given body. -// The argument "noBody" is 1 or 2 and corresponds to which one of the two -// bodies of the constraint we will compute the jacobian part. The argument -// "jacobian" is a 1x6 jacobian matrix of the constraint corresponding to one of -// the two bodies of the constraint. -inline void Contact::computeJacobian(int noBody, Matrix& jacobian) const { - RigidBody* rigidBody; - Vector3D rCrossN; - Vector3D r; - Vector3D norm = normal; - - assert(noBody == 1 || noBody == 2); - assert(jacobian.getNbRow() == 1 && jacobian.getNbColumn() == 6); - - if (noBody == 1) { - rigidBody = dynamic_cast(body1); - assert(rigidBody); - r = point - rigidBody->getPosition(); - rCrossN = r.crossProduct(normal).getOpposite(); - norm = normal.getOpposite(); - } - else { - rigidBody = dynamic_cast(body2); - assert(rigidBody); - r = point - rigidBody->getPosition(); - rCrossN = r.crossProduct(normal); - } - - // Compute the jacobian matrix for the body 1 - jacobian.setValue(0, 0, norm.getX()); - jacobian.setValue(0, 1, norm.getY()); - jacobian.setValue(0, 2, norm.getZ()); - jacobian.setValue(0, 3, rCrossN.getX()); - jacobian.setValue(0, 4, rCrossN.getY()); - jacobian.setValue(0, 5, rCrossN.getZ()); -} - -// Compute a part of the jacobian matrix for an auxiliary constraint (given by "noAuxConstraint") -// and one of the two bodies (given by "noBody") of the contact. The argument "noBody" is 1 or 2 and -// argument auxJacobian is a 1x6 matrix. -inline void Contact::computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const { - Vector3D r; - Vector3D rCrossU; - RigidBody* rigidBody; - double sign; - - assert(noBody == 1 || noBody == 2); - assert(noAuxConstraint == 1 || noAuxConstraint == 2); - assert(auxJacobian.getNbRow() == 1 && auxJacobian.getNbColumn() == 6); - - if (noBody == 1) { - rigidBody = dynamic_cast(body1); - assert(rigidBody); - r = point - rigidBody->getPosition(); - sign = -1.0; - } - else { - rigidBody = dynamic_cast(body2); - assert(rigidBody); - r = point - rigidBody->getPosition(); - sign = 1.0; - } - - rCrossU = r.crossProduct(frictionVectors[noAuxConstraint-1]); - - auxJacobian.setValue(0, 0, sign * frictionVectors[noAuxConstraint-1].getX()); - auxJacobian.setValue(0, 1, sign * frictionVectors[noAuxConstraint-1].getY()); - auxJacobian.setValue(0, 2, sign * frictionVectors[noAuxConstraint-1].getZ()); - auxJacobian.setValue(0, 3, sign * rCrossU.getX()); - auxJacobian.setValue(0, 4, sign * rCrossU.getY()); - auxJacobian.setValue(0, 5, sign * rCrossU.getZ()); -} - -// Compute the lowerbounds for the auxiliary constraints -inline double Contact::computeLowerBound() const { - return 0.0; -} - -// Compute the upperbounds for the auxiliary constraints -inline double Contact::computeUpperBound() const { - return INFINITY_CONST; -} - -// Compute the lowerbounds for the auxiliary constraints. This method fills the "auxLowerBounds" -// vector starting at the index "beginIndex" in this vector. -inline void Contact::computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const { - assert(beginIndex + nbAuxConstraints <= auxLowerBounds.getNbComponent()); - - auxLowerBounds.setValue(beginIndex, -mu_mc_g); - auxLowerBounds.setValue(beginIndex + 1, -mu_mc_g); - -} - -// Compute the upperbounds for the auxiliary constraints. This method fills the "auxUpperBounds" -// vector starting at the index "beginIndex" in this vector. -inline void Contact::computeAuxUpperBounds(int beginIndex, Vector& auxUpperBounds) const { - assert(beginIndex + nbAuxConstraints <= auxUpperBounds.getNbComponent()); - - auxUpperBounds.setValue(beginIndex, mu_mc_g); - auxUpperBounds.setValue(beginIndex + 1, mu_mc_g); -} - -// Compute the error value for the constraint -inline double Contact::computeErrorValue() const { - RigidBody* rigidBody1 = dynamic_cast(body1); - RigidBody* rigidBody2 = dynamic_cast(body2); - - assert(rigidBody1); - assert(rigidBody2); - - Vector3D velocity1 = rigidBody1->getLinearVelocity(); - Vector3D velocity2 = rigidBody2->getLinearVelocity(); - double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution(); - double errorValue = restitutionCoeff * (normal.scalarProduct(velocity1) - normal.scalarProduct(velocity2)) + PENETRATION_FACTOR * penetrationDepth; - return errorValue; -} - -// Compute the errors values of the auxiliary constraints -inline void Contact::computeAuxErrorValues(int beginIndex, Vector& errorValues) const { - assert(beginIndex + nbAuxConstraints <= errorValues.getNbComponent()); - - errorValues.setValue(beginIndex, 0.0); - errorValues.setValue(beginIndex + 1, 0.0); -} - // TODO : Delete this (Used to debug collision detection) inline void Contact::draw() const { glColor3f(1.0, 0.0, 0.0); diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 2476226e..0800a7d0 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -68,7 +68,7 @@ void ConstraintSolver::initialize() { bodyNumberMapping.insert(pair(constraint->getBody2(), bodyNumberMapping.size())); // Update the size of the jacobian matrix - nbConstraints += (1 + constraint->getNbAuxConstraints()); + nbConstraints += constraint->getNbConstraints(); } } @@ -132,7 +132,7 @@ void ConstraintSolver::allocate() { B_sp = new Matrix*[2]; B_sp[0] = new Matrix[nbConstraints]; B_sp[1] = new Matrix[nbConstraints]; - for (uint i=0; icomputeJacobian(1, J_sp[noConstraint][0]); - constraint->computeJacobian(2, J_sp[noConstraint][1]); + constraint->computeJacobian(noConstraint, J_sp); + //constraint->computeJacobian(noConstraint, J_sp); // Fill in the body mapping matrix - bodyMapping[noConstraint][0] = constraint->getBody1(); - bodyMapping[noConstraint][1] = constraint->getBody2(); + for(int i=0; igetNbConstraints(); i++) { + bodyMapping[noConstraint+i][0] = constraint->getBody1(); + bodyMapping[noConstraint+i][1] = constraint->getBody2(); + } // Fill in the limit vectors for the constraint - lowerBounds.setValue(noConstraint, constraint->computeLowerBound()); - upperBounds.setValue(noConstraint, constraint->computeUpperBound()); + constraint->computeLowerBound(noConstraint, lowerBounds); + constraint->computeUpperBound(noConstraint, upperBounds); // Fill in the error vector - errorValues.setValue(noConstraint, constraint->computeErrorValue()); + constraint->computeErrorValue(noConstraint, errorValues); - // If it's a contact constraint + // Set the init lambda values contact = dynamic_cast(constraint); + contactInfo = 0; if (contact) { // Get the lambda init value from the cache if exists - contactInfo = contactCache.getContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint()); - if (contactInfo) { - // The last lambda init value was in the cache - lambdaInit.setValue(noConstraint, contactInfo->lambda); + contactInfo = contactCache.getContactCachingInfo(contact); + } + for (int i=0; igetNbConstraints(); i++) { + if (contactInfo) { // If the last lambda init value is in the cache + lambdaInit.setValue(noConstraint + i, contactInfo->lambdas[i]); } - else { - // The las lambda init value was not in the cache - lambdaInit.setValue(noConstraint, 0.0); + else { // The las lambda init value was not in the cache + lambdaInit.setValue(noConstraint + i, 0.0); } } - else { - // Set the lambda init value - lambdaInit.setValue(noConstraint, 0.0); - } - nbAuxConstraints = constraint->getNbAuxConstraints(); - - // If the current constraint has auxiliary constraints - if (nbAuxConstraints > 0) { - - // For each auxiliary constraints - for (uint i=1; i<=nbAuxConstraints; i++) { - // Fill in the J_sp matrix - J_sp[noConstraint+i][0].changeSize(1, 6); - J_sp[noConstraint+i][1].changeSize(1, 6); - constraint->computeAuxJacobian(1, i, J_sp[noConstraint+i][0]); - constraint->computeAuxJacobian(2, i, J_sp[noConstraint+i][1]); - - // Fill in the body mapping matrix - bodyMapping[noConstraint+i][0] = constraint->getBody1(); - bodyMapping[noConstraint+i][1] = constraint->getBody2(); - - // Fill in the init lambda value for the constraint - lambdaInit.setValue(noConstraint+i, 0.0); - } - - // Fill in the limit vectors for the auxilirary constraints - constraint->computeAuxLowerBounds(noConstraint+1, lowerBounds); - constraint->computeAuxUpperBounds(noConstraint+1, upperBounds); - - // Fill in the errorValues vector for the auxiliary constraints - constraint->computeAuxErrorValues(noConstraint+1, errorValues); - } - - noConstraint += 1 + nbAuxConstraints; + noConstraint += constraint->getNbConstraints(); } // For each current body that is implied in some constraint @@ -359,24 +328,40 @@ void ConstraintSolver::computeVectorVconstraint(double dt) { void ConstraintSolver::updateContactCache() { Contact* contact; ContactCachingInfo* contactInfo; + int index; // Clear the contact cache contactCache.clear(); // For each active constraint - uint noConstraint = 0; - for (uint c=0; c(activeConstraints.at(c)); if (contact) { + + // Get all the contact points of the contact + vector points; + vector lambdas; + for (int i=0; igetNbPoints(); i++) { + points.push_back(contact->getPoint(i)); + } + + // For each constraint of the contact + for (int i=0; igetNbConstraints(); i++) { + // Get the lambda value that have just been computed + lambdas.push_back(lambda.getValue(noConstraint + i)); + } + // Create a new ContactCachingInfo - contactInfo = new ContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint(), lambda.getValue(noConstraint)); + contactInfo = new ContactCachingInfo(contact->getBody1(), contact->getBody2(), points, lambdas); // Add it to the contact cache contactCache.addContactCachingInfo(contactInfo); } - noConstraint += 1 + activeConstraints.at(c)->getNbAuxConstraints(); + noConstraint += activeConstraints.at(c)->getNbConstraints(); } } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 9865ca97..23fb4718 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -134,6 +134,13 @@ inline Vector3D ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2)); } +// Cleanup of the constraint solver +inline void ConstraintSolver::cleanup() { + bodyNumberMapping.clear(); + constraintBodies.clear(); + activeConstraints.clear(); +} + // Solve the current LCP problem inline void ConstraintSolver::solve(double dt) { // Allocate memory for the matrices @@ -159,13 +166,6 @@ inline void ConstraintSolver::solve(double dt) { computeVectorVconstraint(dt); } -// Cleanup of the constraint solver -inline void ConstraintSolver::cleanup() { - bodyNumberMapping.clear(); - constraintBodies.clear(); - activeConstraints.clear(); -} - } // End of ReactPhysics3D namespace #endif \ No newline at end of file diff --git a/src/engine/ContactCache.cpp b/src/engine/ContactCache.cpp index 88b63463..38dc6092 100644 --- a/src/engine/ContactCache.cpp +++ b/src/engine/ContactCache.cpp @@ -46,44 +46,53 @@ void ContactCache::clear() { // Add a new contact caching info in the cache void ContactCache::addContactCachingInfo(ContactCachingInfo* info) { - // Check if there is already an entry for that pair of body in the cache - map, vector >::iterator entry = cache.find(make_pair(info->body1, info->body2)); - if (entry != cache.end()) { - (*entry).second.push_back(info); - } - else { - // Add a new entry in the cache - vector vec; - vec.push_back(info); - cache.insert(make_pair(make_pair(info->body1, info->body2), vec)); - } + // Add a new entry in the cache + cache.insert(make_pair(make_pair(info->body1, info->body2), info)); } // Return the ContactCachingInfo (if exists) corresponding to the arguments // If the contact pair between the two bodies in argument doesn't exist this or there is no ContactCachingInfo // compatible (with approximatively the same position), this method returns NULL. -ContactCachingInfo* ContactCache::getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position) { - // Check if there is an entry for that pair of body in the cache - map, vector >::iterator entry = cache.find(make_pair(body1, body2)); +ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const { + double posX, posY, posZ; + + // Check if there is an entry for that pair of body in the cache + map, ContactCachingInfo* >::const_iterator entry = cache.find(make_pair(contact->getBody1(), contact->getBody2())); if (entry != cache.end()) { - vector vec = (*entry).second; - assert((*entry).first.first == body1); - assert((*entry).first.second == body2); + ContactCachingInfo* contactInfo = (*entry).second; - double posX = position.getX(); - double posY = position.getY(); - double posZ = position.getZ(); + assert(contactInfo); + assert((*entry).first.first == contact->getBody1()); + assert((*entry).first.second == contact->getBody2()); - // Check among all the old contacts for this pair of body if there is an approximative match for the contact position - for(vector::iterator it = vec.begin(); it != vec.end(); it++) { - Vector3D& contactPos = (*it)->position; - if (posX <= contactPos.getX() + POSITION_TOLERANCE && posX >= contactPos.getX() - POSITION_TOLERANCE && - posY <= contactPos.getY() + POSITION_TOLERANCE && posY >= contactPos.getY() - POSITION_TOLERANCE && - posZ <= contactPos.getZ() + POSITION_TOLERANCE && posZ >= contactPos.getZ() - POSITION_TOLERANCE) { - // Return the ContactCachingInfo - return *it; + // If the new contact and the contact caching info doesn't have the same number of contact points + if (contact->getNbPoints() != contactInfo->positions.size()) { + // We return NULL because, the contact doesn't match + return 0; + } + + for (int i=0; ipositions.size(); i++) { + + // Get the position of the current contact + posX = contact->getPoint(i).getX(); + posY = contact->getPoint(i).getY(); + posZ = contact->getPoint(i).getZ(); + + // Get the position of the old contact + Vector3D& contactPos = contactInfo->positions[i]; + + // If the old contact point doesn't match the current one + if (posX > contactPos.getX() + POSITION_TOLERANCE || posX < contactPos.getX() - POSITION_TOLERANCE || + posY > contactPos.getY() + POSITION_TOLERANCE || posY < contactPos.getY() - POSITION_TOLERANCE || + posZ > contactPos.getZ() + POSITION_TOLERANCE || posZ < contactPos.getZ() - POSITION_TOLERANCE) { + + // Return NULL + return 0; } } + + // The old contact positions match the current contact, therefore we return the contact caching info + return contactInfo; } else { return 0; diff --git a/src/engine/ContactCache.h b/src/engine/ContactCache.h index 5455ba79..76837f33 100644 --- a/src/engine/ContactCache.h +++ b/src/engine/ContactCache.h @@ -30,12 +30,13 @@ #include #include #include "ContactCachingInfo.h" +#include "../constraint/Contact.h" // ReactPhysics3D namespace namespace reactphysics3d { // Constant -const double POSITION_TOLERANCE = 0.01 ; // Tolerance used to consider two similar positions to be considered as the same +const double POSITION_TOLERANCE = 0.01; // Tolerance used to consider two similar positions to be considered as the same /* ------------------------------------------------------------------- Class ContactCache : @@ -47,14 +48,15 @@ const double POSITION_TOLERANCE = 0.01 ; // Tolerance used to consider two */ class ContactCache { private: - std::map, std::vector > cache; + std::map, ContactCachingInfo* > cache; public: - ContactCache(); // Constructor - ~ContactCache(); // Destructor - void clear(); // Remove all the contact caching info of the cache - void addContactCachingInfo(ContactCachingInfo* contactCachingInfo); // Add a new contact caching info in the cache - ContactCachingInfo* getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position); // Return the ContactCachingInfo (if exists) corresponding to the arguments + ContactCache(); // Constructor + ~ContactCache(); // Destructor + void clear(); // Remove all the contact caching info of the cache + ContactCachingInfo* getContactCachingInfo(Contact* contact) const; + void addContactCachingInfo(ContactCachingInfo* contactCachingInfo); // Add a new contact caching info in the cache + }; } // End of the ReactPhysics3D namespace diff --git a/src/engine/ContactCachingInfo.cpp b/src/engine/ContactCachingInfo.cpp index 680f24a2..b3b65090 100644 --- a/src/engine/ContactCachingInfo.cpp +++ b/src/engine/ContactCachingInfo.cpp @@ -26,9 +26,10 @@ #include "ContactCachingInfo.h" using namespace reactphysics3d; +using namespace std; // Constructor -ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const Vector3D& position, double lambda) - : body1(body1), body2(body2), position(position), lambda(lambda) { +ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const vector& positions, const vector& lambdas) + : body1(body1), body2(body2), positions(positions), lambdas(lambdas) { } diff --git a/src/engine/ContactCachingInfo.h b/src/engine/ContactCachingInfo.h index 29e07662..5340bae7 100644 --- a/src/engine/ContactCachingInfo.h +++ b/src/engine/ContactCachingInfo.h @@ -41,13 +41,12 @@ namespace reactphysics3d { */ struct ContactCachingInfo { public: - // TODO : Use polymorphism here (change OBB into BoundingVolume to be more general) Body* body1; // Body pointer of the first bounding volume Body* body2; // Body pointer of the second bounding volume - Vector3D position; // Contact point - double lambda; // Last lambda value for the constraint + std::vector positions; // Positions of the contact points + std::vector lambdas; // Last lambdas value for the constraint - ContactCachingInfo(Body* body1, Body* body2, const Vector3D& position, double lambda); // Constructor + ContactCachingInfo(Body* body1, Body* body2, const std::vector& positions, const std::vector& lambdas); // Constructor }; } // End of the ReactPhysics3D namespace diff --git a/src/mathematics/Matrix.cpp b/src/mathematics/Matrix.cpp index 0eb6a657..a70ed75c 100644 --- a/src/mathematics/Matrix.cpp +++ b/src/mathematics/Matrix.cpp @@ -470,6 +470,7 @@ Matrix Matrix::operator*(const Matrix& matrix2) const throw(MathematicsException // Overloaded operator for multiplication with a vector Matrix Matrix::operator*(const Vector& vector) const throw(MathematicsException) { + // Check the sizes of the matrices if (nbColumn == vector.getNbComponent()) { Matrix result(nbRow, 1);