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

using UnityEngine;

namespace UDE_HAND_INTERACTION
{
    public class RegularShapeFit : MonoBehaviour
    {
        public Transform TargetObject;
        public bool SpecialSphere;
        public bool SpecialCube;

        public Vector3 startPoint = new(0f, 0.1f, 0f);
        public Vector3 endPoint = new(0f, -0.1f, 0f);

        public float DrawRadius = 0.1f;

        private const float Epsilon = 0.000001f;
        private Pose RelativePose => PoseManager.DeltaScaled(TargetObject, transform);

        [HideInInspector]
        public float TipDist;
        [HideInInspector]
        public Vector3 CenterPoint;
        [HideInInspector]
        public Vector3 CenterPointToHand;

        private Vector3 OriginalPos;
        [HideInInspector]
        public float OriginalDist;
        public Vector3 RealStart => TargetObject.TransformPoint(startPoint);
        public Vector3 RealEnd => TargetObject.TransformPoint(endPoint);

        public Vector3 LocalDirection
        {
            get
            {
                Vector3 dir = (endPoint - startPoint);
                if (dir.sqrMagnitude <= Epsilon)
                {
                    return Vector3.up;
                }
                return dir.normalized;
            }
        }

        public Vector3 GetArcDir()
        {
            Vector3 localStartArcDir = Quaternion.AngleAxis(0, LocalDirection) * Vector3.ProjectOnPlane(RelativePose.position - startPoint, LocalDirection).normalized;
            return TargetObject.TransformDirection(localStartArcDir);
        }

        void Start()
        {
            OriginalPos = transform.position;
            CalculateVector3();
            OriginalDist = CenterPointToHand.magnitude;
        }

        public void CalculateVector3()
        {
            if(SpecialSphere || SpecialCube)
            {
                CenterPoint = TargetObject.position;
            }
            else
            {
                Vector3 regularDir = RealEnd - RealStart;
                TipDist = Vector3.Cross(regularDir, RealStart - transform.GetChild(0).position).magnitude / regularDir.magnitude;
                float lineLength = regularDir.magnitude;
                regularDir.Normalize();
                CenterPoint = RealStart + Vector3.Project(transform.GetChild(0).position - RealStart, regularDir);
                if ((CenterPoint - RealStart).magnitude > lineLength || (CenterPoint - RealEnd).magnitude > lineLength)
                {
                    CenterPoint = (CenterPoint - RealStart).magnitude > lineLength ? RealEnd : RealStart;
                }
                CenterPointToHand = transform.position - CenterPoint;
            }
        }
    }
}