Beispiel #1
0
// 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.Xx), C.btScalar(m3.Xy), C.btScalar(m3.Xz)
	bbi.rotA[0x4], bbi.rotA[0x5], bbi.rotA[0x6] = C.btScalar(m3.Yx), C.btScalar(m3.Yy), C.btScalar(m3.Yz)
	bbi.rotA[0x8], bbi.rotA[0x9], bbi.rotA[0xA] = C.btScalar(m3.Zx), C.btScalar(m3.Zy), C.btScalar(m3.Zz)
	m3.SetQ(bb.world.Rot)
	bbi.rotB[0x0], bbi.rotB[0x1], bbi.rotB[0x2] = C.btScalar(m3.Xx), C.btScalar(m3.Xy), C.btScalar(m3.Xz)
	bbi.rotB[0x4], bbi.rotB[0x5], bbi.rotB[0x6] = C.btScalar(m3.Yx), C.btScalar(m3.Yy), C.btScalar(m3.Yz)
	bbi.rotB[0x8], bbi.rotB[0x9], bbi.rotB[0xA] = C.btScalar(m3.Zx), C.btScalar(m3.Zy), C.btScalar(m3.Zz)
	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]))
			goc.point.SetS(float64(cc.p[0]), float64(cc.p[1]), float64(cc.p[2]))
		}
		return a, b, c[0:numContacts]
	}
	return a, b, c[0:0] // boxes did not collide.
}
Beispiel #2
0
// 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)
}