Merge pull request #2832 from ousttrue/fix/spring_limit_clamp

[VRMC_springBone_limit] fix clamp

Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs

@@ -1,3 +1,4 @@
+using System;
 using UniGLTF.SpringBoneJobs.Blittables;
 using Unity.Mathematics;
 
@@ -5,62 +6,76 @@ namespace UniGLTF.SpringBoneJobs
 {
     public static class Anglelimit
     {
+        public static readonly float SINGULARITY_EPSILON = 1e-8f;
+
         public static float3 Apply(
-            in BlittableJointImmutable logic, in BlittableJointMutable joint,
-            in quaternion parentRotation, in float3 head, in float3 nextTail)
+            in BlittableJointImmutable logic,
+            in BlittableJointMutable joint,
+            in quaternion parentRotation,
+            in float3 head,
+            in float3 nextTail
+        )
         {
+            if (joint.anglelimitType == AnglelimitTypes.None)
+            {
+                // do nothing
+                return nextTail;
+            }
+
+            var angleSpaceToWorld = anglelimitSpaceToWorld(logic, joint, parentRotation);
+            var tailDir = math.mul(
+                math.inverse(angleSpaceToWorld),
+                math.normalizesafe(nextTail - head)
+            );
+
             switch (joint.anglelimitType)
             {
                 case AnglelimitTypes.None:
-                    // do nothing
-                    return nextTail;
+                    throw new Exception("not reach here");
 
                 case AnglelimitTypes.Cone:
-                    {
-                        var angleSpaceToWorld = anglelimitSpaceToWorld(logic, joint, parentRotation);
-                        var tailDir = math.mul(math.inverse(angleSpaceToWorld), math.normalizesafe(nextTail - head));
-                        tailDir = AnglelimitCone.Apply(tailDir, joint.anglelimit1);
-                        return head + math.mul(angleSpaceToWorld, tailDir) * logic.length;
-                    }
+
+                    tailDir = AnglelimitCone.Apply(tailDir, joint.anglelimit1);
+                    break;
 
                 case AnglelimitTypes.Hinge:
-                    {
-                        var angleSpaceToWorld = anglelimitSpaceToWorld(logic, joint, parentRotation);
-                        var tailDir = math.mul(math.inverse(angleSpaceToWorld), math.normalizesafe(nextTail - head));
-                        tailDir = AnglelimitHinge.Apply(tailDir, joint.anglelimit1);
-                        return head + math.mul(angleSpaceToWorld, tailDir) * logic.length;
-                    }
-
+                    tailDir = AnglelimitHinge.Apply(tailDir, joint.anglelimit1);
+                    break;
 
                 case AnglelimitTypes.Spherical:
-                    {
-                        var angleSpaceToWorld = anglelimitSpaceToWorld(logic, joint, parentRotation);
-                        var tailDir = math.mul(math.inverse(angleSpaceToWorld), math.normalizesafe(nextTail - head));
-                        tailDir = AnglelimitSpherical.Apply(tailDir, joint.anglelimit1, joint.anglelimit2);
-                        return head + math.mul(angleSpaceToWorld, tailDir) * logic.length;
-                    }
+                    tailDir = AnglelimitSpherical.Apply(
+                        tailDir,
+                        joint.anglelimit1,
+                        joint.anglelimit2
+                    );
+                    break;
 
                 default:
-                    throw new System.ArgumentException($"unknown joint.anglelimitType: {joint.anglelimitType}");
+                    throw new System.ArgumentException(
+                        $"unknown joint.anglelimitType: {joint.anglelimitType}"
+                    );
             }
+
+            return head + math.mul(angleSpaceToWorld, tailDir) * logic.length;
         }
 
         /// <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)
+        public static quaternion anglelimitSpaceToWorld(
+            in BlittableJointImmutable logic,
+            in BlittableJointMutable joint,
+            in quaternion parentRotation
+        )
         {
             // Y+方向からjointのheadからtailに向かうベクトルへの最小回転
             var axisRotation = getAxisRotation(logic.boneAxis);
 
             // limitのローカル空間をワールド空間に写像する回転
-            return
-                math.mul(parentRotation,
-                math.mul(logic.localRotation,
-                math.mul(axisRotation,
-                joint.anglelimitOffset)))
-            ;
+            return math.mul(
+                parentRotation,
+                math.mul(logic.localRotation, math.mul(axisRotation, joint.anglelimitOffset))
+            );
         }
 
         /// <summary>
@@ -71,20 +86,22 @@ namespace UniGLTF.SpringBoneJobs
         ///
         /// TODO: Replace with the appropriate link to the specification later
         /// </summary>
-        public static quaternion getAxisRotation(in float3 to)
+        public static quaternion getAxisRotation(in float3 boneAxis)
         {
-            // dot(from, to) + 1
-            var dot1 = to.y + 1f;
+            // headからtailに向かうベクトルとY+方向との内積
+            var dot = boneAxis.y;
 
-            // Handle the case where from and to are parallel and opposite
-            if (dot1 < 1e-8f) // dot is approximately -1
+            if (dot <= -1f + SINGULARITY_EPSILON)
             {
-                return new quaternion(1f, 0f, 0f, 0f);
+                // headからtailに向かうベクトルがY-方向の場合、X軸周りに180度回転させた回転を設定する
+                return new quaternion(1, 0, 0, 0);
+            }
+            else
+            {
+                // それ以外の場合、Y+方向からjointのheadからtailに向かうベクトルへの最小回転を設定する
+                // quaternion(cross(from, to); dot(from, to) + 1).normalized
+                return math.normalizesafe(new quaternion(boneAxis.z, 0, -boneAxis.x, dot + 1));
             }
-
-            // General case
-            // quaternion(cross(from, to); dot(from, to) + 1).normalized
-            return math.normalizesafe(new quaternion(to.z, 0f, -to.x, dot1));
         }
     }
