From 03557bbff66268bc114fb12655356891b1825ffc Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Wed, 10 Aug 2011 16:49:38 +0000 Subject: [PATCH] Changes in the Contact class git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@436 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- src/collision/CollisionDetection.cpp | 7 +- src/constants.h | 2 +- src/constraint/Contact.cpp | 163 +++++++++++---------------- src/constraint/Contact.h | 36 +++--- src/engine/ConstraintSolver.cpp | 5 +- src/engine/ContactCache.cpp | 44 +++++--- 6 files changed, 112 insertions(+), 145 deletions(-) diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 03bb7ff9..96c89479 100644 --- a/src/collision/CollisionDetection.cpp +++ b/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 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); diff --git a/src/constants.h b/src/constants.h index eeac851e..87ea3e31 100644 --- a/src/constants.h +++ b/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 diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index 704839d6..b76ed659 100644 --- a/src/constraint/Contact.cpp +++ b/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& points) - :Constraint(body1, body2, 3*points.size(), true), normal(normal), penetrationDepth(penetrationDepth), points(points), nbPoints(points.size()) { +Contact::Contact(const ContactInfo* contactInfo) + : 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 - 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(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 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 + const Vector3D normal; // Normal vector of the contact (From body1 toward body2) + const double penetrationDepth; // Penetration depth + const Vector3D pointOnBody1; // Contact point on body 1 + const Vector3D pointOnBody2; // Contact point on body 2 + 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 std::vector& points); // Constructor - virtual ~Contact(); // Destructor + Contact(const ContactInfo* contactInfo); // Constructor + virtual ~Contact(); // Destructor 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 + Vector3D getPointOnBody1() const; // Return the contact point on body 1 + 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 -inline Vector3D Contact::getPoint(int index) const { - assert(index >= 0 && index < nbPoints); - return points[index]; +// Return the contact point on body 1 +inline Vector3D Contact::getPointOnBody1() const { + return pointOnBody1; } -// Return the number of contact points -inline int Contact::getNbPoints() const { - return nbPoints; +// Return the contact point on body 2 +inline Vector3D Contact::getPointOnBody2() const { + return pointOnBody2; } // Return the penetration depth of the contact diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index a12d375c..40cfca40 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -353,9 +353,8 @@ void ConstraintSolver::updateContactCache() { // Get all the contact points of the contact vector points; vector lambdas; - for (int i=0; igetNbPoints(); i++) { - points.push_back(contact->getPoint(i)); - } + points.push_back(contact->getPointOnBody1()); + points.push_back(contact->getPointOnBody2()); // For each constraint of the contact for (int i=0; igetNbConstraints(); i++) { diff --git a/src/engine/ContactCache.cpp b/src/engine/ContactCache.cpp index aafd1193..9c739358 100644 --- a/src/engine/ContactCache.cpp +++ b/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 - if (contact->getNbPoints() != contactInfo->positions.size()) { - // We return NULL because, the contact doesn't match + // Get the position of the current contact + posX = contact->getPointOnBody1().getX(); + 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; ipositions.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 - posX = contact->getPoint(i).getX(); - posY = contact->getPoint(i).getY(); - posZ = contact->getPoint(i).getZ(); + // Get the position of the old contact + Vector3D& contactPos2 = contactInfo->positions[1]; - // 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 > 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 - 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 NULL; - } + // Return NULL + return NULL; } // The old contact positions match the current contact, therefore we return the contact caching info