/// <summary> /// INTERNAL USE: Get the most recent values for Tango emulation. /// </summary> /// <param name="poseTimestamp">The new Tango emulation timestamp.</param> /// <param name="posePosition">The new Tango emulation position.</param> /// <param name="poseRotation">The new Tango emulation rotation.</param> private static bool GetTangoEmulationCurrent( out float poseTimestamp, out Vector3 posePosition, out Quaternion poseRotation) { if (m_emulatedPoseHistory.Count > 0) { EmulatedPose pose = m_emulatedPoseHistory[m_emulatedPoseHistory.Count - 1]; poseTimestamp = pose.m_timestamp; posePosition = pose.m_position; poseRotation = Quaternion.Euler(90, 0, 0) * Quaternion.Euler(pose.m_anglesFromForward); return(true); } else { poseTimestamp = 0; posePosition = Vector3.zero; poseRotation = Quaternion.identity; return(false); } }
/// <summary> /// INTERNAL USE: Get a historical value for Tango emulation. /// </summary> /// <returns><c>true</c>, if a historical values was retrieved, <c>false</c> otherwise.</returns> /// <param name="timestamp">Time of the historical value.</param> /// <param name="posePosition">The emulated pose position if retrieved, <c>Vector3.Zero</c> otherwise.</param> /// <param name="poseRotation"> /// The emulated pose rotation if retrieved, <c>Quaternion.Identity</c> otherwise. /// </param> private static bool GetTangoEmulationAtTime( float timestamp, out Vector3 posePosition, out Quaternion poseRotation) { EmulatedPose timestampedPose = new EmulatedPose(); timestampedPose.m_timestamp = timestamp; int index = m_emulatedPoseHistory.BinarySearch(timestampedPose, new CompareEmulatedPoseByTimestamp()); if (index >= 0) { // Found an exact timestamp match EmulatedPose pose = m_emulatedPoseHistory[index]; posePosition = pose.m_position; poseRotation = Quaternion.Euler(90, 0, 0) * Quaternion.Euler(pose.m_anglesFromForward); return(true); } else if (~index == m_emulatedPoseHistory.Count || ~index == 0) { // Out of bounds, no good pose posePosition = Vector3.zero; poseRotation = Quaternion.identity; return(false); } else { // Timestamp is inbetween two pose histories EmulatedPose earlierPose = m_emulatedPoseHistory[~index - 1]; EmulatedPose laterPose = m_emulatedPoseHistory[~index]; float t = Mathf.InverseLerp(earlierPose.m_timestamp, laterPose.m_timestamp, timestamp); posePosition = Vector3.Lerp(earlierPose.m_position, laterPose.m_position, t); Quaternion earlierRot = Quaternion.Euler(90, 0, 0) * Quaternion.Euler(earlierPose.m_anglesFromForward); Quaternion laterRot = Quaternion.Euler(90, 0, 0) * Quaternion.Euler(laterPose.m_anglesFromForward); poseRotation = Quaternion.Slerp(earlierRot, laterRot, t); return(true); } }
/// <summary> /// Initializes a new instance of the <see cref="Tango.PoseProvider+EmulatedPose"/> class. /// </summary> /// <param name="other">Emulated pose to copy.</param> public EmulatedPose(EmulatedPose other) { m_timestamp = other.m_timestamp; m_position = other.m_position; m_angles = other.m_angles; }
/// <summary> /// Get transformation from Start of Service to to the Device frame (minus axis swaps) at a specified time. /// </summary> /// <returns><c>true</c>, if transformation is valid at the given timestamp, <c>false</c> otherwise.</returns> /// <param name="timestamp">Time stamp being queried. /// When querying with a timestamp of 0, this will be adjusted to the current pose's timestamp.</param> /// <param name="transformation">The transformation Start of Service to the Device frame at the specified time. /// If transformation is not valid, this is undefined.</param> private static bool _GetEmulatedMovementTransformAtTime(ref double timestamp, out Matrix4x4 transformation) { bool poseIsValid; Vector3 posePosition; Quaternion poseRotation; // Get pose data. if (timestamp == 0) { // Get the most recent value for Tango emulation. if (m_emulatedPoseHistory.Count > 0) { EmulatedPose pose = m_emulatedPoseHistory[m_emulatedPoseHistory.Count - 1]; timestamp = pose.m_timestamp; posePosition = pose.m_position; poseRotation = Quaternion.Euler(pose.m_angles); poseIsValid = true; } else { posePosition = Vector3.zero; poseRotation = Quaternion.identity; poseIsValid = false; } } else { // Get a historical value for Tango emulation. EmulatedPose timestampedPose = new EmulatedPose(); timestampedPose.m_timestamp = (float)timestamp; int index = m_emulatedPoseHistory.BinarySearch(timestampedPose, new CompareEmulatedPoseByTimestamp()); if (index >= 0) { // Found an exact timestamp match EmulatedPose pose = m_emulatedPoseHistory[index]; posePosition = pose.m_position; poseRotation = Quaternion.Euler(pose.m_angles); poseIsValid = true; } else if (~index == m_emulatedPoseHistory.Count || ~index == 0) { // Out of bounds, no good pose posePosition = Vector3.zero; poseRotation = Quaternion.identity; poseIsValid = false; } else { // Timestamp is inbetween two pose histories EmulatedPose earlierPose = m_emulatedPoseHistory[~index - 1]; EmulatedPose laterPose = m_emulatedPoseHistory[~index]; float t = Mathf.InverseLerp(earlierPose.m_timestamp, laterPose.m_timestamp, (float)timestamp); posePosition = Vector3.Lerp(earlierPose.m_position, laterPose.m_position, t); Quaternion earlierRot = Quaternion.Euler(earlierPose.m_angles); Quaternion laterRot = Quaternion.Euler(laterPose.m_angles); poseRotation = Quaternion.Slerp(earlierRot, laterRot, t); poseIsValid = true; } } // Compose matrix. if (poseIsValid) { transformation = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one); } else { transformation = Matrix4x4.identity; } return(poseIsValid); }
/// <summary> /// INTERNAL USE: Update the Tango emulation state for pose data. /// /// Make sure this is only called once per frame. /// </summary> internal static void UpdateTangoEmulation() { EmulatedPose pose; float now = Time.realtimeSinceStartup; if (m_emulatedPoseHistory.Count > 0) { pose = new EmulatedPose(m_emulatedPoseHistory[m_emulatedPoseHistory.Count - 1]); } else { pose = new EmulatedPose(); m_beginningOfPoseEmulation = now; } if (!Input.GetKey(KeyCode.LeftAlt) && !Input.GetKey(KeyCode.LeftCommand)) { // Update the emulated rotation (do this first to make sure the position is rotated) // // Note: We need to use Input.GetAxis here because Unity3D does not provide a way to get the underlying // mouse delta. if (!Input.GetKey(KeyCode.LeftShift)) { pose.m_angles.y += Input.GetAxis("Mouse X") * MOUSE_LOOK_SENSITIVITY * Time.deltaTime; pose.m_angles.x -= Input.GetAxis("Mouse Y") * MOUSE_LOOK_SENSITIVITY * Time.deltaTime; } else { pose.m_angles.z -= Input.GetAxis("Mouse X") * MOUSE_LOOK_SENSITIVITY * Time.deltaTime; } } // Update the emulated position Quaternion poseRotation = Quaternion.Euler(pose.m_angles); Vector3 directionRight = poseRotation * Vector3.right; Vector3 directionForward = poseRotation * Vector3.forward; Vector3 directionUp = poseRotation * Vector3.up; if (Input.GetKey(KeyCode.W)) { // Forward pose.m_position += directionForward * TRANSLATION_SPEED * Time.deltaTime; } if (Input.GetKey(KeyCode.S)) { // Backward pose.m_position -= directionForward * TRANSLATION_SPEED * Time.deltaTime; } if (Input.GetKey(KeyCode.A)) { // Left pose.m_position -= directionRight * TRANSLATION_SPEED * Time.deltaTime; } if (Input.GetKey(KeyCode.D)) { // Right pose.m_position += directionRight * TRANSLATION_SPEED * Time.deltaTime; } if (Input.GetKey(KeyCode.E)) { // Up pose.m_position += directionUp * TRANSLATION_SPEED * Time.deltaTime; } if (Input.GetKey(KeyCode.Q)) { // Down pose.m_position -= directionUp * TRANSLATION_SPEED * Time.deltaTime; } // Record the current state pose.m_timestamp = now; while (m_emulatedPoseHistory.Count > 0 && m_emulatedPoseHistory[0].m_timestamp < now - EMULATION_POSE_KEEP_TIME_SECS) { m_emulatedPoseHistory.RemoveAt(0); } m_emulatedPoseHistory.Add(pose); m_emulationIsDirty = true; }
/// <summary> /// Initializes a new instance of the <see cref="Tango.PoseProvider+EmulatedPose"/> class. /// </summary> /// <param name="other">Other.</param> public EmulatedPose(EmulatedPose other) { m_timestamp = other.m_timestamp; m_position = other.m_position; m_anglesFromForward = other.m_anglesFromForward; }