HandDriverUPM/HandDriver/SDK/Interaction SDK/Scripts/Runtime/PhysicalHand.cs

using UDE_HAND_INTERACTION;
using UnityEngine;

public class PhysicalHand : MonoBehaviour
{
    public Transform Navigator;

    [SerializeField, Min(0)]
    private float TraceToloranceDist = 0.75f;
    [SerializeField, Min(0)]
    private float VelocityLimitation = 12f;

    private float lastDistance = float.MaxValue;
    private readonly float angularDragValue = 35;
    private readonly float trackPosStrength = 70;
    private readonly float trackRotStrength = 110;

    private Transform VirtualMovePoint;
    private Rigidbody _rigidbody;

    void Start()
    {
        if(GetComponentInParent<InteractableObject>() == null)
        {
            VirtualMovePoint = new GameObject().transform;
            VirtualMovePoint.parent = transform.parent;
            VirtualMovePoint.name = transform.name + "Virtual Move Point";
            _rigidbody = GetComponent<Rigidbody>();
        }
    }

    void FixedUpdate()
    {
        RefreshTransform();
    }

    public void VirtualMovePointReset()
    {
        if (Navigator == null) return;

        VirtualMovePoint.SetPositionAndRotation(Navigator.position, Navigator.rotation);
    }

    void RefreshTransform()
    {
        if (VirtualMovePoint == null) return;
        ResfreshPosition();
        RefreshRotation();
    }

    void ResfreshPosition()
    {
        VirtualMovePointReset();

        var movePos = VirtualMovePoint.position;
        var distance = Vector3.Distance(movePos, transform.position);

        if (lastDistance != distance)
        {
            lastDistance = distance;

            if (distance > TraceToloranceDist)
            {
                HandForceTransform(movePos, transform.rotation);
            }
            distance = Mathf.Clamp(distance, 0, 0.5f);

            Vector3 vel = (movePos - transform.position).normalized * trackPosStrength * distance;
            vel.x = Mathf.Clamp(vel.x, -VelocityLimitation, VelocityLimitation);
            vel.y = Mathf.Clamp(vel.y, -VelocityLimitation, VelocityLimitation);
            vel.z = Mathf.Clamp(vel.z, -VelocityLimitation, VelocityLimitation);
            var towardsVel = new Vector3(
                Mathf.MoveTowards(_rigidbody.velocity.x, vel.x, 0.5f + Mathf.Abs(_rigidbody.velocity.x) * Time.fixedDeltaTime * 60),
                Mathf.MoveTowards(_rigidbody.velocity.y, vel.y, 0.5f + Mathf.Abs(_rigidbody.velocity.y) * Time.fixedDeltaTime * 60),
                Mathf.MoveTowards(_rigidbody.velocity.z, vel.z, 0.5f + Mathf.Abs(_rigidbody.velocity.z) * Time.fixedDeltaTime * 60)
            );

            _rigidbody.velocity = towardsVel;
        }
    }

    void RefreshRotation()
    {
        var delta = (VirtualMovePoint.rotation * Quaternion.Inverse(_rigidbody.rotation));
        delta.ToAngleAxis(out float angle, out Vector3 axis);
        if (float.IsInfinity(axis.x)) return;

        angle = angle > 180f ? angle - 360f : angle;
        var multiLinear = Mathf.Deg2Rad * angle * trackRotStrength;
        Vector3 angular = multiLinear * axis.normalized;
        angle = Mathf.Abs(angle);

        var angleStrengthOffset = Mathf.Lerp(1f, 2f, angle / 45f);
        _rigidbody.angularDrag = Mathf.Lerp(angularDragValue + 10, angularDragValue, angle) * Time.fixedDeltaTime * 60;

        _rigidbody.angularVelocity = new Vector3(
            Mathf.MoveTowards(_rigidbody.angularVelocity.x, angular.x, trackRotStrength * 5f * Time.fixedDeltaTime * 60 * angleStrengthOffset),
            Mathf.MoveTowards(_rigidbody.angularVelocity.y, angular.y, trackRotStrength * 5f * Time.fixedDeltaTime * 60 * angleStrengthOffset),
            Mathf.MoveTowards(_rigidbody.angularVelocity.z, angular.z, trackRotStrength * 5f * Time.fixedDeltaTime * 60 * angleStrengthOffset)
        );
    }

    public virtual void HandForceTransform(Vector3 pos, Quaternion rot)
    {
        transform.SetPositionAndRotation(pos, rot);
        _rigidbody.position = pos;
        _rigidbody.rotation = rot;
        _rigidbody.velocity = Vector3.zero;
        _rigidbody.angularVelocity = Vector3.zero;

        VirtualMovePointReset();
    }
}