// Constructs an arbitrary orthonormal basis for the contact. It's stored
// as a 3x3 matrix where each column is a vector for an axis. The x axis
// is based off of the contact normal and the y and z axis will be generated
// so that they are at right angles to it.
func (c *Contact) calculateContactBasis() {
var contactTangentY m.Vector3
var contactTangentZ m.Vector3
absContactNormalX := m.RealAbs(c.ContactNormal[0])
absContactNormalY := m.RealAbs(c.ContactNormal[1])
// check whether the z axis is nearer to the x or y axis
if absContactNormalX > absContactNormalY {
// generate a scaling factor to ensure results are normalized
s := m.Real(1.0) / m.RealSqrt(c.ContactNormal[2]*c.ContactNormal[2]+c.ContactNormal[0]*c.ContactNormal[0])
// the new x axis is at right angles to the world y axis
contactTangentY[0] = c.ContactNormal[2] * s
contactTangentY[1] = 0
contactTangentY[2] = c.ContactNormal[0] * -s
// the new y axis is at right angles to the new x and z axes
contactTangentZ[0] = c.ContactNormal[1] * contactTangentY[0]
contactTangentZ[1] = c.ContactNormal[2]*contactTangentY[0] - c.ContactNormal[0]*contactTangentY[2]
contactTangentZ[2] = -c.ContactNormal[1] * contactTangentY[0]
} else {
// generate a scaling factor to ensure results are normalized
s := m.Real(1.0) / m.RealSqrt(c.ContactNormal[2]*c.ContactNormal[2]+c.ContactNormal[1]*c.ContactNormal[1])
// the new x axis is at right angles to the world y axis
contactTangentY[0] = 0
contactTangentY[1] = -c.ContactNormal[2] * s
contactTangentY[2] = c.ContactNormal[1] * s
// the new y axis is at right angles to the new x and z axes
contactTangentZ[0] = c.ContactNormal[1]*contactTangentY[2] - c.ContactNormal[2]*contactTangentY[1]
contactTangentZ[1] = -c.ContactNormal[0] * contactTangentY[2]
contactTangentZ[2] = c.ContactNormal[0] * contactTangentY[1]
}
// now set the contactToWorld matrix based off of these three vectors
c.contactToWorld.SetComponents(&c.ContactNormal, &contactTangentY, &contactTangentZ)
}
// CheckAgainstSphere checks the cube against a sphere to see if there's a collision.
func (cube *CollisionCube) CheckAgainstSphere(sphere *CollisionSphere, existingContacts []*Contact) (bool, []*Contact) {
// transform the center of the sphere into cube coordinates
position := sphere.transform.GetAxis(3)
relCenter := cube.transform.TransformInverse(&position)
// check to see if we can exclude contact
if m.RealAbs(relCenter[0])-sphere.Radius > cube.HalfSize[0] ||
m.RealAbs(relCenter[1])-sphere.Radius > cube.HalfSize[1] ||
m.RealAbs(relCenter[2])-sphere.Radius > cube.HalfSize[2] {
return false, existingContacts
}
var closestPoint m.Vector3
// clamp the coordinates to the box
for i := 0; i < 3; i++ {
dist := relCenter[i]
if dist > cube.HalfSize[i] {
dist = cube.HalfSize[i]
} else if dist < -cube.HalfSize[i] {
dist = -cube.HalfSize[i]
}
closestPoint[i] = dist
}
// check to see if we're in contact
distCheck := closestPoint
distCheck.Sub(&relCenter)
dist := distCheck.SquareMagnitude()
if dist > sphere.Radius*sphere.Radius {
return false, existingContacts
}
// transform the contact point
closestPointWorld := cube.transform.MulVector3(&closestPoint)
// we have contact
c := NewContact()
c.ContactPoint = closestPointWorld
c.ContactNormal = closestPointWorld
c.ContactNormal.Sub(&position)
// if the sphere is small enough, or the engine doesn't process fast enough,
// you can end up having a relCenter position that's the same as closestPoint --
// meaning that closestPoint didn't need to be clamped to cube bounds.
//
// since closestPoint is relCenter at this point, transforming it back to
// world coordinates makes it equal to the sphere position which will not
// be able to produce a contact normal.
if m.RealEqual(c.ContactNormal.Magnitude(), 0.0) {
// our hack for this is to simply use the sphere's velocity as the contact
// normal, which is probably not the correct thing to do, but looks okay.
c.ContactNormal = sphere.Body.Velocity
}
c.ContactNormal.Normalize()
c.Penetration = sphere.Radius
if !m.RealEqual(dist, 0.0) {
c.Penetration -= m.RealSqrt(dist)
} else {
c.Penetration = 0.0
}
c.Bodies[0] = cube.Body
c.Bodies[1] = sphere.Body
contacts := append(existingContacts, c)
// FIXME:
// TODO: c.Friction and c.Restitution set here are test constants
c.Friction = 0.9
c.Restitution = 0.1
return true, contacts
}
// 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
}
