mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@297 92aac97c-a6ce-11dd-a772-7fcde58d38e6

Browse Source
This commit is contained in:
chappuis.daniel 2010-03-28 21:54:41 +00:00
parent c8647da879
commit 4eaf404477
3 changed files with 116 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
sources/reactphysics3d/constraint/Constraint.h
View File

@ -50,16 +50,17 @@ class Constraint {
Matrix body1Jacobian; // Jacobian matrix of the constraint for body1 (dimension 1x6) Matrix body1Jacobian; // Jacobian matrix of the constraint for body1 (dimension 1x6)
Matrix body2Jacobian; // Jacobian matrix of the constraint for body2 (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 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 unsigned int nbAuxConstraints; // Number of auxiliary constraints associated with this constraint
double lowerBound; // Lower bound of the constraint double lowerBound; // Lower bound of the constraint
double upperBound; // Upper 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 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 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 : public :
Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, double lowerBound, double upperBound, Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, bool active); // Constructor // Constructor
bool active); // Constructor
virtual ~Constraint(); // Destructor virtual ~Constraint(); // Destructor
Body* const getBody1() const; // Return the reference to the body 1 Body* const getBody1() const; // Return the reference to the body 1
Body* const getBody2() const; // Return the reference to the body 2 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 bool isActive() const; // Return true if the constraint is active
Matrix getBody1Jacobian() const; // Return the jacobian matrix of body 1 Matrix getBody1Jacobian() const; // Return the jacobian matrix of body 1
Matrix getBody2Jacobian() const; // Return the jacobian matrix of body 2 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 void getAuxJacobian(Matrix& auxJacobian) const; // Return the jacobian matrix of auxiliary constraints
double getLowerBound() const; // Return the lower bound value of the constraint double getLowerBound() const; // Return the lower bound value of the constraint
double getUpperBound() const; // Return the upper 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 getAuxLowerBounds(Vector& auxLowerBounds) const; // Return the vector of lower bounds values
void getAuxUpperBounds(Vector& auxUpperBounds) const; // Return the vector of the upper 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 // Return the reference to the body 1
@ -130,6 +132,11 @@ inline void Constraint::getAuxUpperBounds(Vector& auxUpperBounds) const {
auxUpperBounds = this->auxUpperBounds; auxUpperBounds = this->auxUpperBounds;
} }
// Return the error value (bias) of the constraint
inline double Constraint::getErrorValue() const {
return errorValue;
}
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace
#endif #endif

84
sources/reactphysics3d/constraint/Contact.cpp
View File

@ -27,11 +27,14 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector<Vector3D>& points) Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point)
:Constraint(body1, body2, 2, 0, INFINITY_CONST, true), normal(normal), penetrationDepth(penetrationDepth), points(points) { :Constraint(body1, body2, 2, true), normal(normal), penetrationDepth(penetrationDepth), point(point) {
body1Jacobian = Matrix(1, 6); body1Jacobian = Matrix(1, 6);
body2Jacobian = 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 // Destructor
@ -48,9 +51,80 @@ void Contact::evaluate() {
assert(rigidBody1 != 0); assert(rigidBody1 != 0);
assert(rigidBody2 != 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) // TODO : Delete this (Used to debug collision detection)
void Contact::draw() const { void Contact::draw() const {

40
sources/reactphysics3d/constraint/Contact.h
View File

@ -27,45 +27,55 @@
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { 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 : Class Contact :
This class represents a collision contact between two bodies in This class represents a collision contact between two bodies in
the physics engine. The contact class inherits from the the physics engine. The contact class inherits from the
Constraint class. The collision detection system computes Constraint class. The collision detection system computes
contact informations that will be used to perform the collision contact informations that will be used to perform the collision
response. A contact constraint has two auxiliary constraints in response. Each contact contains only one contact point. A contact
order two represent the two friction forces at the contact constraint has two auxiliary constraints in order two represent
surface. the two friction forces at the contact surface.
------------------------------------------------------------------- -------------------------------------------------------------------
*/ */
class Contact : public Constraint { class Contact : public Constraint {
protected : protected :
Vector3D normal; // Normal vector of the contact (From body1 toward body2) const Vector3D normal; // Normal vector of the contact (From body1 toward body2)
double penetrationDepth; // Penetration depth const double penetrationDepth; // Penetration depth
std::vector<Vector3D> points; // Contact points const Vector3D point; // Contact point
std::vector<Vector3D> frictionVectors; // Two orthogonal vectors that span the tangential friction plane std::vector<Vector3D> frictionVectors; // Two orthogonal vectors that span the tangential friction plane
void computeFrictionVectors(); // Compute the two friction vectors that span the tangential friction plane void computeFrictionVectors(); // Compute the two friction vectors that span the tangential friction plane
public : public :
Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector<Vector3D>& points); // Constructor Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point); // Constructor
virtual ~Contact(); // Destructor virtual ~Contact(); // Destructor
Vector3D getNormal() const; // Return the normal vector of the contact Vector3D getNormal() const; // Return the normal vector of the contact
std::vector<Vector3D> getPoints() const; // Return the contact points Vector3D getPoint() const; // Return the contact point
virtual void evaluate(); // Evaluate the constraint 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 // The two vectors have to be such that : v1 x v2 = contactNormal
inline void Contact::computeFrictionVectors() { inline void Contact::computeFrictionVectors() {
// Delete the current friction vectors // Delete the current friction vectors
frictionVectors.clear(); 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 // Return the normal vector of the contact
@ -74,8 +84,8 @@ inline Vector3D Contact::getNormal() const {
} }
// Return the contact points // Return the contact points
inline std::vector<Vector3D> Contact::getPoints() const { inline Vector3D Contact::getPoint() const {
return points; return point;
} }