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 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
}
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)
}
// 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
}
func tryAxis(one *CollisionCube, two *CollisionCube, axis m.Vector3, toCenter *m.Vector3,
index int, smallestPenetration m.Real, smallestCase int) (bool, m.Real, int) {
// make sure we have a normalized axis, and don't check almost parallel axes
if axis.SquareMagnitude() < m.Epsilon {
return true, smallestPenetration, smallestCase
}
axis.Normalize()
penetration := penetrationOnAxis(one, two, &axis, toCenter)
if penetration < 0 {
return false, smallestPenetration, smallestCase
}
if penetration < smallestPenetration {
return true, penetration, index
}
return true, smallestPenetration, smallestCase
}