VRTK SnapTrackObjectGrabAttach

SnapTrackObjectGrabAttach.cs

// adapted from VRTK_TrackObjectGrabAttach

using UnityEngine;

using VRTK;
using VRTK.GrabAttachMechanics;

public class SnapTrackObjectGrabAttach : VRTK_BaseGrabAttach
{
    [Header("Track Options", order = 2)]

    [Tooltip("The maximum distance the grabbing controller is away from the object before it is automatically dropped.")]
    public float detachDistance = 1f;

    public float snapDistance = 0.01f;
    public float snapAngle = 0.01f;

    public float snappingVelocityLimit = 0.5f;
    public float snappingAngularVelocityLimit = 10f;

    [Tooltip("The maximum amount of velocity magnitude that can be applied to the object. Lowering this can prevent physics glitches if objects are moving too fast.")]
    public float snappedVelocityLimit = float.PositiveInfinity;
    [Tooltip("The maximum amount of angular velocity magnitude that can be applied to the object. Lowering this can prevent physics glitches if objects are moving too fast.")]
    public float snappedAngularVelocityLimit = float.PositiveInfinity;

    protected bool isReleasable = true;

    protected bool snapped = false;
    
    protected float angularVelocityLimit
    {
        get { return snapped ? snappedAngularVelocityLimit : snappingAngularVelocityLimit; }
    }

    protected float velocityLimit
    {
        get { return snapped ? snappedVelocityLimit : snappingVelocityLimit; }
    }

    public override bool StartGrab(GameObject grabbingObject, GameObject givenGrabbedObject, Rigidbody givenControllerAttachPoint)
    {
        snapped = false;
        return base.StartGrab(grabbingObject, givenGrabbedObject, givenControllerAttachPoint);
    }

    public override void StopGrab(bool applyGrabbingObjectVelocity)
    {
        snapped = false;
        if (isReleasable)
        {
            ReleaseObject(applyGrabbingObjectVelocity);
        }
        base.StopGrab(applyGrabbingObjectVelocity);
    }

    public override Transform CreateTrackPoint(Transform controllerPoint, GameObject currentGrabbedObject, GameObject currentGrabbingObject, ref bool customTrackPoint)
    {
        Transform trackPoint = null;
        if (precisionGrab)
        {
            trackPoint = new GameObject(VRTK_SharedMethods.GenerateVRTKObjectName(true, currentGrabbedObject.name, "TrackObject", "PrecisionSnap", "AttachPoint")).transform;
            trackPoint.parent = currentGrabbingObject.transform;
            SetTrackPointOrientation(ref trackPoint, currentGrabbedObject.transform, controllerPoint);
            customTrackPoint = true;
        }
        else
        {
            trackPoint = base.CreateTrackPoint(controllerPoint, currentGrabbedObject, currentGrabbingObject, ref customTrackPoint);
        }
        return trackPoint;
    }

    public override void ProcessUpdate()
    {
        if (trackPoint && grabbedObjectScript.IsDroppable())
        {
            float distance = Vector3.Distance(trackPoint.position, initialAttachPoint.position);
            if (distance > detachDistance)
            {
                ForceReleaseGrab();
            }
        }
    }

    public override void ProcessFixedUpdate()
    {
        if (!grabbedObject)
        {
            return;
        }
        
        float maxDistanceDelta = 10f;
        Vector3 positionDelta = trackPoint.position - (grabbedSnapHandle != null ? grabbedSnapHandle.position : grabbedObject.transform.position);
        Quaternion rotationDelta = trackPoint.rotation * Quaternion.Inverse((grabbedSnapHandle != null ? grabbedSnapHandle.rotation : grabbedObject.transform.rotation));

        float angle;
        Vector3 axis;
        rotationDelta.ToAngleAxis(out angle, out axis);

        if (!snapped)
        {
            snapped = (positionDelta.magnitude < snapDistance);
            //snapped = (angle < snapAngle) && (positionDelta.magnitude < snapDistance);
        }

        angle = ((angle > 180) ? angle -= 360 : angle);

        if (angle != 0)
        {
            Vector3 angularTarget = angle * axis;
            Vector3 calculatedAngularVelocity = Vector3.MoveTowards(grabbedObjectRigidBody.angularVelocity, angularTarget, maxDistanceDelta);
            if (angularVelocityLimit == float.PositiveInfinity || calculatedAngularVelocity.sqrMagnitude < angularVelocityLimit)
            {
                grabbedObjectRigidBody.angularVelocity = calculatedAngularVelocity;
            }
        }

        Vector3 velocityTarget = positionDelta / Time.fixedDeltaTime;
        Vector3 calculatedVelocity = Vector3.MoveTowards(grabbedObjectRigidBody.velocity, velocityTarget, maxDistanceDelta);

        if (velocityLimit == float.PositiveInfinity || calculatedVelocity.sqrMagnitude < velocityLimit)
        {
            grabbedObjectRigidBody.velocity = calculatedVelocity;
        }
        else
        {
            grabbedObjectRigidBody.velocity = calculatedVelocity.normalized * velocityLimit;
        }
    }

    protected override void Initialise()
    {
        tracked = true;
        climbable = false;
        kinematic = false;
        FlipSnapHandles();
    }

    protected virtual void SetTrackPointOrientation(ref Transform trackPoint, Transform currentGrabbedObject, Transform controllerPoint)
    {
        trackPoint.position = currentGrabbedObject.position;
        trackPoint.rotation = currentGrabbedObject.rotation;
    }
}