VRMedic/Assets/Oculus/VR/Scripts/Util/OVRRaycastManager.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
using UnityEngine.EventSystems;
using UnityEngine.XR;
public class OVRRaycastManager : MonoBehaviour
{
    public Transform RightHand;
    public OVRPhysicsRaycaster OVRPhysicsRaycaster;
    public LineRenderer LineRendererRay;
    public LineRenderer LineRendererRaySelected;
    public  Vector3 f = new Vector3(0, 0, 1);
    Vector3 u = new Vector3(0, 1, 1);
    Vector3 r = new Vector3(1, 0, 1);
    public bool IsInterationMode;
    private float Distance = 0.15f;
    private float SpeedRotate = 10f;
    private float CentrPositionInInteractMode;
    private Vector3 lastPositionHand;
    public GameObject Ball;
    public GameObject BallVisible;
    public GameObject Plane;
    public bool SelectedObj;
    private Vector3 position;
    private List<Rigidbody> _listRigs = new List<Rigidbody>();
    private List<JointsHighlighter.ChildObject> _listChildObjects = new List<JointsHighlighter.ChildObject>();
    void Awake()
    {
        OVRPhysicsRaycaster = GetComponent<OVRPhysicsRaycaster>();
    }
   
    void FixedUpdate()
    {
       
         float v = OVRInput.Get(OVRInput.Axis1D.PrimaryIndexTrigger, OVRInput.Controller.RTouch);

        if (OVRPhysicsRaycaster!= null && OVRPhysicsRaycaster.CurrentGameObjectInteraction!= null)
        {

            if (v > 0.2f && OVRPhysicsRaycaster.CurrentGameObjectInteraction != null && !SelectedObj)
            {
                
                SelectedObj = true;
                hj = OVRPhysicsRaycaster.CurrentGameObjectInteraction.GetComponent<ColliderProxy>().HingeJoint;
                cj = OVRPhysicsRaycaster.CurrentGameObjectInteraction.GetComponent<ColliderProxy>().ConfJoint;
                InitBall(hj);
                if (hj == null)
                {
                    
                    OVRPhysicsRaycaster.CurrentGameObjectInteraction = cj.gameObject;
                    Ball.transform.SetParent(cj.transform.parent);
                    Ball.transform.position = cj.transform.position + cj.transform.forward * 0.3f;
                    BallVisible.transform.SetParent(cj.transform.parent);
                    BallVisible.transform.position = cj.transform.position + cj.transform.forward * 0.3f;
                    Plane.transform.SetParent(cj.transform);
                    Plane.transform.localPosition = Vector3.zero;
                    Plane.transform.localEulerAngles = Vector3.zero;
                    Plane.transform.SetParent(cj.transform.parent);
                    position = InputTracking.GetLocalPosition(XRNode.RightHand) - offset;
                    lastPositionHand = position;
                    IsInterationMode = true;
                    BallVisible.SetActive(true);
                }
                else
                {
                    _listChildObjects = hj.GetComponent<JointsHighlighter>().GetChildren();
                    Rigidbody[] rigs = hj.transform.GetComponentsInChildren<Rigidbody>();

                    _listRigs = rigs.ToList();
                    if (hj.transform.parent.parent.GetComponent<Rigidbody>() != null)
                    {
                        _listRigs.Add(hj.transform.parent.parent.GetComponent<Rigidbody>());
                    }
                    if (hj.transform.parent.GetComponent<Rigidbody>() != null)
                    {
                        _listRigs.Add(hj.transform.parent.GetComponent<Rigidbody>());
                    }
                    _listRigs.Add(hj.GetComponent<Rigidbody>());
                    foreach (Rigidbody r in _listRigs)
                    {
                        if (hj.useLimits )
                        {
                            r.isKinematic = false;
                        }
                        else
                        {
                            r.isKinematic = true;
                        }
                        r.angularVelocity = Vector3.zero;
                    }
                    Ball.transform.SetParent(hj.transform.parent);
                    Ball.transform.position = hj.transform.position + hj.transform.forward * 0.3f;
                    BallVisible.transform.SetParent(hj.transform.parent);
                    
                    Plane.transform.SetParent(hj.transform);
                    Plane.transform.localPosition = Vector3.zero;
                    Plane.transform.localEulerAngles = Vector3.zero;
                    Plane.transform.SetParent(hj.transform.parent);
                    position = InputTracking.GetLocalPosition(XRNode.RightHand) - offset;
                    lastPositionHand = position;
                    IsInterationMode = true;
                    LineRendererRay.enabled = false;
                    BallVisible.SetActive(true);
                    LineRendererRaySelected.enabled = true;
                }
                
            }
            if (hj == null)
            {
                if (IsInterationMode && OVRPhysicsRaycaster.CurrentGameObjectInteraction != null)
                {
                    UpdateConfigurableJointRotation();
                }
            }
            else
            {
                if (IsInterationMode && OVRPhysicsRaycaster.CurrentGameObjectInteraction != null)
                {
                    UpdateConfigurableJointRotation();
                }
            }
            
            LineRendererRaySelected.SetPosition(0, RightHand.transform.position);
            LineRendererRaySelected.SetPosition(1, BallVisible.transform.position);
        }
        if (v<=0)
        {
            SelectedObj = false;
            IsInterationMode = false;
            LineRendererRay.enabled= true;
            OVRPhysicsRaycaster.CurrentGameObjectInteraction = null;
            hj = null;
            cj = null;
            IsGotFirstPosition = false;
         
            _listChildObjects.Clear();
            foreach (Rigidbody r in _listRigs)
            {
                r.angularVelocity = Vector3.zero;
                r.isKinematic = false;
            }
            LineRendererRaySelected.enabled = false;
            BallVisible.SetActive(false);
        }
    }
    private Vector3 offset;

