func transformToAxis(cube *CollisionCube, axis *m.Vector3) m.Real {
cubeAxisX := cube.transform.GetAxis(0)
cubeAxisY := cube.transform.GetAxis(1)
cubeAxisZ := cube.transform.GetAxis(2)
return cube.HalfSize[0]*m.RealAbs(axis.Dot(&cubeAxisX)) +
cube.HalfSize[1]*m.RealAbs(axis.Dot(&cubeAxisY)) +
cube.HalfSize[2]*m.RealAbs(axis.Dot(&cubeAxisZ))
}
func (c *Contact) calculateDesiredDeltaVelocity(duration m.Real) {
const velocityLimit m.Real = 0.25
var velocityFromAcc m.Real
// calculate the acceleration induced velocity accumlated this frame
var tempVelocity m.Vector3
if c.Bodies[0].IsAwake {
tempVelocity = c.Bodies[0].GetLastFrameAccelleration()
tempVelocity.MulWith(duration)
velocityFromAcc += tempVelocity.Dot(&c.ContactNormal)
}
if c.Bodies[1] != nil && c.Bodies[1].IsAwake {
tempVelocity = c.Bodies[1].GetLastFrameAccelleration()
tempVelocity.MulWith(duration)
velocityFromAcc -= tempVelocity.Dot(&c.ContactNormal)
}
// if the velocity is very slow, limit the restitution
restitution := c.Restitution
if m.RealAbs(c.contactVelocity[0]) < velocityLimit {
restitution = 0.0
}
// combine the bounce velocity with the removed acceleration velocity
c.desiredDeltaVelocity = -c.contactVelocity[0] - restitution*(c.contactVelocity[0]-velocityFromAcc)
}
func contactPoint(pOne *m.Vector3, dOne *m.Vector3, oneSize m.Real,
pTwo *m.Vector3, dTwo *m.Vector3, twoSize m.Real, useOne bool) m.Vector3 {
// If useOne is true, and the contact point is outside
// the edge (in the case of an edge-face contact) then
// we use one's midpoint, otherwise we use two's.
//Vector3 toSt, cOne, cTwo;
//real dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo;
//real denom, mua, mub;
smOne := dOne.SquareMagnitude()
smTwo := dTwo.SquareMagnitude()
dpOneTwo := dTwo.Dot(dOne)
toSt := *pOne
toSt.Sub(pTwo)
dpStaOne := dOne.Dot(&toSt)
dpStaTwo := dTwo.Dot(&toSt)
denom := smOne*smTwo - dpOneTwo*dpOneTwo
// Zero denominator indicates parrallel lines
if m.RealAbs(denom) < m.Epsilon {
if useOne {
return *pOne
}
return *pTwo
}
mua := (dpOneTwo*dpStaTwo - smTwo*dpStaOne) / denom
mub := (smOne*dpStaTwo - dpOneTwo*dpStaOne) / denom
// If either of the edges has the nearest point out
// of bounds, then the edges aren't crossed, we have
// an edge-face contact. Our point is on the edge, which
// we know from the useOne parameter.
if mua > oneSize || mua < -oneSize || mub > twoSize || mub < -twoSize {
if useOne {
return *pOne
}
return *pTwo
}
cOne := *dOne
cOne.MulWith(mua)
cOne.Add(pOne)
cTwo := *dTwo
cTwo.MulWith(mub)
cTwo.Add(pTwo)
cOne.MulWith(0.5)
cTwo.MulWith(0.5)
cOne.Add(&cTwo)
return cOne
}
// penetrationOnAxis checks if the two boxes overlap along a given axis and
// returns the amount of overlap.
func penetrationOnAxis(one *CollisionCube, two *CollisionCube, axis *m.Vector3, toCenter *m.Vector3) m.Real {
// project the half-size of one onto axis
oneProject := transformToAxis(one, axis)
twoProject := transformToAxis(two, axis)
// Project this onto the axis
distance := m.RealAbs(toCenter.Dot(axis))
// Return the overlap (i.e. positive indicates
// overlap, negative indicates separation).
return oneProject + twoProject - distance
}
// 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
}
