Added conversion eulerAnglesByQuaternion

hsteinhaus · Jan 5, 2015 · a631983 · a631983
1 parent 3a8b542
commit a631983
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/src/me/drton/jmavlib/conversion/RotationConversion.java b/src/me/drton/jmavlib/conversion/RotationConversion.java
@@ -23,4 +23,12 @@ public static Matrix rotationMatrixByEulerAngles(double roll, double pitch, doub
         R.set(2, 2, cos(roll) * cos(pitch));
         return R;
     }
+
+    public static double[] eulerAnglesByQuaternion(double[] q) {
+        return new double[]{
+                Math.atan2(2.0 * (q[0] * q[1] + q[2] * q[3]), 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])),
+                Math.asin(2 * (q[0] * q[2] - q[3] * q[1])),
+                Math.atan2(2.0 * (q[0] * q[3] + q[1] * q[2]), 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3])),
+        };
+    }
 }