implement headfree movement

Assets/ZED/SDK/Helpers/Scripts/ZEDManager.cs

@@ -1070,17 +1070,17 @@ public class ZEDManager : MonoBehaviour
         //This won't affect whether the rig is in stereo mode, but allows the cameras to be accessed via GetLeftCamera() and GetRightCamera(). 
         if (cameraLeft == null || cameraRight == null)
         {
-            foreach(Camera cam in monocams)
+            foreach (Camera cam in monocams)
             {
                 ZEDRenderingPlane rendplane = cam.gameObject.GetComponent<ZEDRenderingPlane>();
                 if (!rendplane) continue;
 
-                if(!cameraLeft && (rendplane.viewSide == ZEDRenderingPlane.ZED_CAMERA_SIDE.LEFT || rendplane.viewSide == ZEDRenderingPlane.ZED_CAMERA_SIDE.LEFT_FORCE))
+                if (!cameraLeft && (rendplane.viewSide == ZEDRenderingPlane.ZED_CAMERA_SIDE.LEFT || rendplane.viewSide == ZEDRenderingPlane.ZED_CAMERA_SIDE.LEFT_FORCE))
                 {
                     cameraLeft = cam;
                     camLeftTransform = cam.transform;
                 }
-                else if(!cameraRight && (rendplane.viewSide == ZEDRenderingPlane.ZED_CAMERA_SIDE.RIGHT || rendplane.viewSide == ZEDRenderingPlane.ZED_CAMERA_SIDE.RIGHT_FORCE))
+                else if (!cameraRight && (rendplane.viewSide == ZEDRenderingPlane.ZED_CAMERA_SIDE.RIGHT || rendplane.viewSide == ZEDRenderingPlane.ZED_CAMERA_SIDE.RIGHT_FORCE))
                 {
                     cameraRight = cam;
                     camRightTransform = cam.transform;
@@ -1094,6 +1094,8 @@ public class ZEDManager : MonoBehaviour
             if (camLeftTransform.transform.parent != null)
             {
                 zedRigRoot = camLeftTransform.parent; //Make the camera's parent object (Camera_eyes in the ZED_Rig_Stereo prefab) the new zedRigRoot to be tracked. 
+
+                Debug.Log("set camLeftTransform.position");
             }
 
             if (UnityEngine.XR.XRDevice.isPresent)
@@ -1119,7 +1121,9 @@ public class ZEDManager : MonoBehaviour
                 cameraLeft = caml;
 
                 if (camLeftTransform.transform.parent != null)
+                {
                     zedRigRoot = camLeftTransform.parent;
+                }
             }
             else
             {
@@ -1310,7 +1314,7 @@ public class ZEDManager : MonoBehaviour
         {
             Debug.LogError("ZEDManager on " + gameObject.name + " couldn't connect to camera: " + cameraID +
                ". Check if another ZEDManager is already connected.");
-            this.gameObject.SetActive (false);
+            this.gameObject.SetActive(false);
             return;
         }
         initParameters.inputType = inputType;
@@ -1511,7 +1515,7 @@ public class ZEDManager : MonoBehaviour
         else if (isStereoRig && !UnityEngine.XR.XRDevice.isPresent) //Using stereo rig, but no VR headset. 
         {
             //When no VR HMD is available, simply put the origin at the left camera.
-            if(camLeftTransform) camLeftTransform.localPosition = Vector3.zero;
+            if (camLeftTransform) camLeftTransform.localPosition = Vector3.zero;
             if (camLeftTransform) camLeftTransform.localRotation = Quaternion.identity;
             if (camRightTransform) camRightTransform.localPosition = rightCameraOffset; //Space the eyes apart. 
             if (camRightTransform) camRightTransform.localRotation = Quaternion.identity;
@@ -1560,7 +1564,7 @@ public class ZEDManager : MonoBehaviour
             if (renderingPath != ZEDRenderingMode.FORWARD && renderingPath != ZEDRenderingMode.DEFERRED)
             {
                 Debug.LogError("[ZED Plugin] Only Forward and Deferred Shading rendering path are supported");
-                if(cameraLeft) cameraLeft.renderingPath = RenderingPath.Forward;
+                if (cameraLeft) cameraLeft.renderingPath = RenderingPath.Forward;
                 if (cameraRight) cameraRight.renderingPath = RenderingPath.Forward;
             }
 
@@ -1831,9 +1835,12 @@ public class ZEDManager : MonoBehaviour
     }
 
 
+
+
+
     /// <summary>
     /// Gets the tracking position from the ZED and updates zedRigRoot's position. Also updates the AR tracking if enabled. 
-	/// Only called in Live (not SVO playback) mode. Called in Update().
+    /// Only called in Live (not SVO playback) mode. Called in Update().
     /// </summary>
     private void UpdateTracking()
     {
@@ -1854,7 +1861,8 @@ public class ZEDManager : MonoBehaviour
                     AdjustZEDRigCameraPosition(); //Re-apply the ZED's offset from the VR headset. 
                     calibrationHasChanged = false;
                 }
-
+                //(Mod)
+                /*
                 arRig.ExtractLatencyPose(imageTimeStamp); //Find what HMD's pose was at ZED image's timestamp for latency compensation. 
                 arRig.AdjustTrackingAR(zedPosition, zedOrientation, out r, out v, setIMUPriorInAR);
                 zedRigRoot.localRotation = r;
@@ -1864,6 +1872,91 @@ public class ZEDManager : MonoBehaviour
                 ZEDSyncRotation = r;
                 HMDSyncPosition = arRig.LatencyPose().translation;
                 HMDSyncRotation = arRig.LatencyPose().rotation;
+                */
+
+                //Rotate Image plane with zed rotation
+                r = zedOrientation;
+
+
+                zedRigDisplayer.transform.localRotation = r; //(Mod) zedRigDisplayer should move/rotate the image plane
+                                                             //zedRigDisplayer.transform.localEulerAngles = new Vector3(r.x,r.y,0);
+                                                             //TODO: rotation von hmd soll zedRigDisplayer weiterhin beeinflussen (r.z nicht übernehmen evtl?)
+
+
+
+                //camLeft.transform.SetParent(zedRigDisplayer.transform); //(Mod)
+                //camLeftTransform gets overwritten by hmd transform
+
+
+                //move cams(within camRig) along with zedRig (test, immitates behaviour with cams parent set to zedRigDisplayer)
+                //camRigDisplayer.transform.rotation = zedRigDisplayer.transform.rotation;
+                //camRigDisplayer.transform.position = zedRigDisplayer.transform.position;
+
+                //hmd: camRigDisplayer.transform.eulerAngles.z = pitch
+                //hmd: camRigDisplayer.transform.eulerAngles.y = yaw
+                //hmd: camRigDisplayer.transform.eulerAngles.x = roll
+                //zed roll = z
+
+                //camLeft.transform  <-  Rotation des HMD
+                //zedRigDisplayer.transform <- rotation der ZED (durch zeile oben, fuer umwandlung in euler von quaternion)
+                //camRigDisplayer.transform <- Rotation des HMD (Parent, rotiert nicht mit hmd mit)
+
+                //camRigDisplayer.transform.eulerAngles = new Vector3(0,0,0);
+                //Transform rotAxis = camRigDisplayer.transform; //axis to rotate around
+                //rotAxis.Rotate(camLeftTransform.eulerAngles); //match camera direction
+
+
+                /*GameObject emptyGO = new GameObject();
+                Transform u = emptyGO.transform;
+                u.position = new Vector3(0,0,1); //rotation axis
+                u.Rotate(zedRigDisplayer.transform.eulerAngles); //rotate so that vector above matches rotation direction of zedRigDisplayers coordinate system
+                */
+
+                //camRigDisplayer.transform = camRigDisplayer.transform.rotateAroundVector(u, zedOrientation.z) //TODO
+
+
+
+                //Transform u = rotationTransformhelper.transform;
+                //rotationTransformhelper.transform.position = camLeft.transform.position; //copy data
+                //rotationTransformhelper.transform.rotation = camLeft.transform.rotation; //copy data
+                ///rotationTransformhelper.transform.position = camLeft.transform.localPosition; //copy data
+                ///rotationTransformhelper.transform.rotation = camLeft.transform.localRotation; //copy data
+                //Debug.Log(new Vector3(0, 0, zedRigDisplayer.transform.eulerAngles.z));
+                ///rotationTransformhelper.transform.Rotate(new Vector3(0, 0, zedRigDisplayer.transform.localEulerAngles.z));
+
+                ///rotationTransformhelper.transform.Rotate(-camLeft.transform.localEulerAngles);//rotate back
+                //var newrot =  zedRigDisplayer.transform.eulerAngles - rotationTransformhelper.transform.eulerAngles;
+                //camRigDisplayer.transform.eulerAngles = newrot;
+                //camRigDisplayer.transform.eulerAngles = rotationTransformhelper.transform.eulerAngles;
+
+                //TODO: das hier geht auch noch nicht, rotation noch etwas komisch. ggf weil rotationspunkt von camLeft/Right nicht im ursprung ist?
+
+
+                ///camRigDisplayer.transform.eulerAngles = rotationTransformhelper.transform.eulerAngles; //get cam rotation
+                ///camRigDisplayer.transform.position = zedRigDisplayer.transform.position;
+
+
+                // # Versuch Z Rotation der ZED transformiert auf Rotation des camRig uebertragen #
+                //rotationTransformhelper.transform.rotation = zedRigDisplayer.transform.localRotation;
+                //rotationTransformhelper.transform.localEulerAngles = new Vector3(-zedRigDisplayer.transform.eulerAngles.x, -zedRigDisplayer.transform.eulerAngles.y, 0);
+
+
+
+                Vector3 zedDirection = new Vector3(0, 0, 1); //Create Vector towars Z+
+                zedDirection = zedRigDisplayer.transform.rotation * zedDirection; //rotate vector by zedRigRotation (vector should point towards +Z of zedRigDisplayer)
+                camRigDisplayer.transform.localRotation = Quaternion.AngleAxis(zedRigDisplayer.transform.localEulerAngles.z, zedDirection); //rotate only around zedDirection.z with respect to original coordinate system
+
+                //same for camera Z rotation compensation
+                Vector3 hmdDirection = new Vector3(0, 0, 1); //Create Vector towars Z+
+                hmdDirection = camLeft.transform.rotation * hmdDirection; //rotate vector by cam (HMD) (vector should point towards +Z of HMD)
+                camRigDisplayer.transform.rotation *= Quaternion.AngleAxis(-camLeft.transform.localEulerAngles.z, hmdDirection); //compensate only around hmd.z with respect to original coordinate system
+
+
+
+
+
+                //zedRigRoot.localRotation = r;
+                //zedRigRoot.localPosition = v;
             }
             else //Not AR pass-through mode. 
             {
@@ -1894,6 +1987,12 @@ public class ZEDManager : MonoBehaviour
             arRig.CollectPose(); //Save headset pose with current timestamp. 
     }
 
+    Vector3 rotatePointAroundAxis(Vector3 point, float angle, Vector3 axis)
+    {
+        Quaternion q = Quaternion.AngleAxis(angle, axis);
+        return q * point; //Note: q must be first (point * q wouldn't compile)
+    }
+
     /// <summary>
     /// Updates images, collects HMD poses for latency correction, and applies tracking. 
     /// Called by Unity each frame. 
@@ -2089,6 +2188,9 @@ public class ZEDManager : MonoBehaviour
     /// </summary>
     [HideInInspector]
     public GameObject zedRigDisplayer;
+    public GameObject camRigDisplayer; //(Mod)
+    public GameObject rotationTransformhelper;//(Mod):
+    public GameObject camLeft;//(Mod);
     private ZEDMixedRealityPlugin arRig;
     /// <summary>
 	/// Create a GameObject to display the ZED in an headset (ZED-M Only).
@@ -2102,6 +2204,10 @@ public class ZEDManager : MonoBehaviour
         zedRigDisplayer = new GameObject("ZEDRigDisplayer");
         arRig = zedRigDisplayer.AddComponent<ZEDMixedRealityPlugin>();
 
+        camRigDisplayer = new GameObject("camRigDisplayer"); //(Mod)
+        rotationTransformhelper = new GameObject("rotationTransformhelper"); //(Mod)
+        //rotationTransformhelper.transform.SetParent(zedRigDisplayer.transform);
+
         /*Screens left and right */
         GameObject leftScreen = GameObject.CreatePrimitive(PrimitiveType.Quad);
         leftScreen.name = "Quad - Left";
@@ -2128,8 +2234,11 @@ public class ZEDManager : MonoBehaviour
         rightScreen.layer = arLayer;
 
         /*Camera left and right*/
-        GameObject camLeft = new GameObject("cameraLeft");
+        //GameObject camLeft = new GameObject("cameraLeft"); //(Mod)
-        camLeft.transform.SetParent(zedRigDisplayer.transform);
+        camLeft = new GameObject("cameraLeft");
+        //camLeft.transform.SetParent(zedRigDisplayer.transform); //(Mod)
+        camLeft.transform.SetParent(camRigDisplayer.transform); //(Mod)
+
         Camera camL = camLeft.AddComponent<Camera>();
         camL.stereoTargetEye = StereoTargetEyeMask.Both; //Temporary setting to fix loading screen issue.
         camL.renderingPath = RenderingPath.Forward;//Minimal overhead
@@ -2141,7 +2250,9 @@ public class ZEDManager : MonoBehaviour
         camL.depth = camLeftTransform.GetComponent<Camera>().depth;
 
         GameObject camRight = new GameObject("cameraRight");
-        camRight.transform.SetParent(zedRigDisplayer.transform);
+        //camRight.transform.SetParent(zedRigDisplayer.transform); //(Mod)
+        camRight.transform.SetParent(camRigDisplayer.transform); //(Mod)
+
         Camera camR = camRight.AddComponent<Camera>();
         camR.renderingPath = RenderingPath.Forward;//Minimal overhead
         camR.clearFlags = CameraClearFlags.Color;