Merge pull request #1087 from ousttrue/fix/matrix_extraction

Nodeの行列分解時の負のスケールの限定対応
vrm-c · Jul 1, 2021 · 17c6140 · 17c6140
2 parents ed51975 + 7874e72
commit 17c6140
Show file tree

Hide file tree

Showing 4 changed files with 133 additions and 4 deletions.
diff --git a/Assets/UniGLTF/Runtime/Extensions/UnityExtensions.cs b/Assets/UniGLTF/Runtime/Extensions/UnityExtensions.cs
@@ -180,6 +180,55 @@ public static Vector3 ExtractScale(this Matrix4x4 matrix)
             return scale;
         }
 
+        public static bool Nearly(in Matrix4x4 lhs, in Matrix4x4 rhs, float epsilon = 1e-3f)
+        {
+            if (Mathf.Abs(lhs.m00 - rhs.m00) > epsilon) return false;
+            if (Mathf.Abs(lhs.m01 - rhs.m01) > epsilon) return false;
+            if (Mathf.Abs(lhs.m02 - rhs.m02) > epsilon) return false;
+            if (Mathf.Abs(lhs.m03 - rhs.m03) > epsilon) return false;
+            if (Mathf.Abs(lhs.m10 - rhs.m10) > epsilon) return false;
+            if (Mathf.Abs(lhs.m11 - rhs.m11) > epsilon) return false;
+            if (Mathf.Abs(lhs.m12 - rhs.m12) > epsilon) return false;
+            if (Mathf.Abs(lhs.m13 - rhs.m13) > epsilon) return false;
+            if (Mathf.Abs(lhs.m20 - rhs.m20) > epsilon) return false;
+            if (Mathf.Abs(lhs.m21 - rhs.m21) > epsilon) return false;
+            if (Mathf.Abs(lhs.m22 - rhs.m22) > epsilon) return false;
+            if (Mathf.Abs(lhs.m23 - rhs.m23) > epsilon) return false;
+            if (Mathf.Abs(lhs.m30 - rhs.m30) > epsilon) return false;
+            if (Mathf.Abs(lhs.m31 - rhs.m31) > epsilon) return false;
+            if (Mathf.Abs(lhs.m32 - rhs.m32) > epsilon) return false;
+            if (Mathf.Abs(lhs.m33 - rhs.m33) > epsilon) return false;
+            return true;
+        }
+
+        public static (Vector3 T, Quaternion R, Vector3 S) Extract(this Matrix4x4 m)
+        {
+            if (m.determinant < 0)
+            {
+                // ミラーリングを試行する
+
+                // -X
+                {
+                    var mm = m * Matrix4x4.Scale(new Vector3(-1, 1, 1));
+                    var ss = mm.ExtractScale();
+                    mm = mm * Matrix4x4.Scale(new Vector3(1 / ss.x, 1 / ss.y, 1 / ss.z));
+                    var tt = mm.ExtractPosition();
+                    var rr = mm.ExtractRotation();
+                    ss.x = -ss.x;
+                    var mmm = Matrix4x4.TRS(tt, rr, ss);
+                    if (Nearly(m, mmm))
+                    {
+                        return (tt, rr, ss);
+                    }
+                }
+            }
+
+            var s = m.ExtractScale();
+            var t = m.ExtractPosition();
+            var r = m.ExtractRotation();
+            return (t, r, s);
+        }
+
         public static string RelativePathFrom(this Transform self, Transform root)
         {
             var path = new List<String>();

diff --git a/Assets/UniGLTF/Runtime/UniGLTF/IO/NodeImporter.cs b/Assets/UniGLTF/Runtime/UniGLTF/IO/NodeImporter.cs
@@ -50,9 +50,7 @@ public static GameObject ImportNode(glTFNode node, int nodeIndex)
             if (node.matrix != null && node.matrix.Length > 0)
             {
                 var m = UnityExtensions.MatrixFromArray(node.matrix);
-                go.transform.localRotation = m.ExtractRotation();
-                go.transform.localPosition = m.ExtractPosition();
-                go.transform.localScale = m.ExtractScale();
+                (go.transform.localPosition, go.transform.localRotation, go.transform.localScale) = m.Extract();
             }
             return go;
         }
