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

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

Browse Source
This commit is contained in:
chappuis.daniel 2010-03-10 22:28:40 +00:00
parent a17db9ec67
commit 3eb315cb0c
4 changed files with 81 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
sources/reactphysics3d/constraint/Constraint.cpp
View File

@ -24,8 +24,9 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
Constraint::Constraint(Body* const body1, Body* const body2) Constraint::Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, bool active)
:body1(body1), body2(body2), active(true) { :body1(body1), body2(body2), active(true), nbAuxConstraints(nbAuxConstraints),
lowerBound(lowerBound), upperBound(upperBound), active(active) {
} }

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

@ -27,14 +27,19 @@
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
// Constants
const double kErp = 0.8; // Value between 0 and 1 used to compute the kCorrection value (kCorrection = kErp * kFps)
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
Class Constraint : Class Constraint :
This abstract class represents a constraint in the physics engine. This abstract class represents a constraint in the physics engine.
A constraint can be a collision contact or a joint for 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 { class Constraint {
@ -42,38 +47,32 @@ class Constraint {
Body* const body1; // Pointer to the first body of the constraint Body* const body1; // Pointer to the first body of the constraint
Body* const body2; // Pointer to the second body of the constraint Body* const body2; // Pointer to the second body of the constraint
bool active; // True if the constraint is active bool active; // True if the constraint is active
Matrix body1LinearJacobian; // Linear jacobian matrix of the constraint for body1 Matrix body1Jacobian; // Jacobian matrix of the constraint for body1 (dimension 1x6)
Matrix body2LinearJacobian; // Linear jacobian matrix of the constraint for body2 Matrix body2Jacobian; // Jacobian matrix of the constraint for body2 (dimension 1x6)
Matrix body1AngularJacobian; // Angular jacobian matrix of the constraint for body1 Matrix auxJacobian; // Jacobian matrix for all the auxiliary constraints jacobian associated with this constraint
Matrix body2AngularJacobian; // Angular jacobian matrix of the constraint for body2 // (dimension nx6 where n is the number of auxiliary constraints)
Vector errorVector; // Error term vector of the constraint unsigned int nbAuxConstraints; // Number of auxiliary constraints associated with this constraint
Vector rightHandSideVector; // Right-hand-side vector double lowerBound; // Lower bound of the constraint
Matrix auxiliaryRows; double upperBound; // Upper bound of the constraint
Matrix auxiliaryColumns; Vector auxLowerBounds; // Vector that contains all the lower bounds of the auxiliary constraints
unsigned int jacobianIndex; // Jacobian index Vector auxUpperBounds; // Vector that contains all the upper bounds of the auxiliary constraints
unsigned int auxiliaryIndex; // Auxiliary index
public : public :
Constraint(Body* const body1, Body* const body2); // Constructor Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, double lowerBound, double upperBound,
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
virtual void evaluate(double dt)=0; // Evaluate the constraint virtual void evaluate()=0; // Evaluate the constraint
bool isActive() const; // Return true if the constraint is active 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 Matrix getBody1Jacobian() const; // Return the jacobian matrix of body 1
void setJacobianIndex(unsigned int index); // Set the jacobian index value Matrix getBody2Jacobian() const; // Return the jacobian matrix of body 2
unsigned int getJacobianIndex() const; // Return the jacobian index virtual unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints
Matrix getBody1LinearJacobian() const; // Return the linear jacobian matrix of body 1 void getAuxJacobian(Matrix& auxJacobian) const; // Return the jacobian matrix of auxiliary constraints
Matrix getBody2LinearJacobian() const; // Return the linear jacobian matrix of body 2 double getLowerBound() const; // Return the lower bound value of the constraint
Matrix getBody1AngularJacobian() const; // Return the angular jacobian matrix of body 1 double getUpperBound() const; // Return the upper bound value of the constraint
Matrix getBody2AngularJacobian() const; // Return the angular jacobian matrix of body 2 void getAuxLowerBounds(Vector& auxLowerBounds) const; // Return the vector of lower bounds values
Vector getErrorVector() const; // Return the error vector of the constraint void getAuxUpperBounds(Vector& auxUpperBounds) const; // Return the vector of the upper bounds values
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
}; };
// Return the reference to the body 1 // Return the reference to the body 1
@ -91,63 +90,46 @@ inline bool Constraint::isActive() const {
return active; return active;
} }
// Set the jacobian index value // Return the jacobian matrix of body 1
inline void Constraint::setJacobianIndex(unsigned int index) { inline Matrix Constraint::getBody1Jacobian() const {
this->jacobianIndex = index; return body1Jacobian;
} }
// Return the jacobian index // Return the jacobian matrix of body 2
inline unsigned int Constraint::getJacobianIndex() const { inline Matrix Constraint::getBody2Jacobian() const {
return jacobianIndex; return body2Jacobian;
} }
// Return the linear jacobian matrix of body 1 // Return the number auxiliary constraints
inline Matrix Constraint::getBody1LinearJacobian() const { inline unsigned int Constraint::getNbAuxConstraints() const {
return body1LinearJacobian; return nbAuxConstraints;
} }
// Return the linear jacobian matrix of body 2 // Return the auxiliary jacobian matrix
inline Matrix Constraint::getBody2LinearJacobian() const { inline void Constraint::getAuxJacobian(Matrix& auxJacobian) const {
return body2LinearJacobian; auxJacobian = this->auxJacobian;
} }
// Return the angular jacobian matrix of body 1 // Return the lower bound value of the constraint
inline Matrix Constraint::getBody1AngularJacobian() const { inline double Constraint::getLowerBound() const {
return body1AngularJacobian; return lowerBound;
} }
// Return the angular jacobian matrix of body 2 // Return the upper bound value of the constraint
inline Matrix Constraint::getBody2AngularJacobian() const { inline double Constraint::getUpperBound() const {
return body2AngularJacobian; return upperBound;
} }
// Return the error vector of the constraint // Return the vector of lower bounds values
inline Vector Constraint::getErrorVector() const { inline void Constraint::getAuxLowerBounds(Vector& auxLowerBounds) const {
return errorVector; auxLowerBounds = this->auxLowerBounds;
} }
// Set the auxiliary index // Return the vector of the upper bounds values
inline void Constraint::setAuxiliaryIndex(unsigned int index) { inline void Constraint::getAuxUpperBounds(Vector& auxUpperBounds) const {
auxiliaryIndex = index; 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 } // End of the ReactPhysics3D namespace
#endif #endif

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

@ -28,8 +28,10 @@ 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 std::vector<Vector3D>& points)
:Constraint(body1, body2), normal(normal), penetrationDepth(penetrationDepth), points(points), nbFrictionVectors(NB_FRICTION_VECTORS) { :Constraint(body1, body2, 2, 0, INFINITY_CONST, true), normal(normal), penetrationDepth(penetrationDepth), points(points) {
active = true; body1Jacobian = Matrix(1, 6);
body2Jacobian = Matrix(1, 6);
auxJacobian = Matrix(2, 6);
} }
// Destructor // Destructor
@ -39,75 +41,15 @@ Contact::~Contact() {
// Evaluate the constraint // Evaluate the constraint
// This method computes the jacobian matrices of 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() {
void Contact::evaluate(double dt) {
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1); RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2); RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
assert(frictionVectors.size() == NB_FRICTION_VECTORS);
assert(rigidBody1 != 0); assert(rigidBody1 != 0);
assert(rigidBody2 != 0); assert(rigidBody2 != 0);
// Computation of the linear jacobian for body 1 // Compute the jacobian matrix for the 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());
}
// 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) // TODO : Delete this (Used to debug collision detection)

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

