Tool to differentiate orientation

Some doc and cleanup

Adding 4x4 Qdot to omega transform.

work-in-progress...

Initial version of 4x4 Qdot -> omega transform

Add important clarifying comment.

Author: SylvainBertrand

ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidCoreMissingTools.java

@@ -4,6 +4,8 @@
 import static us.ihmc.euclid.geometry.tools.EuclidGeometryTools.ONE_TRILLIONTH;
 import static us.ihmc.euclid.tools.EuclidCoreTools.normSquared;
 
+import org.ejml.data.DMatrixRMaj;
+
 import us.ihmc.commons.MathTools;
 import us.ihmc.euclid.Axis3D;
 import us.ihmc.euclid.geometry.tools.EuclidGeometryTools;
@@ -21,9 +23,13 @@
 import us.ihmc.euclid.tuple2D.Point2D;
 import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics;
 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.*;
+import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics;
+import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
+import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
+import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DReadOnly;
+import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics;
+import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
 import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics;
 import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
 
@@ -93,7 +99,7 @@ public static void projectRotationOnAxis(QuaternionReadOnly rotation, Vector3DRe
    {
       double dotProduct = rotation.getX() * axis.getX() + rotation.getY() * axis.getY() + rotation.getZ() * axis.getZ();
 
-      double scale = dotProduct / axis.lengthSquared();
+      double scale = dotProduct / axis.normSquared();
       double projectedX = scale * axis.getX();
       double projectedY = scale * axis.getY();
       double projectedZ = scale * axis.getZ();
@@ -1088,14 +1094,14 @@ public static boolean intersectionBetweenRay2DAndLine2D(Point2DReadOnly rayOrigi
     * Just a more thorough version
     */
    public static boolean intersectionBetweenRay2DAndLine2D(double rayOriginX,
-                                                                  double rayOriginY,
-                                                                  double rayDirectionX,
-                                                                  double rayDirectionY,
-                                                                  double lineOriginX,
-                                                                  double lineOriginY,
-                                                                  double lineDirectionX,
-                                                                  double lineDirectionY,
-                                                                  Point2DBasics intersectionToPack)
+                                                           double rayOriginY,
+                                                           double rayDirectionX,
+                                                           double rayDirectionY,
+                                                           double lineOriginX,
+                                                           double lineOriginY,
+                                                           double lineDirectionX,
+                                                           double lineDirectionY,
+                                                           Point2DBasics intersectionToPack)
    {
       double start1x = rayOriginX;
       double start1y = rayOriginY;
@@ -1138,7 +1144,7 @@ private static boolean intersectionBetweenTwoLine2DsImpl(double start1x,
 
       if (Math.abs(determinant) < epsilon)
       { // The lines are parallel
-         // Check if they are collinear
+        // Check if they are collinear
          double dx = start2x - start1x;
          double dy = start2y - start1y;
          double cross = dx * direction1y - dy * direction1x;
@@ -1266,4 +1272,70 @@ private static boolean intersectionBetweenTwoLine2DsImpl(double start1x,
 
       return areIntersecting;
    }
+
+   /**
+    * Calculate the angular velocity by differentiating orientation.
+    * 
+    * @param qStart                the initial orientation at time t.
+    * @param qEnd                  the final orientation at time t + duration.
+    * @param duration              the time interval between the 2 orientations.
+    * @param angularVelocityToPack the angular velocity.
+    */
+   public static void differentiateOrientation(QuaternionReadOnly qStart, QuaternionReadOnly qEnd, double duration, Vector3DBasics angularVelocityToPack)
+   {
+      double q1x = qStart.getX();
+      double q1y = qStart.getY();
+      double q1z = qStart.getZ();
+      double q1s = qStart.getS();
+
+      double q2x = qEnd.getX();
+      double q2y = qEnd.getY();
+      double q2z = qEnd.getZ();
+      double q2s = qEnd.getS();
+
+      double diffx = q1s * q2x - q1x * q2s - q1y * q2z + q1z * q2y;
+      double diffy = q1s * q2y + q1x * q2z - q1y * q2s - q1z * q2x;
+      double diffz = q1s * q2z - q1x * q2y + q1y * q2x - q1z * q2s;
+      double diffs = q1s * q2s + q1x * q2x + q1y * q2y + q1z * q2z;
+
+      if (diffs < 0.0)
+      {
+         diffx = -diffx;
+         diffy = -diffy;
+         diffz = -diffz;
+         diffs = -diffs;
+      }
+
+      double sinHalfTheta = EuclidCoreTools.norm(diffx, diffy, diffz);
+
+      double angle;
+      if (EuclidCoreTools.epsilonEquals(1.0, diffs, 1.0e-12))
+         angle = 2.0 * sinHalfTheta / diffs;
+      else
+         angle = 2.0 * EuclidCoreTools.atan2(sinHalfTheta, diffs);
+      angularVelocityToPack.set(diffx, diffy, diffz);
+      angularVelocityToPack.scale(angle / (sinHalfTheta * duration));
+   }
+
+   // *** NOTE ***: The 4x4 output matrix produced by this method assumes a Quaternion component ordering of:
+   //   Quat = [ Qs
+   //            Qx
+   //            Qy
+   //            Qz ]
+   public static DMatrixRMaj quaternionDotToOmegaTransform(QuaternionReadOnly rotatingFrameQuaternion)
+   {
+      double qs = rotatingFrameQuaternion.getS();
+      double qx = rotatingFrameQuaternion.getX();
+      double qy = rotatingFrameQuaternion.getY();
+      double qz = rotatingFrameQuaternion.getZ();
+
+      DMatrixRMaj E = new DMatrixRMaj(4,4);
+
+      E.set(0,0, qs); E.set(0,1, qx); E.set(0,2, qy); E.set(0,3, qz);
+      E.set(1,0,-qx); E.set(1,1, qs); E.set(1,2, qz); E.set(1,3,-qy);
+      E.set(2,0,-qy); E.set(2,1,-qz); E.set(2,2, qs); E.set(2,3, qx);
+      E.set(3,0,-qz); E.set(3,1, qy); E.set(3,2,-qx); E.set(3,3, qs);
+      
+      return E;
+   }
 }