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
}
// createBaseFrames constructs the joint transform base-pose matricies.
// These are temporary structures used later in genFrame to prepare the
// per-frame animation data.
func (l *loader) createBaseFrames(iqd *IqData, poses []*transform, scr *scratch) {
i3 := lin.NewM3() // scratch
vx, vy, vz := lin.NewV3(), lin.NewV3(), lin.NewV3() // scratch
numJoints := len(poses)
scr.baseframe = make([]*lin.M4, numJoints) // joint transforms.
scr.inversebaseframe = make([]*lin.M4, numJoints) // inverse transforms.
for cnt := 0; cnt < numJoints; cnt++ {
j := poses[cnt]
// Get the joint transform.
j.q.Unit() // ensure unit quaternion.
m4 := lin.NewM4().SetQ(j.q)
m4.Transpose(m4).ScaleSM(j.s.X, j.s.Y, j.s.Z) // apply scale before rotation.
m4.Wx, m4.Wy, m4.Wz, m4.Ww = j.t.X, j.t.Y, j.t.Z, 1 // translation added in, not multiplied.
scr.baseframe[cnt] = m4
// invert the joint transform for frame generation later on.
i3.Inv(i3.SetM4(m4))
itx := -vx.SetS(i3.Xx, i3.Yx, i3.Zx).Dot(j.t)
ity := -vy.SetS(i3.Xy, i3.Yy, i3.Zy).Dot(j.t)
itz := -vz.SetS(i3.Xz, i3.Yz, i3.Zz).Dot(j.t)
i4 := lin.NewM4()
i4.Xx, i4.Xy, i4.Xz, i4.Xw = i3.Xx, i3.Xy, i3.Xz, 0
i4.Yx, i4.Yy, i4.Yz, i4.Yw = i3.Yx, i3.Yy, i3.Yz, 0
i4.Zx, i4.Zy, i4.Zz, i4.Zw = i3.Zx, i3.Zy, i3.Zz, 0
i4.Wx, i4.Wy, i4.Wz, i4.Ww = itx, ity, itz, 1
scr.inversebaseframe[cnt] = i4
// Combine the joint transforms and inverse transform with the parent transform.
parent := iqd.Joints[cnt]
if parent >= 0 {
// childBasePose * parentBasePose
scr.baseframe[cnt].Mult(scr.baseframe[cnt], scr.baseframe[parent])
// childInverseBasePose * parentInverseBasePose
scr.inversebaseframe[cnt].Mult(scr.inversebaseframe[parent], scr.inversebaseframe[cnt])
}
}
}