implement headfree movement

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

@@ -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,8 +1121,10 @@ public class ZEDManager : MonoBehaviour
                 cameraLeft = caml;
 
                 if (camLeftTransform.transform.parent != null)
+                {
                     zedRigRoot = camLeftTransform.parent;
                 }
+            }
             else
             {
                 zedRigRoot = transform;
@@ -1831,6 +1835,9 @@ 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().
@@ -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;