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

Continue to implement the Sequential Impulse solver

Browse Source
This commit is contained in:
Daniel Chappuis 2013-01-16 13:23:37 +01:00
parent 1bcec415a1
commit 5c941cf88b
2 changed files with 100 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
src/engine/ConstraintSolver.cpp
View File

@ -86,6 +86,7 @@ void ConstraintSolver::initialize() {
constraint.massInverseBody1 = body1->getMassInverse();
constraint.massInverseBody2 = body2->getMassInverse();
constraint.nbContacts = contactManifold.nbContacts;
constraint.restitutionFactor = computeMixRestitutionFactor(body1, body2);
// For each contact point of the contact manifold
for (uint c=0; c<contactManifold.nbContacts; c++) {
@ -104,6 +105,7 @@ void ConstraintSolver::initialize() {
contactPointConstraint.r2 = p2 - x2;
contactPointConstraint.frictionVector1 = contact->getFrictionVector1();
contactPointConstraint.frictionVector2 = contact->getFrictionVector2();
contactPointConstraint.penetrationDepth = contact->getPenetrationDepth();
}
mNbContactConstraints++;
@ -200,6 +202,24 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) {
contact.inverseFriction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2);
}
const Vector3& v1 = mLinearVelocities[constraint.indexBody1];
const Vector3& w1 = mAngularVelocities[constraint.indexBody1];
const Vector3& v2 = mLinearVelocities[constraint.indexBody2];
const Vector3& w2 = mAngularVelocities[constraint.indexBody2];
// Compute the restitution velocity bias "b". We compute this here instead
// of inside the solve() method because we need to use the velocity difference
// at the beginning of the contact. Note that if it is a resting contact (normal velocity
// under a given threshold), we don't add a restitution velocity bias
contact.restitutionBias = 0.0;
Vector3 deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1);
decimal deltaVDotN = deltaV.dot(contact.normal);
// TODO : Use a constant here
decimal elasticLinearVelocityThreshold = 0.3;
if (deltaVDotN < -elasticLinearVelocityThreshold) {
contact.restitutionBias = constraint.restitutionFactor * deltaVDotN;
}
// Fill in the J_sp matrix
realContact->computeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration);
realContact->computeJacobianFriction1(contact.J_spBody1Friction1, contact.J_spBody2Friction1);
@ -382,14 +402,38 @@ void ConstraintSolver::solveLCP() {
indexBody1Array = constraint.indexBody1;
indexBody2Array = constraint.indexBody2;
const Vector3& v1 = mLinearVelocities[constraint.indexBody1];
const Vector3& w1 = mAngularVelocities[constraint.indexBody1];
const Vector3& v2 = mLinearVelocities[constraint.indexBody2];
const Vector3& w2 = mAngularVelocities[constraint.indexBody2];
// --------- Penetration --------- //
// Compute J*v
Vector3 deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1);
decimal deltaVDotN = deltaV.dot(contact.normal);
decimal Jv = deltaVDotN;
// Compute the bias "b" of the constraint
decimal beta = 0.2;
// TODO : Use a constant for the slop
decimal slop = 0.005;
decimal biasPenetrationDepth = 0.0;
if (contact.penetrationDepth > slop) biasPenetrationDepth = -(beta/mTimeStep) *
max(0.0f, float(contact.penetrationDepth - slop));
decimal b = biasPenetrationDepth + contact.restitutionBias;
// Compute the Lagrange multiplier
deltaLambda = - (Jv + b);
/*
deltaLambda = 0.0;
for (uint j=0; j<3; j++) {
deltaLambda -= (contact.J_spBody1Penetration[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j] * mLinearVelocities[indexBody2Array][j]);
deltaLambda -= (contact.J_spBody1Penetration[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * mAngularVelocities[indexBody2Array][j]);
}
deltaLambda -= contact.b_Penetration;
*/
//deltaLambda -= contact.b_Penetration;
deltaLambda /= contact.inversePenetrationMass;
lambdaTemp = contact.penetrationImpulse;
contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration));
@ -537,3 +581,23 @@ void ConstraintSolver::solve(decimal dt) {
// Cache the lambda values in order to use them in the next step
cacheLambda();
}
// Apply an impulse to the two bodies of a constraint
void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstraint& constraint) {
// Update the velocities of the bodies by applying the impulse P
if (constraint.isBody1Moving) {
mLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 *
impulse.linearImpulseBody1;
mAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
}
if (constraint.isBody2Moving) {
mLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 *
impulse.linearImpulseBody2;
mAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
}

51
src/engine/ConstraintSolver.h
View File

@ -39,6 +39,24 @@ namespace reactphysics3d {
// Declarations
class DynamicsWorld;
// Structure Impulse
struct Impulse {
public:
Vector3& linearImpulseBody1;
Vector3& linearImpulseBody2;
Vector3& angularImpulseBody1;
Vector3& angularImpulseBody2;
// Constructor
Impulse(Vector3& linearImpulseBody1, Vector3& angularImpulseBody1,
Vector3& linearImpulseBody2, Vector3& angularImpulseBody2)
: linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1),
linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) {
}
};
// Structure ContactPointConstraint
// Internal structure for a contact point constraint
struct ContactPointConstraint {
@ -137,9 +155,7 @@ class ConstraintSolver {
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
bool isErrorCorrectionActive; // True if error correction (with world order) is active
uint mNbIterations; // Number of iterations of the LCP solver
uint nbIterationsLCPErrorCorrection; // Number of iterations of the LCP solver for error correction
uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints)
uint nbBodies; // Current number of bodies in the physics world
RigidBody* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
@ -151,21 +167,8 @@ class ConstraintSolver {
decimal B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation
// This array contains for each constraint two 6x1 matrices (one for each body of the constraint)
// a 6x1 matrix
decimal J_spError[NB_MAX_CONSTRAINTS][2*6]; // Same as J_sp for error correction projection (only contact constraints)
decimal B_spError[2][6*NB_MAX_CONSTRAINTS]; // Same as B_sp for error correction projection (only contact constraints)
decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem
decimal bError[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem for error correction projection
decimal d[NB_MAX_CONSTRAINTS]; // Vector "d"
decimal aError[6*NB_MAX_BODIES]; // Vector "a" for error correction projection
decimal penetrationDepths[NB_MAX_CONSTRAINTS]; // Array of penetration depths for error correction projection
decimal lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem
decimal lambdaError[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem for error correction projections
decimal lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver
decimal errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints
decimal lowerBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the low limits for the variables of the LCP problem
decimal upperBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the high limits for the variables of the LCP problem
decimal lowerBoundsError[NB_MAX_CONSTRAINTS]; // Same as "lowerBounds" but for error correction projections
decimal upperBoundsError[NB_MAX_CONSTRAINTS]; // Same as "upperBounds" but for error correction projections
Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass
// This is an array of size nbBodies that contains in each cell a 6x6 matrix
@ -193,7 +196,13 @@ class ConstraintSolver {
void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
void warmStart(); // Compute the vector a used in the solve() method
void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
// Apply an impulse to the two bodies of a constraint
void applyImpulse(const Impulse& impulse, const ContactConstraint& constraint);
// Compute the collision restitution factor from the restitution factor of each body
decimal computeMixRestitutionFactor(const RigidBody *body1, const RigidBody *body2) const;
public:
ConstraintSolver(DynamicsWorld* world); // Constructor
virtual ~ConstraintSolver(); // Destructor
@ -249,6 +258,16 @@ inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
mNbIterations = nbIterations;
}
// Compute the collision restitution factor from the restitution factor of each body
inline decimal ConstraintSolver::computeMixRestitutionFactor(const RigidBody* body1,
const RigidBody* body2) const {
decimal restitution1 = body1->getRestitution();
decimal restitution2 = body2->getRestitution();
// Return the largest restitution factor
return (restitution1 > restitution2) ? restitution1 : restitution2;
}
} // End of ReactPhysics3D namespace
#endif