From 6e843d76e7e1773522aeb4c8380a3306fea5d7f1 Mon Sep 17 00:00:00 2001 From: Philipp Kramer Date: Tue, 26 Nov 2019 15:08:36 +0100 Subject: [PATCH] implement headfree movement --- Assets/ZED/SDK/Helpers/Scripts/ZEDManager.cs | 133 +++++++++++++++++-- 1 file changed, 122 insertions(+), 11 deletions(-) diff --git a/Assets/ZED/SDK/Helpers/Scripts/ZEDManager.cs b/Assets/ZED/SDK/Helpers/Scripts/ZEDManager.cs index 549b295..f92315f 100644 --- a/Assets/ZED/SDK/Helpers/Scripts/ZEDManager.cs +++ b/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(); 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 } + + + /// /// 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(). /// 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) + } + /// /// Updates images, collects HMD poses for latency correction, and applies tracking. /// Called by Unity each frame. @@ -2089,6 +2188,9 @@ public class ZEDManager : MonoBehaviour /// [HideInInspector] public GameObject zedRigDisplayer; + public GameObject camRigDisplayer; //(Mod) + public GameObject rotationTransformhelper;//(Mod): + public GameObject camLeft;//(Mod); private ZEDMixedRealityPlugin arRig; /// /// 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(); + 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"); - camLeft.transform.SetParent(zedRigDisplayer.transform); + //GameObject camLeft = new GameObject("cameraLeft"); //(Mod) + camLeft = new GameObject("cameraLeft"); + //camLeft.transform.SetParent(zedRigDisplayer.transform); //(Mod) + camLeft.transform.SetParent(camRigDisplayer.transform); //(Mod) + Camera camL = camLeft.AddComponent(); 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().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(); camR.renderingPath = RenderingPath.Forward;//Minimal overhead camR.clearFlags = CameraClearFlags.Color;