@@ -158,7 +156,7 @@ public static void FixCoordinate(glTF gltf, List<TransformWithSkin> nodes, IAxis
             }
         }
 
-        public static void SetupSkinning(glTF gltf,  List<TransformWithSkin> nodes, int i, IAxisInverter inverter)
+        public static void SetupSkinning(glTF gltf, List<TransformWithSkin> nodes, int i, IAxisInverter inverter)
         {
             var x = nodes[i];
             var skinnedMeshRenderer = x.Transform.GetComponent<SkinnedMeshRenderer>();

diff --git a/Assets/UniGLTF/Tests/UniGLTF/MatrixTests.cs b/Assets/UniGLTF/Tests/UniGLTF/MatrixTests.cs
@@ -0,0 +1,71 @@
+using NUnit.Framework;
+using UnityEngine;
+
+namespace UniGLTF
+{
+    public class MatrixTests
+    {
+        static void AssertFloat(float expected, float value, string template, float epsilon = 1e-5f)
+        {
+            Assert.True(Mathf.Abs(value - expected) < epsilon, string.Format(template, expected, value));
+        }
+
+        static void AssertQuaternion(in Quaternion expected, in Quaternion value, string message)
+        {
+            AssertFloat(expected.x, value.x, $"{message}" + ".X: {0} != {1}");
+            AssertFloat(expected.y, value.y, $"{message}" + ".Y: {0} != {1}");
+            AssertFloat(expected.z, value.z, $"{message}" + ".Z: {0} != {1}");
+            AssertFloat(expected.w, value.w, $"{message}" + ".W: {0} != {1}");
+        }
+
+        static void AssertVec3(in Vector3 expected, in Vector3 value, string message)
+        {
+            AssertFloat(expected.x, value.x, $"{message}" + ".X: {0} != {1}");
+            AssertFloat(expected.y, value.y, $"{message}" + ".Y: {0} != {1}");
+            AssertFloat(expected.z, value.z, $"{message}" + ".Z: {0} != {1}");
+        }
+
+        static void AssertExtract(in Vector3 t, in Quaternion r, in Vector3 s)
+        {
+            var m = Matrix4x4.TRS(t, r, s);
+            var (et, er, es) = m.Extract();
+            AssertVec3(t, et, "T");
+            AssertQuaternion(r, er, "R");
+            AssertVec3(s, es, "S");
+        }
+
+        [Test]
+        public void ExtractTest()
+        {
+            AssertExtract(new Vector3(1, 2, 3), Quaternion.Euler(0, 90, -90), new Vector3(1, 2, 3));
+        }
+
+        [Test]
+        public void ExtractMirror_MinusX()
+        {
+            AssertExtract(new Vector3(1, 2, 3), Quaternion.Euler(0, 90, -90), new Vector3(-1, 2, 3));
+        }
+
+#if false
+        // TODO:
+        [Test]
+        public void ExtractMirror_MinusY()
+        {
+
+            AssertExtract(new Vector3(1, 2, 3), Quaternion.Euler(0, 90, -90), new Vector3(1, -2, 3));
+        }
+
+        [Test]
+        public void ExtractMirror_MinusZ()
+        {
+            AssertExtract(new Vector3(1, 2, 3), Quaternion.Euler(0, 90, -90), new Vector3(1, 2, -3));
+        }
+
+        [Test]
+        public void ExtractMirror_MinusXYZ()
+        {
+            AssertExtract(new Vector3(1, 2, 3), Quaternion.Euler(0, 90, -90), new Vector3(-1, -2, -3));
+        }
+#endif
+    }
+}
diff --git a/Assets/UniGLTF/Tests/UniGLTF/MatrixTests.cs.meta b/Assets/UniGLTF/Tests/UniGLTF/MatrixTests.cs.meta