-}
+}

Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs

@@ -4,30 +4,41 @@ namespace UniGLTF.SpringBoneJobs
 {
     public static class AnglelimitCone
     {
-        /// <param name="src">AngleLimit空間の方向ベクトル</param>
-        /// <param name="angleLimit">radius</param>
+        /// <param name="tailDir">AngleLimit空間の方向ベクトル</param>
+        /// <param name="limitAngle">radius</param>
         /// <returns>AngleLimit空間の方向ベクトル</returns>
-        public static float3 Apply(in float3 src, float angleLimit)
+        public static float3 Apply(float3 tailDir, float limitAngle)
         {
-            // tailDirのy要素をjointに設定されたangleの余弦と比較する
-            var cosAngle = math.cos(angleLimit);
-            if (src.y >= cosAngle)
-            {
-                return src;
-            }
+            // angleを0以上π以下に制限する
+            limitAngle = math.clamp(limitAngle, 0.0f, math.PI);
 
-            var tailDir = src;
+            // tailDirのy要素をlimitに設定されたangleの余弦と比較する
+            var cosLimitAngle = math.cos(limitAngle);
+            if (tailDir.y < cosLimitAngle)
             {
                 // x・z要素を、tailDirの正弦とjointに設定されたangleの正弦の比を用いてスケールする
-                var ratio = math.sqrt((1.0f - cosAngle * cosAngle) / (1.0f - tailDir.y * tailDir.y));
-                tailDir.x *= ratio;
-                tailDir.z *= ratio;
+                var horizontalLengthSquared = 1.0f - tailDir.y * tailDir.y;
 
-                // y要素を、jointに設定されたangleの余弦とする
-                tailDir.y = cosAngle;
+                if (horizontalLengthSquared <= Anglelimit.SINGULARITY_EPSILON)
+                {
+                    // tailDirがy軸負方向の場合、z軸正方向側を選択する
+                    tailDir.x = 0.0f;
+                    tailDir.z = math.sqrt(1.0f - cosLimitAngle * cosLimitAngle);
+                }
+                else
+                {
+                    var scale = math.sqrt(
+                        (1.0f - cosLimitAngle * cosLimitAngle) / horizontalLengthSquared
+                    );
+                    tailDir.x *= scale;
+                    tailDir.z *= scale;
+                }
+
+                // y要素をlimitに設定されたangleの余弦とする
+                tailDir.y = cosLimitAngle;
             }
 
             return tailDir;
         }
     }
-}
+}

Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs

