From d8970a0363c071c529459104a41139597f1d4f85 Mon Sep 17 00:00:00 2001
From: Golui <golui889@gmail.com>
Date: Sun, 10 May 2020 00:26:18 +0200
Subject: [PATCH] Basic SwSh Animation previewing

---
 .../Pokemon/GFLX/GFBANIM/GFBANM.cs            | 61 +++++++++++++++----
 File_Format_Library/Main.cs                   |  2 +-
 2 files changed, 50 insertions(+), 13 deletions(-)

diff --git a/File_Format_Library/FileFormats/Pokemon/GFLX/GFBANIM/GFBANM.cs b/File_Format_Library/FileFormats/Pokemon/GFLX/GFBANIM/GFBANM.cs
index 94d43264..50c5f7a4 100644
--- a/File_Format_Library/FileFormats/Pokemon/GFLX/GFBANIM/GFBANM.cs
+++ b/File_Format_Library/FileFormats/Pokemon/GFLX/GFBANIM/GFBANM.cs
@@ -208,20 +208,15 @@ namespace FirstPlugin
 
                             Console.WriteLine("3ds X bits " + Convert.ToString(14, 2));
 
-                            float x = PackedToQuat(value1);
-                            float y = PackedToQuat(value2);
-                            float z = PackedToQuat(value3);
-                            float w = (float)Math.Sqrt(1 - x - y - z);
-
                             if (b.Text == "Waist") {
                                 var quat = EulerToQuat(1.570796f, -1.313579f, 0.03490628f);
                                 Console.WriteLine($"quat og {quat.X} {quat.Y} {quat.Z} {quat.W}");
                                 Console.WriteLine("group " + b.Text);
                                 Console.WriteLine($"packed rot {value1} {value2} {value3}");
-                                Console.WriteLine($"quat rot X {x} Y {y} Z {z} W {w}");
+                                // Console.WriteLine($"quat rot X {x} Y {y} Z {z} W {w}");
                             }
-
-                            b.rot = new Quaternion(x, y, z, w);
+                                
+                            b.rot = PackedToQuat(value1, value2, value3);
                         }
                         else
                         {
@@ -238,12 +233,54 @@ namespace FirstPlugin
 
            // private static readonly ushort _flagsMask = 0b11000011_11111111;
 
-            private static float PackedToQuat(short val)
+            private static short UnpackS15(short u15)
             {
-             //   Console.WriteLine("bin1 " + Convert.ToString(val, 2));
-              //  Console.WriteLine("bin2 " + Convert.ToString(val, 2));
+                int sign = (u15 >> 14) & 1;
+                u15 &= 0x3FFF;
+                if (sign == 0) u15 -= 0x4000;
+                return u15;
+            }
+
+            static int[][] QUATERNION_SWIZZLES = { new int[] { 0, 3, 2, 1 }, new int[] { 3, 0, 2, 1 }, new int[] { 3, 2, 0, 1 }, new int[] { 3, 2, 1, 0 } };
+
+            private static Quaternion PackedToQuat(short z, short y, short x)
+            {
+
+                const int count = 15;
+                const int BASE = (1 << count) - 1;
+                float maxval = 1 / (0x399E * (float)Math.Sqrt(2.0)); // such obvious, so constant, wow
+
+                long cq = x & 0xFFFF;
+                cq <<= 16;
+                cq |= ((uint)y) & 0xFFFF;
+                cq <<= 16;
+                cq |= ((uint)z) & 0xFFFF;
+
+
+                short extra = (short)(cq & 0x7);
+
+                long num = cq >> 3;
+
+                x = UnpackS15((short)((num >> (count * 2)) & BASE));
+                y = UnpackS15((short)((num >> (count * 1)) & BASE));
+                z = UnpackS15((short)((num >> (count * 0)) & BASE));
+
+                float fx = x * maxval;
+                float fy = y * maxval;
+                float fz = z * maxval;
+
+                float[] quat = {
+                    (float)Math.Sqrt(1 - fx * fx - fy * fy - fz * fz),
+                    fx,
+                    fy,
+                    fz };
+
+                int[] qmap = QUATERNION_SWIZZLES[extra & 0b11];
+                Quaternion q = new Quaternion(quat[qmap[0]], quat[qmap[1]], quat[qmap[2]], quat[qmap[3]]);
+                if ((extra >> 2) != 0) q *= -1;
+
+                return q;
 
-                return val / 0x8000f;
             }
 
             public enum RotationFlags : ushort
diff --git a/File_Format_Library/Main.cs b/File_Format_Library/Main.cs
index 6113deb1..957d3ed7 100644
--- a/File_Format_Library/Main.cs
+++ b/File_Format_Library/Main.cs
@@ -392,7 +392,7 @@ namespace FirstPlugin
             Formats.Add(typeof(IGA_PAK));
             Formats.Add(typeof(MKAGPDX_Model));
             Formats.Add(typeof(GFBMDL));
-         //  Formats.Add(typeof(GFBANM));
+            Formats.Add(typeof(GFBANM));
             Formats.Add(typeof(GFBANMCFG));
             Formats.Add(typeof(Turbo.Course_MapCamera_bin));
             Formats.Add(typeof(SDF));