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 }