/// <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;
}
