diff --git a/mgl32/quat.go b/mgl32/quat.go index a2ede3e..138a419 100644 --- a/mgl32/quat.go +++ b/mgl32/quat.go @@ -345,6 +345,121 @@ func AnglesToQuat(angle1, angle2, angle3 float32, order RotationOrder) Quat { return ret } +// Convert quaternion to Euler angles +// Based off matlab code from: https://github.com/mjcarroll/mech7710-final/blob/master/automowfilter/quat2angle.m + +func QuatToAngles(q Quat, order RotationOrder) (float32, float32, float32) { + angle1 := float32(0.0) + angle2 := float32(0.0) + angle3 := float32(0.0) + q = q.Normalize() + + switch order { + case ZYX: + r11 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r12 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + r21 := -2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r31 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r32 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case ZYZ: + r11 := 2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r12 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r21 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + r31 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r32 := -2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + case ZXY: + r11 := -2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r12 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + r21 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r31 := -2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r32 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case ZXZ: + r11 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r12 := -2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r21 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + r31 := 2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r32 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + case YXZ: + r11 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r12 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + r21 := -2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r31 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r32 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case YXY: + r11 := 2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r12 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r21 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + r31 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r32 := -2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + case YZX: + r11 := -2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r12 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + r21 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r31 := -2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r32 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case YZY: + r11 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r12 := -2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r21 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + r31 := 2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r32 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + case XYZ: + r11 := -2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r12 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + r21 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r31 := -2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r32 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case XYX: + r11 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r12 := -2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r21 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + r31 := 2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r32 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + case XZY: + r11 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r12 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + r21 := -2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r31 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r32 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case XZX: + r11 := 2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r12 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r21 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + r31 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r32 := -2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + default: + panic("Unsupported rotation order") + } + return angle1, angle2, angle3 +} + +// Find angles for rotations about X, Y, and Z axes +func threeAxisRot(r11, r12, r21, r31, r32 float32) (float32, float32, float32) { + r1 := math.Atan2(float64(r31), float64(r32)) + r2 := math.Asin(float64(r21)) + r3 := math.Atan2(float64(r11), float64(r12)) + return float32(r1), float32(r2), float32(r3) +} + +func twoAxisRot(r11, r12, r21, r31, r32 float32) (float32, float32, float32) { + r1 := math.Atan2(float64(r11), float64(r12)) + r2 := math.Acos(float64(r21)) + r3 := math.Atan2(float64(r31), float64(r32)) + return float32(r1), float32(r2), float32(r3) +} + // Mat4ToQuat converts a pure rotation matrix into a quaternion func Mat4ToQuat(m Mat4) Quat { // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm diff --git a/mgl32/quat_test.go b/mgl32/quat_test.go index 049bfa0..e4e0b3a 100644 --- a/mgl32/quat_test.go +++ b/mgl32/quat_test.go @@ -113,6 +113,24 @@ func TestAnglesToQuatZYX(t *testing.T) { } } +func TestQuatToAngles(t *testing.T) { + t.Parallel() + + a_x := float32(math.Pi / 16.0) + a_y := float32(math.Pi / 8.0) + a_z := float32(math.Pi / 4.0) + a1, a2, a3 := QuatToAngles(AnglesToQuat(a_x, a_y, a_z, XYZ), XYZ) + if !FloatEqualThreshold(a1, a_z, 1e-6) { + t.Errorf("Angle1 incorrect. Got: %f Expected: %f", a1, a_z) + } + if !FloatEqualThreshold(a2, a_y, 1e-6) { + t.Errorf("Angle1 incorrect. Got: %f Expected: %f", a2, a_y) + } + if !FloatEqualThreshold(a3, a_x, 1e-6) { + t.Errorf("Angle1 incorrect. Got: %f Expected: %f", a3, a_x) + } +} + func TestQuatMatRotateY(t *testing.T) { t.Parallel() diff --git a/mgl64/matstack/matstack.go b/mgl64/matstack/matstack.go index 9c0a489..139d016 100644 --- a/mgl64/matstack/matstack.go +++ b/mgl64/matstack/matstack.go @@ -1,4 +1,4 @@ -// This file is generated from mgl32/matstack\matstack.go; DO NOT EDIT +// This file is generated from mgl32/matstack/matstack.go; DO NOT EDIT package matstack diff --git a/mgl64/matstack/transformStack.go b/mgl64/matstack/transformStack.go index 2357bb3..d2df449 100644 --- a/mgl64/matstack/transformStack.go +++ b/mgl64/matstack/transformStack.go @@ -1,4 +1,4 @@ -// This file is generated from mgl32/matstack\transformStack.go; DO NOT EDIT +// This file is generated from mgl32/matstack/transformStack.go; DO NOT EDIT package matstack diff --git a/mgl64/matstack/transformstack_test.go b/mgl64/matstack/transformstack_test.go index c42a32a..e8cabde 100644 --- a/mgl64/matstack/transformstack_test.go +++ b/mgl64/matstack/transformstack_test.go @@ -1,4 +1,4 @@ -// This file is generated from mgl32/matstack\transformstack_test.go; DO NOT EDIT +// This file is generated from mgl32/matstack/transformstack_test.go; DO NOT EDIT package matstack diff --git a/mgl64/quat.go b/mgl64/quat.go index 5e4f7bf..dcde259 100644 --- a/mgl64/quat.go +++ b/mgl64/quat.go @@ -347,6 +347,121 @@ func AnglesToQuat(angle1, angle2, angle3 float64, order RotationOrder) Quat { return ret } +// Convert quaternion to Euler angles +// Based off matlab code from: https://github.com/mjcarroll/mech7710-final/blob/master/automowfilter/quat2angle.m + +func QuatToAngles(q Quat, order RotationOrder) (float64, float64, float64) { + angle1 := float64(0.0) + angle2 := float64(0.0) + angle3 := float64(0.0) + q = q.Normalize() + + switch order { + case ZYX: + r11 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r12 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + r21 := -2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r31 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r32 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case ZYZ: + r11 := 2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r12 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r21 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + r31 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r32 := -2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + case ZXY: + r11 := -2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r12 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + r21 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r31 := -2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r32 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case ZXZ: + r11 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r12 := -2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r21 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + r31 := 2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r32 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + case YXZ: + r11 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r12 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + r21 := -2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r31 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r32 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case YXY: + r11 := 2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r12 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r21 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + r31 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r32 := -2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + case YZX: + r11 := -2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r12 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + r21 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r31 := -2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r32 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case YZY: + r11 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r12 := -2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r21 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + r31 := 2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r32 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + case XYZ: + r11 := -2.0 * (q.V[1]*q.V[2] - q.W*q.V[0]) + r12 := q.W*q.W - q.V[0]*q.V[0] - q.V[1]*q.V[1] + q.V[2]*q.V[2] + r21 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r31 := -2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r32 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case XYX: + r11 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r12 := -2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r21 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + r31 := 2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r32 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + case XZY: + r11 := 2.0 * (q.V[1]*q.V[2] + q.W*q.V[0]) + r12 := q.W*q.W - q.V[0]*q.V[0] + q.V[1]*q.V[1] - q.V[2]*q.V[2] + r21 := -2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + r31 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r32 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + angle1, angle2, angle3 = threeAxisRot(r11, r12, r21, r31, r32) + case XZX: + r11 := 2.0 * (q.V[0]*q.V[2] - q.W*q.V[1]) + r12 := 2.0 * (q.V[0]*q.V[1] + q.W*q.V[2]) + r21 := q.W*q.W + q.V[0]*q.V[0] - q.V[1]*q.V[1] - q.V[2]*q.V[2] + r31 := 2.0 * (q.V[0]*q.V[2] + q.W*q.V[1]) + r32 := -2.0 * (q.V[0]*q.V[1] - q.W*q.V[2]) + angle1, angle2, angle3 = twoAxisRot(r11, r12, r21, r31, r32) + default: + panic("Unsupported rotation order") + } + return angle1, angle2, angle3 +} + +// Find angles for rotations about X, Y, and Z axes +func threeAxisRot(r11, r12, r21, r31, r32 float64) (float64, float64, float64) { + r1 := math.Atan2(float64(r31), float64(r32)) + r2 := math.Asin(float64(r21)) + r3 := math.Atan2(float64(r11), float64(r12)) + return float64(r1), float64(r2), float64(r3) +} + +func twoAxisRot(r11, r12, r21, r31, r32 float64) (float64, float64, float64) { + r1 := math.Atan2(float64(r11), float64(r12)) + r2 := math.Acos(float64(r21)) + r3 := math.Atan2(float64(r31), float64(r32)) + return float64(r1), float64(r2), float64(r3) +} + // Mat4ToQuat converts a pure rotation matrix into a quaternion func Mat4ToQuat(m Mat4) Quat { // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm diff --git a/mgl64/quat_test.go b/mgl64/quat_test.go index 735b5d0..9093603 100644 --- a/mgl64/quat_test.go +++ b/mgl64/quat_test.go @@ -115,6 +115,24 @@ func TestAnglesToQuatZYX(t *testing.T) { } } +func TestQuatToAngles(t *testing.T) { + t.Parallel() + + a_x := float64(math.Pi / 16.0) + a_y := float64(math.Pi / 8.0) + a_z := float64(math.Pi / 4.0) + a1, a2, a3 := QuatToAngles(AnglesToQuat(a_x, a_y, a_z, XYZ), XYZ) + if !FloatEqualThreshold(a1, a_z, 1e-6) { + t.Errorf("Angle1 incorrect. Got: %f Expected: %f", a1, a_z) + } + if !FloatEqualThreshold(a2, a_y, 1e-6) { + t.Errorf("Angle1 incorrect. Got: %f Expected: %f", a2, a_y) + } + if !FloatEqualThreshold(a3, a_x, 1e-6) { + t.Errorf("Angle1 incorrect. Got: %f Expected: %f", a3, a_x) + } +} + func TestQuatMatRotateY(t *testing.T) { t.Parallel()