// collideBoxBox uses the Separating Axis Test to check for overlap. If there
// is overlap then the axis of least penetration is used as the contact normal.
// For more background see:
// Real-Time Collision Detection by Christer Ericson. Sections 4.4.1, 5.2.1
// http://www.jkh.me/files/tutorials/Separating%20Axis%20Theorem%20for%20Oriented%20Bounding%20Boxes.pdf
// metanetsoftware.com/technique/tutorialA.html
// Up to 4 contact points can be returned.
func collideBoxBox(a, b Body, c []*pointOfContact) (i, j Body, k []*pointOfContact) {
aa, bb := a.(*body), b.(*body)
sa, sb := aa.shape.(*box), bb.shape.(*box)
// Translate box rotation transforms into 4x3 rotation matrix.
bbi, bbr, m3 := aa.coi, aa.cor, aa.m0
bbi.orgA[0] = C.btScalar(aa.world.Loc.X)
bbi.orgA[1] = C.btScalar(aa.world.Loc.Y)
bbi.orgA[2] = C.btScalar(aa.world.Loc.Z)
bbi.orgB[0] = C.btScalar(bb.world.Loc.X)
bbi.orgB[1] = C.btScalar(bb.world.Loc.Y)
bbi.orgB[2] = C.btScalar(bb.world.Loc.Z)
bbi.lenA[0] = C.btScalar(sa.Hx + margin)
bbi.lenA[1] = C.btScalar(sa.Hy + margin)
bbi.lenA[2] = C.btScalar(sa.Hz + margin)
bbi.lenB[0] = C.btScalar(sb.Hx + margin)
bbi.lenB[1] = C.btScalar(sb.Hy + margin)
bbi.lenB[2] = C.btScalar(sb.Hz + margin)
m3.SetQ(aa.world.Rot)
bbi.rotA[0x0], bbi.rotA[0x1], bbi.rotA[0x2] = C.btScalar(m3.X0), C.btScalar(m3.Y0), C.btScalar(m3.Z0)
bbi.rotA[0x4], bbi.rotA[0x5], bbi.rotA[0x6] = C.btScalar(m3.X1), C.btScalar(m3.Y1), C.btScalar(m3.Z1)
bbi.rotA[0x8], bbi.rotA[0x9], bbi.rotA[0xA] = C.btScalar(m3.X2), C.btScalar(m3.Y2), C.btScalar(m3.Z2)
m3.SetQ(bb.world.Rot)
bbi.rotB[0x0], bbi.rotB[0x1], bbi.rotB[0x2] = C.btScalar(m3.X0), C.btScalar(m3.Y0), C.btScalar(m3.Z0)
bbi.rotB[0x4], bbi.rotB[0x5], bbi.rotB[0x6] = C.btScalar(m3.X1), C.btScalar(m3.Y1), C.btScalar(m3.Z1)
bbi.rotB[0x8], bbi.rotB[0x9], bbi.rotB[0xA] = C.btScalar(m3.X2), C.btScalar(m3.Y2), C.btScalar(m3.Z2)
bbr.ncp, bbr.code = 0, 0
C.boxBoxClosestPoints(bbi, bbr)
// Translate the returned c-lang contact information into go-lang Contact information.
if bbr.code > 0 {
numContacts := int(bbr.ncp)
if numContacts < 0 || numContacts > 4 {
log.Printf("Dev error: should be 0-4 contacts %d.", numContacts)
numContacts = int(lin.Clamp(0, 4, float64(numContacts)))
}
for cnt := 0; cnt < numContacts; cnt++ {
cc := bbr.bbc[cnt]
goc := c[cnt]
goc.depth = float64(cc.d) // depth is 0 for identical centers.
goc.normal.SetS(float64(cc.n[0]), float64(cc.n[1]), float64(cc.n[2])) // normal
goc.point.SetS(float64(cc.p[0]), float64(cc.p[1]), float64(cc.p[2])) // point
}
return a, b, c[0:numContacts]
}
return a, b, c[0:0] // boxes did not collide.
}
// combinedFriction calculates the combined friction of the two bodies.
// Returned friction value clamped to reasonable range.
func (b *body) combinedFriction(a *body) float64 {
return lin.Clamp(a.friction*b.friction, -maxFriction, maxFriction)
}