So I had the same problem when I first started implementing my own physics engine. I was using Chris Hecker's GDM article and it's a great start, but it produces jiggly results.
The more advanced (and more accurate) way to do it is to use a constraint solver. You're probably thinking "I don't need constraints, I'm not doing joints/rag-doll yet", but yeah it turns out you can have a constraint called a "contact constraint" that does collision response for you. You need to write a contact point refreshing engine and then you have to create constraints from your contacts. Then you solve your contact constraints. I will try to help you with what I wrote for my own engine:
Code: Select all
void MEHPhysicsRigidBody::UpdateContactPoints()
{
scalar contactBreakingThreshold = BASE_CONTACT_BREAKING_THRESHOLD * MEHMAX(MINIMUM_CONTACT_RADIUS, GetMaxRadius());
for (s32 i = 0; i < (s32)m_contactPoints.GetSize(); i++)
{
if (m_contactPoints[i].penetrationDist >= contactBreakingThreshold)
{
RemoveManifoldPoint(i);
i--;
}
else
{
MEHVector3 projPt = m_contactPoints[i].transOtherNormal;
projPt *= -m_contactPoints[i].penetrationDist;
projPt += m_contactPoints[i].transPoint;
MEHVector3 projDiff = m_contactPoints[i].transOtherPoint;
projDiff -= projPt;
if (projDiff.Magnitude2() >= contactBreakingThreshold*contactBreakingThreshold)
{
RemoveManifoldPoint(i);
i--;
}
}
}
}
This is my code for refreshing contact points. You need to call it once per frame for overlapping/non-sleeping objects. It is essentially derived from Bullet's methodology. Then you need a constraint solver. Something like:
Code: Select all
void MEHConstraint::ToNormal(const MEHVector3 &radius1, const MEHVector3 &radius2, const MEHVector3 &normal,
MEHPhysicsRigidBody *bodyA, MEHPhysicsRigidBody *bodyB, scalar penetrationVelocity,
scalar erp, scalar cfm)
{
m_jacobiVectors[0] = normal;
m_jacobiVectors[1] = radius1;
m_jacobiVectors[1].ToCrossProductWith(normal);
m_jacobiVectors[2] = normal;
m_jacobiVectors[2] *= (scalar)-1.0;
m_jacobiVectors[3] = normal;
m_jacobiVectors[3].ToCrossProductWith(radius2);
scalar num1 = 0.0;
num1 = m_jacobiVectors[0].DotProductWith(bodyA->GetVelocity()) + m_jacobiVectors[1].DotProductWith(bodyA->GetAngularVelocity());
scalar num2 = 0.0;
if (bodyB && bodyB->GetMass() != MEHMAXSCALAR)
num2 = m_jacobiVectors[2].DotProductWith(bodyB->GetVelocity()) + m_jacobiVectors[3].DotProductWith(bodyB->GetAngularVelocity());
scalar numerator = (scalar)(-1.0 + 0.0)*(num1 + num2) - (penetrationVelocity > 0.0 ? penetrationVelocity : penetrationVelocity * erp);
m_constraint = numerator;
m_erp = 1.0;
m_cfm = cfm;
m_calcDamping = false;
m_isContact = true;
m_lowerBound = 0.0;
m_upperBound = (scalar)1e10;
}
erp and cfm are old variables introduced into the physics community by the open source ODE engine. There's a way to express them in terms of spring constant and damp ratio, that is one of Erin's papers. But for contact constraints I use an "erp" of 0.2 and a cfm of 0.0. The m_erp = 1.0 that you see is to make sure I don't do ERP twice for contact constraints. The thing with ERP for contact constraints is that you want to affect "position error" but not "velocity error" (I think Bullet is like this too). By error we're talking about how much we need to move and how much we need to speed up to resolve a collision. The movement (the amount of overlap) is divided by the time-step to make it a velocity and then you can just add it to your velocity and make it all a velocity error. That's essentially what this function is doing.
Finally, you need a constraint solver. Something like:
Code: Select all
void MEHJacobian::SolveSingleIteration(scalar elapsedSecs)
{
if (m_numConstraints == 0)
return;
if (elapsedSecs == 0.0)
return;
// the equation we want to solve is:
// 1/dt * c - J*(v/dt + M^-1F) = JM^-1J^T * lambda
// where:
// dt = elapsedSecs
// c = m_constraint from the constraint data structure (this is a motor or error values)
// J = a Jacobian row (the recipe for the constraint dynamic)
// v = either <v_{m_owner}, v_{bodyB}> or <w_{m_owner}, w_{bodyB}> depending on linear or angular constraint
// M = the masses matrix
// lambda = impulse vector
// we need to compute everything but lambda and then get it into Ax = b form to solve via Projected Gauss-Seidel where x is lambda
scalar b[MAX_CONSTRAINTS_PER_JACOBIAN];
u32 j;
scalar oneOverDT = (scalar)1.0 / elapsedSecs;
MEHVector3 linearDeltaAccelA = MEHVector3::Zero;
MEHVector3 angularDeltaAccelA = MEHVector3::Zero;
if (!m_bodyA->IsSleeping())
{
linearDeltaAccelA = m_bodyA->GetDeltaLinearVelocityFromSolver();
linearDeltaAccelA *= oneOverDT;
angularDeltaAccelA = m_bodyA->GetDeltaAngularVelocityFromSolver();
angularDeltaAccelA *= oneOverDT;
}
MEHVector3 linearDeltaAccelB = MEHVector3::Zero;
MEHVector3 angularDeltaAccelB = MEHVector3::Zero;
if (m_bodyB && !m_bodyB->IsSleeping())
{
linearDeltaAccelB = m_bodyB->GetDeltaLinearVelocityFromSolver();
linearDeltaAccelB *= oneOverDT;
angularDeltaAccelB = m_bodyB->GetDeltaAngularVelocityFromSolver();
angularDeltaAccelB *= oneOverDT;
}
for (u32 i = 0; i < m_numConstraints; i++)
{
scalar aDP, bDP;
aDP = m_constraints[i][0].DotProductWith(linearDeltaAccelA) + m_constraints[i][1].DotProductWith(angularDeltaAccelA);
if (m_bodyB)
bDP = m_constraints[i][2].DotProductWith(linearDeltaAccelB) + m_constraints[i][3].DotProductWith(angularDeltaAccelB);
else
bDP = 0.0;
b[i] = m_C[i] - (aDP + bDP);
}
// now we solve for x (lambda) ((impulses)) using Projected Gauss-Seidel (basically Gauss-Seidel with a clamp)
for (u32 i = 0; i < m_numConstraints; i++)
{
scalar AxSum = 0.0;
for (j = 0; j < 3; j++)
{
AxSum += m_constraints[i][0][j] * m_a[j];
AxSum += m_constraints[i][1][j] * m_a[j+3];
AxSum += m_constraints[i][2][j] * m_a[j+6];
AxSum += m_constraints[i][3][j] * m_a[j+9];
}
scalar rowDiff = b[i] - AxSum;
scalar deltaLambda;
if (MEHISZERO(m_d[i]))
deltaLambda = 0.0;
else
deltaLambda = rowDiff / m_d[i];
m_lastImpulses[i] = m_lambda[i];
m_lambda[i] = m_lastImpulses[i] + deltaLambda;
if (m_lambda[i] < m_constraints[i].GetLowerBound())
m_lambda[i] = m_constraints[i].GetLowerBound();
else if (m_lambda[i] > m_constraints[i].GetUpperBound())
m_lambda[i] = m_constraints[i].GetUpperBound();
deltaLambda = m_lambda[i] - m_lastImpulses[i];
//MEHASSERT(m_lastImpulses[i] == 0.0 || MEHFABS(deltaLambda) < 100.0);
for (j = 0; j < 12; j++)
m_a[j] += m_B[j][i] * deltaLambda;
}
// we need to solve for v2 in M(v2 - v1) = dt*(J^T*lambda + Fext)
// we have v2 = dt*(M^-1J^T*lambda) + M^-1*Fext*dt + v1
MEHVector3 newLinearVelocityA;
MEHVector3 newLinearVelocityB;
MEHVector3 newAngularVelocityA;
MEHVector3 newAngularVelocityB;
MEHVector3 linearJTlambdaA = MEHVector3::Zero;
MEHVector3 linearJTlambdaB = MEHVector3::Zero;
MEHVector3 angularJTlambdaA = MEHVector3::Zero;
MEHVector3 angularJTlambdaB = MEHVector3::Zero;
for (j = 0; j < m_numConstraints; j++)
{
for (u32 i = 0; i < 3; i++)
{
linearJTlambdaA[i] += m_constraints[j][0][i] * m_lambda[j];
angularJTlambdaA[i] += m_constraints[j][1][i] * m_lambda[j];
linearJTlambdaB[i] += m_constraints[j][2][i] * m_lambda[j];
angularJTlambdaB[i] += m_constraints[j][3][i] * m_lambda[j];
}
m_aggregateImpulses[j] = m_lambda[j];
}
newLinearVelocityA = linearJTlambdaA;
newLinearVelocityA *= elapsedSecs;
newLinearVelocityA /= m_bodyA->GetMass();
m_bodyA->GetDeltaLinearVelocityFromSolver() += newLinearVelocityA;
newAngularVelocityA = angularJTlambdaA;
newAngularVelocityA *= elapsedSecs;
newAngularVelocityA *= m_bodyA->GetInverseInertiaTensor();
MEHASSERT(newAngularVelocityA.Magnitude() < 10000);
m_bodyA->GetDeltaAngularVelocityFromSolver() += newAngularVelocityA;
if (m_bodyB)
{
newLinearVelocityB = linearJTlambdaB;
newLinearVelocityB *= elapsedSecs;
newLinearVelocityB /= m_bodyB->GetMass();
m_bodyB->GetDeltaLinearVelocityFromSolver() += newLinearVelocityB;
newAngularVelocityB = angularJTlambdaB;
newAngularVelocityB *= elapsedSecs;
newAngularVelocityB *= m_bodyB->GetInverseInertiaTensor();
MEHASSERT(newAngularVelocityB.Magnitude() < 10000);
m_bodyB->GetDeltaAngularVelocityFromSolver() += newAngularVelocityB;
}
}
The naming is not entirely accurate because a Jacobian is not for solving--an equation is, lol. But anyway, it's tempting to do something in a for loop for solving a positive definite matrix using PGS. But I quickly found out that's not optimal either. You want ACCUMULATE the deltas and iterate through ALL your physics objects once each for each iteration of solving PGS. This is the true strength of an iterative method to find "happy ground" between a large amount of collisions. So it's still a for loop, but it's a for loop that goes through each physics object once for each iteration of the algorithm.
If you do something like that, it should look a lot better, given that your collision engine is good.
Let me know if you have any questions.
Mike