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
}
// sample calculates the colour value for a given ray (origin and direction)
// shot into the scene. It relies on the trace() method to determine what the
// ray hit and recursively calls itself to add in the colour values of any
// child rays. This method performs the job of a rasterization pipeline shader
// by calculating colour based on light and normals wherever rays hit scene
// objects.
func (rt *rtrace) sample(orig, dir lin.V3, seed *uint32) (colour lin.V3) {
rt.sampleCalls += 1 // track number of times called
st, dist, bounce := rt.trace(orig, dir) // check ray scene collision.
obounce := bounce
// generate a sky colour if the ray is going up.
if st == missHigh {
p := 1 - dir.Z // make the sky colour lighter closer to the horizon.
p = p * p
p = p * p
colour.SetS(0.7, 0.6, 1)
return *colour.Scale(&colour, p)
}
// add randomness to light for soft shadows.
hitAt, lightDir, tmpv := lin.NewV3(), lin.NewV3(), lin.NewV3()
hitAt.Add(&orig, tmpv.Scale(&dir, dist))
lightDir.SetS(9+rnd(seed), 9+rnd(seed), 16).Add(lightDir, tmpv.Scale(hitAt, -1)).Unit()
lightIntensity := lightDir.Dot(&bounce) // lambertian factor based on angle of light source.
// check if the spot is in shadow by tracing a ray from the
// intersection point to the light. Its in shadow if there is a hit.
shadowFactor := 1.0
if lightIntensity < 0 {
lightIntensity = 0
shadowFactor = 0
} else {
var hitStatus int
if hitStatus, dist, bounce = rt.trace(*hitAt, *lightDir); hitStatus != missHigh {
lightIntensity = 0
shadowFactor = 0
}
}
// generate a floor colour if the ray was going down.
if st == missLow {
hitAt.Scale(hitAt, 0.2)
colour.SetS(3, 3, 3) // gray floor squares.
if int(math.Ceil(hitAt.X)+math.Ceil(hitAt.Y))&1 == 1 {
colour.SetS(1, 1, 3) // blue floor squares.
}
return *(colour.Scale(&colour, lightIntensity*0.2+0.1))
}
// calculate the colour 'rgb' with diffuse and specular component.
// r is the reflection vector.
reflectDir := lin.NewV3()
reflectDir.Add(&dir, tmpv.Scale(&obounce, obounce.Dot(tmpv.Scale(&dir, -2))))
rgb := lightDir.Dot(reflectDir.Scale(reflectDir, shadowFactor))
rgb = math.Pow(rgb, 99)
// cast a child ray from where the parent ray hit.
// Add in the result of the colour from the child ray.
colour.SetS(rgb, rgb, rgb)
addColour := rt.sample(*hitAt, *reflectDir, seed)
addColour.Scale(&addColour, 0.5)
return *(colour.Add(&colour, &addColour))
}
// 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)
}
}
// render one row of pixels by calculating a colour for each pixel.
// The image pixel row number is r. Fill the pixel colour into the
// image after the colour has been calculated.
func (r row) render(rt *rtrace, a, b, c lin.V3, img *image.NRGBA, seed *uint32) {
rgba := color.NRGBA{0, 0, 0, 255}
t, v1, v2 := lin.NewV3(), lin.NewV3(), lin.NewV3() // temp vectors.
colour, orig, dir := lin.NewV3(), lin.NewV3(), lin.NewV3()
for x := (rt.iw - 1); x >= 0; x-- {
colour.SetS(13, 13, 13) // Use a very dark default colour.
// Cast 64 rays per pixel for blur (stochastic sampling) and soft-shadows.
for cnt := 0; cnt < 64; cnt++ {
// Add randomness to the camera origin 17,16,8
t.Scale(&a, rnd(seed)-0.5).Scale(t, 99).Add(t, v1.Scale(&b, rnd(seed)-0.5).Scale(v1, 99))
orig.SetS(17, 16, 8).Add(orig, t)
// Add randomness to the camera direction.
rnda := rnd(seed) + float64(x)
rndb := float64(r) + rnd(seed)
dir.Scale(t, -1)
dir.Add(dir, v1.Scale(&a, rnda).Add(v1, v2.Scale(&b, rndb)).Add(v1, &c).Scale(v1, 16))
dir.Unit()
// accumulate the colour from each of the 64 rays.
sample := rt.sample(*orig, *dir, seed)
colour = sample.Scale(&sample, 3.5).Add(&sample, colour)
}
// set the final pixel colour in the image.
rgba.R = byte(colour.X) // red
rgba.G = byte(colour.Y) // green
rgba.B = byte(colour.Z) // blue
img.SetNRGBA(rt.iw-x, int(r), rgba)
}
}
// 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])
}
}
}
// newSolverBody allocates space for body specific solver information.
// This is expected to be called for a movable body, ie. 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
}
// rayTrace creates a single ray traced image.
// Its job is to create the workers that will render each row of the image.
// createScene() needs to have been called first.
func (rt *rtrace) rayTrace() *image.NRGBA {
start := time.Now() // track total raytrace time.
rt.procs = runtime.NumCPU() // equals GOMAXPROCS since Go 1.5.
img := image.NewNRGBA(image.Rect(0, 0, rt.iw, rt.ih))
// we're nominally tracing rays for pixel (x,y) in the direction of ax+by+c.
// At the image midpoint, this should be `g`
g, a, b, c := lin.NewV3(), lin.NewV3(), lin.NewV3(), lin.NewV3()
g.SetS(-5.5, -16, 0).Unit() // camera direction.
a.SetS(0, 0, 1).Cross(a, g).Unit().Scale(a, 0.002) // camera up vector.
b.Cross(g, a).Unit().Scale(b, 0.002) // right vector.
// Comment from Aeg:
// "offset from the eye point (ignoring lens perturbation `t`)
// to the corner of the focal plane."
c.Add(a, b).Scale(c, -256).Add(c, g)
// create one worker goroutine per processor.
rows := make(chan row, rt.ih)
var wg sync.WaitGroup
wg.Add(rt.procs)
for i := 0; i < rt.procs; i++ {
go rt.worker(*a, *b, *c, img, rows, &wg) // pass vectors by value.
}
// start assigning image rows to the workers.
for y := (rt.ih - 1); y >= 0; y-- {
rows <- row(y)
}
close(rows) // closing the worker comm channel causes workers to terminate...
wg.Wait() // ... once they finish their current assignment.
// dump some render statistics.
used := time.Since(start)
log.Printf("Sample:%d Trace:%d Time:%fs ", rt.sampleCalls, rt.traceCalls, used.Seconds())
return img
}
// 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
}
// trace casts a ray (origin, direction) to see if the ray hits any
// of the spheres in the scene. The possible return values are:
// missHigh : no hit and ray goes up.
// missLow : no hit and ray goes down.
// hit : hit so return distance and reflection ray.
func (rt *rtrace) trace(orig, dir lin.V3) (hitStatus int, minHitDistance float64, bounce lin.V3) {
rt.traceCalls += 1 // track number of times called.
minHitDistance = 1e9
hitStatus = missHigh
s := -orig.Z / dir.Z
if 0.01 < s {
minHitDistance = s
bounce.SetS(0, 0, 1)
hitStatus = missLow
}
// cast the ray against each sphere in the scene.
// http://www.lighthouse3d.com/tutorials/maths/ray-sphere-intersection/
// http://kylehalladay.com/blog/tutorial/math/2013/12/24/Ray-Sphere-Intersection.html
tempv := lin.NewV3()
for i, _ := range rt.scene {
tempv.Add(&orig, &(rt.scene[i])) // ray origin + sphere center.
b := tempv.Dot(&dir) // represent the intersection ray...
c := tempv.Dot(tempv) - 1 // ... and the sphere radius
b2 := b * b // ... using squared magnitudes.
// if the ray intersected the sphere.
if b2 > c {
q := b2 - c // convert the squared length values....
hitDistance := -b - math.Sqrt(q) // ... to the actual collision distance.
// remember the minimum hit distance.
if hitDistance < minHitDistance && hitDistance > 0.01 {
minHitDistance = hitDistance
bounce = *tempv
hitStatus = hit
}
}
}
if hitStatus == hit {
bounce.Add(&bounce, tempv.Scale(&dir, minHitDistance)).Unit()
}
return
}
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))
}
}