@ -27,16 +27,15 @@
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
// Constants
const unsigned int NB_FRICTION_VECTORS = 4; // Number of vectors used to describe the contact friction
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
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 systems compute 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. response. A contact constraint has two auxiliary constraints in
order two represent the two friction forces at the contact
surface.
------------------------------------------------------------------- -------------------------------------------------------------------
*/ */
class Contact : public Constraint { class Contact : public Constraint {
@ -44,11 +43,9 @@ class Contact : public Constraint {
Vector3D normal; // Normal vector of the contact (From body1 toward body2) Vector3D normal; // Normal vector of the contact (From body1 toward body2)
double penetrationDepth; // Penetration depth double penetrationDepth; // Penetration depth
std::vector<Vector3D> points; // Contact points std::vector<Vector3D> points; // Contact points
unsigned int nbFrictionVectors; // Number of vectors used to describe the friction std::vector<Vector3D> frictionVectors; // Two orthogonal vectors that span the tangential friction plane
// TODO : Implement the computation of the frictionVectors in the collision detection
std::vector<Vector3D> frictionVectors; // Set of 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 : 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 std::vector<Vector3D>& points); // Constructor
@ -56,26 +53,19 @@ class Contact : public Constraint {
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 std::vector<Vector3D> getPoints() const; // Return the contact points
virtual unsigned int getNbJacobianRows() const; // Return the number of rows of the Jacobian matrix virtual void evaluate(); // Evaluate the constraint
virtual void evaluate(double dt); // Evaluate the constraint virtual unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints
virtual int getNbAuxiliaryVars() const; // Return the number of auxiliary variables
void draw() const; // TODO : Delete this (Used to debug collision detection) void draw() const; // TODO : Delete this (Used to debug collision detection)
}; };
// Compute all the friction vectors from two vectors that span the friction cone // Compute the two orthogonal vectors "v1" and "v2" that span the tangential friction plane
// The vectors "vector1" and "vector2" are two vectors that span the friction cone // The two vectors have to be such that : v1 x v2 = contactNormal
inline void Contact::computeFrictionVectors(const Vector3D& vector1, const Vector3D& vector2) { inline void Contact::computeFrictionVectors() {
// Delete the current friction vectors // Delete the current friction vectors
frictionVectors.clear(); frictionVectors.clear();
Vector3D vector; // TODO : Implement this method ...
// 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);
}
} }
// Return the normal vector of the contact // Return the normal vector of the contact
@ -88,16 +78,6 @@ inline std::vector<Vector3D> Contact::getPoints() const {
return points; 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 } // End of the ReactPhysics3D namespace