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); } } } }