Streamingle_URP/Assets/Scripts/KindRetargeting/TwoBoneIKSolver.cs

using UnityEngine;
using RootMotion.FinalIK;

namespace KindRetargeting
{
    /// <summary>
    /// FinalIK IKSolverTrigonometric.Solve()를 사용하는 IK 래퍼.
    /// 4개 사지(양팔, 양다리)에 대해 FinalIK의 검증된 코사인 법칙 솔버를 호출합니다.
    /// </summary>
    [DefaultExecutionOrder(6)]
    public class TwoBoneIKSolver : MonoBehaviour
    {
        [System.Serializable]
        public class LimbIK
        {
            public bool enabled = true;
            public Transform target;
            public Transform bendGoal;

            [Range(0f, 1f)] public float positionWeight = 0f;
            [Range(0f, 1f)] public float rotationWeight = 0f;
            [Range(0f, 1f)] public float bendGoalWeight = 0f;

            [HideInInspector] public Transform upper;
            [HideInInspector] public Transform lower;
            [HideInInspector] public Transform end;
            [HideInInspector] public float upperLength;
            [HideInInspector] public float lowerLength;

            // 초기 벤드 법선 (upper 본 로컬 공간 — FinalIK 방식)
            [HideInInspector] public Vector3 localBendNormal;
        }

        [HideInInspector] public Animator animator;

        [Header("팔")]
        public LimbIK leftArm = new LimbIK();
        public LimbIK rightArm = new LimbIK();

        [Header("다리")]
        public LimbIK leftLeg = new LimbIK();
        public LimbIK rightLeg = new LimbIK();

        private bool isInitialized;

        private void Start()
        {
            Initialize();
        }

        public void Initialize()
        {
            if (animator == null)
                animator = GetComponent<Animator>();
            if (animator == null || !animator.isHuman) return;

            CacheLimb(leftArm, HumanBodyBones.LeftUpperArm, HumanBodyBones.LeftLowerArm, HumanBodyBones.LeftHand);
            CacheLimb(rightArm, HumanBodyBones.RightUpperArm, HumanBodyBones.RightLowerArm, HumanBodyBones.RightHand);
            CacheLimb(leftLeg, HumanBodyBones.LeftUpperLeg, HumanBodyBones.LeftLowerLeg, HumanBodyBones.LeftFoot);
            CacheLimb(rightLeg, HumanBodyBones.RightUpperLeg, HumanBodyBones.RightLowerLeg, HumanBodyBones.RightFoot);

            isInitialized = true;
        }

        private void CacheLimb(LimbIK limb, HumanBodyBones upperBone, HumanBodyBones lowerBone, HumanBodyBones endBone)
        {
            limb.upper = animator.GetBoneTransform(upperBone);
            limb.lower = animator.GetBoneTransform(lowerBone);
            limb.end = animator.GetBoneTransform(endBone);

            if (limb.upper == null || limb.lower == null || limb.end == null) return;

            limb.upperLength = Vector3.Distance(limb.upper.position, limb.lower.position);
            limb.lowerLength = Vector3.Distance(limb.lower.position, limb.end.position);

            // 초기 벤드 법선을 upper 본의 로컬 공간에 캐싱 (FinalIK TrigonometricBone 방식)
            // 런타임에 upper.rotation * localBendNormal로 안정적인 월드 법선 획득
            Vector3 ab = limb.lower.position - limb.upper.position;
            Vector3 bc = limb.end.position - limb.lower.position;
            Vector3 bendNormal = Vector3.Cross(ab, bc);

            if (bendNormal.sqrMagnitude < 0.0001f)
            {
                bendNormal = Vector3.Cross(ab.normalized, Vector3.up);
                if (bendNormal.sqrMagnitude < 0.0001f)
                    bendNormal = Vector3.Cross(ab.normalized, Vector3.forward);
            }

            bendNormal.Normalize();
            limb.localBendNormal = Quaternion.Inverse(limb.upper.rotation) * bendNormal;
        }

        private void Update()
        {
            if (!isInitialized) return;

            SolveLimb(leftArm);
            SolveLimb(rightArm);
            SolveLimb(leftLeg);
            SolveLimb(rightLeg);
        }

        private void SolveLimb(LimbIK limb)
        {
            if (!limb.enabled || limb.target == null || limb.positionWeight < 0.001f) return;
            if (limb.upper == null || limb.lower == null || limb.end == null) return;

            // 벤드 법선 계산
            Vector3 bendNormal = GetBendNormal(limb);

            // bendGoal 적용
            if (limb.bendGoal != null && limb.bendGoalWeight > 0.001f)
            {
                Vector3 goalNormal = Vector3.Cross(
                    limb.bendGoal.position - limb.upper.position,
                    limb.target.position - limb.upper.position
                );
                if (goalNormal.sqrMagnitude > 0.0001f)
                {
                    bendNormal = Vector3.Lerp(bendNormal, goalNormal.normalized, limb.bendGoalWeight);
                }
            }

            // FinalIK 정적 솔버 호출
            IKSolverTrigonometric.Solve(
                limb.upper,
                limb.lower,
                limb.end,
                limb.target.position,
                bendNormal,
                limb.positionWeight
            );

            // 끝단 회전
            if (limb.rotationWeight > 0.001f)
            {
                limb.end.rotation = Quaternion.Slerp(
                    limb.end.rotation,
                    limb.target.rotation,
                    limb.rotationWeight
                );
            }
        }

        /// <summary>
        /// 벤드 법선을 upper 본의 회전에서 유도합니다 (FinalIK 방식).
        /// 위치 기반 Cross(ab, bc)는 직선 근처에서 불안정하지만,
        /// 회전 기반은 본의 회전을 그대로 따르므로 안정적입니다.
        /// </summary>
        private Vector3 GetBendNormal(LimbIK limb)
        {
            return limb.upper.rotation * limb.localBendNormal;
        }

        /// <summary>
        /// 히프 높이 자동 보정값을 계산합니다.
        /// </summary>
        public float CalculateAutoFloorHeight(float comfortRatio = 0.98f)
        {
            if (animator == null || leftLeg.upper == null || leftLeg.lower == null || leftLeg.end == null) return 0f;

            float upperLen = Vector3.Distance(leftLeg.upper.position, leftLeg.lower.position);
            float lowerLen = Vector3.Distance(leftLeg.lower.position, leftLeg.end.position);
            float totalLegLength = upperLen + lowerLen;
            float comfortHeight = totalLegLength * comfortRatio;

            Transform hips = animator.GetBoneTransform(HumanBodyBones.Hips);
            Transform foot = leftLeg.end;
            if (hips == null || foot == null) return 0f;

            float currentHipToFoot = hips.position.y - foot.position.y;
            float heightDiff = currentHipToFoot - comfortHeight;

            return -heightDiff;
        }
    }
}