git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@290 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
a17db9ec67
commit
3eb315cb0c
|
|
@ -24,8 +24,9 @@
|
|||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
Constraint::Constraint(Body* const body1, Body* const body2)
|
||||
:body1(body1), body2(body2), active(true) {
|
||||
Constraint::Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, bool active)
|
||||
:body1(body1), body2(body2), active(true), nbAuxConstraints(nbAuxConstraints),
|
||||
lowerBound(lowerBound), upperBound(upperBound), active(active) {
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -27,53 +27,52 @@
|
|||
// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
// Constants
|
||||
const double kErp = 0.8; // Value between 0 and 1 used to compute the kCorrection value (kCorrection = kErp * kFps)
|
||||
|
||||
/* -------------------------------------------------------------------
|
||||
Class Constraint :
|
||||
This abstract class represents a constraint in the physics engine.
|
||||
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 {
|
||||
protected :
|
||||
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
|
||||
Matrix body1LinearJacobian; // Linear jacobian matrix of the constraint for body1
|
||||
Matrix body2LinearJacobian; // Linear jacobian matrix of the constraint for body2
|
||||
Matrix body1AngularJacobian; // Angular jacobian matrix of the constraint for body1
|
||||
Matrix body2AngularJacobian; // Angular jacobian matrix of the constraint for body2
|
||||
Vector errorVector; // Error term vector of the constraint
|
||||
Vector rightHandSideVector; // Right-hand-side vector
|
||||
Matrix auxiliaryRows;
|
||||
Matrix auxiliaryColumns;
|
||||
unsigned int jacobianIndex; // Jacobian index
|
||||
unsigned int auxiliaryIndex; // Auxiliary index
|
||||
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
|
||||
Matrix body1Jacobian; // Jacobian matrix of the constraint for body1 (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
|
||||
// (dimension nx6 where n is the number of auxiliary constraints)
|
||||
unsigned int nbAuxConstraints; // Number of auxiliary constraints associated with this constraint
|
||||
double lowerBound; // Lower 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 auxUpperBounds; // Vector that contains all the upper bounds of the auxiliary constraints
|
||||
|
||||
public :
|
||||
Constraint(Body* const body1, Body* const body2); // 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
|
||||
virtual void evaluate(double dt)=0; // Evaluate the constraint
|
||||
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
|
||||
void setJacobianIndex(unsigned int index); // Set the jacobian index value
|
||||
unsigned int getJacobianIndex() const; // Return the jacobian index
|
||||
Matrix getBody1LinearJacobian() const; // Return the linear jacobian matrix of body 1
|
||||
Matrix getBody2LinearJacobian() const; // Return the linear jacobian matrix of body 2
|
||||
Matrix getBody1AngularJacobian() const; // Return the angular jacobian matrix of body 1
|
||||
Matrix getBody2AngularJacobian() const; // Return the angular jacobian matrix of body 2
|
||||
Vector getErrorVector() const; // Return the error vector of the constraint
|
||||
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
|
||||
Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, double lowerBound, double upperBound,
|
||||
bool active); // 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
|
||||
virtual void evaluate()=0; // Evaluate the constraint
|
||||
bool isActive() const; // Return true if the constraint is active
|
||||
Matrix getBody1Jacobian() const; // Return the jacobian matrix of body 1
|
||||
Matrix getBody2Jacobian() const; // Return the jacobian matrix of body 2
|
||||
virtual unsigned int getNbAuxConstraints() const; // Return the number 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 getUpperBound() const; // Return the upper bound value of the constraint
|
||||
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
|
||||
};
|
||||
|
||||
// Return the reference to the body 1
|
||||
|
|
@ -91,63 +90,46 @@ inline bool Constraint::isActive() const {
|
|||
return active;
|
||||
}
|
||||
|
||||
// Set the jacobian index value
|
||||
inline void Constraint::setJacobianIndex(unsigned int index) {
|
||||
this->jacobianIndex = index;
|
||||
// Return the jacobian matrix of body 1
|
||||
inline Matrix Constraint::getBody1Jacobian() const {
|
||||
return body1Jacobian;
|
||||
}
|
||||
|
||||
// Return the jacobian index
|
||||
inline unsigned int Constraint::getJacobianIndex() const {
|
||||
return jacobianIndex;
|
||||
// Return the jacobian matrix of body 2
|
||||
inline Matrix Constraint::getBody2Jacobian() const {
|
||||
return body2Jacobian;
|
||||
}
|
||||
|
||||
// Return the linear jacobian matrix of body 1
|
||||
inline Matrix Constraint::getBody1LinearJacobian() const {
|
||||
return body1LinearJacobian;
|
||||
// Return the number auxiliary constraints
|
||||
inline unsigned int Constraint::getNbAuxConstraints() const {
|
||||
return nbAuxConstraints;
|
||||
}
|
||||
|
||||
// Return the linear jacobian matrix of body 2
|
||||
inline Matrix Constraint::getBody2LinearJacobian() const {
|
||||
return body2LinearJacobian;
|
||||
// Return the auxiliary jacobian matrix
|
||||
inline void Constraint::getAuxJacobian(Matrix& auxJacobian) const {
|
||||
auxJacobian = this->auxJacobian;
|
||||
}
|
||||
|
||||
// Return the angular jacobian matrix of body 1
|
||||
inline Matrix Constraint::getBody1AngularJacobian() const {
|
||||
return body1AngularJacobian;
|
||||
// Return the lower bound value of the constraint
|
||||
inline double Constraint::getLowerBound() const {
|
||||
return lowerBound;
|
||||
}
|
||||
|
||||
// Return the angular jacobian matrix of body 2
|
||||
inline Matrix Constraint::getBody2AngularJacobian() const {
|
||||
return body2AngularJacobian;
|
||||
// Return the upper bound value of the constraint
|
||||
inline double Constraint::getUpperBound() const {
|
||||
return upperBound;
|
||||
}
|
||||
|
||||
// Return the error vector of the constraint
|
||||
inline Vector Constraint::getErrorVector() const {
|
||||
return errorVector;
|
||||
// Return the vector of lower bounds values
|
||||
inline void Constraint::getAuxLowerBounds(Vector& auxLowerBounds) const {
|
||||
auxLowerBounds = this->auxLowerBounds;
|
||||
}
|
||||
|
||||
// Set the auxiliary index
|
||||
inline void Constraint::setAuxiliaryIndex(unsigned int index) {
|
||||
auxiliaryIndex = index;
|
||||
// Return the vector of the upper bounds values
|
||||
inline void Constraint::getAuxUpperBounds(Vector& auxUpperBounds) const {
|
||||
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
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -28,8 +28,10 @@ using namespace reactphysics3d;
|
|||
|
||||
// Constructor
|
||||
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) {
|
||||
active = true;
|
||||
:Constraint(body1, body2, 2, 0, INFINITY_CONST, true), normal(normal), penetrationDepth(penetrationDepth), points(points) {
|
||||
body1Jacobian = Matrix(1, 6);
|
||||
body2Jacobian = Matrix(1, 6);
|
||||
auxJacobian = Matrix(2, 6);
|
||||
}
|
||||
|
||||
// Destructor
|
||||
|
|
@ -39,75 +41,15 @@ Contact::~Contact() {
|
|||
|
||||
// Evaluate 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(double dt) {
|
||||
void Contact::evaluate() {
|
||||
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
|
||||
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
|
||||
|
||||
assert(frictionVectors.size() == NB_FRICTION_VECTORS);
|
||||
assert(rigidBody1 != 0);
|
||||
assert(rigidBody2 != 0);
|
||||
|
||||
// Computation of the linear jacobian for 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());
|
||||
}
|
||||
// Compute the jacobian matrix for the body 1
|
||||
|
||||
// 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)
|
||||
|
|
|
|||
|
|
@ -27,16 +27,15 @@
|
|||
// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
// Constants
|
||||
const unsigned int NB_FRICTION_VECTORS = 4; // Number of vectors used to describe the contact friction
|
||||
|
||||
/* -------------------------------------------------------------------
|
||||
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 systems compute
|
||||
Constraint class. The collision detection system computes
|
||||
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 {
|
||||
|
|
@ -44,11 +43,9 @@ class Contact : public Constraint {
|
|||
Vector3D normal; // Normal vector of the contact (From body1 toward body2)
|
||||
double penetrationDepth; // Penetration depth
|
||||
std::vector<Vector3D> points; // Contact points
|
||||
unsigned int nbFrictionVectors; // Number of vectors used to describe the friction
|
||||
// TODO : Implement the computation of the frictionVectors in the collision detection
|
||||
std::vector<Vector3D> frictionVectors; // Set of vectors that span the tangential friction plane
|
||||
std::vector<Vector3D> frictionVectors; // Two orthogonal 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 :
|
||||
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
|
||||
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(double dt); // Evaluate the constraint
|
||||
virtual int getNbAuxiliaryVars() const; // Return the number of auxiliary variables
|
||||
virtual void evaluate(); // Evaluate the constraint
|
||||
virtual unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints
|
||||
|
||||
void draw() const; // TODO : Delete this (Used to debug collision detection)
|
||||
};
|
||||
|
||||
// Compute all the friction vectors from two vectors that span the friction cone
|
||||
// The vectors "vector1" and "vector2" are two vectors that span the friction cone
|
||||
inline void Contact::computeFrictionVectors(const Vector3D& vector1, const Vector3D& vector2) {
|
||||
// Compute the two 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();
|
||||
|
||||
Vector3D vector;
|
||||
|
||||
// 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);
|
||||
}
|
||||
// TODO : Implement this method ...
|
||||
}
|
||||
|
||||
// Return the normal vector of the contact
|
||||
|
|
@ -88,16 +78,6 @@ inline std::vector<Vector3D> Contact::getPoints() const {
|
|||
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
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user