    private void UpdateConfigurableJointRotation()
    {
        if (hj == null)
        {
            hj = OVRPhysicsRaycaster.CurrentGameObjectInteraction.GetComponent<JointController>().HingeJoint;
        }
        if (hj != null)
        {
            float p = 0;
            var position = InputTracking.GetLocalPosition(XRNode.RightHand) - offset;
            var delta = lastPositionHand - position;
            Ball.transform.position -= delta;
            BallVisible.transform.position -= delta;
            lastPositionHand = position;
            Vector3 Point = Vector3.zero;
            Math3d.LinePlaneIntersection(out Point, BallVisible.transform.position, Plane.transform.up, Plane.transform.up * -1, Plane.transform.position);
            Ball.transform.position = Point;

            var pos = hj.transform.position + (Ball.transform.position - hj.transform.position).normalized * Distance;
            offset += Ball.transform.position - pos;
            //BallVisible.transform.position -= offset;
            Ball.transform.position = pos;

            var lighter = OVRPhysicsRaycaster.CurrentGameObjectInteraction.GetComponent<ColliderProxy>().HingeJoint.GetComponent<JointsHighlighter>();
            lighter.Highlighted();
            var rig = OVRPhysicsRaycaster.CurrentGameObjectInteraction.GetComponent<ColliderProxy>().HingeJoint.GetComponent<Rigidbody>();

            var go = new GameObject();
            go.transform.position = rig.transform.position;
            go.transform.LookAt(Ball.transform);
            go.transform.SetParent(rig.transform.parent);


            
            if (!IsGotFirstPosition)
            {
                LastPosition = go.transform.rotation;
                IsGotFirstPosition = true;
                Normal = go.transform.up;
                return;
            }

            var deltaRotation = Quaternion.Angle(go.transform.rotation, LastPosition);
            if (deltaRotation > 160)
            {
                deltaRotation -= 180;
            }
            Vector3 forward = rig.transform.TransformDirection(Vector3.forward);
            Vector3 toOther = Ball.transform.position - rig.transform.position;
            float sign = 1;
            if (Vector3.Dot(forward, toOther) < 0)
            {
                sign = -1;
            }

            rig.MoveRotation(rig.rotation * Quaternion.AngleAxis(deltaRotation * sign /** Time.deltaTime * 30*/, Vector3.up) /*Quaternion.Euler(deltaRotation * sign *Vector3.up)*/);
            Destroy(go);
            LastPosition = go.transform.rotation;
            Normal = go.transform.up;
        }
    }

    private bool IsPartHinge;
    HingeJoint hj;
    ConfigurableJoint cj;
    private bool IsGotFirstPosition;
    private Quaternion LastPosition;
    private Vector3 Normal;

    

