RotationUtil: more functions for quaternions

microavia · Apr 22, 2020 · 4613433 · 4613433
1 parent 92ea81e
commit 4613433
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 6 deletions.
diff --git a/jmalib-log/pom.xml b/jmalib-log/pom.xml
@@ -5,7 +5,7 @@
     <parent>
         <groupId>com.microavia</groupId>
         <artifactId>jmalib</artifactId>
-        <version>0.1.1</version>
+        <version>0.1.2</version>
     </parent>
     <modelVersion>4.0.0</modelVersion>
 

diff --git a/jmalib-math/pom.xml b/jmalib-math/pom.xml
@@ -5,7 +5,7 @@
     <parent>
         <artifactId>jmalib</artifactId>
         <groupId>com.microavia</groupId>
-        <version>0.1.1</version>
+        <version>0.1.2</version>
     </parent>
     <modelVersion>4.0.0</modelVersion>
 

diff --git a/jmalib-math/src/main/java/com/microavia/jmalib/math/RotationUtil.java b/jmalib-math/src/main/java/com/microavia/jmalib/math/RotationUtil.java
@@ -2,8 +2,7 @@
 
 import java.util.Arrays;
 
-import static java.lang.Math.cos;
-import static java.lang.Math.sin;
+import static java.lang.Math.*;
 
 /**
  * User: ton Date: 02.06.13 Time: 20:20
@@ -23,6 +22,22 @@ public static double[] rotationMatrixByEulerAngles(double roll, double pitch, do
         };
     }
 
+    public static double[] quaternionByAngleAxis(double[] angleAxis) {
+        double a = sqrt(angleAxis[0] * angleAxis[0] + angleAxis[1] * angleAxis[1] + angleAxis[2] * angleAxis[2]);
+        if (a > 1.0e-10) {
+            double b = sin(a / 2) / a;
+            return new double[]{cos(a / 2), angleAxis[0] * b, angleAxis[1] * b, angleAxis[2] * b};
+        } else {
+            return new double[]{1, 0, 0, 0};
+        }
+    }
+
+    public static double[] quaternionByEulerAngles(double roll, double pitch, double yaw) {
+        double[] q = quaternionByAngleAxis(new double[]{0, 0, yaw});
+        q = quaternionMult(quaternionByAngleAxis(new double[]{0, pitch, 0}), q);
+        return quaternionMult(quaternionByAngleAxis(new double[]{roll, 0, 0}), q);
+    }
+
     public static double[] rotationMatrixByQuaternion(double[] q) {
         double aSq = q[0] * q[0];
         double bSq = q[1] * q[1];
@@ -55,7 +70,7 @@ public static double[] rotateByQuaternion(double[] v, double[] q) {
         return Arrays.copyOfRange(quaternionMult(quaternionMult(q, qV), qConj), 1, 4);
     }
 
-    private static double[] quaternionMult(double[] q1, double[] q2) {
+    public static double[] quaternionMult(double[] q1, double[] q2) {
         return new double[]{
                 q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3],
                 q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2],

diff --git a/pom.xml b/pom.xml
@@ -7,7 +7,7 @@
     <groupId>com.microavia</groupId>
     <artifactId>jmalib</artifactId>
     <packaging>pom</packaging>
-    <version>0.1.1</version>
+    <version>0.1.2</version>
     <modules>
         <module>jmalib-log</module>
         <module>jmalib-math</module>