@@ -4,29 +4,39 @@ namespace UniGLTF.SpringBoneJobs
 {
     public static class AnglelimitHinge
     {
-        /// <param name="src">AngleLimit空間の方向ベクトル</param>
-        /// <param name="angleLimit">radius</param>
+        /// <param name="tailDir">AngleLimit空間の方向ベクトル</param>
+        /// <param name="limitAngle">radius</param>
         /// <returns>AngleLimit空間の方向ベクトル</returns>
-        public static float3 Apply(in float3 src, float limitAngle)
+        public static float3 Apply(float3 tailDir, float limitAngle)
         {
-            // x要素を0にし、正規化する
-            float3 tailDir = src;
-            tailDir.x = 0.0f;
-            tailDir = math.normalizesafe(tailDir);
+            // angleを0以上π以下に制限する
+            limitAngle = math.clamp(limitAngle, 0.0f, math.PI);
 
-            // tailDirのy要素をjointに設定されたangleの余弦と比較する
-            var cosAngle = math.cos(limitAngle);
-            if (tailDir.y < cosAngle)
+            var projectedLengthSquared = tailDir.y * tailDir.y + tailDir.z * tailDir.z;
+            if (projectedLengthSquared <= Anglelimit.SINGULARITY_EPSILON)
             {
-                // z要素を、tailDirの正弦とjointに設定されたangleの正弦の比を用いてスケールする
-                var ratio = math.sqrt((1.0f - cosAngle * cosAngle) / (1.0f - tailDir.y * tailDir.y));
-                tailDir.z *= ratio;
-
-                // y要素を、jointに設定されたangleの余弦とする
-                tailDir.y = cosAngle;
+                // tailDirがx軸正方向または負方向の場合、Y軸正方向を選択する
+                tailDir = math.float3(0.0f, 1.0f, 0.0f);
             }
+            else
+            {
+                // tailDirをヒンジのYZ平面へ射影する
+                tailDir =
+                    math.float3(0.0f, tailDir.y, tailDir.z) / math.sqrt(projectedLengthSquared);
 
+                // tailDirのy要素をlimitに設定されたangleの余弦と比較する
+                var cosLimitAngle = math.cos(limitAngle);
+                if (tailDir.y < cosLimitAngle)
+                {
+                    var sinLimitAngle = math.sqrt(1.0f - cosLimitAngle * cosLimitAngle);
+
+                    // zの符号を維持し、z==0 の場合は z軸正方向側を選択する
+                    var zSign = (tailDir.z < 0.0f) ? -1.0f : 1.0f;
+                    tailDir.y = cosLimitAngle;
+                    tailDir.z = sinLimitAngle * zSign;
+                }
+            }
             return tailDir;
         }
     }
-}
+}

Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs

