Streamingle_URP/Assets/Scripts/KindRetargeting/TwoBoneIKSolver.cs

using UnityEngine;

namespace KindRetargeting
{
    /// <summary>
    /// 커스텀 Two-Bone IK 솔버.
    /// 소스 무릎 위치를 비율 스케일하여 타겟 무릎을 직접 배치합니다.
    /// cosine law 없이 동작하므로 180° 특이점이 없고 역관절도 자연스럽게 지원합니다.
    /// </summary>
    [System.Serializable]
    public class TwoBoneIKSolver
    {
        [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;

            [HideInInspector] public Vector3 localBendNormal;

            // 소스 본 참조 (소스 무릎 위치 기반 IK용)
            [HideInInspector] public Transform sourceUpper;
            [HideInInspector] public Transform sourceLower;
            [HideInInspector] public Transform sourceEnd;
        }

        [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;

        public void Initialize(Animator targetAnimator)
        {
            animator = targetAnimator;
            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);

            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;
        }

        public void OnUpdate()
        {
            if (!isInitialized) return;

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

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

            float weight = limb.positionWeight;
            if (weight < 0.0001f) return; // 완전히 0일 때만 스킵

            // FK 회전 저장 (IK 결과와 블렌딩용)
            Quaternion fkUpperRot = limb.upper.rotation;
            Quaternion fkLowerRot = limb.lower.rotation;
            Quaternion fkEndRot = limb.end.rotation;

            float upperLen = Vector3.Distance(limb.upper.position, limb.lower.position);
            float lowerLen = Vector3.Distance(limb.lower.position, limb.end.position);

            Vector3 targetPos = limb.target.position;

            // --- 무릎 위치 결정 ---
            Vector3 kneePos;

            if (limb.sourceUpper != null && limb.sourceLower != null && limb.sourceEnd != null)
            {
                kneePos = ComputeKneePosFromSource(limb, upperLen, lowerLen, targetPos);
            }
            else
            {
                kneePos = ComputeKneePosFromBendGoal(limb, upperLen, lowerLen, targetPos);
            }

            // --- IK 회전 계산 ---
            Vector3 currentUpperDir = (limb.lower.position - limb.upper.position).normalized;
            Vector3 desiredUpperDir = (kneePos - limb.upper.position).normalized;
            Quaternion ikUpperRot = Quaternion.FromToRotation(currentUpperDir, desiredUpperDir) * limb.upper.rotation;

            // upper 적용 후 lower 계산 (자식 위치가 바뀌므로 임시 적용)
            limb.upper.rotation = ikUpperRot;
            Vector3 currentLowerDir = (limb.end.position - limb.lower.position).normalized;
            Vector3 desiredLowerDir = (targetPos - limb.lower.position).normalized;
            Quaternion ikLowerRot = Quaternion.FromToRotation(currentLowerDir, desiredLowerDir) * limb.lower.rotation;

            // --- FK/IK 블렌딩: weight로 부드럽게 전환 ---
            limb.upper.rotation = Quaternion.Slerp(fkUpperRot, ikUpperRot, weight);
            limb.lower.rotation = Quaternion.Slerp(fkLowerRot, ikLowerRot, weight);

            if (limb.rotationWeight > 0.0001f)
            {
                Quaternion ikEndRot = limb.target.rotation;
                limb.end.rotation = Quaternion.Slerp(fkEndRot, ikEndRot, limb.rotationWeight);
            }
        }

