commit a4999c9dd024024ffbc62223f3889220ac8a4449
Author: Leszek Koltunski <leszek@koltunski.pl>
Date:   Tue May 23 18:37:51 2023 +0200

    speedup

diff --git a/src/main/java/org/distorted/library/helpers/QuatHelper.java b/src/main/java/org/distorted/library/helpers/QuatHelper.java
index 8631e96..474723c 100644
--- a/src/main/java/org/distorted/library/helpers/QuatHelper.java
+++ b/src/main/java/org/distorted/library/helpers/QuatHelper.java
@@ -49,6 +49,29 @@ public class QuatHelper
     return new Static4D(tx,ty,tz,tw);
     }
 
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// return quat1*quat2
+
+  public static float[] quatMultiply( float[] quat1, float[] quat2 )
+    {
+    float qx = quat1[0];
+    float qy = quat1[1];
+    float qz = quat1[2];
+    float qw = quat1[3];
+
+    float rx = quat2[0];
+    float ry = quat2[1];
+    float rz = quat2[2];
+    float rw = quat2[3];
+
+    float tx = rw*qx - rz*qy + ry*qz + rx*qw;
+    float ty = rw*qy + rz*qx + ry*qw - rx*qz;
+    float tz = rw*qz + rz*qw - ry*qx + rx*qy;
+    float tw = rw*qw - rz*qz - ry*qy - rx*qx;
+
+    return new float[] {tx,ty,tz,tw};
+    }
+
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 // return quat1*(rx,ry,rz,rw)
 
@@ -166,6 +189,22 @@ public class QuatHelper
     quatMultiply(output,tmp[0],tmp[1],tmp[2],tmp[3],-qx,-qy,-qz,qw);
     }
 
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// rotate (x1,x2,x3,x4) by quat  ( i.e. return quat*vector*(quat^-1) )
+
+  public static void rotateVectorByQuat(float[] output, float x, float y, float z, float w, float[] quat)
+    {
+    float[] tmp = new float[4];
+
+    float qx = quat[0];
+    float qy = quat[1];
+    float qz = quat[2];
+    float qw = quat[3];
+
+    quatMultiply(tmp,qx,qy,qz,qw,x,y,z,w);
+    quatMultiply(output,tmp[0],tmp[1],tmp[2],tmp[3],-qx,-qy,-qz,qw);
+    }
+
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 // rotate vec by quat ( i.e. return quat*vector*(quat^-1) )