From 1863fa973640cb6e2972f6786dbafdd680481a46 Mon Sep 17 00:00:00 2001 From: Sylvain Date: Thu, 18 Jan 2024 21:47:46 +0100 Subject: [PATCH 1/7] Implemented finite difference for orientation 3D. --- .../RotationMatrixConversion.java | 3 +- .../RotationVectorConversion.java | 22 +- .../us/ihmc/euclid/tools/AxisAngleTools.java | 47 +- .../euclid/tools/EuclidCoreRandomTools.java | 11 +- .../us/ihmc/euclid/tools/EuclidCoreTools.java | 156 +++++-- .../us/ihmc/euclid/tools/QuaternionTools.java | 248 ++++++++++- .../euclid/tools/RotationMatrixTools.java | 409 ++++++++++++++++- .../ihmc/euclid/tools/YawPitchRollTools.java | 118 ++++- .../RotationVectorConversionTest.java | 66 ++- .../euclid/tools/EuclidCoreToolsTest.java | 414 +++++++++++------- .../euclid/tools/QuaternionToolsTest.java | 150 ++++++- 11 files changed, 1342 insertions(+), 302 deletions(-) diff --git a/src/main/java/us/ihmc/euclid/rotationConversion/RotationMatrixConversion.java b/src/main/java/us/ihmc/euclid/rotationConversion/RotationMatrixConversion.java index 5bf3929cf..b80f55971 100644 --- a/src/main/java/us/ihmc/euclid/rotationConversion/RotationMatrixConversion.java +++ b/src/main/java/us/ihmc/euclid/rotationConversion/RotationMatrixConversion.java @@ -39,7 +39,7 @@ private RotationMatrixConversion() } /** - * Sets the given rotation matrix to represent a counter clockwise rotation around the z-axis of an + * Sets the given rotation matrix to represent a counter-clockwise rotation around the z-axis of an * angle {@code yaw}. * * 
@@ -429,7 +429,6 @@ public static void convertYawPitchRollToMatrix(double yaw, double pitch, double
          ((RotationMatrixBasics) matrixToPack).setUnsafe(m00, m01, m02, m10, m11, m12, m20, m21, m22);
       else
          matrixToPack.set(m00, m01, m02, m10, m11, m12, m20, m21, m22);
-
    }
 
    /**
diff --git a/src/main/java/us/ihmc/euclid/rotationConversion/RotationVectorConversion.java b/src/main/java/us/ihmc/euclid/rotationConversion/RotationVectorConversion.java
index 2340160d1..c09463480 100644
--- a/src/main/java/us/ihmc/euclid/rotationConversion/RotationVectorConversion.java
+++ b/src/main/java/us/ihmc/euclid/rotationConversion/RotationVectorConversion.java
@@ -179,7 +179,7 @@ public static void convertMatrixToRotationVector(RotationMatrixReadOnly rotation
       double m21 = rotationMatrix.getM21();
       double m22 = rotationMatrix.getM22();
 
-      convertMatrixToRotationVectorImpl(m00, m01, m02, m10, m11, m12, m20, m21, m22, rotationVectorToPack);
+      convertMatrixToRotationVector(m00, m01, m02, m10, m11, m12, m20, m21, m22, rotationVectorToPack);
    }
 
    /**
@@ -220,16 +220,16 @@ public static void convertMatrixToRotationVector(RotationMatrixReadOnly rotation
     *                             conversion.
     * @param rotationVectorToPack the vector in which the result is stored. Modified.
     */
