void CollisionSolver::step(const Vector<ContactManifold*>& manifolds)
{
for (int h = -1; ++h < manifolds.size();)
{
ContactManifold* manifold = manifolds[h];
CollisionObject* a = manifold->getA();
CollisionObject* b = manifold->getB();
RigidBody* ra = a->getBody();
RigidBody* rb = b->getBody();
for (int i = -1; ++i < manifold->numContacts();)
{
Contact *c = manifold->getContact(i);
if (!c) continue;
Vector3 vela = ra ? ra->getVelocity(c->mRcp1) : Vector3::ZERO;
Vector3 velb = rb ? rb->getVelocity(c->mRcp2) : Vector3::ZERO;
Vector3 rvel = velb - vela;
float veln = rvel.dot(c->mNormal);
float deltaImpulse = c->mMassNormal * -veln;
float pn0 = c->mPN;
c->mPN = Math::max(pn0 + deltaImpulse, c->mDV);
deltaImpulse = c->mPN - pn0;
Vector3 impulse = c->mNormal * deltaImpulse;
if (ra) ra->applyImpulse(-impulse, c->mRcp1);
if (rb) rb->applyImpulse(impulse, c->mRcp2);
vela = ra ? ra->getBiasVelocity(c->mRcp1) : Vector3::ZERO;
velb = rb ? rb->getBiasVelocity(c->mRcp2) : Vector3::ZERO;
rvel = velb - vela;
float velnb = rvel.dot(c->mNormal);
float dPnb = c->mMassNormal * (-velnb + c->mBias);
float pnb0 = c->mPNB;
c->mPNB = Math::max(pnb0 + dPnb, c->mDV);
dPnb = c->mPNB - pnb0;
Vector3 impulseb = c->mNormal * dPnb;
if (ra) ra->applyBiasImpulse(-impulseb, c->mRcp1);
if (rb) rb->applyBiasImpulse(impulseb, c->mRcp2);
if (c->mTangent != Vector3::ZERO)
{
vela = ra ? ra->getVelocity(c->mRcp1) : Vector3::ZERO;
velb = rb ? rb->getVelocity(c->mRcp2) : Vector3::ZERO;
rvel = velb - vela;
float velt = c->mTangent.dot(rvel);
float deltaImpulseTangent = c->mMassTangent * -velt;
float fa = ra ? ra->getFriction() : 0.0f;
float fb = rb ? rb->getFriction() : 0.0f;
float maxPt = Math::max(fa, fb) * c->mPN;
float oldTangentImpulse = c->mPT;
c->mPT = Math::clamp(oldTangentImpulse + deltaImpulseTangent, -maxPt, maxPt);
deltaImpulseTangent = c->mPT - oldTangentImpulse;
Vector3 frictionImpulse = c->mTangent * deltaImpulseTangent;
if (ra) ra->applyImpulse(-frictionImpulse, c->mRcp1);
if (rb) rb->applyImpulse(frictionImpulse, c->mRcp2);
}
}
}
}
