/// <summary>
/// Sets the listener coordinate frame pairs.
/// </summary>
/// <param name="count">Count.</param>
/// <param name="frames">Frames.</param>
public static void SetListenerCoordinateFramePairs(int count,
ref TangoCoordinateFramePair frames)
{
int returnValue = PoseProviderAPI.TangoService_setPoseListenerFrames (count, ref frames);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".SetListenerCoordinateFramePairs() Could not set frame pairs");
}
}
/// <summary>
/// Sets the listener coordinate frame pairs.
/// </summary>
/// <param name="count">Count.</param>
/// <param name="frames">Frames.</param>
public static void SetListenerCoordinateFramePairs(int count,
ref TangoCoordinateFramePair frames)
{
int returnValue = PoseProviderAPI.TangoService_setPoseListenerFrames(count, ref frames);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".SetListenerCoordinateFramePairs() Could not set frame pairs");
}
}
/// <summary>
/// Get a pose at a given timestamp from the base to the target frame.
///
/// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if
/// they were marked as TANGO_POSE_INITIALIZING in the callback poses.
///
/// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID.
/// </summary>
/// <param name="poseData">The pose to return.</param>
/// <param name="timeStamp">
/// Time specified in seconds.
///
/// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0,
/// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is
/// contained in the pose output structure and may differ from the queried timestamp.
/// </param>
/// <param name="framePair">
/// A pair of coordinate frames specifying the transformation to be queried for.
///
/// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base
/// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE .
/// </param>
public static void GetPoseAtTime([In, Out] TangoPoseData poseData,
double timeStamp,
TangoCoordinateFramePair framePair)
{
int returnValue = PoseProviderAPI.TangoService_getPoseAtTime(timeStamp, framePair, poseData);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp);
}
}
/// <summary>
/// Get a pose at a given timestamp from the base to the target frame.
///
/// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if
/// they were marked as TANGO_POSE_INITIALIZING in the callback poses.
///
/// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID.
/// </summary>
/// <param name="poseData">The pose to return.</param>
/// <param name="timeStamp">
/// Time specified in seconds.
///
/// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0,
/// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is
/// contained in the pose output structure and may differ from the queried timestamp.
/// </param>
/// <param name="framePair">
/// A pair of coordinate frames specifying the transformation to be queried for.
///
/// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base
/// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE .
/// </param>
public static void GetPoseAtTime([In, Out] TangoPoseData poseData,
double timeStamp,
TangoCoordinateFramePair framePair)
{
#if UNITY_EDITOR
poseData.framePair = framePair;
bool pairIsValid = true;
Matrix4x4 baseToDevice;
Matrix4x4 targetToDevice;
double adjustedTimeStamp1;
double adjustedTimeStamp2;
bool a = !GetFrameToDeviceTransformation(framePair.baseFrame, timeStamp, out adjustedTimeStamp1, out baseToDevice);
bool b = !GetFrameToDeviceTransformation(framePair.targetFrame, timeStamp, out adjustedTimeStamp2, out targetToDevice);
if (a || b)
{
pairIsValid = false;
}
Matrix4x4 baseToTarget = baseToDevice * targetToDevice.inverse;
Quaternion rotation = Quaternion.LookRotation(baseToTarget.GetColumn(2), baseToTarget.GetColumn(1));
poseData.translation[0] = baseToTarget.m03;
poseData.translation[1] = baseToTarget.m13;
poseData.translation[2] = baseToTarget.m23;
poseData.orientation[0] = rotation.x;
poseData.orientation[1] = rotation.y;
poseData.orientation[2] = rotation.z;
poseData.orientation[3] = rotation.w;
if (pairIsValid)
{
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
}
else
{
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
Debug.Log(string.Format(
CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : ts={0}, framePair={1},{2}",
timeStamp, framePair.baseFrame, framePair.targetFrame));
}
// Let most recent timestamp involved in the transformation be the timestamp
// (relevant when using GetPoseAtTime(0)).
// Behaviour may need to be updated after implmenting Area Description emulation.
poseData.timestamp = System.Math.Max(adjustedTimeStamp1, adjustedTimeStamp2);
#else
int returnValue = PoseProviderAPI.TangoService_getPoseAtTime(timeStamp, framePair, poseData);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp);
}
#endif
}
/// <summary>
/// Sets the listener coordinate frame pairs.
/// </summary>
/// <param name="count">Count.</param>
/// <param name="frames">Frames.</param>
public static void SetListenerCoordinateFramePairs(int count,
ref TangoCoordinateFramePair frames)
{
int returnValue = PoseProviderAPI.TangoService_setPoseListenerFrames(count, ref frames);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
DebugLogger.GetInstance.WriteToLog(DebugLogger.EDebugLevel.DEBUG_ERROR,
CLASS_NAME + ".SetListenerCoordinateFramePairs() Could not set frame pairs");
}
}
/// <summary>
/// Get a pose at a given timestamp from the base to the target frame.
///
/// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if
/// they were marked as TANGO_POSE_INITIALIZING in the callback poses.
///
/// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID.
/// </summary>
/// <param name="poseData">The pose to return.</param>
/// <param name="timeStamp">
/// Time specified in seconds.
///
/// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0,
/// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is
/// contained in the pose output structure and may differ from the queried timestamp.
/// </param>
/// <param name="framePair">
/// A pair of coordinate frames specifying the transformation to be queried for.
///
/// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base
/// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE .
/// </param>
public static void GetPoseAtTime([In, Out] TangoPoseData poseData,
double timeStamp,
TangoCoordinateFramePair framePair)
{
int returnValue = PoseProviderAPI.TangoService_getPoseAtTime(timeStamp, framePair, poseData);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp);
}
}
/// <summary>
/// Sets the callback to be used when a new Pose is
/// presented by the Tango Service.
/// </summary>
/// <param name="callback">Callback.</param>
public static void SetCallback(TangoCoordinateFramePair[] framePairs, TangoService_onPoseAvailable callback)
{
int returnValue = PoseProviderAPI.TangoService_connectOnPoseAvailable(framePairs.Length, framePairs, callback);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".SetCallback() Callback was not set!");
}
else
{
Debug.Log(CLASS_NAME + ".SetCallback() OnPose callback was set!");
}
}
/// <summary>
/// Get a pose at a given timestamp from the base to the target frame.
///
/// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if
/// they were marked as TANGO_POSE_INITIALIZING in the callback poses.
///
/// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID.
/// </summary>
/// <param name="poseData">The pose to return.</param>
/// <param name="timeStamp">
/// Time specified in seconds.
///
/// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0,
/// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is
/// contained in the pose output structure and may differ from the queried timestamp.
/// </param>
/// <param name="framePair">
/// A pair of coordinate frames specifying the transformation to be queried for.
///
/// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base
/// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE .
/// </param>
public static void GetPoseAtTime([In, Out] TangoPoseData poseData,
double timeStamp,
TangoCoordinateFramePair framePair)
{
#if UNITY_EDITOR
GetEmulatedPoseAtTime(poseData, timeStamp, framePair);
#else
int returnValue = API.TangoService_getPoseAtTime(timeStamp, framePair, poseData);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp);
}
#endif
}
public static extern int TangoService_setPoseListenerFrames(int count,
ref TangoCoordinateFramePair frames);
public static extern int TangoService_getPoseAtTime(double timestamp,
TangoCoordinateFramePair framePair,
[In, Out] TangoPoseData pose);
public static extern int TangoService_connectOnPoseAvailable(int count,
TangoCoordinateFramePair[] framePairs,
TangoService_onPoseAvailable onPoseAvailable);
/// <summary>
/// Registers the callback.
/// </summary>
/// <param name="framePairs">Frame pairs.</param>
public virtual void SetCallback(TangoCoordinateFramePair[] framePairs)
{
m_poseAvailableCallback = new Tango.PoseProvider.TangoService_onPoseAvailable(_OnPoseAvailable);
Tango.PoseProvider.SetCallback(framePairs, m_poseAvailableCallback);
}
public static int TangoService_setPoseListenerFrames(int count,
ref TangoCoordinateFramePair frames)
{
return(Common.ErrorType.TANGO_SUCCESS);
}
/// <summary>
/// Register to get Tango pose callbacks for specific reference frames.
///
/// NOTE: Tango pose callbacks happen on a different thread than the main
/// Unity thread.
/// </summary>
/// <param name="framePairs">The reference frames to get callbacks for.</param>
internal static void SetCallback(TangoCoordinateFramePair[] framePairs)
{
if (m_poseAvailableCallback != null)
{
Debug.Log("PoseListener.SetCallback() called when callback is already set.");
return;
}
Debug.Log("PoseListener.SetCallback()");
m_poseAvailableCallback = new PoseProvider.APIOnPoseAvailable(_OnPoseAvailable);
PoseProvider.SetCallback(framePairs, m_poseAvailableCallback);
}
public static int TangoService_setPoseListenerFrames(int count,
ref TangoCoordinateFramePair frames)
{
return Common.ErrorType.TANGO_SUCCESS;
}
/// <summary>
/// Emulation for PoseProvider.GetPoseAtTime().
/// </summary>
/// <param name="poseData">Pose data.</param>
/// <param name="timeStamp">Requested time stamp.</param>
/// <param name="framePair">Requested frame pair.</param>
internal static void GetEmulatedPoseAtTime(TangoPoseData poseData, double timeStamp,
TangoCoordinateFramePair framePair)
{
poseData.framePair = framePair;
double adjustedTimeStamp1 = timeStamp;
double adjustedTimeStamp2 = timeStamp;
Matrix4x4 baseToDevice;
Matrix4x4 targetToDevice;
TangoEnums.TangoPoseStatusType status1;
TangoEnums.TangoPoseStatusType status2;
_GetFrameToDevicePose(framePair.baseFrame, ref adjustedTimeStamp1, out baseToDevice, out status1);
_GetFrameToDevicePose(framePair.targetFrame, ref adjustedTimeStamp2, out targetToDevice, out status2);
// Composit base->device and target->device into base->target.
Matrix4x4 baseToTarget = baseToDevice * targetToDevice.inverse;
Quaternion rotation = Quaternion.LookRotation(baseToTarget.GetColumn(2), baseToTarget.GetColumn(1));
poseData.translation[0] = baseToTarget.m03;
poseData.translation[1] = baseToTarget.m13;
poseData.translation[2] = baseToTarget.m23;
poseData.orientation[0] = rotation.x;
poseData.orientation[1] = rotation.y;
poseData.orientation[2] = rotation.z;
poseData.orientation[3] = rotation.w;
// Use the 'less successful' of the two statuses.
if (status1 == TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN ||
status2 == TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN)
{
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN;
}
else if (status1 == TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID ||
status2 == TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID)
{
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
}
else if (status1 == TangoEnums.TangoPoseStatusType.TANGO_POSE_INITIALIZING ||
status2 == TangoEnums.TangoPoseStatusType.TANGO_POSE_INITIALIZING)
{
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INITIALIZING;
}
else if (status1 == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID &&
status2 == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
}
else
{
poseData.status_code = TangoEnums.TangoPoseStatusType.NA;
Debug.Log(string.Format(
CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : ts={0}, framePair={1},{2}",
timeStamp, framePair.baseFrame, framePair.targetFrame));
}
// Let most recent timestamp involved in the transformation be the timestamp
// (relevant when using GetPoseAtTime(0)),
// Except when getting relocalization pose (area description <-> start of service),
// in which case the timestamp should be the (theoretical) relocalization time.
if ((framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE)
||
(framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION))
{
if (poseData.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// First assume that relocalization happens at start (e.g. Area Learning for new areas).
poseData.timestamp = m_beginningOfPoseEmulation;
if (!EmulatedAreaDescriptionHelper.m_areaDescriptionFramesAvailableAtStart)
{
// Then add EMULATED_RELOCALIZATION_TIME second if an area description was loaded.
poseData.timestamp += EMULATED_RELOCALIZATION_TIME;
}
// The initially requested timestamp is only valid if it:
// A.) Is 0
// B.) Falls within the range of delivered relocalization frames
// (and we only deliver one in emulation, so it must match that one exactly)
bool validRelocalizationTimestamp = (timeStamp == 0) || (timeStamp == poseData.timestamp);
if (!validRelocalizationTimestamp)
{
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
}
}
}
else
{
poseData.timestamp = System.Math.Max(adjustedTimeStamp1, adjustedTimeStamp2);
}
}
/// <summary>
/// Set callbacks on all PoseListener objects.
/// </summary>
/// <param name="framePairs">Frame pairs.</param>
private void _SetMotionTrackingCallbacks(TangoCoordinateFramePair[] framePairs)
{
if (m_poseListener != null)
{
m_poseListener.AutoReset = m_motionTrackingAutoReset;
m_poseListener.SetCallback(framePairs);
}
}
/// <summary>
/// Set callbacks on all PoseListener objects.
/// </summary>
/// <param name="framePairs">Frame pairs.</param>
private void _SetMotionTrackingCallbacks(TangoCoordinateFramePair[] framePairs)
{
PoseListener[] poseListeners = FindObjectsOfType<PoseListener>();
foreach (PoseListener poseListener in poseListeners)
{
if (poseListener != null)
{
poseListener.AutoReset = m_motionTrackingAutoReset;
poseListener.SetCallback(framePairs);
}
}
}
/// <summary>
/// Set callbacks on all PoseListener objects.
/// </summary>
/// <param name="framePairs">Frame pairs.</param>
private void _SetMotionTrackingCallbacks(TangoCoordinateFramePair[] framePairs)
{
Debug.Log("TangoApplication._SetMotionTrackingCallbacks()");
if (m_poseListener != null)
{
m_poseListener.AutoReset = m_motionTrackingAutoReset;
m_poseListener.SetCallback(framePairs);
}
}
/// <summary>
/// Read pose from file.
/// </summary>
/// <returns>The pose from file.</returns>
/// <param name="reader">File reader.</param>
/// <param name="pose">Tango pose data.</param>
public int ReadPoseFromFile(BinaryReader reader, ref TangoPoseData pose)
{
if (reader == null)
{
return -1;
}
string frameMarker;
try
{
frameMarker = reader.ReadString();
}
catch (EndOfStreamException x)
{
reader.BaseStream.Position = 0;
Reset();
print("Restarting log file: " + x.ToString());
frameMarker = reader.ReadString();
}
if (frameMarker.CompareTo("poseframe\n") != 0)
{
m_debugText = "Failed to read pose";
return -1;
}
pose.timestamp = double.Parse(reader.ReadString());
TangoCoordinateFramePair pair = new TangoCoordinateFramePair();
pair.baseFrame = (Tango.TangoEnums.TangoCoordinateFrameType)reader.ReadInt32();
pair.targetFrame = (Tango.TangoEnums.TangoCoordinateFrameType)reader.ReadInt32();
pose.framePair = pair;
pose.status_code = (Tango.TangoEnums.TangoPoseStatusType)reader.ReadInt32();
pose.translation[0] = reader.ReadDouble();
pose.translation[1] = reader.ReadDouble();
pose.translation[2] = reader.ReadDouble();
pose.orientation[0] = reader.ReadDouble();
pose.orientation[1] = reader.ReadDouble();
pose.orientation[2] = reader.ReadDouble();
pose.orientation[3] = reader.ReadDouble();
return 0;
}
/// <summary>
/// Registers the callback.
/// </summary>
/// <param name="framePairs">Frame pairs.</param>
public void SetCallback(TangoCoordinateFramePair[] framePairs)
{
m_poseAvailableCallback = new Tango.PoseProvider.TangoService_onPoseAvailable(_OnPoseAvailable);
Tango.PoseProvider.SetCallback(framePairs, m_poseAvailableCallback);
m_motionTrackingData = new TangoPoseData();
m_areaLearningData = new TangoPoseData();
m_relocalizationData = new TangoPoseData();
}
public static int TangoService_connectOnPoseAvailable(int count,
TangoCoordinateFramePair[] framePairs,
TangoService_onPoseAvailable onPoseAvailable)
{
return Common.ErrorType.TANGO_SUCCESS;
}
public static extern int TangoService_setPoseListenerFrames(int count,
ref TangoCoordinateFramePair frames);
public static int TangoService_getPoseAtTime(double timestamp,
TangoCoordinateFramePair framePair,
[In, Out] TangoPoseData pose)
{
return Common.ErrorType.TANGO_SUCCESS;
}
public static int TangoService_getPoseAtTime(double timestamp,
TangoCoordinateFramePair framePair,
[In, Out] TangoPoseData pose)
{
return(Common.ErrorType.TANGO_SUCCESS);
}
public static extern int TangoService_getPoseAtTime(double timestamp,
TangoCoordinateFramePair framePair,
[In, Out] TangoPoseData pose);
/// <summary>
/// Get a pose at a given timestamp from the base to the target frame.
///
/// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if
/// they were marked as TANGO_POSE_INITIALIZING in the callback poses.
///
/// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID.
/// </summary>
/// <param name="poseData">The pose to return.</param>
/// <param name="timeStamp">
/// Time specified in seconds.
///
/// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0,
/// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is
/// contained in the pose output structure and may differ from the queried timestamp.
/// </param>
/// <param name="framePair">
/// A pair of coordinate frames specifying the transformation to be queried for.
///
/// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base
/// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE .
/// </param>
public static void GetPoseAtTime([In, Out] TangoPoseData poseData,
double timeStamp,
TangoCoordinateFramePair framePair)
{
#if UNITY_EDITOR
bool baseFrameIsWorld =
framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
bool targetFrameIsWorld =
framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
// Area Descriptions are explicitly not supported yet.
if (framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION ||
framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION)
{
Debug.Log(CLASS_NAME + ".GetPoseAtTime() emulation does not support Area Descriptions.");
poseData.framePair = framePair;
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
poseData.timestamp = 0;
poseData.translation[0] = Vector3.zero.x;
poseData.translation[1] = Vector3.zero.y;
poseData.translation[2] = Vector3.zero.z;
poseData.orientation[0] = Quaternion.identity.x;
poseData.orientation[1] = Quaternion.identity.y;
poseData.orientation[2] = Quaternion.identity.z;
poseData.orientation[3] = Quaternion.identity.w;
return;
}
if (baseFrameIsWorld && !targetFrameIsWorld)
{
if (timeStamp == 0)
{
float poseTimestamp;
Vector3 posePosition;
Quaternion poseRotation;
if (GetTangoEmulationCurrent(out poseTimestamp, out posePosition, out poseRotation))
{
poseData.framePair = framePair;
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
poseData.timestamp = poseTimestamp;
poseData.translation[0] = posePosition.x;
poseData.translation[1] = posePosition.y;
poseData.translation[2] = posePosition.z;
poseData.orientation[0] = poseRotation.x;
poseData.orientation[1] = poseRotation.y;
poseData.orientation[2] = poseRotation.z;
poseData.orientation[3] = poseRotation.w;
return;
}
}
else
{
Vector3 posePosition;
Quaternion poseRotation;
if (GetTangoEmulationAtTime((float)timeStamp, out posePosition, out poseRotation))
{
poseData.framePair = framePair;
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
poseData.timestamp = timeStamp;
poseData.translation[0] = posePosition.x;
poseData.translation[1] = posePosition.y;
poseData.translation[2] = posePosition.z;
poseData.orientation[0] = poseRotation.x;
poseData.orientation[1] = poseRotation.y;
poseData.orientation[2] = poseRotation.z;
poseData.orientation[3] = poseRotation.w;
return;
}
}
}
else if (baseFrameIsWorld == targetFrameIsWorld)
{
poseData.framePair = framePair;
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
poseData.timestamp = timeStamp;
poseData.translation[0] = Vector3.zero.x;
poseData.translation[1] = Vector3.zero.y;
poseData.translation[2] = Vector3.zero.z;
poseData.orientation[0] = Quaternion.identity.x;
poseData.orientation[1] = Quaternion.identity.y;
poseData.orientation[2] = Quaternion.identity.z;
poseData.orientation[3] = Quaternion.identity.w;
return;
}
Debug.Log(string.Format(
CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : ts={0}, framePair={1},{2}",
timeStamp, framePair.baseFrame, framePair.targetFrame));
poseData.framePair = framePair;
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
poseData.timestamp = 0;
poseData.translation[0] = Vector3.zero.x;
poseData.translation[1] = Vector3.zero.y;
poseData.translation[2] = Vector3.zero.z;
poseData.orientation[0] = Quaternion.identity.x;
poseData.orientation[1] = Quaternion.identity.y;
poseData.orientation[2] = Quaternion.identity.z;
poseData.orientation[3] = Quaternion.identity.w;
#else
int returnValue = PoseProviderAPI.TangoService_getPoseAtTime(timeStamp, framePair, poseData);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp);
}
#endif
}
