Slerp + sort animation fix

Author: iam4722202468

src/main/java/net/worldseed/multipart/Quaternion.java

@@ -3,21 +3,25 @@
 import net.minestom.server.coordinate.Point;
 import net.minestom.server.coordinate.Vec;
 
-final class Quaternion {
+final public class Quaternion {
     private final double x;
     private final double y;
     private final double z;
 
     private final double w;
 
-    Quaternion(double x, double y, double z, double w) {
+    public Quaternion(double x, double y, double z, double w) {
         this.x = x;
         this.y = y;
         this.z = z;
         this.w = w;
     }
 
-    Quaternion(Point p) {
+    public Quaternion() {
+        this(0, 0, 0, 0);
+    }
+
+    public Quaternion(Point p) {
         p = ModelMath.toRadians(p);
 
         double cy = Math.cos(p.z() * 0.5);
@@ -33,7 +37,23 @@ final class Quaternion {
         z = cr * cp * sy - sr * sp * cy;
     }
 
-    Point toEuler() {
+    public double x() {
+        return x;
+    }
+
+    public double y() {
+        return y;
+    }
+
+    public double z() {
+        return z;
+    }
+
+    public double w() {
+        return w;
+    }
+
+    public Point toEuler() {
         double t0 = (x+z)*(x-z);        // x^2-z^2
         double t1 = (w+y)*(w-y);        // w^2-y^2
         double xx = 0.5*(t0+t1);        // 1/2 x of x'
@@ -84,7 +104,7 @@ Point threeAxisRot(double r11, double r12, double r21, double r31, double r32){
         return new Vec(x,z,y);
     }
 
-    Point toEulerYZX() {
+    public Point toEulerYZX() {
         Quaternion q = this;
 
         return ModelMath.toDegrees(threeAxisRot( -2*(q.x*q.z - q.w*q.y),

src/main/java/net/worldseed/multipart/animations/Interpolator.java

@@ -2,6 +2,7 @@
 
 import net.minestom.server.coordinate.Point;
 import net.minestom.server.coordinate.Pos;
+import net.worldseed.multipart.Quaternion;
 
 import java.util.LinkedHashMap;
 
@@ -23,17 +24,53 @@ private static StartEnd getStartEnd(double time, LinkedHashMap<Double, Point> tr
         return new StartEnd(lastPoint, lastPoint, lastTime, animationTime);
     }
 
+    static Quaternion slerp(Quaternion qa, Quaternion qb, double t) {
+        // quaternion to return
+        // Calculate angle between them.
+        double cosHalfTheta = qa.w() * qb.w() + qa.x() * qb.x() + qa.y() * qb.y() + qa.z() * qb.z();
+        // if qa=qb or qa=-qb then theta = 0 and we can return qa
+        if (Math.abs(cosHalfTheta) >= 1.0){
+            double qmw = qa.w();
+            double qmx = qa.x();
+            double qmy = qa.y();
+            double qmz = qa.z();
+            return new Quaternion(qmx, qmy, qmz, qmw);
+        }
+        // Calculate temporary values.
+        double halfTheta = Math.acos(cosHalfTheta);
+        double sinHalfTheta = Math.sqrt(1.0 - cosHalfTheta*cosHalfTheta);
+        // if theta = 180 degrees then result is not fully defined
+        // we could rotate around any axis normal to qa or qb
+        if (Math.abs(sinHalfTheta) < 0.001){ // fabs is floating point absolute
+            double qmw = (qa.w() * 0.5 + qb.w() * 0.5);
+            double qmx = (qa.x() * 0.5 + qb.x() * 0.5);
+            double qmy = (qa.y() * 0.5 + qb.y() * 0.5);
+            double qmz = (qa.z() * 0.5 + qb.z() * 0.5);
+            return new Quaternion(qmx, qmy, qmz, qmw);
+        }
+        double ratioA = Math.sin((1 - t) * halfTheta) / sinHalfTheta;
+        double ratioB = Math.sin(t * halfTheta) / sinHalfTheta;
+        //calculate Quaternion.
+        double qmw = (qa.w() * ratioA + qb.w() * ratioB);
+        double qmx = (qa.x() * ratioA + qb.x() * ratioB);
+        double qmy = (qa.y() * ratioA + qb.y() * ratioB);
+        double qmz = (qa.z() * ratioA + qb.z() * ratioB);
+        return new Quaternion(qmx, qmy, qmz, qmw);
+    }
+
     static Point interpolate(double time, LinkedHashMap<Double, Point> transform, double animationTime) {
         StartEnd points = getStartEnd(time, transform, animationTime);
 
         double timeDiff = points.et - points.st;
 
+        Quaternion qa = new Quaternion(points.s);
+        Quaternion qb = new Quaternion(points.e);
+
         if (timeDiff == 0)
             return points.s;
 
         double timePercent = (time - points.st) / timeDiff;
-
-        return points.e.sub(points.s).mul(timePercent).add(points.s);
+        return slerp(qa, qb, timePercent).toEuler();
     }
 
 }

src/main/java/net/worldseed/multipart/animations/ModelAnimation.java

@@ -76,6 +76,8 @@ public ModelAnimation(String modelName, String animationName, ModelBone bone, Js
                 transform.put(0.0, point);
             }
 
+            System.out.println("[AnimationLoader] Generating cache for " + bone.getName() + "/" + animationName);
+
             if (this.type == AnimationLoader.AnimationType.ROTATION) {
                 found = calculateAllTransforms(animationTime, transform);
                 AnimationLoader.addToRotationCache(modelName, bone.getName() + "/" + animationName, found);

src/main/java/net/worldseed/multipart/parser/generator/AnimationGenerator.java

@@ -4,8 +4,7 @@
 import com.google.gson.JsonElement;
 import com.google.gson.JsonObject;
 
-import java.util.Map;
-import java.util.Set;
+import java.util.*;
 
 public class AnimationGenerator {
     public static JsonObject generate(JsonArray animationRaw) {
@@ -25,8 +24,8 @@ public static JsonObject generate(JsonArray animationRaw) {
 
                 String boneName = animator.get("name").getAsString();
 
-                JsonObject rotation = new JsonObject();
-                JsonObject position = new JsonObject();
+                List<Map.Entry<Double, JsonObject>> rotation = new ArrayList<>();
+                List<Map.Entry<Double, JsonObject>> position = new ArrayList<>();
 
                 JsonArray keyframes = animator.get("keyframes").getAsJsonArray();
 
@@ -44,15 +43,29 @@ public static JsonObject generate(JsonArray animationRaw) {
                     built.addProperty("lerp_mode", interpolation);
 
                     if (channel.equals("rotation")) {
-                        rotation.add(String.valueOf(time), built);
+                        rotation.add(Map.entry(time, built));
                     } else if (channel.equals("position")) {
-                        position.add(String.valueOf(time), built);
+                        position.add(Map.entry(time, built));
                     }
                 }
 
+                rotation.sort(Map.Entry.comparingByKey());
+                position.sort(Map.Entry.comparingByKey());
+
+                JsonObject rotationJson = new JsonObject();
+                JsonObject positionJson = new JsonObject();
+
+                for (var rotation_ : rotation) {
+                    rotationJson.add(rotation_.getKey().toString(), rotation_.getValue());
+                }
+
+                for (var position_ : position) {
+                    positionJson.add(position_.getKey().toString(), position_.getValue());
+                }
+
                 JsonObject built = new JsonObject();
-                built.add("rotation", rotation);
-                built.add("position", position);
+                built.add("rotation", rotationJson);
+                built.add("position", positionJson);
 
                 bones.add(boneName, built);
             }