From dd95697f8134c7bb9377a05d8cf2dd60d85374e9 Mon Sep 17 00:00:00 2001
From: ricklentz <rlentz@gmail.com>
Date: Tue, 6 Oct 2026 22:38:18 -0400
Subject: [PATCH] Fix Euler rotation matrix convention in Matrix3f and Matrix4f

Matrix3f.setEulers had m[1][1] and m[2][1] swapped compared to the
standard R=Rh(Y)*Rp(X)*Rr(Z) rotation matrix that both Quaternion
and the Vince reference use. Matrix3f.getEulers correspondingly
extracted cosr from the wrong element.

Matrix4f.setEulers used a different convention (heading about Z,
pitch about Y, roll about X) instead of the Y-X-Z convention used
by Quaternion. Aligned it to match.

Closes #34
---
 src/edu/nps/moves/legacy/math/Matrix3f.java |  6 +++---
 src/edu/nps/moves/legacy/math/Matrix4f.java | 18 +++++++++---------
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/edu/nps/moves/legacy/math/Matrix3f.java b/src/edu/nps/moves/legacy/math/Matrix3f.java
index 6cd8284572..b56c5f1f60 100644
--- a/src/edu/nps/moves/legacy/math/Matrix3f.java
+++ b/src/edu/nps/moves/legacy/math/Matrix3f.java
@@ -206,7 +206,7 @@ public void getEulers(float hpr[]) // self factored - tested ok
         {
         cosh = m[2][2] / cosp;
         sinh = m[0][2] / cosp;
-        cosr = m[2][1] / cosp;
+        cosr = m[1][1] / cosp;
         sinr = m[1][0] / cosp;
         } 
     else 
@@ -251,10 +251,10 @@ public void setEulers(float h, float p, float r) // Vince p.26 - tested ok
     m[0][1] = - cosh * sinr + sinh * sinp * cosr;
     m[0][2] = + sinh * cosp;
     m[1][0] = + cosp * sinr;
-    m[1][1] = + sinh * sinr + cosh * sinp * cosr;
+    m[1][1] = + cosp * cosr;
     m[1][2] = - sinp;
     m[2][0] = - sinh * cosr + cosh * sinp * sinr;
-    m[2][1] = + cosp * cosr;
+    m[2][1] = + sinh * sinr + cosh * sinp * cosr;
     m[2][2] = + cosh * cosp;
     }
 
diff --git a/src/edu/nps/moves/legacy/math/Matrix4f.java b/src/edu/nps/moves/legacy/math/Matrix4f.java
index e6689b9f45..8de81cad47 100644
--- a/src/edu/nps/moves/legacy/math/Matrix4f.java
+++ b/src/edu/nps/moves/legacy/math/Matrix4f.java
@@ -186,19 +186,19 @@ public void setEulers(float h, float p, float r) // Shoemake
     cosr = (float)Math.cos(r);
     sinr = (float)Math.sin(r);
 
-    m[0][0] = cosh * cosp;
-    m[0][1] = cosh * sinp * sinr - sinh * cosr;
-    m[0][2] = cosh * sinp * cosr + sinh * sinr;
+    m[0][0] = + cosh * cosr + sinh * sinp * sinr;
+    m[0][1] = - cosh * sinr + sinh * sinp * cosr;
+    m[0][2] = + sinh * cosp;
     m[0][3] = 0.0f;
 
-    m[1][0] = sinh * cosp;
-    m[1][1] = cosh * cosr + sinh * sinp * sinr;
-    m[1][2] = sinh * sinp * cosr - cosh * sinr;
+    m[1][0] = + cosp * sinr;
+    m[1][1] = + cosp * cosr;
+    m[1][2] = - sinp;
     m[1][3] = 0.0f;
 
-    m[2][0] = -sinp;
-    m[2][1] = cosp * sinr;
-    m[2][2] = cosp * cosr;
+    m[2][0] = - sinh * cosr + cosh * sinp * sinr;
+    m[2][1] = + sinh * sinr + cosh * sinp * cosr;
+    m[2][2] = + cosh * cosp;
     m[2][3] = 0.0f;
 
     m[3][0] = 0.0f;