@@ -5,33 +5,55 @@ namespace UniGLTF.SpringBoneJobs
     public static class AnglelimitSpherical
     {
         /// <param name="tailDir">AngleLimit空間の方向ベクトル</param>
-        /// <param name="limitAnglePhi">radius</param>
-        /// <param name="limitAngleTheta">radius</param>
+        /// <param name="limitPitch">radius</param>
+        /// <param name="limitYaw">radius</param>
         /// <returns>AngleLimit空間の方向ベクトル</returns>
-        public static float3 Apply(in float3 tailDir, float limitAnglePhi, float limitAngleTheta)
+        public static float3 Apply(float3 tailDir, float limitPitch, float limitYaw)
         {
-            // tailDirのphi・thetaを計算する
-            var phi = math.atan2(tailDir.z, tailDir.y);
-            var theta = math.asin(tailDir.x);
+            // pitchを0以上π以下、yawを0以上π/2以下に制限する
+            limitPitch = math.clamp(limitPitch, 0.0f, math.PI);
+            limitYaw = math.clamp(limitYaw, 0.0f, math.PI / 2.0f);
 
-            // phi・thetaをjointに設定されたphi・thetaを用いて制限する
-            // var isLimited = false;
-            if (math.abs(phi) > limitAnglePhi)
+            // tailDirのpitch・yawを計算する
+            float pitch;
+            if (tailDir.y <= -1.0f + Anglelimit.SINGULARITY_EPSILON)
+            {
+                // tailDirがy軸負方向の場合、Z軸正方向側の境界を選択するため、pitchをπとする
+                pitch = math.PI;
+            }
+            else if (math.abs(tailDir.x) >= 1.0f - Anglelimit.SINGULARITY_EPSILON)
+            {
+                // tailDirがx軸正方向または負方向の場合、pitchを0とする
+                pitch = 0.0f;
+            }
+            else
+            {
+                pitch = math.atan2(tailDir.z, tailDir.y);
+            }
+            var yaw = math.asin(math.clamp(tailDir.x, -1f, 1f));
+
+            // pitchをlimitに設定されたpitchを用いて制限する
+            if (math.abs(pitch) > limitPitch)
             {
                 // isLimited = true;
-                phi = limitAnglePhi * math.sign(phi);
+                pitch = limitPitch * math.sign(pitch);
             }
 
-            // thetaをjointに設定されたthetaを用いて制限する
-            if (math.abs(theta) > limitAngleTheta)
+            // yawをlimitに設定されたyawを用いて制限する
+            if (math.abs(yaw) > limitYaw)
             {
-                //   isLimited = true;
-                theta = limitAngleTheta * math.sign(theta);
+                // isLimited = true;
+                yaw = limitYaw * math.sign(yaw);
             }
 
-            // tailDirをphi・thetaを用いて再計算する
-            var cos_theta = math.cos(theta);
-            return new float3(math.sin(theta), cos_theta * math.cos(phi), cos_theta * math.sin(phi));
+            // tailDirをpitch・yawを用いて再計算する
+            tailDir = math.float3(
+                math.sin(yaw),
+                math.cos(yaw) * math.cos(pitch),
+                math.cos(yaw) * math.sin(pitch)
+            );
+
+            return tailDir;
         }
     }
-}
+}

Packages/VRM10/Runtime/IO/Vrm10Importer.cs

@@ -1,11 +1,10 @@
 using System;
 using System.Collections.Generic;
-using System.ComponentModel;
 using System.Linq;
 using System.Threading.Tasks;
 using UniGLTF;
-using UniGLTF.Extensions.VRMC_springBone_limit;
 using UniGLTF.Utils;
+using Unity.Mathematics;
 using UnityEngine;
 
 namespace UniVRM10
@@ -655,20 +654,20 @@ namespace UniVRM10
                                 {
                                     joint.m_anglelimitType = UniGLTF.SpringBoneJobs.AnglelimitTypes.Cone;
                                     joint.m_limitSpaceOffset = QuaternionFromFloat4(cone.Rotation);
-                                    joint.m_pitch = cone.Angle.GetValueOrDefault();
+                                    joint.m_pitch = math.clamp(cone.Angle.GetValueOrDefault(), 0.0f, math.PI);
                                 }
                                 else if (extensionSpringBoneLimit.Limit.Hinge is UniGLTF.Extensions.VRMC_springBone_limit.HingeLimit hinge)
                                 {
                                     joint.m_anglelimitType = UniGLTF.SpringBoneJobs.AnglelimitTypes.Hinge;
                                     joint.m_limitSpaceOffset = QuaternionFromFloat4(hinge.Rotation);
-                                    joint.m_pitch = hinge.Angle.GetValueOrDefault();
+                                    joint.m_pitch = math.clamp(hinge.Angle.GetValueOrDefault(), 0.0f, math.PI);
                                 }
                                 else if (extensionSpringBoneLimit.Limit.Spherical is UniGLTF.Extensions.VRMC_springBone_limit.SphericalLimit spherical)
                                 {
                                     joint.m_anglelimitType = UniGLTF.SpringBoneJobs.AnglelimitTypes.Spherical;
                                     joint.m_limitSpaceOffset = QuaternionFromFloat4(spherical.Rotation);
-                                    joint.m_pitch = spherical.Pitch.GetValueOrDefault();
-                                    joint.m_yaw = spherical.Yaw.GetValueOrDefault();
+                                    joint.m_pitch = math.clamp(spherical.Pitch.GetValueOrDefault(), 0.0f, math.PI);
+                                    joint.m_yaw = math.clamp(spherical.Yaw.GetValueOrDefault(), 0.0f, math.PI / 2);
                                 }
                             }