From abafd14ff2c4db4ff0f86cf37114d8e50b80ac6a Mon Sep 17 00:00:00 2001
From: ousttrue <ousttrue@gmail.com>
Date: Fri, 23 Apr 2021 14:17:05 +0900
Subject: [PATCH] WIP ObjectSpace.model

---
 .../Components/Constraint/ConstraintDestination.cs | 12 ++++++++++--
 Assets/VRM10/Runtime/Components/Constraint/TRS.cs  | 10 ++++++++++
 .../Constraint/VRM10PositionConstraint.cs          | 12 +++++++++++-
 .../Constraint/VRM10RotationConstraint.cs          | 14 ++++++++++++--
 4 files changed, 43 insertions(+), 5 deletions(-)

diff --git a/Assets/VRM10/Runtime/Components/Constraint/ConstraintDestination.cs b/Assets/VRM10/Runtime/Components/Constraint/ConstraintDestination.cs
index 54dbb4525..505c387d8 100644
--- a/Assets/VRM10/Runtime/Components/Constraint/ConstraintDestination.cs
+++ b/Assets/VRM10/Runtime/Components/Constraint/ConstraintDestination.cs
@@ -11,7 +11,7 @@ namespace UniVRM10
 
         readonly TRS m_initial;
 
-        public ConstraintDestination(Transform t, ObjectSpace coords)
+        public ConstraintDestination(Transform t, ObjectSpace coords, Transform modelRoot = null)
         {
             m_transform = t;
             m_coords = coords;
@@ -26,6 +26,10 @@ namespace UniVRM10
                     m_initial = TRS.GetLocal(t);
                     break;
 
+                case ObjectSpace.model:
+                    m_initial = TRS.GetRelative(t, modelRoot.worldToLocalMatrix);
+                    break;
+
                 default:
                     throw new NotImplementedException();
             }
@@ -49,7 +53,7 @@ namespace UniVRM10
             }
         }
 
-        public void ApplyRotation(Quaternion delta, float weight)
+        public void ApplyRotation(Quaternion delta, float weight, Transform modelRoot = null)
         {
             // 0~1 で clamp しない slerp
             var value = Quaternion.LerpUnclamped(Quaternion.identity, delta, weight) * m_initial.Rotation;
@@ -63,6 +67,10 @@ namespace UniVRM10
                     m_transform.localRotation = value;
                     break;
 
+                case ObjectSpace.model:
+                    m_transform.rotation = modelRoot.rotation * value;
+                    break;
+
                 default:
                     throw new NotImplementedException();
             }
diff --git a/Assets/VRM10/Runtime/Components/Constraint/TRS.cs b/Assets/VRM10/Runtime/Components/Constraint/TRS.cs
index 1881b77ac..fffdf7c55 100644
--- a/Assets/VRM10/Runtime/Components/Constraint/TRS.cs
+++ b/Assets/VRM10/Runtime/Components/Constraint/TRS.cs
@@ -27,5 +27,15 @@ namespace UniVRM10
                 Scale = t.localScale,
             };
         }
+
+        public static TRS GetRelative(Transform t, Matrix4x4 toRelative)
+        {
+            return new TRS
+            {
+                Translation = toRelative.MultiplyPoint(t.position),
+                Rotation = toRelative.rotation * t.rotation,
+                Scale = toRelative.MultiplyVector(t.lossyScale),
+            };
+        }
     }
 }
diff --git a/Assets/VRM10/Runtime/Components/Constraint/VRM10PositionConstraint.cs b/Assets/VRM10/Runtime/Components/Constraint/VRM10PositionConstraint.cs
index 614b69d1d..a8b9e1b6d 100644
--- a/Assets/VRM10/Runtime/Components/Constraint/VRM10PositionConstraint.cs
+++ b/Assets/VRM10/Runtime/Components/Constraint/VRM10PositionConstraint.cs
@@ -42,6 +42,16 @@ namespace UniVRM10
             m_dst = null;
         }
 
+        void Reset()
+        {
+            var current = transform;
+            while (current.parent != null)
+            {
+                current = current.parent;
+            }
+            ModelRoot = current;
+        }
+
         public override void Process()
         {
             if (Source == null)
@@ -56,7 +66,7 @@ namespace UniVRM10
             }
             if (m_dst == null)
             {
-                m_dst = new ConstraintDestination(transform, DestinationCoordinate);
+                m_dst = new ConstraintDestination(transform, DestinationCoordinate, ModelRoot);
             }
 
             var delta = FreezeAxes.Freeze(m_src.TranslationDelta);
diff --git a/Assets/VRM10/Runtime/Components/Constraint/VRM10RotationConstraint.cs b/Assets/VRM10/Runtime/Components/Constraint/VRM10RotationConstraint.cs
index b747417bd..0263fbbec 100644
--- a/Assets/VRM10/Runtime/Components/Constraint/VRM10RotationConstraint.cs
+++ b/Assets/VRM10/Runtime/Components/Constraint/VRM10RotationConstraint.cs
@@ -43,6 +43,16 @@ namespace UniVRM10
             m_dst = null;
         }
 
+        void Reset()
+        {
+            var current = transform;
+            while (current.parent != null)
+            {
+                current = current.parent;
+            }
+            ModelRoot = current;
+        }
+
         /// <summary>
         /// SourceのUpdateよりも先か後かはその時による。
         /// 厳密に制御するのは無理。
@@ -61,7 +71,7 @@ namespace UniVRM10
             }
             if (m_dst == null)
             {
-                m_dst = new ConstraintDestination(transform, DestinationCoordinate);
+                m_dst = new ConstraintDestination(transform, DestinationCoordinate, ModelRoot);
             }
 
             // 軸制限をしたオイラー角
@@ -70,7 +80,7 @@ namespace UniVRM10
             var rotation = Quaternion.Euler(fleezed);
             // Debug.Log($"{delta} => {rotation}");
             // オイラー角を再度Quaternionへ。weight を加味してSlerpする
-            m_dst.ApplyRotation(rotation, Weight);
+            m_dst.ApplyRotation(rotation, Weight, ModelRoot);
         }
     }
 }