From 6eed17e3ce1463f186483415839f1b1969e2f5db Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 25 Jan 2026 16:30:04 -0800 Subject: [PATCH 1/2] rotate: thorough test of joint target for revolute and PlaneXZ -- all axes tested -- no specific issue with Y axis for sure. --- go.mod | 6 +- go.sum | 10 +-- physics/builder/joint.go | 6 +- physics/physics_test.go | 138 +++++++++++++++++++++++++++++++++++++++ physics/step_joint.go | 5 +- physics/step_joint.goal | 7 +- 6 files changed, 154 insertions(+), 18 deletions(-) create mode 100644 physics/physics_test.go diff --git a/go.mod b/go.mod index 86f79184..dc112f52 100644 --- a/go.mod +++ b/go.mod @@ -8,7 +8,7 @@ go 1.25.6 // https://github.com/googleapis/go-genproto/issues/1015 require ( - cogentcore.org/core v0.3.14 + cogentcore.org/core v0.3.15 github.com/cogentcore/readline v0.1.3 github.com/cogentcore/yaegi v0.0.0-20260116172027-700fbf8949f3 github.com/mitchellh/go-homedir v1.1.0 @@ -67,8 +67,8 @@ require ( golang.org/x/sync v0.19.0 // indirect golang.org/x/sys v0.40.0 // indirect golang.org/x/text v0.33.0 // indirect - google.golang.org/genproto v0.0.0-20260114163908-3f89685c29c3 // indirect - google.golang.org/genproto/googleapis/rpc v0.0.0-20251222181119-0a764e51fe1b // indirect + google.golang.org/genproto v0.0.0-20260122232226-8e98ce8d340d // indirect + google.golang.org/genproto/googleapis/rpc v0.0.0-20260120174246-409b4a993575 // indirect gopkg.in/yaml.v3 v3.0.1 // indirect modernc.org/knuth v0.5.4 // indirect modernc.org/token v1.1.0 // indirect diff --git a/go.sum b/go.sum index de7e33ff..82b7bf7d 100644 --- a/go.sum +++ b/go.sum @@ -1,7 +1,7 @@ codeberg.org/go-pdf/fpdf v0.11.0 h1:n3I8WISQ1cr0S2rvx9DOlE/GypbcimMWqLpel3slHmY= codeberg.org/go-pdf/fpdf v0.11.0/go.mod h1:Y0DGRAdZ0OmnZPvjbMp/1bYxmIPxm0ws4tfoPOc4LjU= -cogentcore.org/core v0.3.14 h1:dfkws5ET/68Q8UsF1P8+F5RtpA/CoZYK4gd8i+66t5A= -cogentcore.org/core v0.3.14/go.mod h1:IQBEleZr+h6O/UjnymY875ve9kY0yokLK8YJ/JUPPBg= +cogentcore.org/core v0.3.15 h1:szgSwZ5HFPhLTmyzo9juggvCO+BGJRQAaU+x/uVvE8o= +cogentcore.org/core v0.3.15/go.mod h1:IQBEleZr+h6O/UjnymY875ve9kY0yokLK8YJ/JUPPBg= git.sr.ht/~sbinet/cmpimg v0.1.0 h1:E0zPRk2muWuCqSKSVZIWsgtU9pjsw3eKHi8VmQeScxo= git.sr.ht/~sbinet/cmpimg v0.1.0/go.mod h1:FU12psLbF4TfNXkKH2ZZQ29crIqoiqTZmeQ7dkp/pxE= github.com/Bios-Marcel/wastebasket/v2 v2.0.3 h1:TkoDPcSqluhLGE+EssHu7UGmLgUEkWg7kNyHyyJ3Q9g= @@ -165,10 +165,12 @@ golang.org/x/tools v0.41.0/go.mod h1:XSY6eDqxVNiYgezAVqqCeihT4j1U2CCsqvH3WhQpnlg golang.org/x/xerrors v0.0.0-20191204190536-9bdfabe68543/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0= gonum.org/v1/gonum v0.17.0 h1:VbpOemQlsSMrYmn7T2OUvQ4dqxQXU+ouZFQsZOx50z4= gonum.org/v1/gonum v0.17.0/go.mod h1:El3tOrEuMpv2UdMrbNlKEh9vd86bmQ6vqIcDwxEOc1E= -google.golang.org/genproto v0.0.0-20260114163908-3f89685c29c3 h1:rUamZFBwsWVWg4Yb7iTbwYp81XVHUvOXNdrFCoYRRNE= -google.golang.org/genproto v0.0.0-20260114163908-3f89685c29c3/go.mod h1:wE6SUYr3iNtF/D0GxVAjT+0CbDFktQNssYs9PVptCt4= +google.golang.org/genproto v0.0.0-20260122232226-8e98ce8d340d h1:hUplc9kLwH374NIY3PreRUK3Unc0xLm/W7MDsm0gCNo= +google.golang.org/genproto v0.0.0-20260122232226-8e98ce8d340d/go.mod h1:SpjiK7gGN2j/djoQMxLl3QOe/J/XxNzC5M+YLecVVWU= google.golang.org/genproto/googleapis/rpc v0.0.0-20251222181119-0a764e51fe1b h1:Mv8VFug0MP9e5vUxfBcE3vUkV6CImK3cMNMIDFjmzxU= google.golang.org/genproto/googleapis/rpc v0.0.0-20251222181119-0a764e51fe1b/go.mod h1:j9x/tPzZkyxcgEFkiKEEGxfvyumM01BEtsW8xzOahRQ= +google.golang.org/genproto/googleapis/rpc v0.0.0-20260120174246-409b4a993575 h1:vzOYHDZEHIsPYYnaSYo60AqHkJronSu0rzTz/s4quL0= +google.golang.org/genproto/googleapis/rpc v0.0.0-20260120174246-409b4a993575/go.mod h1:j9x/tPzZkyxcgEFkiKEEGxfvyumM01BEtsW8xzOahRQ= google.golang.org/grpc v1.76.0 h1:UnVkv1+uMLYXoIz6o7chp59WfQUYA2ex/BXQ9rHZu7A= google.golang.org/grpc v1.76.0/go.mod h1:Ju12QI8M6iQJtbcsV+awF5a4hfJMLi4X0JLo94ULZ6c= google.golang.org/protobuf v1.36.11 h1:fV6ZwhNocDyBLK0dj+fg8ektcVegBBuEolpbTQyBNVE= diff --git a/physics/builder/joint.go b/physics/builder/joint.go index 5d695e5d..45f1cdc4 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -383,7 +383,8 @@ func (jd *Joint) AddTargetPos(dof int32, pos, stiff float32) { // which is enforced, and values near the edge can be unstable at higher // stiffness levels. func (jd *Joint) SetTargetAngle(dof int32, angDeg, stiff float32) { - pos := math32.WrapPi(math32.DegToRad(angDeg)) + // pos := math32.WrapPi(math32.DegToRad(angDeg)) + pos := math32.DegToRad(angDeg) d := jd.DoF(int(dof)) d.Current.Pos = pos d.Current.Stiff = stiff @@ -399,7 +400,8 @@ func (jd *Joint) SetTargetAngle(dof int32, angDeg, stiff float32) { // stiffness levels. func (jd *Joint) AddTargetAngle(dof int32, angDeg, stiff float32) { d := jd.DoF(int(dof)) - d.Current.Pos = math32.WrapPi(d.Current.Pos + math32.DegToRad(angDeg)) + // d.Current.Pos = math32.WrapPi(d.Current.Pos + math32.DegToRad(angDeg)) + d.Current.Pos = d.Current.Pos + math32.DegToRad(angDeg) d.Current.Stiff = stiff physics.SetJointTargetPos(jd.JointIndex, dof, d.Current.Pos, stiff) } diff --git a/physics/physics_test.go b/physics/physics_test.go new file mode 100644 index 00000000..598af45a --- /dev/null +++ b/physics/physics_test.go @@ -0,0 +1,138 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "fmt" + "testing" + + "cogentcore.org/core/math32" + "github.com/stretchr/testify/assert" +) + +func testModel() *Model { + model := NewModel() + model.GPU = false + return model +} + +func TestJointRevolute(t *testing.T) { + ml := testModel() + params := GetParams(0) + params.Gravity.Y = 0 + params.SubSteps = 1 + params.Dt = 0.001 + rot := math32.NewQuatIdentity() + hsz := math32.Vec3(0.2, 0.2, 0.2) + mass := float32(0.1) + stiff := float32(1000) + damp := float32(20) + steps := 100 + tol := 1.0e-1 // this is pretty bad, but whatever + dim := math32.Z + var axis, off math32.Vector3 + axis.SetDim(dim, 1) + fmt.Println("#### dim:", dim, axis) + + bi, di := ml.NewDynamic(Box, mass, hsz, math32.Vec3(0, 0, 0), rot) + _ = bi + ml.NewObject() + ji := ml.NewJointRevolute(-1, di, math32.Vec3(0, 0, 0), off, axis) + + ml.Config() + // fmt.Println("inertia:", BodyInertia(bi)) + + SetJointTargetVel(ji, 0, 0, damp) + + for trg := float32(-5); trg <= 5.0; trg += 0.5 { + SetJointTargetPos(ji, 0, trg, stiff) + for range steps { + ml.Step() + // q := DynamicQuat(di, params.Next) + // a := q.ToAxisAngle() + // fmt.Println("trg:", trg, math32.WrapPi(trg), a.W, q) + } + q := DynamicQuat(di, params.Next) + a := q.ToAxisAngle() + // fmt.Println(trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim))) + assert.InDelta(t, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim)), tol) + } + + // return + // zooming in around Pi transition + for trg := float32(2.5); trg <= 3.5; trg += 0.01 { + SetJointTargetPos(ji, 0, trg, stiff) + for range steps { + ml.Step() + } + if math32.Abs(trg-3.13) < 0.001 { // flips a bit here + continue + } + q := DynamicQuat(di, params.Next) + a := q.ToAxisAngle() + // fmt.Println(trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim))) + assert.InDelta(t, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim)), tol) + } +} + +func TestJointPlaneXZ(t *testing.T) { + ml := testModel() + params := GetParams(0) + params.Gravity.Y = 0 + params.SubSteps = 1 + params.Dt = 0.001 + rot := math32.NewQuatIdentity() + hsz := math32.Vec3(0.2, 0.2, 0.2) + mass := float32(0.1) + stiff := float32(1000) + damp := float32(20) + steps := 100 + tol := 1.0e-1 // this is pretty bad, but whatever + dim := math32.Y + var axis, off math32.Vector3 + axis.SetDim(dim, 1) + fmt.Println("#### dim:", dim, axis) + + bi, di := ml.NewDynamic(Box, mass, hsz, math32.Vec3(0, 0, 0), rot) + _ = bi + ml.NewObject() + ji := ml.NewJointPlaneXZ(-1, di, math32.Vec3(0, 0, 0), off) + SetJointAxis(ji, 2, axis) + + ml.Config() + // fmt.Println("inertia:", BodyInertia(bi)) + + SetJointTargetVel(ji, 0, 0, damp) + + for trg := float32(-5); trg <= 5.0; trg += 0.5 { + SetJointTargetPos(ji, 2, trg, stiff) + for range steps { + ml.Step() + // q := DynamicQuat(di, params.Next) + // a := q.ToAxisAngle() + // fmt.Println("trg:", trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim)), q) + } + q := DynamicQuat(di, params.Next) + a := q.ToAxisAngle() + // fmt.Println(trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim))) + assert.InDelta(t, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim)), tol) + } + + return + // zooming in around Pi transition + for trg := float32(2.5); trg <= 3.5; trg += 0.01 { + SetJointTargetPos(ji, 2, trg, stiff) + for range steps { + ml.Step() + } + if math32.Abs(trg-3.13) < 0.001 { // flips a bit here + continue + } + q := DynamicQuat(di, params.Next) + a := q.ToAxisAngle() + // fmt.Println(trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim))) + assert.InDelta(t, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim)), tol) + } +} diff --git a/physics/step_joint.go b/physics/step_joint.go index 4b0fe754..59ed8c50 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -390,7 +390,7 @@ func StepSolveJoint(ji int32) { relQ.W*invs, relQ.X*(relQ.Z*relQ.X-relQ.W*relQ.Y)*invscube) grad0 = slmath.QuatMulScalar(grad0, 2.0/math32.Abs(qtwist.W)) - // # grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) + // grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) // derivative of asin(x) = 1/sqrt(1-x^2) // rescale swing swing_sq := qswing.W * qswing.W @@ -485,9 +485,6 @@ func StepSolveJoint(ji int32) { compliance = 1.0 / kd damping = kd } - // if ji == 0 && dim == 1 { - // fmt.Println(targetPos, e, err) - // } } // lambdaIn := slmath.Dim3(lambdaPrev, dim) lambdaIn := float32(0) diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 1f96acaf..7e19278f 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -388,8 +388,8 @@ func StepSolveJoint(ji int32) { relQ.W*invs, relQ.X*(relQ.Z*relQ.X-relQ.W*relQ.Y)*invscube) grad0 = slmath.QuatMulScalar(grad0, 2.0/math32.Abs(qtwist.W)) - // # grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) - + // grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) // derivative of asin(x) = 1/sqrt(1-x^2) + // rescale swing swing_sq := qswing.W * qswing.W // if swing axis magnitude close to zero vector, just treat in quaternion space @@ -483,9 +483,6 @@ func StepSolveJoint(ji int32) { compliance = 1.0 / kd damping = kd } - // if ji == 0 && dim == 1 { - // fmt.Println(targetPos, e, err) - // } } // lambdaIn := slmath.Dim3(lambdaPrev, dim) lambdaIn := float32(0) From cf6c5069372757188b1dc2d630da269fcd025398 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 25 Jan 2026 21:55:12 -0800 Subject: [PATCH 2/2] rotate: multi planexz test case -- issue arises from friction + gravity. todo. --- physics/builder/joint.go | 8 +- physics/examples/virtroom/virtroom.go | 9 ++- physics/physics_test.go | 104 +++++++++++++++++++++++--- 3 files changed, 106 insertions(+), 15 deletions(-) diff --git a/physics/builder/joint.go b/physics/builder/joint.go index 45f1cdc4..9f7797d2 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -383,8 +383,8 @@ func (jd *Joint) AddTargetPos(dof int32, pos, stiff float32) { // which is enforced, and values near the edge can be unstable at higher // stiffness levels. func (jd *Joint) SetTargetAngle(dof int32, angDeg, stiff float32) { - // pos := math32.WrapPi(math32.DegToRad(angDeg)) - pos := math32.DegToRad(angDeg) + pos := math32.WrapPi(math32.DegToRad(angDeg)) + // pos := math32.DegToRad(angDeg) d := jd.DoF(int(dof)) d.Current.Pos = pos d.Current.Stiff = stiff @@ -400,8 +400,8 @@ func (jd *Joint) SetTargetAngle(dof int32, angDeg, stiff float32) { // stiffness levels. func (jd *Joint) AddTargetAngle(dof int32, angDeg, stiff float32) { d := jd.DoF(int(dof)) - // d.Current.Pos = math32.WrapPi(d.Current.Pos + math32.DegToRad(angDeg)) - d.Current.Pos = d.Current.Pos + math32.DegToRad(angDeg) + d.Current.Pos = math32.WrapPi(d.Current.Pos + math32.DegToRad(angDeg)) + // d.Current.Pos = d.Current.Pos + math32.DegToRad(angDeg) d.Current.Stiff = stiff physics.SetJointTargetPos(jd.JointIndex, dof, d.Current.Pos, stiff) } diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 2d179fde..61bc2d91 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -147,6 +147,9 @@ func (ev *Env) MakeModel(sc *xyz.Scene) { // params.ControlDt = 0.1 params.Dt = 0.001 params.SubSteps = 1 + params.Gravity.Y = 0 // note: critical to not have gravity for full rotation + // https://github.com/cogentcore/lab/issues/47 + // params.MaxForce = 1.0e3 // params.AngularDamping = 0.5 // params.SubSteps = 1 @@ -311,13 +314,15 @@ func (ev *Env) MakeEmer(wl *builder.World, em *Emer, name string) { obj := wl.NewObject() em.Obj = obj sc := ev.Physics.Scene - emr := obj.NewDynamicSkin(sc, name+"_body", physics.Box, "purple", mass, math32.Vec3(hw, hh, hd), math32.Vec3(0, hh, 0), rot) + off := float32(0.01) // note: critical to float slightly off the plane! + // otherwise, this is where the problems in rotation come in. + emr := obj.NewDynamicSkin(sc, name+"_body", physics.Box, "purple", mass, math32.Vec3(hw, hh, hd), math32.Vec3(0, hh+off, 0), rot) // body := physics.NewCapsule(emr, "body", math32.Vec3(0, hh, 0), hh, hw) // body := physics.NewCylinder(emr, "body", math32.Vec3(0, hh, 0), hh, hw) em.XZ = obj.NewJointPlaneXZ(nil, emr, math32.Vec3(0, 0, 0), math32.Vec3(0, -hh, 0)) // emr.Group = 0 // no collide (temporary) - headPos := math32.Vec3(0, 2*hh+headsz, 0) + headPos := math32.Vec3(0, 2*hh+headsz+off, 0) head := obj.NewDynamicSkin(sc, name+"_head", physics.Box, "tan", mass*.1, math32.Vec3(headsz, headsz, headsz), headPos, rot) // head.Group = 0 hdsk := head.Skin diff --git a/physics/physics_test.go b/physics/physics_test.go index 598af45a..f0dffad3 100644 --- a/physics/physics_test.go +++ b/physics/physics_test.go @@ -57,7 +57,7 @@ func TestJointRevolute(t *testing.T) { q := DynamicQuat(di, params.Next) a := q.ToAxisAngle() // fmt.Println(trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim))) - assert.InDelta(t, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim)), tol) + assert.InDelta(t, 0.0, math32.MinAngleDiff(trg, a.W*a.Dim(dim)), tol) } // return @@ -67,13 +67,10 @@ func TestJointRevolute(t *testing.T) { for range steps { ml.Step() } - if math32.Abs(trg-3.13) < 0.001 { // flips a bit here - continue - } q := DynamicQuat(di, params.Next) a := q.ToAxisAngle() // fmt.Println(trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim))) - assert.InDelta(t, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim)), tol) + assert.InDelta(t, 0.0, math32.MinAngleDiff(trg, a.W*a.Dim(dim)), tol) } } @@ -93,6 +90,7 @@ func TestJointPlaneXZ(t *testing.T) { dim := math32.Y var axis, off math32.Vector3 axis.SetDim(dim, 1) + off.SetDim(dim, -hsz.Dim(dim)) fmt.Println("#### dim:", dim, axis) bi, di := ml.NewDynamic(Box, mass, hsz, math32.Vec3(0, 0, 0), rot) @@ -117,10 +115,98 @@ func TestJointPlaneXZ(t *testing.T) { q := DynamicQuat(di, params.Next) a := q.ToAxisAngle() // fmt.Println(trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim))) - assert.InDelta(t, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim)), tol) + assert.InDelta(t, 0.0, math32.MinAngleDiff(trg, a.W*a.Dim(dim)), tol) + } + + // return + // zooming in around Pi transition + for trg := float32(2.5); trg <= 3.5; trg += 0.01 { + SetJointTargetPos(ji, 2, trg, stiff) + for range steps { + ml.Step() + } + q := DynamicQuat(di, params.Next) + a := q.ToAxisAngle() + // fmt.Println(trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim))) + assert.InDelta(t, 0.0, math32.MinAngleDiff(trg, a.W*a.Dim(dim)), tol) + } +} + +func TestJointMultiPlaneXZ(t *testing.T) { + ml := testModel() + params := GetParams(0) + params.Gravity.Y = 0 + params.SubSteps = 1 + params.Dt = 0.001 + rot := math32.NewQuatIdentity() + + hh := float32(1.0) / 2 + hw := hh * .4 + hd := hh * .15 + headsz := hd * 1.5 + eyesz := headsz * .2 + mass := float32(1) // kg + + stiff := float32(1000) + damp := float32(20) + steps := 600 + tol := 1.0e-1 // this is pretty bad, but whatever + dim := math32.Y + var axis math32.Vector3 + axis.SetDim(dim, 1) + fmt.Println("#### dim:", dim, axis) + + // todo: this is based on emer, virtroom + // the issue https://github.com/cogentcore/lab/issues/47 + // arises from gravity + friction on a plane. Need to investigate that here. + // can presumably get rid of the rest of the elements + // or make a separate test. + + ebi, edi := ml.NewDynamic(Box, mass, math32.Vec3(hw, hh, hd), math32.Vec3(0, hh, 0), rot) + _ = ebi + ml.NewObject() + ji := ml.NewJointPlaneXZ(-1, edi, math32.Vec3(0, 0, 0), math32.Vec3(0, -hh, 0)) + SetJointAxis(ji, 2, axis) + + headPos := math32.Vec3(0, 2*hh+headsz, 0) + hbi, hdi := ml.NewDynamic(Box, mass*.1, math32.Vec3(headsz, headsz, headsz), headPos, rot) + _ = hbi + nji := ml.NewJointRevolute(edi, hdi, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0), math32.Vec3(0, 1, 0)) + SetJointParentFixed(nji, true) + SetJointNoLinearRotation(nji, true) + + eyeoff := math32.Vec3(-headsz*.6, headsz*.1, -(headsz + eyesz*.3)) + lbi, ldi := ml.NewDynamic(Box, mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) + _ = lbi + lji := ml.NewJointFixed(hdi, ldi, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) + SetJointParentFixed(lji, true) + + eyeoff.X = headsz * .6 + rbi, rdi := ml.NewDynamic(Box, mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) + _ = rbi + rji := ml.NewJointFixed(hdi, rdi, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) + SetJointParentFixed(rji, true) + + ml.Config() + // fmt.Println("inertia:", BodyInertia(bi)) + + SetJointTargetVel(ji, 0, 0, damp) + + for trg := float32(0); trg <= 5.0; trg += 0.5 { + SetJointTargetPos(ji, 2, trg, stiff) + for range steps { + ml.Step() + // q := DynamicQuat(di, params.Next) + // a := q.ToAxisAngle() + // fmt.Println("trg:", trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim)), q) + } + q := DynamicQuat(edi, params.Next) + a := q.ToAxisAngle() + // fmt.Println(trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim))) + assert.InDelta(t, 0.0, math32.MinAngleDiff(trg, a.W*a.Dim(dim)), tol) } - return + // return // zooming in around Pi transition for trg := float32(2.5); trg <= 3.5; trg += 0.01 { SetJointTargetPos(ji, 2, trg, stiff) @@ -130,9 +216,9 @@ func TestJointPlaneXZ(t *testing.T) { if math32.Abs(trg-3.13) < 0.001 { // flips a bit here continue } - q := DynamicQuat(di, params.Next) + q := DynamicQuat(edi, params.Next) a := q.ToAxisAngle() // fmt.Println(trg, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim))) - assert.InDelta(t, math32.WrapPi(trg), math32.WrapPi(a.W*a.Dim(dim)), tol) + assert.InDelta(t, 0.0, math32.MinAngleDiff(trg, a.W*a.Dim(dim)), tol) } }