Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[jcxue/quat2angle] add utility function to convert quaternion to angles #70

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

[jcxue/quat2angle] add utility function to convert quaternion to angles #70

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
115 changes: 115 additions & 0 deletions mgl32/quat.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
18 changes: 18 additions & 0 deletions mgl32/quat_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could we add a test which checks all of the possible RotationOrder?

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()

Expand Down
2 changes: 1 addition & 1 deletion mgl64/matstack/matstack.go
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please can you revert this change. codegen.go obviously needs a bugfix.


package matstack

Expand Down
2 changes: 1 addition & 1 deletion mgl64/matstack/transformStack.go
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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

Expand Down
2 changes: 1 addition & 1 deletion mgl64/matstack/transformstack_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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

Expand Down
115 changes: 115 additions & 0 deletions mgl64/quat.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
18 changes: 18 additions & 0 deletions mgl64/quat_test.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()

Expand Down