impl Anglelimit

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs

@@ -0,0 +1,84 @@
+using UniGLTF.SpringBoneJobs.Blittables;
+using Unity.Mathematics;
+
+namespace UniGLTF.SpringBoneJobs
+{
+    public static class Anglelimit
+    {
+        public static float3 Apply(
+            in BlittableJointImmutable logic, in BlittableJointMutable joint,
+            in quaternion parentRotation, in float3 head, in float3 nextTail)
+        {
+            switch (joint.anglelimitType)
+            {
+                case AnglelimitTypes.None:
+                    // do nothing
+                    return nextTail;
+
+                case AnglelimitTypes.Cone:
+                    {
+                        var angleSpaceToWorld = anglelimitSpaceToWorld(logic, joint, parentRotation);
+                        var tailDir = math.mul(math.inverse(angleSpaceToWorld), nextTail - head);
+                        tailDir = AnglelimitCone.Apply(tailDir, joint.anglelimit1);
+                        return math.mul(angleSpaceToWorld, tailDir);
+                    }
+
+                case AnglelimitTypes.Hinge:
+                    {
+                        var angleSpaceToWorld = anglelimitSpaceToWorld(logic, joint, parentRotation);
+                        var tailDir = math.mul(math.inverse(angleSpaceToWorld), nextTail - head);
+                        tailDir = AnglelimitHinge.Apply(tailDir, joint.anglelimit1);
+                        return math.mul(angleSpaceToWorld, tailDir);
+                    }
+
+
+                case AnglelimitTypes.Spherical:
+                    {
+                        var angleSpaceToWorld = anglelimitSpaceToWorld(logic, joint, parentRotation);
+                        var tailDir = math.mul(math.inverse(angleSpaceToWorld), nextTail - head);
+                        tailDir = AnglelimitSpherical.Apply(tailDir, joint.anglelimit1, joint.anglelimit2);
+                        return math.mul(angleSpaceToWorld, tailDir);
+                    }
+
+                default:
+                    throw new System.ArgumentException($"unknown joint.anglelimitType: {joint.anglelimitType}");
+            }
+        }
+
+        /// <param name="nextTail">nextTail(position vector in world space)</param>
+        /// <returns>tailDir(directionay vector in angle space)</returns>
+        /// <exception cref="System.NotImplementedException"></exception>
+        public static quaternion anglelimitSpaceToWorld(in BlittableJointImmutable logic, in BlittableJointMutable joint,
+        in quaternion parentRotation)
+        {
+            // Y+方向からjointのheadからtailに向かうベクトルへの最小回転
+            var axisRotation = fromToQuaternion(new float3(0, 1, 0), logic.boneAxis);
+
+            // limitのローカル空間をワールド空間に写像する回転
+            return
+                math.mul(parentRotation,
+                math.mul(logic.localRotation,
+                math.mul(axisRotation,
+                joint.anglelimitOffset)))
+            ;
+        }
+
+        // https://discussions.unity.com/t/unity-mathematics-equivalent-to-quaternion-fromtorotation/237459
+        public static quaternion fromToQuaternion(in float3 from, in float3 to)
+        {
+            return quaternion.AxisAngle(
+                  angle: math.acos(
+                    math.clamp(
+                      math.dot(
+                        math.normalize(from),
+                        math.normalize(to)
+                      ),
+                    -1f,
+                  1f)
+                  ),
+                  axis: math.normalize(math.cross(from, to))
+                );
+        }
+
+    }
+}

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs.meta

@@ -1,5 +1,5 @@
 fileFormatVersion: 2
-guid: e38e2998df511664cb0d6e582e6a05a3
+guid: 3c2f98858052fbe4c89b445bad4c979b
 MonoImporter:
   externalObjects: {}
   serializedVersion: 2

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs

@@ -1,36 +0,0 @@
-using Unity.Mathematics;
-
-namespace UniGLTF.SpringBoneJobs
-{
-    public static class AnglelimitSpace
-    {
-        public static float3 ToTailDir(in quaternion src)
-        {
-            // // Y+方向からjointのheadからtailに向かうベクトルへの最小回転
-            // let axisRotation = fromToQuaternion(vec3(0, 1, 0), boneAxis);
-
-            // // limitのローカル空間をワールド空間に写像する回転
-            // let rotation =
-            //   joint.parent.worldRotation *
-            //   joint.localSpaceInitialRotation *
-            //   axisRotation *
-            //   joint.limit.rotation;
-
-            // // tailの向きをlimitのローカル空間に写像する
-            // tailDir = tailDir.applyQuaternion(rotation.inverse);
-
-            // // limitを適用する
-            // joint.limit.apply(tailDir);
-
-            // // tailの向きをワールド空間に戻す
-            // tailDir = tailDir.applyQuaternion(rotation);
-
-            throw new System.NotImplementedException();
-        }
-
-        public static quaternion FromTailDir(in float3 src)
-        {
-            throw new System.NotImplementedException();
-        }
-    }
-}

Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs

@@ -94,6 +94,8 @@ namespace UniGLTF.SpringBoneJobs
                 // 長さをboneLengthに強制
                 nextTail = headTransform.position + math.normalize(nextTail - headTransform.position) * logic.length;
 
+                nextTail = Anglelimit.Apply(logic, joint, parentRotation, head: headTransform.position, nextTail: nextTail);
+
                 // Collisionで移動
                 for (var colliderIndex = colliderSpan.startIndex; colliderIndex < colliderSpan.startIndex + colliderSpan.count; ++colliderIndex)
                 {
@@ -131,6 +133,8 @@ namespace UniGLTF.SpringBoneJobs
                     }
                 }
 
+                nextTail = Anglelimit.Apply(logic, joint, parentRotation, head: headTransform.position, nextTail: nextTail);
+
                 NextTail[logicIndex] = centerTransform.HasValue
                     ? MathHelper.MultiplyPoint3x4(centerTransform.Value.worldToLocalMatrix, nextTail)
                     : nextTail;