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 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
}