diff --git a/Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs b/Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs index 45f291929..6010d94be 100644 --- a/Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs +++ b/Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs @@ -4,29 +4,39 @@ namespace UniGLTF.SpringBoneJobs { public static class AnglelimitHinge { - /// AngleLimit空間の方向ベクトル - /// radius + /// AngleLimit空間の方向ベクトル + /// radius /// AngleLimit空間の方向ベクトル - 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 == 0.0f) { - // 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); + + // tailDirがy軸負方向の場合、z軸正方向側を選択する + var zSign = (tailDir.z < 0.0f) ? -1.0f : 1.0f; + tailDir.y = cosLimitAngle; + tailDir.z = sinLimitAngle * zSign; + } + } return tailDir; } } -} \ No newline at end of file +}