Inverse kinematics for holding an object inside Unity

InverseKinematics.cs

using System.Collections.Generic;

using UnityEngine;

/// <summary>
/// Inverse kinematics. Attaches hands to a weapon and rotates a head in the direction it's looking. 
/// </summary>
[DefaultExecutionOrder(1000)]
public class InverseKinematics : MonoBehaviour
{
    [Header("Settings")]
    [SerializeField] private Vector3 _targetOffset;
    [SerializeField, NotNull] private Transform _targetOffsetFrom;

    [Header("Arms")]
    [SerializeField, NotNull] private Transform _armLeftTarget;
    [SerializeField, NotNull] private Transform[] _armLeftHierarchy;
    [SerializeField, NotNull] private Transform _armRightTarget;
    [SerializeField, NotNull] private Transform[] _armRightHierarchy;

    [Header("Hint")]
    [SerializeField] private Transform _hint;
    [SerializeField, Range(0f, 1f)] private float _weightHint;
    public Vector3 OffsetPosition => (_baseOffset ? transform.TransformDirection(transform.localPosition) : Vector3.zero)
                                    + _targetOffsetFrom.TransformDirection(_targetOffset);

    private bool _maintainTargetPositionOffset;
    private bool _maintainTargetRotationOffset;

    private const float KSqrEpsilon = 1e-8f;
    
    void LateUpdate()
    {
        Calculate(
            _armLeftHierarchy, 
            _armLeftTarget.position + OffsetPosition,
            _armLeftTarget.rotation
        );

        Calculate(
            _armRightHierarchy,
            _armRightTarget.position + OffsetPosition,
            _armRightTarget.rotation
        );
    }

    private void Calculate(IReadOnlyList<Transform> hierarchy, Vector3 targetPosition, Quaternion targetRotation)
    {
        Vector3 targetOffsetPosition = Vector3.zero;
        Quaternion targetOffsetRotation = Quaternion.identity;

        if (_maintainTargetPositionOffset)
            targetOffsetPosition = hierarchy[2].position - targetPosition;
        if (_maintainTargetRotationOffset)
            targetOffsetRotation = Quaternion.Inverse(targetRotation) * hierarchy[2].rotation;

        Vector3 aPosition = hierarchy[0].position;
        Vector3 bPosition = hierarchy[1].position;
        Vector3 cPosition = hierarchy[2].position;
        Vector3 targetPos = targetPosition;
        Quaternion targetRot = targetRotation;
        Vector3 tPosition = Vector3.Lerp(cPosition, targetPos + targetOffsetPosition, 1f);
        Quaternion tRotation = Quaternion.Lerp(hierarchy[2].rotation, targetRot * targetOffsetRotation, 1f);
        bool hasHint = _hint != null && _weightHint > 0f;

        Vector3 ab = bPosition - aPosition;
        Vector3 bc = cPosition - bPosition;
        Vector3 ac = cPosition - aPosition;
        Vector3 at = tPosition - aPosition;

        float abLen = ab.magnitude;
        float bcLen = bc.magnitude;
        float acLen = ac.magnitude;
        float atLen = at.magnitude;

        float oldAbcAngle = TriangleAngle(acLen, abLen, bcLen);
        float newAbcAngle = TriangleAngle(atLen, abLen, bcLen);

        Vector3 axis = Vector3.Cross(ab, bc);
        if (axis.sqrMagnitude < KSqrEpsilon)
        {
            axis = hasHint ? Vector3.Cross(_hint.position - aPosition, bc) : Vector3.zero;

            if (axis.sqrMagnitude < KSqrEpsilon)
                axis = Vector3.Cross(at, bc);

            if (axis.sqrMagnitude < KSqrEpsilon)
                axis = Vector3.up;
        }
        axis = Vector3.Normalize(axis);

        float a = 0.5f * (oldAbcAngle - newAbcAngle);
        float sin = Mathf.Sin(a);
        float cos = Mathf.Cos(a);
        Quaternion deltaR = new Quaternion(axis.x * sin, axis.y * sin, axis.z * sin, cos);
        hierarchy[1].rotation = deltaR * hierarchy[1].rotation;

        cPosition = hierarchy[2].position;
        ac = cPosition - aPosition;
        hierarchy[0].rotation = Quaternion.FromToRotation(ac, at) * hierarchy[0].rotation;

        if (hasHint)
        {
            float acSqrMag = ac.sqrMagnitude;
            if (acSqrMag > 0f)
            {
                bPosition = hierarchy[1].position;
                cPosition = hierarchy[2].position;
                ab = bPosition - aPosition;
                ac = cPosition - aPosition;

                Vector3 acNorm = ac / Mathf.Sqrt(acSqrMag);
                Vector3 ah = _hint.position - aPosition;
                Vector3 abProj = ab - acNorm * Vector3.Dot(ab, acNorm);
                Vector3 ahProj = ah - acNorm * Vector3.Dot(ah, acNorm);

                float maxReach = abLen + bcLen;
                if (abProj.sqrMagnitude > (maxReach * maxReach * 0.001f) && ahProj.sqrMagnitude > 0f)
                {
                    Quaternion hintR = Quaternion.FromToRotation(abProj, ahProj);
                    hintR.x *= _weightHint;
                    hintR.y *= _weightHint;
                    hintR.z *= _weightHint;
                    hintR = Quaternion.Normalize(hintR);
                    hierarchy[0].rotation = hintR * hierarchy[0].rotation;
                }
            }
        }

        hierarchy[2].rotation = tRotation;
    }

    private static float TriangleAngle(float aLen, float aLen1, float aLen2)
    {
        float c = Mathf.Clamp((aLen1 * aLen1 + aLen2 * aLen2 - aLen * aLen) / (aLen1 * aLen2) / 2.0f, -1.0f, 1.0f);
        return Mathf.Acos(c);
    }
}