/******************************************************************************** * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * * Copyright (c) 2010 Daniel Chappuis * ********************************************************************************* * * * Permission is hereby granted, free of charge, to any person obtaining a copy * * of this software and associated documentation files (the "Software"), to deal * * in the Software without restriction, including without limitation the rights * * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * * copies of the Software, and to permit persons to whom the Software is * * furnished to do so, subject to the following conditions: * * * * The above copyright notice and this permission notice shall be included in * * all copies or substantial portions of the Software. * * * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * * THE SOFTWARE. * ********************************************************************************/ #ifndef CONTACT_H #define CONTACT_H // Libraries #include "../typeDefinitions.h" #include "Constraint.h" #include "../body/RigidBody.h" #include "../mathematics/mathematics.h" #include // TODO : Remove this in the final version #include // TODO : Remove this in the final version // ReactPhysics3D namespace namespace reactphysics3d { // Constants const double FRICTION_COEFFICIENT = 0.3; // Friction coefficient const double PENETRATION_FACTOR = 0.0; // 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. 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 : 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 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 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) }; // 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(); // 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 inline Vector3D Contact::getNormal() const { return normal; } // Return the contact points inline Vector3D Contact::getPoint() const { return point; } // Return the penetration depth of the contact 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); glutSolidSphere(0.3, 20, 20); } } // End of the ReactPhysics3D namespace #endif