Main Physics Update
- Update velocity (add external forces, eg. gravity)
- Predict position for all bodies (temporarily update position)
- Collision detection
- Collision resolution
- Restore positions for all bodies
- Add stored impulses to velocity, reset impulses
- Update position for all bodies (using the new velocity)
Collision Detection
- Broadphase detection
- GJK+EPA
- Add/update contacts to the contact manifold
Code: Select all
contact.pointA = triangle.v1.support_a*u + triangle.v2.support_a*v + triangle.v3.support_a*w
contact.pointB = triangle.v1.support_b*u + triangle.v2.support_b*v + triangle.v3.support_b*w
Collision Resolution
- For each contact: solve penetrations, warmstart impulses, calculate b for baumgarte stabilization
- For each contact: For each iteration: find delta lambda and add impulse
Code: Select all
contact.rA = contact.pointA - bodyA.position
contact.tangent = contact.rA.cross(contact.normal)
bodyA.d = bodyA.invMass + contact.normal.dot( contact.tangent.cross(contact.rA) * bodyA.invInertia )
contact.d = 1.0 / (bodyA.d + bodyB.d)
dVA = contact.normal.dot( bodyA.velocity ) + contact.tangent.dot( bodyA.angularVelocity )
immediateLinearImpulse = contact.normal * body.invMass * (contact.depth - (dVA * contact.d) - (dVB * contact.d))
immediateAngularImpulse = contact.tangent * body.invInertia * (contact.depth - (dVA * contact.d) - (dVB * contact.d))
Warmstart Impulses
Dampen contact's stored impulse and apply to each body
Code: Select all
contact.impulse *= warmstart
body.applyLinearImpulse( contact.normal * body.invMass * contact.impulse )
body.applyAngularImpulse( contact.tangent * body.invInertia * contact.impulse )
Baumgarte Stabilization
Code: Select all
relativeVelocityA = contact.normal.dot(bodyA.velocity) + contact.tangent.dot(bodyA.angularVelocity)
contact.b = contact.d * (contact.depth + slop) * baumgarte / dt + contact.d * (relativeVelocityA + relativeVelocityB) * damping
Code: Select all
delta = contact.b - contact.impulse
dVA = contact.normal.dot(bodyA.impulseV) + contact.tangent.dot(bodyA.impulseW)
delta -= dVA * contact.d + dVB * contact.d
contact.impulse = clamp(contact.impulse + delta, 0, Infinity)
body.applyLinearImpulse( contact.normal * body.invMass * delta )
body.applyAngularImpulse( contact.tangent * body.invInertia * delta )
I could really use some advice, if anybody here knows what I'm doing wrong or if I misunderstood something please let me know.
Video 1: https://www.youtube.com/watch?v=nA8X4PINldE
Notice that the first two attempts the body simply topples over. The 3rd attempt the body stabilizes but runs into the resting contact problem.