        /// <summary>
        /// 소스 무릎 위치를 hip→foot 직선 기준으로 분해(투영+수직)한 뒤
        /// 타겟 비율로 스케일하여 타겟 무릎 위치를 결정합니다.
        /// 소스가 역관절이면 수직 성분이 반대쪽 → 타겟도 자연스럽게 역관절.
        /// </summary>
        private Vector3 ComputeKneePosFromSource(LimbIK limb, float upperLen, float lowerLen, Vector3 targetPos)
        {
            // 소스 체인 길이
            float sourceUpperLen = Vector3.Distance(limb.sourceUpper.position, limb.sourceLower.position);
            float sourceLowerLen = Vector3.Distance(limb.sourceLower.position, limb.sourceEnd.position);
            float sourceChain = sourceUpperLen + sourceLowerLen;
            float targetChain = upperLen + lowerLen;

            if (sourceChain < 0.001f || targetChain < 0.001f)
                return limb.lower.position;

            // 소스 hip → foot 방향
            Vector3 sourceHipToFoot = limb.sourceEnd.position - limb.sourceUpper.position;
            float sourceHipToFootMag = sourceHipToFoot.magnitude;
            if (sourceHipToFootMag < 0.001f)
                return limb.lower.position;
            Vector3 sourceHipToFootDir = sourceHipToFoot / sourceHipToFootMag;

            // 소스 hip → knee 벡터를 투영(직선 성분)과 수직(오프셋 성분)으로 분해
            Vector3 sourceHipToKnee = limb.sourceLower.position - limb.sourceUpper.position;
            float projection = Vector3.Dot(sourceHipToKnee, sourceHipToFootDir);
            Vector3 rejection = sourceHipToKnee - projection * sourceHipToFootDir;

            // 소스 체인 길이 대비 비율로 정규화 → 타겟 체인 길이로 스케일
            float scale = targetChain / sourceChain;
            float scaledProjection = projection * scale;
            Vector3 scaledRejection = rejection * scale;

            // 타겟 hip → foot 방향
            Vector3 targetHipToFoot = targetPos - limb.upper.position;
            float targetHipToFootMag = targetHipToFoot.magnitude;
            if (targetHipToFootMag < 0.001f)
                return limb.lower.position;
            Vector3 targetHipToFootDir = targetHipToFoot / targetHipToFootMag;

            // 소스 프레임 → 타겟 프레임으로 수직 성분 회전
            // (소스와 타겟의 사지 방향이 다를 때 팔꿈치/무릎 오프셋 방향 보정)
            Quaternion frameRotation = Quaternion.FromToRotation(sourceHipToFootDir, targetHipToFootDir);
            Vector3 rotatedRejection = frameRotation * scaledRejection;

            // 타겟 관절 위치: 타겟 upper에서 투영 + 회전된 수직 성분 적용
            Vector3 kneePos = limb.upper.position
                            + targetHipToFootDir * scaledProjection
                            + rotatedRejection;

            return kneePos;
        }

        /// <summary>
        /// bendGoal 기반 fallback (소스 참조가 없는 사지용).
        /// 기존 cosine law + bendNormal 방식.
        /// </summary>
        private Vector3 ComputeKneePosFromBendGoal(LimbIK limb, float upperLen, float lowerLen, Vector3 targetPos)
        {
            float chainLength = upperLen + lowerLen;
            Vector3 toTarget = targetPos - limb.upper.position;
            float targetDist = toTarget.magnitude;
            if (targetDist < 0.001f) return limb.lower.position;
            Vector3 toTargetDir = toTarget / targetDist;

            // bendGoal의 수직 성분(rejection)으로 팔꿈치/무릎 방향 직접 결정
            Vector3 bendDir;
            if (limb.bendGoal != null && limb.bendGoalWeight > 0.001f)
            {
                Vector3 toBendGoal = limb.bendGoal.position - limb.upper.position;
                Vector3 rejection = toBendGoal - Vector3.Dot(toBendGoal, toTargetDir) * toTargetDir;
                if (rejection.sqrMagnitude > 0.0001f)
                {
                    // T-포즈 기반 기본 방향
                    Vector3 defaultBendDir = GetDefaultBendDir(limb, toTargetDir);
                    bendDir = Vector3.Lerp(defaultBendDir, rejection.normalized, limb.bendGoalWeight);
                }
                else
                {
                    bendDir = GetDefaultBendDir(limb, toTargetDir);
                }
            }
            else
            {
                bendDir = GetDefaultBendDir(limb, toTargetDir);
            }
            bendDir.Normalize();

            // 코사인 법칙으로 upper 각도 계산
            float clampedDist = Mathf.Clamp(targetDist, Mathf.Abs(upperLen - lowerLen) + 0.001f, chainLength - 0.001f);
            float cosAngle = (clampedDist * clampedDist + upperLen * upperLen - lowerLen * lowerLen)
                           / (2f * clampedDist * upperLen);
            cosAngle = Mathf.Clamp(cosAngle, -1f, 1f);
            float angle = Mathf.Acos(cosAngle);

            // 무릎/팔꿈치 위치: toTargetDir + bendDir 방향으로 angle만큼 오프셋
            // sin(angle) = 수직 성분, cos(angle) = 직선 성분
            Vector3 kneePos = limb.upper.position
                            + toTargetDir * (upperLen * Mathf.Cos(angle))
                            + bendDir * (upperLen * Mathf.Sin(angle));

            return kneePos;
        }

        /// <summary>
        /// T-포즈 기반 기본 굽힘 방향 (bendGoal이 없거나 불안정할 때)
        /// </summary>
        private Vector3 GetDefaultBendDir(LimbIK limb, Vector3 toTargetDir)
        {
            Vector3 bendNormal = limb.upper.rotation * limb.localBendNormal;
            Vector3 bendDir = Vector3.Cross(bendNormal, toTargetDir);
            if (bendDir.sqrMagnitude < 0.0001f)
                bendDir = Vector3.Cross(Vector3.up, toTargetDir);
            return bendDir.normalized;
        }

        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;
        }
    }
}