Implement the split impulse technique for position correction
This commit is contained in:
Daniel Chappuis
parent
d546d8208f
commit
8cde68f5b9
|
|
@ -31,11 +31,11 @@
|
|||
using namespace reactphysics3d;
|
||||
using namespace std;
|
||||
|
||||
|
||||
// Constructor
|
||||
ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
|
||||
:world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0),
|
||||
mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(false) {
|
||||
mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true),
|
||||
mIsSplitImpulseActive(true) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -118,6 +118,8 @@ void ConstraintSolver::initialize() {
|
|||
|
||||
mLinearVelocities = new Vector3[nbBodies];
|
||||
mAngularVelocities = new Vector3[nbBodies];
|
||||
mSplitLinearVelocities = new Vector3[nbBodies];
|
||||
mSplitAngularVelocities = new Vector3[nbBodies];
|
||||
|
||||
assert(mMapBodyToIndex.size() == nbBodies);
|
||||
}
|
||||
|
|
@ -136,6 +138,8 @@ void ConstraintSolver::initializeBodies() {
|
|||
|
||||
mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce();
|
||||
mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
||||
mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0);
|
||||
mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -204,7 +208,7 @@ void ConstraintSolver::initializeContactConstraints() {
|
|||
contact.restitutionBias = 0.0;
|
||||
decimal deltaVDotN = deltaV.dot(contact.normal);
|
||||
// TODO : Use a constant here
|
||||
if (!contact.isRestingContact) {
|
||||
if (deltaVDotN < 1.0f) {
|
||||
contact.restitutionBias = constraint.restitutionFactor * deltaVDotN;
|
||||
}
|
||||
|
||||
|
|
@ -212,6 +216,9 @@ void ConstraintSolver::initializeContactConstraints() {
|
|||
contact.penetrationImpulse = realContact->getCachedLambda(0);
|
||||
contact.friction1Impulse = realContact->getCachedLambda(1);
|
||||
contact.friction2Impulse = realContact->getCachedLambda(2);
|
||||
|
||||
// Initialize the split impulses to zero
|
||||
contact.penetrationSplitImpulse = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -323,17 +330,19 @@ void ConstraintSolver::solveContactConstraints() {
|
|||
// Compute the bias "b" of the constraint
|
||||
decimal beta = 0.2;
|
||||
// TODO : Use a constant for the slop
|
||||
decimal slop = 0.005;
|
||||
decimal slop = 0.01;
|
||||
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 -= contact.b_Penetration;
|
||||
deltaLambda *= contact.inversePenetrationMass;
|
||||
if (mIsSplitImpulseActive) {
|
||||
deltaLambda = - (Jv + contact.restitutionBias) * contact.inversePenetrationMass;
|
||||
}
|
||||
else {
|
||||
deltaLambda = - (Jv + b) * contact.inversePenetrationMass;
|
||||
}
|
||||
lambdaTemp = contact.penetrationImpulse;
|
||||
contact.penetrationImpulse = std::max(contact.penetrationImpulse + deltaLambda, 0.0f);
|
||||
deltaLambda = contact.penetrationImpulse - lambdaTemp;
|
||||
|
|
@ -349,6 +358,32 @@ void ConstraintSolver::solveContactConstraints() {
|
|||
// Apply the impulse to the bodies of the constraint
|
||||
applyImpulse(impulsePenetration, constraint);
|
||||
|
||||
// If the split impulse position correction is active
|
||||
if (mIsSplitImpulseActive) {
|
||||
|
||||
// Split impulse (position correction)
|
||||
const Vector3& v1Split = mSplitLinearVelocities[constraint.indexBody1];
|
||||
const Vector3& w1Split = mSplitAngularVelocities[constraint.indexBody1];
|
||||
const Vector3& v2Split = mSplitLinearVelocities[constraint.indexBody2];
|
||||
const Vector3& w2Split = mSplitAngularVelocities[constraint.indexBody2];
|
||||
Vector3 deltaVSplit = v2Split + w2Split.cross(contact.r2) - v1Split - w1Split.cross(contact.r1);
|
||||
decimal JvSplit = deltaVSplit.dot(contact.normal);
|
||||
decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contact.inversePenetrationMass;
|
||||
decimal lambdaTempSplit = contact.penetrationSplitImpulse;
|
||||
contact.penetrationSplitImpulse = std::max(contact.penetrationSplitImpulse + deltaLambdaSplit, 0.0f);
|
||||
deltaLambda = contact.penetrationSplitImpulse - lambdaTempSplit;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
linearImpulseBody1 = -contact.normal * deltaLambdaSplit;
|
||||
angularImpulseBody1 = -contact.r1CrossN * deltaLambdaSplit;
|
||||
linearImpulseBody2 = contact.normal * deltaLambdaSplit;
|
||||
angularImpulseBody2 = contact.r2CrossN * deltaLambdaSplit;
|
||||
const Impulse splitImpulsePenetration(linearImpulseBody1, angularImpulseBody1,
|
||||
linearImpulseBody2, angularImpulseBody2);
|
||||
|
||||
applySplitImpulse(splitImpulsePenetration, constraint);
|
||||
}
|
||||
|
||||
// --------- Friction 1 --------- //
|
||||
|
||||
// Compute J*v
|
||||
|
|
@ -467,6 +502,24 @@ void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstra
|
|||
}
|
||||
}
|
||||
|
||||
// Apply an impulse to the two bodies of a constraint
|
||||
void ConstraintSolver::applySplitImpulse(const Impulse& impulse, const ContactConstraint& constraint) {
|
||||
|
||||
// Update the velocities of the bodies by applying the impulse P
|
||||
if (constraint.isBody1Moving) {
|
||||
mSplitLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 *
|
||||
impulse.linearImpulseBody1;
|
||||
mSplitAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 *
|
||||
impulse.angularImpulseBody1;
|
||||
}
|
||||
if (constraint.isBody2Moving) {
|
||||
mSplitLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 *
|
||||
impulse.linearImpulseBody2;
|
||||
mSplitAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 *
|
||||
impulse.angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
|
||||
// The two vectors have to be such that : t1 x t2 = contactNormal
|
||||
void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity,
|
||||
|
|
|
|||
|
|
@ -64,6 +64,7 @@ struct ContactPointConstraint {
|
|||
decimal penetrationImpulse; // Accumulated normal impulse
|
||||
decimal friction1Impulse; // Accumulated impulse in the 1st friction direction
|
||||
decimal friction2Impulse; // Accumulated impulse in the 2nd friction direction
|
||||
decimal penetrationSplitImpulse; // Accumulated split impulse for penetration correction
|
||||
Vector3 normal; // Normal vector of the contact
|
||||
Vector3 frictionVector1; // First friction vector in the tangent plane
|
||||
Vector3 frictionVector2; // Second friction vector in the tangent plane
|
||||
|
|
@ -146,6 +147,13 @@ class ConstraintSolver {
|
|||
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
|
||||
Vector3* mLinearVelocities; // Array of constrained linear velocities
|
||||
Vector3* mAngularVelocities; // Array of constrained angular velocities
|
||||
|
||||
// Split linear velocities for the position contact solver (split impulse)
|
||||
Vector3* mSplitLinearVelocities;
|
||||
|
||||
// Split angular velocities for the position contact solver (split impulse)
|
||||
Vector3* mSplitAngularVelocities;
|
||||
|
||||
decimal mTimeStep; // Current time step
|
||||
|
||||
// Contact constraints
|
||||
|
|
@ -163,6 +171,9 @@ class ConstraintSolver {
|
|||
// True if the warm starting of the solver is active
|
||||
bool mIsWarmStartingActive;
|
||||
|
||||
// True if the split impulse position correction is active
|
||||
bool mIsSplitImpulseActive;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
// Initialize the constraint solver
|
||||
|
|
@ -187,6 +198,9 @@ class ConstraintSolver {
|
|||
// Apply an impulse to the two bodies of a constraint
|
||||
void applyImpulse(const Impulse& impulse, const ContactConstraint& constraint);
|
||||
|
||||
// Apply an impulse to the two bodies of a constraint
|
||||
void applySplitImpulse(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;
|
||||
|
||||
|
|
@ -214,9 +228,15 @@ class ConstraintSolver {
|
|||
// Return the constrained linear velocity of a body after solving the constraints
|
||||
Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body);
|
||||
|
||||
// Return the split linear velocity
|
||||
Vector3 getSplitLinearVelocityOfBody(RigidBody* body);
|
||||
|
||||
// Return the constrained angular velocity of a body after solving the constraints
|
||||
Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body);
|
||||
|
||||
// Return the split angular velocity
|
||||
Vector3 getSplitAngularVelocityOfBody(RigidBody* body);
|
||||
|
||||
// Clean up the constraint solver
|
||||
void cleanup();
|
||||
|
||||
|
|
@ -232,15 +252,29 @@ inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
|
|||
// Return the constrained linear velocity of a body after solving the constraints
|
||||
inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBodyArray = mMapBodyToIndex[body];
|
||||
return mLinearVelocities[indexBodyArray];
|
||||
uint indexBody = mMapBodyToIndex[body];
|
||||
return mLinearVelocities[indexBody];
|
||||
}
|
||||
|
||||
// Return the split linear velocity
|
||||
inline Vector3 ConstraintSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBody = mMapBodyToIndex[body];
|
||||
return mSplitLinearVelocities[indexBody];
|
||||
}
|
||||
|
||||
// Return the constrained angular velocity of a body after solving the constraints
|
||||
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBodyArray = mMapBodyToIndex[body];
|
||||
return mAngularVelocities[indexBodyArray];
|
||||
uint indexBody = mMapBodyToIndex[body];
|
||||
return mAngularVelocities[indexBody];
|
||||
}
|
||||
|
||||
// Return the split angular velocity
|
||||
inline Vector3 ConstraintSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBody = mMapBodyToIndex[body];
|
||||
return mSplitAngularVelocities[indexBody];
|
||||
}
|
||||
|
||||
// Clean up the constraint solver
|
||||
|
|
|
|||
|
|
@ -120,8 +120,8 @@ void DynamicsWorld::updateAllBodiesMotion() {
|
|||
// If it's a constrained body
|
||||
if (mConstraintSolver.isConstrainedBody(*it)) {
|
||||
// Get the constrained linear and angular velocities from the constraint solver
|
||||
newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it);
|
||||
newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it);
|
||||
newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(rigidBody);
|
||||
newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(rigidBody);
|
||||
}
|
||||
else {
|
||||
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
|
||||
|
|
@ -147,8 +147,8 @@ void DynamicsWorld::updateAllBodiesMotion() {
|
|||
// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
|
||||
// orientation of the body
|
||||
void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody,
|
||||
const Vector3& newLinVelocity,
|
||||
const Vector3& newAngVelocity) {
|
||||
Vector3 newLinVelocity,
|
||||
Vector3 newAngVelocity) {
|
||||
decimal dt = mTimer.getTimeStep();
|
||||
|
||||
assert(rigidBody);
|
||||
|
|
@ -159,6 +159,12 @@ void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody,
|
|||
// Update the linear and angular velocity of the body
|
||||
rigidBody->setLinearVelocity(newLinVelocity);
|
||||
rigidBody->setAngularVelocity(newAngVelocity);
|
||||
|
||||
// Split velocity (only used to update the position)
|
||||
if (mConstraintSolver.isConstrainedBody(rigidBody)) {
|
||||
newLinVelocity += mConstraintSolver.getSplitLinearVelocityOfBody(rigidBody);
|
||||
newAngVelocity += mConstraintSolver.getSplitAngularVelocityOfBody(rigidBody);
|
||||
}
|
||||
|
||||
// Get current position and orientation of the body
|
||||
const Vector3& currentPosition = rigidBody->getTransform().getPosition();
|
||||
|
|
|
|||
|
|
@ -96,8 +96,8 @@ class DynamicsWorld : public CollisionWorld {
|
|||
void updateAllBodiesMotion();
|
||||
|
||||
// Update the position and orientation of a body
|
||||
void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity,
|
||||
const Vector3& newAngVelocity);
|
||||
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
|
||||
Vector3 newAngVelocity);
|
||||
|
||||
// Compute and set the interpolation factor to all bodies
|
||||
void setInterpolationFactorToAllBodies();
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user