diff --git a/Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs b/Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs index b15ce10fa..06090da62 100644 --- a/Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs +++ b/Packages/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs @@ -1,37 +1,61 @@ +using System; using Unity.Mathematics; +using UnityEngine; namespace UniGLTF.SpringBoneJobs { public static class AnglelimitSpherical { /// AngleLimit空間の方向ベクトル - /// radius - /// radius + /// radius + /// radius /// AngleLimit空間の方向ベクトル - 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.0) + { + // tailDirがy軸負方向の場合、Z軸正方向側の境界を選択するため、pitchをπとする + pitch = math.PI; + } + else if (math.abs(tailDir.x) == 1.0f) + { + // tailDirがx軸正方向または負方向の場合、pitchを0とする + pitch = 0.0f; + } + else + { + pitch = math.atan2(tailDir.z, tailDir.y); + } + var yaw = math.asin(tailDir.x); + + // 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; } } -} \ No newline at end of file +}