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;