Ipseity-Demo/Packages/OctoCore/Runtime/Scripts/Components/OctoIkArms.cs

using UnityEngine;
using static UnityEngine.GraphicsBuffer;

public class OctoIkArms : MonoBehaviour
{
    private class RaycastInfos
    {
        public bool HasHit;
        public float Distance;
        public float MaxDistance;
        public Vector3 Position;
        public Vector3 Normal;
        public Transform Transform;
    }

    [System.Serializable]
    private class RaycastRig
    {
        public Transform Root;
        public float Distance = 1;
    }

    [System.Serializable]
    private class ArmRig
    {
        public bool Enabled;
        public OctoIkLimb Arm;
        public Vector3 RotationOffset;
        public RaycastRig[] Raycasts;

        [HideInInspector] public Vector3 TargetOriginPos;
        [HideInInspector] public Quaternion TargetOriginRot;
        [HideInInspector] public Vector3 PoleOriginPos;

        public void Init()
        {
            if (Arm)
            {
                if (Arm.Target)
                {
                    TargetOriginPos = Arm.Target.localPosition;
                    TargetOriginRot = Arm.Target.localRotation;
                }

                if (Arm.Pole)
                {
                    PoleOriginPos = Arm.Pole.localPosition;
                }
            }
        }
    }

    [System.Serializable]
    private class Settings
    {
        public LayerMask LayerMask;
    }

    [SerializeField] private ArmRig _leftArm;
    [SerializeField] private ArmRig _rightArm;
    [SerializeField] private Settings _settings;

    private ArmRig[] _arms;
    private RaycastInfos[][] _raycasts = new RaycastInfos[2][];
    private RaycastInfos[] _nearHitRaycasts = new RaycastInfos[2];

    void Awake()
    {
        _arms = new ArmRig[2] { _leftArm, _rightArm };
        foreach (ArmRig armRig in _arms)
            armRig.Init();

        _raycasts = new RaycastInfos[_arms.Length][];
        for (int i = 0; i < _arms.Length; i++)
        {
            _raycasts[i] = new RaycastInfos[_arms[i].Raycasts.Length];
            for (int u = 0; u < _raycasts[i].Length; u++)
                _raycasts[i][u] = new RaycastInfos() { MaxDistance = 1 };
        }

        _nearHitRaycasts = new RaycastInfos[_arms.Length];
        for (int i = 0; i < _nearHitRaycasts.Length; i++)
            _nearHitRaycasts[i] = new RaycastInfos() { MaxDistance = 1 };
    }

    void OnDrawGizmos()//Selected()
    {
        if (Application.isPlaying || !enabled) return;

        foreach (ArmRig armRig in new ArmRig[2] { _leftArm, _rightArm })
        {
            for (int i = 0; i < armRig.Raycasts.Length; i++)
            {
                RaycastRig raycastRig = armRig.Raycasts[i];
                Debug.DrawRay(raycastRig.Root.position, raycastRig.Root.TransformDirection(Vector3.forward) * raycastRig.Distance, Color.red);
            }
        }
    }

    void Update()
    {
        // Get raycats infos

        for (int i = 0; i < _arms.Length; i++)
        {
            ArmRig armRig = _arms[i];

            if (!armRig.Enabled)
                continue;

            for (int u = 0; u < armRig.Raycasts.Length; u++)
            {
                RaycastRig raycastRig = armRig.Raycasts[u];
                RaycastInfos raycastInfos = _raycasts[i][u];

                RaycastHit hit;

                if (Physics.Raycast(raycastRig.Root.position, raycastRig.Root.TransformDirection(Vector3.forward), out hit, raycastRig.Distance, _settings.LayerMask))
                {
                    Debug.DrawRay(raycastRig.Root.position, raycastRig.Root.TransformDirection(Vector3.forward) * hit.distance, Color.green);

                    raycastInfos.HasHit = true;
                    raycastInfos.Distance = hit.distance;
                    raycastInfos.MaxDistance = raycastRig.Distance;
                    raycastInfos.Position = hit.point;
                    raycastInfos.Normal = hit.normal;
                    raycastInfos.Transform = hit.transform;
                }
                else
                {
                    Debug.DrawRay(raycastRig.Root.position, raycastRig.Root.TransformDirection(Vector3.forward) * raycastRig.Distance, Color.red);

                    raycastInfos.HasHit = false;
                    raycastInfos.Distance = 1;
                    raycastInfos.MaxDistance = 1;
                    raycastInfos.Position = Vector3.zero;
                    raycastInfos.Normal = Vector3.zero;
                    raycastInfos.Transform = null;
                }
            }
        }

        // Evaluate near hit

        for (int i = 0; i < _arms.Length; i++)
        {
            ArmRig armRig = _arms[i];
            _nearHitRaycasts[i].HasHit = false;

            for (int u = 0; u < armRig.Raycasts.Length; u++)
            {
                RaycastInfos raycastInfos = _raycasts[i][u];
                
                if (raycastInfos.HasHit)
                {
                    _nearHitRaycasts[i].HasHit = raycastInfos.HasHit;
                    _nearHitRaycasts[i].Distance = _raycasts[i][u].Distance;
                    _nearHitRaycasts[i].MaxDistance = _raycasts[i][u].MaxDistance;
                    _nearHitRaycasts[i].Position = _raycasts[i][u].Position;
                    _nearHitRaycasts[i].Normal = _raycasts[i][u].Normal;
                    _nearHitRaycasts[i].Transform = _raycasts[i][u].Transform;

                    break;
                }
            }
        }

        // Move hands

        for (int i = 0; i < _arms.Length; i++)
        {
            ArmRig armRig = _arms[i];
            RaycastInfos raycastInfos = _nearHitRaycasts[i];

            Vector3 targetPos = armRig.TargetOriginPos;
            Quaternion targetRot = armRig.TargetOriginRot;

            float sticknessPos = 0;
            float sticknessRot = 0;

            if (raycastInfos.HasHit)
            {
                float doorForward = transform.InverseTransformDirection(raycastInfos.Transform.forward).z;
                bool isGoodArm = i == 0 ? (doorForward < 0) : (doorForward > 0);

                if (isGoodArm)
                {
                    targetPos = armRig.Arm.Target.parent.InverseTransformPoint(raycastInfos.Position);
                    targetRot = Quaternion.LookRotation(-armRig.Arm.Target.parent.InverseTransformDirection(raycastInfos.Normal), transform.up) * Quaternion.Euler(armRig.RotationOffset);

                    float t = 1 - Mathf.Clamp01(raycastInfos.Distance / raycastInfos.MaxDistance);

                    sticknessPos = Mathf.SmoothStep(0, .7f, t);
                    sticknessRot = Mathf.SmoothStep(.5f, .9f, t);
                }
            }

            float tPos = Mathf.SmoothStep(Time.deltaTime * 3, 1, sticknessPos);
            float tRot = Mathf.SmoothStep(Time.deltaTime * 5, 1, sticknessRot);

            armRig.Arm.Target.localPosition = Vector3.Lerp(armRig.Arm.Target.localPosition, targetPos, tPos);
            armRig.Arm.Target.localRotation = Quaternion.Lerp(armRig.Arm.Target.localRotation, targetRot, tRot);
        }
    }
}