func newBody(shape Shape) *body {
b := &body{}
b.shape = shape
b.imass = 0 // no mass, static body by default
b.friction = 0.5 // good to have some friction
b.world = lin.NewT().SetI() // world transform
b.guess = lin.NewT().SetI() // predicted world transform
// allocate linear and angular motion data
b.lvel = lin.NewV3()
b.lfor = lin.NewV3()
b.avel = lin.NewV3()
b.afor = lin.NewV3()
b.iitw = lin.NewM3().Set(lin.M3I)
b.iit = lin.NewV3()
// allocate scratch variables
b.coi = &C.BoxBoxInput{}
b.cor = &C.BoxBoxResults{}
b.m0 = &lin.M3{}
b.m1 = &lin.M3{}
b.v0 = &lin.V3{}
b.t0 = lin.NewT()
// create a unique body identifier
bodyUuidMutex.Lock()
b.bid = bodyUuid
if bodyUuid++; bodyUuid == 0 {
log.Printf("Overflow: dev error. Unique body id wrapped.")
}
bodyUuidMutex.Unlock()
return b
}
func TestLargestArea(t *testing.T) {
con := &contactPair{}
con.v0, con.v1, con.v2 = lin.NewV3(), lin.NewV3(), lin.NewV3()
// Existing points: essentially 14,0,+-1, 16,0,+-1
manifold := newManifold()
manifold[0].sp.localA.SetS(13.993946, 25.000000, -0.999210) // 14,0,-1
manifold[1].sp.localA.SetS(14.006243, 25.000000, 0.979937) // 14,0,1
manifold[2].sp.localA.SetS(15.989870, 25.000000, 0.996212) // 16,0,1
manifold[3].sp.localA.SetS(15.993749, 25.000000, -0.999743) // 16,0,-1
// new point A should replace existing point 0.
ptA := newPoc()
ptA.sp.localA.SetS(14.024626, 25.000000, -1.020002) // 14,0,-1
if index := con.largestArea(manifold, ptA); index != 0 {
t.Errorf("Wrong replacement ptA for best contact area %d", index)
}
// new point A should replace existing point 1.
ptB := newPoc()
ptB.sp.localA.SetS(14.008444, 25.000000, 0.979925) // 14,0,1
if index := con.largestArea(manifold, ptB); index != 1 {
t.Errorf("Wrong replacement ptB for best contact area %d", index)
}
}
// newPoc allocates space for, and returns, a pointOfContact structure.
func newPoc() *pointOfContact {
poc := &pointOfContact{}
poc.point = lin.NewV3()
poc.normal = lin.NewV3()
poc.sp = newSolverPoint()
poc.v0 = lin.NewV3()
return poc
}
// newSolverConstraint allocates the memory needed for a solver constraint.
func newSolverConstraint() *solverConstraint {
sc := &solverConstraint{}
sc.normal = lin.NewV3()
sc.relpos1CrossNormal = lin.NewV3()
sc.relpos2CrossNormal = lin.NewV3()
sc.angularComponentA = lin.NewV3()
sc.angularComponentB = lin.NewV3()
return sc
}
// newSolver creates the necessary space for the solver to work.
// This is expected to be called once on engine startup.
func newSolver() *solver {
sol := &solver{}
sol.info = newSolverInfo()
sol.constC = []*solverConstraint{}
sol.constF = []*solverConstraint{}
sol.v0 = lin.NewV3()
sol.v1 = lin.NewV3()
sol.v2 = lin.NewV3()
sol.ra = lin.NewV3()
sol.rb = lin.NewV3()
return sol
}
// newContactPair creates a contact between two bodies. This is expected to be
// used only for contacting bodies that are not already being tracked by the
// mover.
func newContactPair(bodyA, bodyB *body) *contactPair {
con := &contactPair{}
con.bodyA, con.bodyB = bodyA, bodyB
if bodyA != nil && bodyB != nil {
con.pid = bodyA.pairId(bodyB)
}
con.pocs = newManifold() // allocate space for 4 contacts.
con.pocs = con.pocs[0:0] // start with zero contacts.
con.breakingLimit = 0.02
con.processingLimit = lin.LARGE
con.v0 = lin.NewV3()
con.v1 = lin.NewV3()
con.v2 = lin.NewV3()
return con
}
func TestGetVelocityInLocalPoint(t *testing.T) {
b := newBody(NewSphere(1)).SetMaterial(0.5, 0.8).(*body)
b.lvel.SetS(2, 2, 2)
b.avel.SetS(3, 3, 3)
v, p, want := lin.NewV3(), lin.NewV3S(1, 1, 1), "{2.0 2.0 2.0}"
if b.getVelocityInLocalPoint(p, v); dumpV3(v) != want {
t.Errorf("Expecting local velocity %s, got %s", dumpV3(v), want)
}
}
// newSolverBody allocates space for body specific solver information.
// This is expected to be called for a movable body, i.e. one that has mass
// and can have velocity.
func newSolverBody(bod *body) *solverBody {
sb := &solverBody{}
sb.oBody = bod // reference
sb.world = lin.NewT().Set(bod.world)
sb.linearVelocity = lin.NewV3().Set(bod.lvel)
sb.angularVelocity = lin.NewV3().Set(bod.avel)
sb.deltaLinearVelocity = lin.NewV3()
sb.deltaAngularVelocity = lin.NewV3()
sb.pushVelocity = lin.NewV3()
sb.turnVelocity = lin.NewV3()
sb.invMass = lin.NewV3().SetS(bod.imass, bod.imass, bod.imass)
sb.t0 = lin.NewT()
sb.v0 = lin.NewV3()
return sb
}
// fixedSolverBody lazy initializes and returns the single fixed
// solver body that is used by all static solver bodies.
func fixedSolverBody() *solverBody {
if fsb == nil {
fsb = &solverBody{}
fsb.oBody = nil
fsb.world = lin.NewT().SetI()
fsb.linearVelocity = lin.NewV3()
fsb.angularVelocity = lin.NewV3()
fsb.deltaLinearVelocity = lin.NewV3()
fsb.deltaAngularVelocity = lin.NewV3()
fsb.pushVelocity = lin.NewV3()
fsb.turnVelocity = lin.NewV3()
fsb.invMass = lin.NewV3()
fsb.t0 = lin.NewT()
fsb.v0 = lin.NewV3()
}
return fsb
}
func TestSphereInertia(t *testing.T) {
sp, inertia, want := Shape(NewSphere(1.25)), lin.NewV3(), "{0.6 0.6 0.6}"
if sp.Inertia(1, inertia); dumpV3(inertia) != want {
t.Errorf("Expected sphere inertia %s, got %s", want, dumpV3(inertia))
}
}
func TestBoxInertia(t *testing.T) {
bx, inertia, want := Shape(NewBox(1, 1, 1)), lin.NewV3(), "{0.7 0.7 0.7}"
if bx.Inertia(1, inertia); dumpV3(inertia) != want {
t.Errorf("Expected box inertia %s, got %s", want, dumpV3(inertia))
}
}