// calculateFrictionImpulse calculates the impulse needed to resolve this contact,
// given that the contact has a non-zero coefficient of friction.
func (c *Contact) calculateFrictionImpulse(inverseInertiaTensors [2]m.Matrix3) (impulseContact m.Vector3) {
inverseMass := c.Bodies[0].GetInverseMass()
// the equivalent of a cross product in matrices is multiplication
// by a skew symmetric matrix - we build the matrix for converting
// between linear and angular quantities.
var impulseToTorque m.Matrix3
setSkewSymmetric(&impulseToTorque, &c.relativeContactPosition[0])
// build the matrix to convert contact impulse to change in velocity in
// world coordinates
deltaVelWorld := impulseToTorque.MulMatrix3(&inverseInertiaTensors[0])
deltaVelWorld = deltaVelWorld.MulMatrix3(&impulseToTorque)
deltaVelWorld.MulWith(-1.0)
// check to see if we need to add the second body's data
if c.Bodies[1] != nil {
// set the cross product matrix
setSkewSymmetric(&impulseToTorque, &c.relativeContactPosition[1])
// calculate the velocity change matrix
deltaVelWorld2 := impulseToTorque.MulMatrix3(&inverseInertiaTensors[1])
deltaVelWorld2 = deltaVelWorld2.MulMatrix3(&impulseToTorque)
deltaVelWorld2.MulWith(-1.0)
// add to the total delta velocity
deltaVelWorld.Add(&deltaVelWorld2)
// add to the inverse mass
inverseMass += c.Bodies[1].GetInverseMass()
}
// do a change of basis to convert into contact coordinates
deltaVelocity := c.contactToWorld.Transpose()
deltaVelocity = deltaVelocity.MulMatrix3(&deltaVelWorld)
deltaVelocity = deltaVelocity.MulMatrix3(&c.contactToWorld)
// add in the linear velocity change
deltaVelocity[0] += inverseMass
deltaVelocity[4] += inverseMass
deltaVelocity[8] += inverseMass
// invert to get the impulse needed per unit velocity
impulseMatrix := deltaVelocity.Invert()
// find the target velocities to kill
velKill := m.Vector3{
c.desiredDeltaVelocity,
-c.contactVelocity[1],
-c.contactVelocity[2],
}
// find the impulse to kill target velocities
impulseContact = impulseMatrix.MulVector3(&velKill)
// check for exceeding friction
planarImpulse := m.RealSqrt(impulseContact[1]*impulseContact[1] + impulseContact[2]*impulseContact[2])
if planarImpulse > impulseContact[0]*c.Friction {
// we need to use dynamic friction
impulseContact[1] /= planarImpulse
impulseContact[2] /= planarImpulse
impulseContact[0] = deltaVelocity[0] +
deltaVelocity[3]*c.Friction*impulseContact[1] +
deltaVelocity[6]*c.Friction*impulseContact[2]
impulseContact[0] = c.desiredDeltaVelocity / impulseContact[0]
impulseContact[1] *= c.Friction * impulseContact[0]
impulseContact[2] *= c.Friction * impulseContact[0]
}
return
}