-   static void convertMatrixToRotationVectorImpl(double m00,
-                                                 double m01,
-                                                 double m02,
-                                                 double m10,
-                                                 double m11,
-                                                 double m12,
-                                                 double m20,
-                                                 double m21,
-                                                 double m22,
-                                                 Vector3DBasics rotationVectorToPack)
+   public static void convertMatrixToRotationVector(double m00,
+                                                    double m01,
+                                                    double m02,
+                                                    double m10,
+                                                    double m11,
+                                                    double m12,
+                                                    double m20,
+                                                    double m21,
+                                                    double m22,
+                                                    Vector3DBasics rotationVectorToPack)
    {
       if (EuclidCoreTools.containsNaN(m00, m01, m02, m10, m11, m12, m20, m21, m22))
       {
diff --git a/src/main/java/us/ihmc/euclid/tools/AxisAngleTools.java b/src/main/java/us/ihmc/euclid/tools/AxisAngleTools.java
index 952be8a43..48cc8a630 100644
--- a/src/main/java/us/ihmc/euclid/tools/AxisAngleTools.java
+++ b/src/main/java/us/ihmc/euclid/tools/AxisAngleTools.java
@@ -13,6 +13,7 @@
 import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly;
 import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
 import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
+import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics;
 import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics;
 import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
 import us.ihmc.euclid.tuple4D.interfaces.Vector4DBasics;
@@ -156,7 +157,7 @@ public static void addTransform(AxisAngleReadOnly axisAngle, Tuple3DReadOnly tup
     * @param checkIfTransformInXYPlane whether this method should assert that the axis-angle represents
     *                                  a transformation in the XY plane.
     * @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the axis-angle
-    *                               does not represent a transformation in the XY plane.
+    *       does not represent a transformation in the XY plane.
     */
    public static void transform(AxisAngleReadOnly axisAngle, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane)
    {
@@ -181,7 +182,7 @@ public static void transform(AxisAngleReadOnly axisAngle, Tuple2DReadOnly tupleO
     * @param checkIfTransformInXYPlane whether this method should assert that the axis-angle represents
     *                                  a transformation in the XY plane.
     * @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the axis-angle
-    *                               does not represent a transformation in the XY plane.
+    *       does not represent a transformation in the XY plane.
     */
    public static void inverseTransform(AxisAngleReadOnly axisAngle,
                                        Tuple2DReadOnly tupleOriginal,
@@ -548,9 +549,8 @@ public static void multiply(Orientation3DReadOnly orientation1,
       }
 
       double beta, u2x, u2y, u2z;
-      if (orientation2 instanceof AxisAngleReadOnly)
+      if (orientation2 instanceof AxisAngleReadOnly aa2)
       { // In this case orientation2 might be the same object as axisAngleToPack, so let's save its components first.
-         AxisAngleReadOnly aa2 = (AxisAngleReadOnly) orientation2;
          beta = aa2.getAngle();
          u2x = aa2.getX();
          u2y = aa2.getY();
@@ -1118,7 +1118,7 @@ public static void appendRollRotation(AxisAngleReadOnly axisAngleOriginal, doubl
     * @param orientation3D the orientation3D to be used for comparison. Not modified
     * @param limitToPi     limits the result to [0 , pi].
     * @return angular distance between the two orientations in range: [0, 2pi] when limitToPi =
-    *         false.
+    *       false.
     */
    public static double distance(AxisAngleReadOnly axisAngle, Orientation3DReadOnly orientation3D, boolean limitToPi)
    {
@@ -1151,7 +1151,7 @@ public static double distance(AxisAngleReadOnly axisAngle, Orientation3DReadOnly
     * @param quaternion the quaternion to be used for comparison. Not modified
     * @param limitToPi  limits the result to [0 , pi] if set true.
     * @return angular distance between the two orientations in range: [0, 2pi] when limitToPi =
-    *         false.
+    *       false.
     */
    public static double distance(AxisAngleReadOnly axisAngle, QuaternionReadOnly quaternion, boolean limitToPi)
    {
@@ -1242,7 +1242,7 @@ public static double distance(AxisAngleReadOnly axisAngle, RotationMatrixReadOnl
     * @param yawPitchRoll the yawPitchRoll to be used for comparison. Not modified
     * @param limitToPi    Limits the result to [0, pi].
     * @return angular distance between the two orientations in range: [0, 2pi] when limitToPi =
-    *         false.
+    *       false.
     */
    public static double distance(AxisAngleReadOnly axisAngle, YawPitchRollReadOnly yawPitchRoll, boolean limitToPi)
    {
@@ -1305,7 +1305,7 @@ public static double distance(AxisAngleReadOnly axisAngle, YawPitchRollReadOnly
     * @param aa2       the second axis-angle to measure the distance. Not modified.
     * @param limitToPi Limits the result to [0, pi].
     * @return the angle representing the distance between the two axis-angles. It is contained in [0,
-    *         2pi] when limitToPi = false.
+    *       2pi] when limitToPi = false.
     */
    public static double distance(AxisAngleReadOnly aa1, AxisAngleReadOnly aa2, boolean limitToPi)
    {
@@ -1352,4 +1352,35 @@ static double distance(AxisAngleReadOnly aa1, double u2x, double u2y, double u2z
       }
       return Math.abs(gamma);
    }
+
+   /**
+    * Computes the angular velocity from the finite difference of two orientations.
+    *
+    * @param previousOrientation   the orientation at the previous time step. Not modified.
+    * @param currentOrientation    the orientation at the current time step. Not modified.
+    * @param dt                    the time step.
+    * @param angularVelocityToPack the vector used to store the angular velocity expressed in the orientation's local coordinates. Modified.
+    * @see EuclidCoreTools#finiteDifference(Orientation3DReadOnly, Orientation3DReadOnly, double, Vector3DBasics)
+    */
+   public static void finiteDifference(AxisAngleReadOnly previousOrientation,
+                                       AxisAngleReadOnly currentOrientation,
+                                       double dt,
+                                       Vector3DBasics angularVelocityToPack)
+   {
+      double halfAnglePrev = 0.5 * previousOrientation.getAngle();
+      double sinHalfAnglePrev = Math.sin(halfAnglePrev);
+      double xPrev = previousOrientation.getX() * sinHalfAnglePrev;
+      double yPrev = previousOrientation.getY() * sinHalfAnglePrev;
+      double zPrev = previousOrientation.getZ() * sinHalfAnglePrev;
+      double sPrev = Math.cos(halfAnglePrev);
+
+      double halfAngleCurr = 0.5 * currentOrientation.getAngle();
+      double sinHalfAngleCurr = Math.sin(halfAngleCurr);
+      double xCurr = currentOrientation.getX() * sinHalfAngleCurr;
+      double yCurr = currentOrientation.getY() * sinHalfAngleCurr;
+      double zCurr = currentOrientation.getZ() * sinHalfAngleCurr;
+      double sCurr = Math.cos(halfAngleCurr);
+
+      QuaternionTools.finiteDifference(xPrev, yPrev, zPrev, sPrev, xCurr, yCurr, zCurr, sCurr, dt, angularVelocityToPack);
+   }
 }
diff --git a/src/main/java/us/ihmc/euclid/tools/EuclidCoreRandomTools.java b/src/main/java/us/ihmc/euclid/tools/EuclidCoreRandomTools.java
index 86f5073ea..0b828addd 100644
--- a/src/main/java/us/ihmc/euclid/tools/EuclidCoreRandomTools.java
+++ b/src/main/java/us/ihmc/euclid/tools/EuclidCoreRandomTools.java
@@ -1,9 +1,6 @@
 package us.ihmc.euclid.tools;
 
-import java.util.Random;
-
 import org.ejml.data.DMatrixRMaj;
-
 import us.ihmc.euclid.Axis2D;
 import us.ihmc.euclid.Axis3D;
 import us.ihmc.euclid.axisAngle.AxisAngle;
@@ -40,6 +37,8 @@
 import us.ihmc.euclid.tuple4D.Vector4D32;
 import us.ihmc.euclid.yawPitchRoll.YawPitchRoll;
 
+import java.util.Random;
+
 /**
  * This class provides random generators to generate random geometry objects.
  * 

@@ -1556,11 +1555,9 @@ public static void randomizeAxisAngle(Random random, AxisAngleBasics axisAngleTo
     */
    public static void randomizeAxisAngle(Random random, double minMaxAngle, AxisAngleBasics axisAngleToRandomize)
    {
-      // Generate uniformly random point on unit sphere (based on http://mathworld.wolfram.com/SpherePointPicking.html )
-      double height = 2.0 * random.nextDouble() - 1.0;
+      // Generate uniformly random point on the unit-sphere (based on http://mathworld.wolfram.com/SpherePointPicking.html )
       double angle = nextDouble(random, minMaxAngle);
-      double radius = EuclidCoreTools.squareRoot(1.0 - height * height);
-      axisAngleToRandomize.set(radius * EuclidCoreTools.cos(angle), radius * EuclidCoreTools.sin(angle), height, angle);
+      axisAngleToRandomize.set(EuclidCoreRandomTools.nextVector3D(random), angle);
    }
 
    /**
diff --git a/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java b/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java
index 02c9f7121..a80d83348 100644
--- a/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java
+++ b/src/main/java/us/ihmc/euclid/tools/EuclidCoreTools.java
@@ -1,20 +1,25 @@
 package us.ihmc.euclid.tools;
 
-import java.util.Collections;
-import java.util.List;
-
 import org.ejml.MatrixDimensionException;
 import org.ejml.data.Matrix;
-
+import us.ihmc.euclid.axisAngle.interfaces.AxisAngleReadOnly;
 import us.ihmc.euclid.interfaces.EuclidGeometry;
 import us.ihmc.euclid.matrix.interfaces.Matrix3DReadOnly;
+import us.ihmc.euclid.matrix.interfaces.RotationMatrixReadOnly;
+import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly;
 import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;
 import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly;
 import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly;
 import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly;
 import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
+import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics;
 import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
 import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
+import us.ihmc.euclid.yawPitchRoll.interfaces.YawPitchRollReadOnly;
+
+import java.util.Collections;
+import java.util.List;
+import java.util.Objects;
 
 /**
  * This class provides a variety of generic tools such as fast square-root algorithm
@@ -467,7 +472,7 @@ public static double fastSquareRoot(double squaredValueClosedToOne)
     * @param a the first element.
     * @param b the second element.
     * @return {@code true} if at least one element is equal to {@link Double#NaN}, {@code false}
-    *         otherwise.
+    *       otherwise.
     */
    public static boolean containsNaN(double a, double b)
    {
@@ -481,7 +486,7 @@ public static boolean containsNaN(double a, double b)
     * @param b the second element.
     * @param c the third element.
     * @return {@code true} if at least one element is equal to {@link Double#NaN}, {@code false}
-    *         otherwise.
+    *       otherwise.
     */
    public static boolean containsNaN(double a, double b, double c)
    {
@@ -496,7 +501,7 @@ public static boolean containsNaN(double a, double b, double c)
     * @param c the third element.
     * @param d the fourth element.
     * @return {@code true} if at least one element is equal to {@link Double#NaN}, {@code false}
-    *         otherwise.
+    *       otherwise.
     */
    public static boolean containsNaN(double a, double b, double c, double d)
    {
@@ -516,7 +521,7 @@ public static boolean containsNaN(double a, double b, double c, double d)
     * @param a7 the eighth element.
     * @param a8 the ninth element.
     * @return {@code true} if at least one element is equal to {@link Double#NaN}, {@code false}
-    *         otherwise.
+    *       otherwise.
     */
    public static boolean containsNaN(double a0, double a1, double a2, double a3, double a4, double a5, double a6, double a7, double a8)
    {
@@ -524,9 +529,7 @@ public static boolean containsNaN(double a0, double a1, double a2, double a3, do
          return true;
       if (Double.isNaN(a3) || Double.isNaN(a4) || Double.isNaN(a5))
          return true;
-      if (Double.isNaN(a6) || Double.isNaN(a7) || Double.isNaN(a8))
-         return true;
-      return false;
+      return Double.isNaN(a6) || Double.isNaN(a7) || Double.isNaN(a8);
    }
 
    /**
@@ -534,7 +537,7 @@ public static boolean containsNaN(double a0, double a1, double a2, double a3, do
     *
     * @param array the array containing the elements to test for {@link Double#NaN}. Not modified.
     * @return {@code true} if at least one element is equal to {@link Double#NaN}, {@code false}
-    *         otherwise.
+    *       otherwise.
     */
    public static boolean containsNaN(double[] array)
    {
@@ -806,7 +809,7 @@ public static final double max(double a, double b, double c)
     * @param d the fourth argument to compare.
     * @return the maximum value of the four arguments.
     */
-   public static final double max(double a, double b, double c, double d)
+   public static double max(double a, double b, double c, double d)
    {
       if (a > b)
       {
@@ -849,7 +852,7 @@ public static final double min(double a, double b, double c)
     * @param d the fourth argument to compare.
     * @return the minimum value of the four arguments.
     */
-   public static final double min(double a, double b, double c, double d)
+   public static double min(double a, double b, double c, double d)
    {
       if (a < b)
       {
@@ -942,7 +945,7 @@ public static boolean epsilonEquals(double expectedValue, double actualValue, do
     */
    public static boolean equals(EuclidGeometry a, EuclidGeometry b)
    {
-      return (a == b) || (a != null && a.equals(b));
+      return Objects.equals(a, b);
    }
 
    /**
@@ -1051,7 +1054,7 @@ public static boolean areAllZero(double x, double y, double z, double s, double
     * @param actualAngle   the second angle in the comparison.
     * @param epsilon       the tolerance to use for the test.
     * @return {@code true} if the two angles are considered to be geometrically equal, {@code false}
-    *         otherwise.
+    *       otherwise.
     */
    public static boolean angleGeometricallyEquals(double expectedAngle, double actualAngle, double epsilon)
    {
@@ -1081,11 +1084,10 @@ public static boolean isAngleZero(double angle, double epsilon)
     *
     * @param value  value
     * @param minMax inclusive absolute boundary
-    * @return
-    *         
  • {@code -minMax} if {@code value} is less than {@code -minMax}
    • 
-    *         
  • {@code minMax} if {@code value} is greater than {@code minMax}
    • 
-    *         
  • {@code value} if {@code value} is between or equal to {@code -minMax} and
-    *         {@code minMax}
    • 
+    * @return 
  • {@code -minMax} if {@code value} is less than {@code -minMax}
    • 
+    *       
  • {@code minMax} if {@code value} is greater than {@code minMax}
    • 
+    *       
  • {@code value} if {@code value} is between or equal to {@code -minMax} and
+    *       {@code minMax}
    • 
     */
    public static double clamp(double value, double minMax)
    {
@@ -1098,11 +1100,10 @@ public static double clamp(double value, double minMax)
     * @param value value
     * @param min   inclusive boundary start
     * @param max   inclusive boundary end
-    * @return
-    *         
  • {@code min} if {@code value} is less than {@code min}
    • 
-    *         
  • {@code max} if {@code value} is greater than {@code max}
    • 
-    *         
  • {@code value} if {@code value} is between or equal to {@code min} and
-    *         {@code max}
    • 
+    * @return 
  • {@code min} if {@code value} is less than {@code min}
    • 
+    *       
  • {@code max} if {@code value} is greater than {@code max}
    • 
+    *       
  • {@code value} if {@code value} is between or equal to {@code min} and
+    *       {@code max}
    • 
     */
    public static double clamp(double value, double min, double max)
    {
@@ -1163,8 +1164,8 @@ public static double atan(double a)
     * @param y the ordinate coordinate
     * @param x the abscissa coordinate
     * @return the theta component of the point (r, theta) in polar
-    *         coordinates that corresponds to the point (x, y) in Cartesian
-    *         coordinates.
+    *       coordinates that corresponds to the point (x, y) in Cartesian
+    *       coordinates.
     * @see Math#atan2(double, double)
     */
    public static double atan2(double y, double x)
@@ -1253,8 +1254,9 @@ public static void checkMatrixMinimumSize(int minRows, int minColumns, Matrix ma
    {
       if (matrixToTest.getNumCols() < minColumns || matrixToTest.getNumRows() < minRows)
       {
-         throw new MatrixDimensionException("The matrix is too small, expected: [nRows >= " + minRows + ", nColumns >= " + minColumns + "], was: [nRows = "
-               + matrixToTest.getNumRows() + ", nCols = " + matrixToTest.getNumCols() + "].");
+         throw new MatrixDimensionException(
+               "The matrix is too small, expected: [nRows >= " + minRows + ", nColumns >= " + minColumns + "], was: [nRows = " + matrixToTest.getNumRows()
+               + ", nCols = " + matrixToTest.getNumCols() + "].");
       }
    }
 
@@ -1265,7 +1267,7 @@ public static void checkMatrixMinimumSize(int minRows, int minColumns, Matrix ma
     * This method is garbage free and is equivalent to
     * {@code Collections.reverse(list.subList(fromIndex, toIndex))}.
     * 
    
-    * 
+    *
     * @param list      the list whose elements are to be reversed. Modified.
     * @param fromIndex low endpoint (inclusive) of the range to be reversed.
     * @param toIndex   high endpoint (exclusive) of the range to be reversed.
@@ -1301,7 +1303,7 @@ public static void reverse(List list, int fromIndex, int toIndex)
     * {@code [9, 2, 0, 1, 9]}.
     * 
     * 
    
-    * 
+    *
     * @param        the element type.
     * @param list      the list whose elements are to be rotated. Modified.
     * @param fromIndex low endpoint (inclusive) of the range to be rotated.
@@ -1365,7 +1367,7 @@ public static int wrap(int index, int listSize)
    }
 
    /**
-    * Increments then recomputes the given {@code index} such that it is ∈ [0, {@code listSize}[.
+    * Increments the given {@code index} such that it is ∈ [0, {@code listSize}[.
     * Examples:
     *