/// <summary>
/// Constructs an ApiPoseData equivalent to a unity PoseData.
/// </summary>
public ApiPoseData(UnityTango.PoseData unityPoseData)
{
// TODO (mtsmall): Tell unity version and accuracy should be uint.
version = (int)unityPoseData.version;
timestamp = unityPoseData.timestamp;
orientation = new DVector4(unityPoseData.orientation_x, unityPoseData.orientation_y,
unityPoseData.orientation_z, unityPoseData.orientation_w);
translation = new DVector3(unityPoseData.orientation_x, unityPoseData.orientation_y,
unityPoseData.orientation_z);
statusCode = unityPoseData.statusCode.ToApiType();
framePair = new ApiCoordinateFramePair(unityPoseData.frame);
confidence = (int)unityPoseData.confidence;
accuracy = unityPoseData.accuracy;
}
/// <summary>
/// Gets a unity PoseData equivalent to the ApiPoseData.
/// </summary>
/// <returns>A unity PoseData equivalent to the ApiPoseData.</returns>
public UnityTango.PoseData ToUnityType()
{
UnityTango.PoseData unityPose = new UnityTango.PoseData();
unityPose.version = (uint)version;
unityPose.timestamp = timestamp;
unityPose.statusCode = statusCode.ToUnityType();
unityPose.frame = framePair.ToUnityType();
unityPose.confidence = (uint)confidence;
unityPose.accuracy = accuracy;
if (framePair.baseFrame != ApiCoordinateFrameType.StartOfService)
{
ARDebug.LogErrorFormat("apiPlaneData's base frame is not supported.");
unityPose.translation_x = translation.x;
unityPose.translation_y = translation.y;
unityPose.translation_z = translation.z;
unityPose.orientation_x = orientation.x;
unityPose.orientation_y = orientation.y;
unityPose.orientation_z = orientation.z;
unityPose.orientation_w = orientation.w;
return(unityPose);
}
Matrix4x4 startService_T_plane = Matrix4x4.TRS(translation.ToVector3(),
orientation.ToQuaternion(), Vector3.one);
Matrix4x4 unityTransform = Constants.UNITY_WORLD_T_START_SERVICE *
startService_T_plane *
Constants.UNITY_WORLD_T_START_SERVICE.inverse;
Vector3 position = unityTransform.GetColumn(3);
unityPose.translation_x = position.x;
unityPose.translation_y = position.y;
unityPose.translation_z = position.z;
Quaternion rotation = Quaternion.LookRotation(unityTransform.GetColumn(2),
unityTransform.GetColumn(1));
unityPose.orientation_x = rotation.x;
unityPose.orientation_y = rotation.y;
unityPose.orientation_z = rotation.z;
unityPose.orientation_w = rotation.w;
return(unityPose);
}