    private void InitBall(HingeJoint hj)
    {

        //Vector3 point;
        //Math3d.LinePlaneIntersection(out point, RightHand.position, RightHand.forward, Plane.transform.up * -1, Plane.transform.position);

        Vector3 Point = Vector3.zero;
        Math3d.LinePlaneIntersection(out Point, BallVisible.transform.position, Plane.transform.up, Plane.transform.up * -1, Plane.transform.position);
        Ball.transform.position = Point;

        BallVisible.transform.position = OVRPhysicsRaycaster.PosBall;

        var pos = hj.transform.position + (Ball.transform.position - hj.transform.position) * Distance ;
        offset = Ball.transform.position - pos;
        Ball.transform.position = pos;
    }

    //private void UpdateObjectRotation()
    //{
    //    foreach (var childobject in _listChildObjects)
    //    {
    //        childobject.Update();
    //    }
    //    hj = OVRPhysicsRaycaster.CurrentGameObjectInteraction.GetComponent<ColliderProxy>().HingeJoint;
        
    //    if(hj == null)
    //    {
    //        hj = OVRPhysicsRaycaster.CurrentGameObjectInteraction.GetComponent<JointController>().HingeJoint;
    //        OVRPhysicsRaycaster.CurrentGameObjectInteraction = hj.gameObject;
    //    }

    //    float p = 0;
    //    var position = InputTracking.GetLocalPosition(XRNode.RightHand);
      
    //    var delta = lastPositionHand - position;
    //    Ball.transform.position -= delta;
    //    lastPositionHand = position;

    //    Vector3 Point = Vector3.zero;
    //    Math3d.LinePlaneIntersection(out Point, RightHand.position, RightHand.forward, Plane.transform.up * -1, Plane.transform.position);
    //    Ball.transform.position = Point;

    //    var lighter = OVRPhysicsRaycaster.CurrentGameObjectInteraction.GetComponent<ColliderProxy>().HingeJoint.GetComponent<JointsHighlighter>();
    //    lighter.Highlighted();
    //    var rig = OVRPhysicsRaycaster.CurrentGameObjectInteraction.GetComponent<ColliderProxy>().HingeJoint.GetComponent<Rigidbody>();
      
    //    var go = new GameObject();
    //    go.transform.position = rig.transform.position;
    //    go.transform.LookAt(Ball.transform);
    //    go.transform.SetParent(rig.transform.parent);

    //    if(!IsGotFirstPosition)
    //    {
    //        LastPosition = go.transform.rotation;
    //        IsGotFirstPosition = true;
    //        Normal = go.transform.up;
    //        return;
    //    }
        
    //    var deltaRotation = Quaternion.Angle(go.transform.rotation, LastPosition);
    //    if(deltaRotation>160)
    //    {
    //        deltaRotation -= 180;
    //    }
    //    Vector3 forward = rig.transform.TransformDirection(Vector3.forward);
    //    Vector3 toOther = Ball.transform.position - rig.transform.position;
    //    float sign = 1;
    //    if (Vector3.Dot(forward, toOther) < 0)
    //    {
    //        sign = -1;
    //    }
        
    //    rig.MoveRotation(rig.rotation * Quaternion.AngleAxis(deltaRotation * sign /** Time.deltaTime * 30*/, Vector3.up) /*Quaternion.Euler(deltaRotation * sign *Vector3.up)*/);
    //    Destroy(go);
    //    LastPosition = go.transform.rotation;
    //    Normal = go.transform.up;
    //}

    void OnDrawGizmos()
    {
        if (IsInterationMode &&  hj !=  null)
        {
            // Draws a blue line from this transform to the target
            Gizmos.color = Color.blue;
            Gizmos.DrawLine(hj.transform.position, Ball.transform.position);
        }
    }

    Quaternion GetAngleBeetweenQuaternions(Quaternion a, Quaternion b)
    {
        var v1 = a.eulerAngles;
        var v2 = b.eulerAngles;

        var cross = Vector3.Cross(v1, v2);
        var w = Mathf.Sqrt(v1.normalized.magnitude * v1.normalized.magnitude * v2.normalized.magnitude * v2.normalized.magnitude) + Vector3.Dot(v1,v2);

        return new Quaternion(cross.x, cross.y, cross.z, w);
    }
}