/// <summary>
/// Handle the callback sent by the Tango Service
/// when a new pose is sampled.
/// </summary>
/// <param name="pose">Pose.</param>
public void OnTangoPoseAvailable(Tango.TangoPoseData pose)
{
// Get out of here if the pose is null
if (pose == null)
{
Debug.Log("TangoPoseDate is null.");
return;
}
// The callback pose is for device with respect to start of service pose.
if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
// Update the stats for the pose for the debug text
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// Create new Quaternion and Vec3 from the pose data received in the event.
m_tangoPosition = new Vector3((float)pose.translation[0],
(float)pose.translation[1],
(float)pose.translation[2]);
m_tangoRotation = new Quaternion((float)pose.orientation[0],
(float)pose.orientation[1],
(float)pose.orientation[2],
(float)pose.orientation[3]);
// Reset the current status frame count if the status code changed.
if (pose.status_code != m_status)
{
m_frameCount = 0;
}
m_frameCount++;
// Compute delta frame timestamp.
m_frameDeltaTime = (float)pose.timestamp - m_prevFrameTimestamp;
m_prevFrameTimestamp = (float)pose.timestamp;
// Construct the start of service with respect to device matrix from the pose.
Matrix4x4 matrixssTd = Matrix4x4.TRS(m_tangoPosition, m_tangoRotation, Vector3.one);
// Converting from Tango coordinate frame to Unity coodinate frame.
Matrix4x4 matrixuwTuc = m_matrixuwTss * matrixssTd * m_matrixdTuc;
// Extract new local position
transform.position = matrixuwTuc.GetColumn(3);
// Extract new local rotation
transform.rotation = Quaternion.LookRotation(matrixuwTuc.GetColumn(2), matrixuwTuc.GetColumn(1));
}
else
{
// if the current pose is not valid we set the pose to identity
m_tangoPosition = Vector3.zero;
m_tangoRotation = Quaternion.identity;
}
// Finally, apply the new pose status
m_status = pose.status_code;
}
}
/// <summary>
/// Set controller's transformation based on received pose.
/// </summary>
/// <param name="pose">Received Tango pose data.</param>
private void _UpdateTransformationFromPose(TangoPoseData pose)
{
// Remember the previous position, so you can do delta motion
m_prevTangoPosition = m_tangoPosition;
m_prevTangoRotation = m_tangoRotation;
// The callback pose is for device with respect to start of service pose.
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
Vector3 position;
Quaternion rotation;
TangoSupport.TangoPoseToWorldTransform(pose, out position, out rotation);
m_uwTuc = Matrix4x4.TRS(position, rotation, Vector3.one);
Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc;
m_tangoPosition = uwOffsetTuc.GetColumn(3);
m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1));
// Other pose data -- Pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
// Other pose data -- Pose delta time.
m_poseDeltaTime = (float)pose.timestamp - m_poseTimestamp;
m_poseTimestamp = (float)pose.timestamp;
}
m_poseStatus = pose.status_code;
if (m_clutchActive)
{
// When clutching, preserve position.
m_tangoPosition = m_prevTangoPosition;
// When clutching, preserve yaw, keep changes in pitch, roll.
Vector3 rotationAngles = m_tangoRotation.eulerAngles;
rotationAngles.y = m_prevTangoRotation.eulerAngles.y;
m_tangoRotation.eulerAngles = rotationAngles;
}
// Calculate final position and rotation deltas and apply them.
Vector3 deltaPosition = m_tangoPosition - m_prevTangoPosition;
Quaternion deltaRotation = m_tangoRotation * Quaternion.Inverse(m_prevTangoRotation);
if (m_characterMotion && m_characterController != null)
{
m_characterController.Move(deltaPosition);
transform.rotation = deltaRotation * transform.rotation;
}
else
{
transform.position = transform.position + deltaPosition;
transform.rotation = deltaRotation * transform.rotation;
}
}
/// <summary>
/// Unity callback when application is paused.
/// </summary>
/// <param name="pauseStatus">The pauseStatus as reported by Unity.</param>
public void OnApplicationPause(bool pauseStatus)
{
m_poseDeltaTime = -1.0f;
m_poseTimestamp = -1.0f;
m_poseCount = -1;
m_poseStatus = TangoEnums.TangoPoseStatusType.NA;
}
/// <summary>
/// Construct readable string from TangoPoseStatusType.
/// </summary>
/// <param name="status">Pose status from Tango.</param>
/// <returns>Readable string corresponding to status.</returns>
private string _GetLoggingStringFromPoseStatus(TangoEnums.TangoPoseStatusType status)
{
string statusString;
switch (status)
{
case TangoEnums.TangoPoseStatusType.TANGO_POSE_INITIALIZING:
statusString = "initializing";
break;
case TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID:
statusString = "invalid";
break;
case TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN:
statusString = "unknown";
break;
case TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID:
statusString = "valid";
break;
default:
statusString = "N/A";
break;
}
return(statusString);
}
/// <summary>
/// Unity callback when application is paused.
/// </summary>
private void OnApplicationPause(bool pauseStatus)
{
m_frameDeltaTime = -1.0f;
m_prevFrameTimestamp = -1.0f;
m_frameCount = -1;
m_status = TangoEnums.TangoPoseStatusType.NA;
}
/// <summary>
/// Awake is called when the script instance is being loaded.
/// </summary>
public void Awake()
{
// Constant matrix converting start of service frame to Unity world frame.
m_uwTss = new Matrix4x4();
m_uwTss.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f));
m_uwTss.SetColumn(1, new Vector4(0.0f, 0.0f, 1.0f, 0.0f));
m_uwTss.SetColumn(2, new Vector4(0.0f, 1.0f, 0.0f, 0.0f));
m_uwTss.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
// Constant matrix converting Unity world frame frame to device frame.
m_dTuc = new Matrix4x4();
m_dTuc.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f));
m_dTuc.SetColumn(1, new Vector4(0.0f, 1.0f, 0.0f, 0.0f));
m_dTuc.SetColumn(2, new Vector4(0.0f, 0.0f, -1.0f, 0.0f));
m_dTuc.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
m_poseDeltaTime = -1.0f;
m_poseTimestamp = -1.0f;
m_poseCount = -1;
m_poseStatus = TangoEnums.TangoPoseStatusType.NA;
m_prevTangoRotation = m_tangoRotation = Quaternion.identity;
m_prevTangoPosition = m_tangoPosition = Vector3.zero;
m_uwTuc = Matrix4x4.identity;
m_uwOffsetTuw = Matrix4x4.identity;
}
/// @cond
/// <summary>
/// Awake is called when the script instance is being loaded.
/// </summary>
public void Awake()
{
m_poseTimestamp = -1.0f;
m_poseCount = -1;
m_poseStatus = TangoEnums.TangoPoseStatusType.NA;
m_tangoRotation = Quaternion.identity;
m_tangoPosition = Vector3.zero;
}
/// <summary>
/// Unity callback when application is paused.
/// </summary>
private void OnApplicationPause(bool pauseStatus)
{
m_frameDeltaTime = -1.0f;
m_prevFrameTimestamp = -1.0f;
m_frameCount = -1;
m_status = TangoEnums.TangoPoseStatusType.NA;
m_tangoRotation = Quaternion.identity;
m_tangoPosition = Vector3.zero;
}
/// <summary>
/// Update the transformation to the pose for that timestamp.
/// </summary>
/// <param name="timestamp">Time in seconds to update the transformation to.</param>
private void _UpdateTransformation(double timestamp)
{
TangoPoseData pose = new TangoPoseData();
TangoCoordinateFramePair pair;
if (!m_useAreaDescriptionPose)
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
else
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
PoseProvider.GetPoseAtTime(pose, timestamp, pair);
// The callback pose is for device with respect to start of service pose.
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// Construct matrix for the start of service with respect to device from the pose.
Vector3 posePosition = new Vector3((float)pose.translation[0],
(float)pose.translation[1],
(float)pose.translation[2]);
Quaternion poseRotation = new Quaternion((float)pose.orientation[0],
(float)pose.orientation[1],
(float)pose.orientation[2],
(float)pose.orientation[3]);
Matrix4x4 ssTd = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one);
// Calculate matrix for the camera in the Unity world, taking into account offsets.
Matrix4x4 uwTuc = m_uwTss * ssTd * m_dTuc;
// Extract final position, rotation.
m_tangoPosition = uwTuc.GetColumn(3);
m_tangoRotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));
// Other pose data -- Pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
// Other pose data -- Pose time.
m_poseTimestamp = timestamp;
}
m_poseStatus = pose.status_code;
// Apply final position and rotation.
transform.position = m_tangoPosition;
transform.rotation = m_tangoRotation;
}
/// <summary>
/// Initializes a new instance of the <see cref="Tango.TangoPoseData"/> class.
/// </summary>
public TangoPoseData()
{
version = 0;
timestamp = 0.0;
orientation = new double[4];
translation = new double[3];
status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN;
framePair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
framePair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
confidence = 0;
}
/// @cond
/// <summary>
/// Awake is called when the script instance is being loaded.
/// </summary>
public void Awake()
{
m_poseDeltaTime = -1.0f;
m_poseTimestamp = -1.0f;
m_poseCount = -1;
m_poseStatus = TangoEnums.TangoPoseStatusType.NA;
m_prevTangoRotation = m_tangoRotation = Quaternion.identity;
m_prevTangoPosition = m_tangoPosition = Vector3.zero;
m_uwTuc = Matrix4x4.identity;
m_uwOffsetTuw = Matrix4x4.identity;
}
/// <summary>
/// This function will be called the there's a new pose available in the system.
/// when a new pose is sampled.
/// </summary>
/// <param name="pose">Pose data that we get from the estimation.</param>
public void OnTangoPoseAvailable(Tango.TangoPoseData pose)
{
// Get out of here if the pose is null
if (pose == null)
{
Debug.Log("TangoPoseDate is null.");
return;
}
// The callback pose is for device with respect to start of service pose.
if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
Vector3 m_tangoPosition = new Vector3((float)pose.translation [0],
(float)pose.translation [1],
(float)pose.translation [2]);
Quaternion m_tangoRotation = new Quaternion((float)pose.orientation [0],
(float)pose.orientation [1],
(float)pose.orientation [2],
(float)pose.orientation [3]);
Matrix4x4 ssTd = Matrix4x4.TRS(m_tangoPosition, m_tangoRotation, Vector3.one);
// Here we are getting the pose of Unity camaer frame with respect to Unity world.
// This is the transformation of our current pose within the Unity coordinate frame.
Matrix4x4 uwTuc = m_uwTss * ssTd * Matrix4x4.Inverse(m_imuTd) * m_imuTc * m_cTuc;
// Extract new local position
transform.position = uwTuc.GetColumn(3);
// Extract new local rotation
transform.rotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));
}
}
// Reset the current status frame count if the status code changed.
if (pose.status_code != m_status)
{
m_frameCount = 0;
}
// Update the stats for the pose for the debug text
m_status = pose.status_code;
m_frameCount++;
// Compute delta frame timestamp.
m_frameDeltaTime = (float)pose.timestamp - m_prevFrameTimestamp;
m_prevFrameTimestamp = (float)pose.timestamp;
}
/// <summary>
/// Set controller's transformation based on received pose.
/// </summary>
/// <param name="pose">Received Tango pose data.</param>
private void _UpdateTransformationFromPose(TangoPoseData pose)
{
// Remember the previous position, so you can do delta motion
m_prevTangoPosition = m_tangoPosition;
m_prevTangoRotation = m_tangoRotation;
// The callback pose is for device with respect to start of service pose.
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// Construct matrix for the start of service with respect to device from the pose.
Vector3 posePosition = new Vector3((float)pose.translation[0],
(float)pose.translation[1],
(float)pose.translation[2]);
Quaternion poseRotation = new Quaternion((float)pose.orientation[0],
(float)pose.orientation[1],
(float)pose.orientation[2],
(float)pose.orientation[3]);
Matrix4x4 ssTd = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one);
// Calculate matrix for the camera in the Unity world, taking into account offsets.
m_uwTuc = m_uwTss * ssTd * m_dTuc;
Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc;
// Extract final position, rotation.
m_tangoPosition = uwOffsetTuc.GetColumn(3);
m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1));
// Other pose data -- Pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
}
m_poseStatus = pose.status_code;
if (m_clutchActive)
{
// When clutching, preserve position.
m_tangoPosition = m_prevTangoPosition;
// When clutching, preserve yaw, keep changes in pitch, roll.
Vector3 rotationAngles = m_tangoRotation.eulerAngles;
rotationAngles.y = m_prevTangoRotation.eulerAngles.y;
m_tangoRotation.eulerAngles = rotationAngles;
}
// Calculate final position and rotation deltas and apply them.
transform.localPosition = m_tangoPosition;
transform.localRotation = m_tangoRotation;
}
/// <summary>
/// Handle the callback sent by the Tango Service
/// when a new pose is sampled.
/// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION!
/// </summary>
/// <param name="callbackContext">Callback context.</param>
/// <param name="pose">Pose.</param>
public void OnTangoPoseAvailable(Tango.TangoPoseData pose)
{
// Get out of here if the pose is null
if (pose == null)
{
Debug.Log("TangoPoseDate is null.");
return;
}
// The callback pose is for device with respect to start of service pose.
if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// Cache the position and rotation to be set in the update function.
// This needs to be done because this callback does not
// happen in the main game thread.
m_tangoPosition = new Vector3((float)pose.translation [0],
(float)pose.translation [1],
(float)pose.translation [2]);
m_tangoRotation = new Quaternion((float)pose.orientation [0],
(float)pose.orientation [1],
(float)pose.orientation [2],
(float)pose.orientation [3]);
}
else // if the current pose is not valid we set the pose to identity
{
m_tangoPosition = Vector3.zero;
m_tangoRotation = Quaternion.identity;
}
}
// Reset the current status frame count if the status code changed.
if (pose.status_code != m_status)
{
m_frameCount = 0;
}
// Update the stats for the pose for the debug text
m_status = pose.status_code;
m_frameCount++;
// Compute delta frame timestamp.
m_frameDeltaTime = (float)pose.timestamp - m_prevFrameTimestamp;
m_prevFrameTimestamp = (float)pose.timestamp;
// Switch m_isDirty to true, so that the new pose get rendered in update.
m_isDirty = (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID);
}
/// <summary>
/// GetEmulatedPoseAtTime functions by getting pose information for both base and target
/// frames relative to the Device frame; this handles compositing information for one of
/// those frames.
/// </summary>
/// <param name="frame">Frame to get frame -> device frame for.</param>
/// <param name="timeStamp">Time stamp; if 0 may be modified with most up-to-date timestamp.</param>
/// <param name="frameToDeviceTransformation">Specified frame to device frame transformation.</param>
/// <param name="status">Status of the given frame to device frame pose.
/// See comments on GetEmulatedPoseAtTime().</param>
private static void _GetFrameToDevicePose(TangoEnums.TangoCoordinateFrameType frame,
ref double timeStamp,
out Matrix4x4 frameToDeviceTransformation,
out TangoEnums.TangoPoseStatusType status)
{
frameToDeviceTransformation = Matrix4x4.identity;
status = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
bool frameIsWorld = (frame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE) ||
(frame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION);
// Check that emulation supports this frame:
if (!_EmulationSupportsReferenceFrame(frame))
{
status = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
}
// Get mouse/keyboard-based emulation if appropriate.
if (frameIsWorld)
{
if (!_GetEmulatedMovementTransformAtTime(ref timeStamp, out frameToDeviceTransformation))
{
status = TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN;
}
if (frame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION)
{
bool areaDescriptionFramesAreValid =
EmulatedAreaDescriptionHelper.m_usingEmulatedDescriptionFrames &&
(EmulatedAreaDescriptionHelper.m_areaDescriptionFramesAvailableAtStart ||
_GetAreaDescriptionSyncDelayIsOver());
if (areaDescriptionFramesAreValid)
{
frameToDeviceTransformation.m03 += EmulatedAreaDescriptionHelper.m_emulationAreaOffset.x;
frameToDeviceTransformation.m13 += EmulatedAreaDescriptionHelper.m_emulationAreaOffset.y;
frameToDeviceTransformation.m23 += EmulatedAreaDescriptionHelper.m_emulationAreaOffset.z;
}
else
{
status = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
}
}
}
_GetFrameToDeviceAxisSwaps(frame, ref frameToDeviceTransformation);
}
/// <summary>
/// Performs a deep copy of pose data.
/// </summary>
/// <param name="poseToCopy">Pose to copy.</param>
public void DeepCopy(TangoPoseData poseToCopy)
{
this.version = poseToCopy.version;
this.timestamp = poseToCopy.timestamp;
this.status_code = poseToCopy.status_code;
this.framePair.baseFrame = poseToCopy.framePair.baseFrame;
this.framePair.targetFrame = poseToCopy.framePair.targetFrame;
this.confidence = poseToCopy.confidence;
for (int i = 0; i < 4; ++i)
{
this.orientation[i] = poseToCopy.orientation[i];
}
for (int i = 0; i < 3; ++i)
{
this.translation[i] = poseToCopy.translation[i];
}
}
/// <summary>
/// Handle the callback sent by the Tango Service
/// when a new pose is sampled.
/// </summary>
/// <param name="callbackContext">Callback context.</param>
/// <param name="pose">Pose.</param>
private void _OnPoseAvailable(IntPtr callbackContext, TangoPoseData pose)
{
m_latestPoseStatus = pose.status_code;
// MotionTracking
if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
m_motionTrackingData.framePair = pose.framePair;
m_motionTrackingData.status_code = pose.status_code;
m_motionTrackingData.orientation = pose.orientation;
m_motionTrackingData.translation = pose.translation;
m_motionTrackingData.timestamp = pose.timestamp;
m_motionTrackingData.confidence = pose.confidence;
m_motionTrackingData.accuracy = pose.accuracy;
m_hasNewMotionTrackingData = true;
}
// ADF Localized
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
m_areaLearningData.framePair = pose.framePair;
m_areaLearningData.status_code = pose.status_code;
m_areaLearningData.orientation = pose.orientation;
m_areaLearningData.translation = pose.translation;
m_areaLearningData.timestamp = pose.timestamp;
m_areaLearningData.confidence = pose.confidence;
m_areaLearningData.accuracy = pose.accuracy;
m_hasNewAreaLearningData = true;
}
// Relocalized against ADF
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE)
{
m_relocalizationData.framePair = pose.framePair;
m_relocalizationData.status_code = pose.status_code;
m_relocalizationData.orientation = pose.orientation;
m_relocalizationData.translation = pose.translation;
m_relocalizationData.timestamp = pose.timestamp;
m_relocalizationData.confidence = pose.confidence;
m_relocalizationData.accuracy = pose.accuracy;
m_hasNewRelocalizationData = true;
}
m_isDirty = true;
}
/// <summary>
/// Initialize the controller.
/// </summary>
private void Awake()
{
// Constant matrix converting start of service frame to Unity world frame.
m_uwTss = new Matrix4x4();
m_uwTss.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f));
m_uwTss.SetColumn(1, new Vector4(0.0f, 0.0f, 1.0f, 0.0f));
m_uwTss.SetColumn(2, new Vector4(0.0f, 1.0f, 0.0f, 0.0f));
m_uwTss.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
// Constant matrix converting Unity world frame frame to device frame.
m_cTuc.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f));
m_cTuc.SetColumn(1, new Vector4(0.0f, -1.0f, 0.0f, 0.0f));
m_cTuc.SetColumn(2, new Vector4(0.0f, 0.0f, 1.0f, 0.0f));
m_cTuc.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
m_frameDeltaTime = -1.0f;
m_prevFrameTimestamp = -1.0f;
m_frameCount = -1;
m_status = TangoEnums.TangoPoseStatusType.NA;
}
/// <summary>
/// Update the camera gameobject's transformation to the pose that on current timestamp.
/// </summary>
private void _UpdateTransformation(double timestamp)
{
TangoPoseData pose = new TangoPoseData();
TangoCoordinateFramePair pair;
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(pose, timestamp, pair);
m_status = pose.status_code;
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
Vector3 m_tangoPosition = new Vector3((float)pose.translation [0],
(float)pose.translation [1],
(float)pose.translation [2]);
Quaternion m_tangoRotation = new Quaternion((float)pose.orientation [0],
(float)pose.orientation [1],
(float)pose.orientation [2],
(float)pose.orientation [3]);
Matrix4x4 ssTd = Matrix4x4.TRS(m_tangoPosition, m_tangoRotation, Vector3.one);
// Here we are getting the pose of Unity camera frame with respect to Unity world.
// This is the transformation of our current pose within the Unity coordinate frame.
Matrix4x4 uwTuc = m_uwTss * ssTd * m_dTuc;
// Extract new local position
m_renderCamera.transform.position = uwTuc.GetColumn(3);
// Extract new local rotation
m_renderCamera.transform.rotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));
m_frameCount++;
}
else
{
m_frameCount = 0;
}
}
/// <summary>
/// Initialize the controller.
/// </summary>
public void Awake()
{
// Constant matrix converting start of service frame to Unity world frame.
m_matrixuwTss = new Matrix4x4();
m_matrixuwTss.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f));
m_matrixuwTss.SetColumn(1, new Vector4(0.0f, 0.0f, 1.0f, 0.0f));
m_matrixuwTss.SetColumn(2, new Vector4(0.0f, 1.0f, 0.0f, 0.0f));
m_matrixuwTss.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
// Constant matrix converting Unity world frame frame to device frame.
m_matrixdTuc = new Matrix4x4();
m_matrixdTuc.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f));
m_matrixdTuc.SetColumn(1, new Vector4(0.0f, 1.0f, 0.0f, 0.0f));
m_matrixdTuc.SetColumn(2, new Vector4(0.0f, 0.0f, -1.0f, 0.0f));
m_matrixdTuc.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
m_frameDeltaTime = -1.0f;
m_prevFrameTimestamp = -1.0f;
m_frameCount = -1;
m_status = TangoEnums.TangoPoseStatusType.NA;
m_tangoRotation = Quaternion.identity;
m_tangoPosition = Vector3.zero;
}
/// <summary>
/// Unity callback when application is paused.
/// </summary>
///
// ReSharper disable once InconsistentNaming
// ReSharper disable once UnusedMember.Local
// ReSharper disable once UnusedParameter.Local
private void OnApplicationPause(bool pauseStatus)
{
FrameDeltaTime = -1.0f;
mPrevFrameTimestamp = -1.0f;
FrameCount = -1;
Status = TangoEnums.TangoPoseStatusType.NA;
mTangoRotation = Quaternion.identity;
mTangoPosition = Vector3.zero;
}
/// <summary>
/// Update the camera gameobject's transformation to the pose that on current timestamp.
/// </summary>
/// <param name="timestamp">Time to update the camera to.</param>
private void _UpdateTransformation(double timestamp)
{
TangoPoseData pose = new TangoPoseData();
TangoCoordinateFramePair pair;
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(pose, timestamp, pair);
m_status = pose.status_code;
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
Vector3 m_tangoPosition = new Vector3((float)pose.translation[0],
(float)pose.translation[1],
(float)pose.translation[2]);
Quaternion m_tangoRotation = new Quaternion((float)pose.orientation[0],
(float)pose.orientation[1],
(float)pose.orientation[2],
(float)pose.orientation[3]);
Matrix4x4 ssTd = Matrix4x4.TRS(m_tangoPosition, m_tangoRotation, Vector3.one);
// Here we are getting the pose of Unity camera frame with respect to Unity world.
// This is the transformation of our current pose within the Unity coordinate frame.
Matrix4x4 uwTuc = m_uwTss * ssTd * m_dTuc;
// Extract new local position
m_renderCamera.transform.position = uwTuc.GetColumn(3);
// Extract new local rotation
m_renderCamera.transform.rotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));
m_frameCount++;
}
else
{
m_frameCount = 0;
}
}
/// <summary>
/// Set controller's transformation based on received pose.
/// </summary>
/// <param name="pose">Received Tango pose data.</param>
private void _UpdateTransformationFromPose(TangoPoseData pose)
{
// Remember the previous position, so you can do delta motion
m_prevTangoPosition = m_tangoPosition;
m_prevTangoRotation = m_tangoRotation;
// The callback pose is for device with respect to start of service pose.
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
Vector3 position;
Quaternion rotation;
TangoSupport.TangoPoseToWorldTransform(pose, out position, out rotation);
m_uwTuc = Matrix4x4.TRS(position, rotation, Vector3.one);
Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc;
m_tangoPosition = uwOffsetTuc.GetColumn(3);
m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1));
// Other pose data -- Pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
// Other pose data -- Pose delta time.
m_poseDeltaTime = (float)pose.timestamp - m_poseTimestamp;
m_poseTimestamp = (float)pose.timestamp;
}
m_poseStatus = pose.status_code;
if (m_clutchActive)
{
// When clutching, preserve position.
m_tangoPosition = m_prevTangoPosition;
// When clutching, preserve yaw, keep changes in pitch, roll.
Vector3 rotationAngles = m_tangoRotation.eulerAngles;
rotationAngles.y = m_prevTangoRotation.eulerAngles.y;
m_tangoRotation.eulerAngles = rotationAngles;
}
// Calculate final position and rotation deltas and apply them.
Vector3 deltaPosition = m_tangoPosition - m_prevTangoPosition;
Quaternion deltaRotation = m_tangoRotation * Quaternion.Inverse(m_prevTangoRotation);
if (m_characterMotion && m_characterController != null)
{
m_characterController.Move(deltaPosition);
transform.rotation = deltaRotation * transform.rotation;
}
else
{
transform.position = transform.position + deltaPosition;
transform.rotation = deltaRotation * transform.rotation;
}
}
/// @cond
/// <summary>
/// Awake is called when the script instance is being loaded.
/// </summary>
public void Awake()
{
m_poseDeltaTime = -1.0f;
m_poseTimestamp = -1.0f;
m_poseCount = -1;
m_poseStatus = TangoEnums.TangoPoseStatusType.NA;
m_prevTangoRotation = m_tangoRotation = Quaternion.identity;
m_prevTangoPosition = m_tangoPosition = Vector3.zero;
m_uwTuc = Matrix4x4.identity;
m_uwOffsetTuw = Matrix4x4.identity;
}
/// <summary>
/// Handle the callback sent by the Tango Service
/// when a new pose is sampled.
/// </summary>
/// <param name="callbackContext">Callback context.</param>
/// <param name="pose">Pose.</param>
private void _OnPoseAvailable(IntPtr callbackContext, TangoPoseData pose)
{
m_latestPoseStatus = pose.status_code;
// MotionTracking
if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
m_motionTrackingData.framePair = pose.framePair;
m_motionTrackingData.status_code = pose.status_code;
m_motionTrackingData.orientation = pose.orientation;
m_motionTrackingData.translation = pose.translation;
m_motionTrackingData.timestamp = pose.timestamp;
m_motionTrackingData.confidence = pose.confidence;
m_motionTrackingData.accuracy = pose.accuracy;
m_hasNewMotionTrackingData = true;
}
// ADF Localized
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
m_areaLearningData.framePair = pose.framePair;
m_areaLearningData.status_code = pose.status_code;
m_areaLearningData.orientation = pose.orientation;
m_areaLearningData.translation = pose.translation;
m_areaLearningData.timestamp = pose.timestamp;
m_areaLearningData.confidence = pose.confidence;
m_areaLearningData.accuracy = pose.accuracy;
m_hasNewAreaLearningData = true;
}
// Relocalized against ADF
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE)
{
m_relocalizationData.framePair = pose.framePair;
m_relocalizationData.status_code = pose.status_code;
m_relocalizationData.orientation = pose.orientation;
m_relocalizationData.translation = pose.translation;
m_relocalizationData.timestamp = pose.timestamp;
m_relocalizationData.confidence = pose.confidence;
m_relocalizationData.accuracy = pose.accuracy;
m_hasNewRelocalizationData = true;
}
m_isDirty = true;
}
/// <summary>
/// Handle the callback sent by the Tango Service
/// when a new pose is sampled.
/// </summary>
/// <param name="pose">Pose.</param>
public void OnTangoPoseAvailable(Tango.TangoPoseData pose)
{
// Get out of here if the pose is null
if (pose == null)
{
Debug.Log("TangoPoseDate is null.");
return;
}
// The callback pose is for device with respect to start of service pose.
if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
// Update the stats for the pose for the debug text
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// Create new Quaternion and Vec3 from the pose data received in the event.
m_tangoPosition = new Vector3((float)pose.translation[0],
(float)pose.translation[1],
(float)pose.translation[2]);
m_tangoRotation = new Quaternion((float)pose.orientation[0],
(float)pose.orientation[1],
(float)pose.orientation[2],
(float)pose.orientation[3]);
// Reset the current status frame count if the status code changed.
if (pose.status_code != m_status)
{
m_frameCount = 0;
}
m_frameCount++;
// Compute delta frame timestamp.
m_frameDeltaTime = (float)pose.timestamp - m_prevFrameTimestamp;
m_prevFrameTimestamp = (float)pose.timestamp;
// Construct the start of service with respect to device matrix from the pose.
Matrix4x4 matrixssTd = Matrix4x4.TRS(m_tangoPosition, m_tangoRotation, Vector3.one);
// Converting from Tango coordinate frame to Unity coodinate frame.
Matrix4x4 matrixuwTuc = m_matrixuwTss * matrixssTd * m_matrixdTuc;
// Extract new local position
transform.position = matrixuwTuc.GetColumn(3);
// Extract new local rotation
transform.rotation = Quaternion.LookRotation(matrixuwTuc.GetColumn(2), matrixuwTuc.GetColumn(1));
}
else
{
// if the current pose is not valid we set the pose to identity
m_tangoPosition = Vector3.zero;
m_tangoRotation = Quaternion.identity;
}
// Finally, apply the new pose status
m_status = pose.status_code;
}
}
/// <summary>
/// Awake is called when the script instance is being loaded.
/// </summary>
public void Awake()
{
// Constant matrix converting start of service frame to Unity world frame.
m_uwTss = new Matrix4x4();
m_uwTss.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f));
m_uwTss.SetColumn(1, new Vector4(0.0f, 0.0f, 1.0f, 0.0f));
m_uwTss.SetColumn(2, new Vector4(0.0f, 1.0f, 0.0f, 0.0f));
m_uwTss.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
// Constant matrix converting Unity world frame frame to device frame.
m_dTuc = new Matrix4x4();
m_dTuc.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f));
m_dTuc.SetColumn(1, new Vector4(0.0f, 1.0f, 0.0f, 0.0f));
m_dTuc.SetColumn(2, new Vector4(0.0f, 0.0f, -1.0f, 0.0f));
m_dTuc.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
m_poseCount = -1;
m_poseStatus = TangoEnums.TangoPoseStatusType.NA;
m_prevTangoRotation = m_tangoRotation = Quaternion.identity;
m_prevTangoPosition = m_tangoPosition = Vector3.zero;
m_uwTuc = Matrix4x4.identity;
m_uwOffsetTuw = Matrix4x4.identity;
}
/// <summary>
/// Set controller's transformation based on received pose.
/// </summary>
/// <param name="pose">Received Tango pose data.</param>
private void _UpdateTransformationFromPose(TangoPoseData pose)
{
// Remember the previous position, so you can do delta motion
m_prevTangoPosition = m_tangoPosition;
m_prevTangoRotation = m_tangoRotation;
// The callback pose is for device with respect to start of service pose.
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// Construct matrix for the start of service with respect to device from the pose.
Vector3 posePosition = new Vector3((float)pose.translation[0],
(float)pose.translation[1],
(float)pose.translation[2]);
Quaternion poseRotation = new Quaternion((float)pose.orientation[0],
(float)pose.orientation[1],
(float)pose.orientation[2],
(float)pose.orientation[3]);
Matrix4x4 ssTd = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one);
// Calculate matrix for the camera in the Unity world, taking into account offsets.
m_uwTuc = m_uwTss * ssTd * m_dTuc;
Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc;
// Extract final position, rotation.
m_tangoPosition = uwOffsetTuc.GetColumn(3);
m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1));
// Other pose data -- Pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
}
m_poseStatus = pose.status_code;
if (m_clutchActive)
{
// When clutching, preserve position.
m_tangoPosition = m_prevTangoPosition;
// When clutching, preserve yaw, keep changes in pitch, roll.
Vector3 rotationAngles = m_tangoRotation.eulerAngles;
rotationAngles.y = m_prevTangoRotation.eulerAngles.y;
m_tangoRotation.eulerAngles = rotationAngles;
}
// Calculate final position and rotation deltas and apply them.
transform.localPosition = m_tangoPosition;
transform.localRotation = m_tangoRotation;
}
/// <summary>
/// Handle the callback sent by the Tango Service
/// when a new pose is sampled.
/// </summary>
/// <param name="callbackContext">Callback context.</param>
/// <param name="pose">Pose.</param>
private void _OnPoseAvailable(IntPtr callbackContext, TangoPoseData pose)
{
m_latestPoseStatus = pose.status_code;
// MotionTracking
if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
// Only set new pose once the previous pose has been returned.
if(m_motionTrackingData == null)
{
TangoPoseData currentPose = m_poseDataPool.Pop();
if(currentPose == null)
{
return;
}
else
{
currentPose.DeepCopy(pose);
m_motionTrackingData = currentPose;
}
}
}
// ADF Localized
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
// Only set new pose once the previous pose has been returned.
if(m_areaLearningData == null)
{
TangoPoseData currentPose = m_poseDataPool.Pop();
if(currentPose == null)
{
return;
}
else
{
currentPose.DeepCopy(pose);
m_areaLearningData = currentPose;
}
}
}
// Relocalized against ADF
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE)
{
// Only set new pose once the previous pose has been returned.
if(m_relocalizationData == null)
{
TangoPoseData currentPose = m_poseDataPool.Pop();
if(currentPose == null)
{
return;
}
else
{
currentPose.DeepCopy(pose);
m_relocalizationData = currentPose;
}
}
}
m_isDirty = true;
}
/// <summary>
/// Handle the callback sent by the Tango Service
/// when a new pose is sampled.
/// </summary>
/// <param name="callbackContext">Callback context.</param>
/// <param name="pose">Pose.</param>
private void _OnPoseAvailable(IntPtr callbackContext, TangoPoseData pose)
{
m_latestPoseStatus = pose.status_code;
// MotionTracking
if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
// Only set new pose once the previous pose has been returned.
if (m_motionTrackingData == null)
{
TangoPoseData currentPose = m_poseDataPool.Pop();
if (currentPose == null)
{
return;
}
else
{
currentPose.DeepCopy(pose);
m_motionTrackingData = currentPose;
}
}
}
// ADF Localized
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
// Only set new pose once the previous pose has been returned.
if (m_areaLearningData == null)
{
TangoPoseData currentPose = m_poseDataPool.Pop();
if (currentPose == null)
{
return;
}
else
{
currentPose.DeepCopy(pose);
m_areaLearningData = currentPose;
}
}
}
// Relocalized against ADF
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE)
{
// Only set new pose once the previous pose has been returned.
if (m_relocalizationData == null)
{
TangoPoseData currentPose = m_poseDataPool.Pop();
if (currentPose == null)
{
return;
}
else
{
currentPose.DeepCopy(pose);
m_relocalizationData = currentPose;
}
}
}
m_isDirty = true;
}
/// <summary>
/// Unity callback when application is paused.
/// </summary>
/// <param name="pauseStatus">The pauseStatus as reported by Unity.</param>
public void OnApplicationPause(bool pauseStatus)
{
m_poseTimestamp = -1.0f;
m_poseCount = -1;
m_poseStatus = TangoEnums.TangoPoseStatusType.NA;
}
/// @endcond
/// <summary>
/// Updates the transformation to the pose for that timestamp.
/// </summary>
/// <param name="timestamp">Time in seconds to update the transformation to.</param>
private void _UpdateTransformation(double timestamp)
{
TangoPoseData pose = new TangoPoseData();
TangoCoordinateFramePair pair;
if (m_useAreaDescriptionPose)
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
else
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
PoseProvider.GetPoseAtTime(pose, timestamp, pair);
// The callback pose is for device with respect to start of service pose.
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// Construct matrix for the start of service with respect to device from the pose.
Matrix4x4 ssTd = pose.ToMatrix4x4();
// Calculate matrix for the camera in the Unity world, taking into account offsets.
Matrix4x4 uwTuc = m_uwTss * ssTd * m_dTuc * TangoSupport.m_colorCameraPoseRotation;
// Extract final position, rotation.
m_tangoPosition = uwTuc.GetColumn(3);
m_tangoRotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));
// Other pose data -- Pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
// Other pose data -- Pose time.
m_poseTimestamp = timestamp;
}
m_poseStatus = pose.status_code;
// Apply final position and rotation.
transform.position = m_tangoPosition;
transform.rotation = m_tangoRotation;
}
/// <summary>
/// Unity callback when application is paused.
/// </summary>
/// <param name="pauseStatus">The pauseStatus as reported by Unity.</param>
public void OnApplicationPause(bool pauseStatus)
{
m_frameDeltaTime = -1.0f;
m_prevFrameTimestamp = -1.0f;
m_frameCount = -1;
m_status = TangoEnums.TangoPoseStatusType.NA;
m_tangoRotation = Quaternion.identity;
m_tangoPosition = Vector3.zero;
}
/// <summary>
/// OnTangoPoseAvailable is called from Tango when a new Pose is available.
/// </summary>
/// <param name="pose">The new Tango pose.</param>
public void OnTangoPoseAvailable(TangoPoseData pose)
{
// Get out of here if the pose is null
if (pose == null)
{
Debug.Log("TangoPoseDate is null.");
return;
}
// Only interested in pose updates relative to the start of service pose.
if (pose.framePair.baseFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE
|| pose.framePair.targetFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
return;
}
// Remember the previous position, so you can do delta motion
m_prevTangoPosition = m_tangoPosition;
m_prevTangoRotation = m_tangoRotation;
// The callback pose is for device with respect to start of service pose.
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// Construct matrix for the start of service with respect to device from the pose.
Vector3 posePosition = new Vector3((float)pose.translation[0],
(float)pose.translation[1],
(float)pose.translation[2]);
Quaternion poseRotation = new Quaternion((float)pose.orientation[0],
(float)pose.orientation[1],
(float)pose.orientation[2],
(float)pose.orientation[3]);
Matrix4x4 ssTd = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one);
// Calculate matrix for the camera in the Unity world, taking into account offsets.
m_uwTuc = m_uwTss * ssTd * m_dTuc;
Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc;
// Extract final position, rotation.
m_tangoPosition = uwOffsetTuc.GetColumn(3);
m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1));
// Other pose data -- Pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
// Other pose data -- Pose delta time.
m_poseDeltaTime = (float)pose.timestamp - m_poseTimestamp;
m_poseTimestamp = (float)pose.timestamp;
}
m_poseStatus = pose.status_code;
if (m_clutchActive)
{
// When clutching, preserve position.
m_tangoPosition = m_prevTangoPosition;
// When clutching, preserve yaw, keep changes in pitch, roll.
Vector3 rotationAngles = m_tangoRotation.eulerAngles;
rotationAngles.y = m_prevTangoRotation.eulerAngles.y;
m_tangoRotation.eulerAngles = rotationAngles;
}
// Calculate final position and rotation deltas and apply them.
Vector3 deltaPosition = m_tangoPosition - m_prevTangoPosition;
Quaternion deltaRotation = m_tangoRotation * Quaternion.Inverse(m_prevTangoRotation);
if (m_characterMotion)
{
m_characterController.Move(deltaPosition);
transform.rotation = deltaRotation * transform.rotation;
}
else
{
transform.position = transform.position + deltaPosition;
transform.rotation = deltaRotation * transform.rotation;
}
}
/// <summary>
/// Initializes a new instance of the <see cref="Tango.TangoPoseData"/> class.
/// </summary>
public TangoPoseData()
{
version = 0;
timestamp = 0.0;
orientation = new double[4];
translation = new double[3];
status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN;
framePair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
framePair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
confidence = 0;
}
/// @endcond
/// <summary>
/// Updates the transformation to the pose for that timestamp.
/// </summary>
/// <param name="timestamp">Time in seconds to update the transformation to.</param>
private void _UpdateTransformation(double timestamp)
{
TangoPoseData pose = new TangoPoseData();
TangoCoordinateFramePair pair;
// Choose the proper pair according to the properties of this controller
if (m_useAreaDescriptionPose)
{
if (m_tangoApplication.m_enableCloudADF)
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_GLOBAL_WGS84;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
else
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
}
else
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
PoseProvider.GetPoseAtTime(pose, timestamp, pair);
// Update properties from pose
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
DMatrix4x4 globalTLocal;
bool success = m_tangoApplication.GetGlobalTLocal(out globalTLocal);
if (!success)
{
Debug.LogError("Unable to obtain GlobalTLocal from TangoApplication.");
return;
}
DMatrix4x4 uwTDevice = DMatrix4x4.FromMatrix4x4(TangoSupport.UNITY_WORLD_T_START_SERVICE) *
globalTLocal.Inverse * DMatrix4x4.TR(pose.translation, pose.orientation);
// Calculate matrix for the camera in the Unity world, taking into account offsets.
Matrix4x4 uwTuc = uwTDevice.ToMatrix4x4() * m_dTuc * TangoSupport.m_colorCameraPoseRotation;
// Extract final position, rotation.
m_tangoPosition = uwTuc.GetColumn(3);
m_tangoRotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));
// Other pose data -- Pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
// Other pose data -- Pose time.
m_poseTimestamp = timestamp;
}
m_poseStatus = pose.status_code;
// Apply final position and rotation.
transform.position = m_tangoPosition;
transform.rotation = m_tangoRotation;
}
/// <summary>
/// OnTangoPoseAvailable is called from Tango when a new Pose is available.
/// </summary>
/// <param name="pose">The new Tango pose.</param>
public void OnTangoPoseAvailable(TangoPoseData pose)
{
// Get out of here if the pose is null
if (pose == null)
{
Debug.Log("TangoPoseDate is null.");
return;
}
// Only interested in pose updates relative to the start of service pose.
if (pose.framePair.baseFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE ||
pose.framePair.targetFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
return;
}
// Remember the previous position, so you can do delta motion
m_prevTangoPosition = m_tangoPosition;
m_prevTangoRotation = m_tangoRotation;
// The callback pose is for device with respect to start of service pose.
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// Construct matrix for the start of service with respect to device from the pose.
Vector3 posePosition = new Vector3((float)pose.translation[0],
(float)pose.translation[1],
(float)pose.translation[2]);
Quaternion poseRotation = new Quaternion((float)pose.orientation[0],
(float)pose.orientation[1],
(float)pose.orientation[2],
(float)pose.orientation[3]);
Matrix4x4 ssTd = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one);
// Calculate matrix for the camera in the Unity world, taking into account offsets.
m_uwTuc = m_uwTss * ssTd * m_dTuc;
Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc;
// Extract final position, rotation.
m_tangoPosition = uwOffsetTuc.GetColumn(3);
m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1));
// Other pose data -- Pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
// Other pose data -- Pose delta time.
m_poseDeltaTime = (float)pose.timestamp - m_poseTimestamp;
m_poseTimestamp = (float)pose.timestamp;
}
m_poseStatus = pose.status_code;
if (m_clutchActive)
{
// When clutching, preserve position.
m_tangoPosition = m_prevTangoPosition;
// When clutching, preserve yaw, keep changes in pitch, roll.
Vector3 rotationAngles = m_tangoRotation.eulerAngles;
rotationAngles.y = m_prevTangoRotation.eulerAngles.y;
m_tangoRotation.eulerAngles = rotationAngles;
}
// Calculate final position and rotation deltas and apply them.
Vector3 deltaPosition = m_tangoPosition - m_prevTangoPosition;
Quaternion deltaRotation = m_tangoRotation * Quaternion.Inverse(m_prevTangoRotation);
if (m_characterMotion)
{
m_characterController.Move(deltaPosition);
transform.rotation = deltaRotation * transform.rotation;
}
else
{
transform.position = transform.position + deltaPosition;
transform.rotation = deltaRotation * transform.rotation;
}
}
/// <summary>
/// Handle the callback sent by the Tango Service
/// when a new pose is sampled.
/// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION!
/// </summary>
/// <param name="callbackContext">Callback context.</param>
/// <param name="pose">Pose.</param>
public void OnTangoPoseAvailable(Tango.TangoPoseData pose)
{
// Get out of here if the pose is null
if (pose == null)
{
Debug.Log("TangoPoseDate is null.");
return;
}
// The callback pose is for device with respect to start of service pose.
if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
if(pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// Cache the position and rotation to be set in the update function.
// This needs to be done because this callback does not
// happen in the main game thread.
m_tangoPosition = new Vector3((float)pose.translation [0],
(float)pose.translation [1],
(float)pose.translation [2]);
m_tangoRotation = new Quaternion((float)pose.orientation [0],
(float)pose.orientation [1],
(float)pose.orientation [2],
(float)pose.orientation [3]);
}
else // if the current pose is not valid we set the pose to identity
{
m_tangoPosition = Vector3.zero;
m_tangoRotation = Quaternion.identity;
}
}
// Reset the current status frame count if the status code changed.
if (pose.status_code != m_status)
{
m_frameCount = 0;
}
// Update the stats for the pose for the debug text
m_status = pose.status_code;
m_frameCount++;
// Compute delta frame timestamp.
m_frameDeltaTime = (float)pose.timestamp - m_prevFrameTimestamp;
m_prevFrameTimestamp = (float)pose.timestamp;
// Switch m_isDirty to true, so that the new pose get rendered in update.
m_isDirty = (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID);
}
/// <summary>
/// Set controller's transformation based on received pose.
/// </summary>
/// <param name="pose">Received Tango pose data.</param>
private void _UpdateTransformationFromPose(TangoPoseData pose)
{
// Remember the previous position so you can do delta motion.
m_prevTangoPosition = m_tangoPosition;
m_prevTangoRotation = m_tangoRotation;
// The callback pose is for device with respect to start of service pose.
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
DMatrix4x4 globalTLocal;
bool success = m_tangoApplication.GetGlobalTLocal(out globalTLocal);
if (!success)
{
Debug.LogError("Unable to obtain GlobalTLocal from Tango application.");
return;
}
DMatrix4x4 startOfServiceTDevice = globalTLocal.Inverse * DMatrix4x4.TR(pose.translation, pose.orientation);
m_uwTuc = TangoSupport.UNITY_WORLD_T_START_SERVICE * startOfServiceTDevice.ToMatrix4x4()
* TangoSupport.DEVICE_T_UNITY_CAMERA * TangoSupport.m_devicePoseRotation;
Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc;
m_tangoPosition = uwOffsetTuc.GetColumn(3);
// apply scale for Daydream
m_tangoPosition.x *= m_motionMappingScaleXZ;
m_tangoPosition.z *= m_motionMappingScaleXZ;
m_tangoPosition.y *= m_motionMappingScaleY;
m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1));
// Other pose data: pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
// Other pose data: pose delta time.
m_poseDeltaTime = (float)pose.timestamp - m_poseTimestamp;
m_poseTimestamp = (float)pose.timestamp;
}
m_poseStatus = pose.status_code;
if (m_clutchActive)
{
// When clutching, preserve position.
m_tangoPosition = m_prevTangoPosition;
// When clutching, preserve yaw, keep changes in pitch, roll.
Vector3 rotationAngles = m_tangoRotation.eulerAngles;
rotationAngles.y = m_prevTangoRotation.eulerAngles.y;
m_tangoRotation.eulerAngles = rotationAngles;
}
// edit: following https://blogs.unity3d.com/jp/2017/10/18/mobile-inside-out-vr-tracking-now-readily-available-on-your-phone-with-unity/
// Calculate final position and rotation deltas and apply them.
Vector3 deltaPosition = m_tangoPosition - m_prevTangoPosition;
Quaternion deltaRotation = m_tangoRotation * Quaternion.Inverse(m_prevTangoRotation);
if (m_characterMotion && m_characterController != null)
{
m_characterController.Move(deltaPosition);
//transform.rotation = deltaRotation * transform.rotation;
}
else
{
transform.position = transform.position + deltaPosition;
//transform.rotation = deltaRotation * transform.rotation;
}
}
/// <summary>
/// Deep copy from poseToCopy into this.
/// </summary>
/// <param name="poseToCopy">Pose to copy.</param>
public void DeepCopy(TangoPoseData poseToCopy)
{
this.version = poseToCopy.version;
this.timestamp = poseToCopy.timestamp;
this.status_code = poseToCopy.status_code;
this.framePair.baseFrame = poseToCopy.framePair.baseFrame;
this.framePair.targetFrame = poseToCopy.framePair.targetFrame;
this.confidence = poseToCopy.confidence;
for (int i = 0; i < 4; ++i)
{
this.orientation[i] = poseToCopy.orientation[i];
}
for (int i = 0; i < 3; ++i)
{
this.translation[i] = poseToCopy.translation[i];
}
}
/// <summary>
/// Update the transformation to the pose for that timestamp.
/// </summary>
/// <param name="timestamp">Time in seconds to update the transformation to.</param>
private void _UpdateTransformation(double timestamp)
{
TangoPoseData pose = new TangoPoseData();
TangoCoordinateFramePair pair;
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(pose, timestamp, pair);
// The callback pose is for device with respect to start of service pose.
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
// Construct matrix for the start of service with respect to device from the pose.
Vector3 posePosition = new Vector3((float)pose.translation[0],
(float)pose.translation[1],
(float)pose.translation[2]);
Quaternion poseRotation = new Quaternion((float)pose.orientation[0],
(float)pose.orientation[1],
(float)pose.orientation[2],
(float)pose.orientation[3]);
Matrix4x4 ssTd = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one);
// Calculate matrix for the camera in the Unity world, taking into account offsets.
Matrix4x4 uwTuc = m_uwTss * ssTd * m_dTuc;
// Extract final position, rotation.
m_tangoPosition = uwTuc.GetColumn(3);
m_tangoRotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));
// Other pose data -- Pose count gets reset if pose status just became valid.
if (pose.status_code != m_poseStatus)
{
m_poseCount = 0;
}
m_poseCount++;
// Other pose data -- Pose time.
m_poseTimestamp = timestamp;
}
m_poseStatus = pose.status_code;
// Apply final position and rotation.
transform.position = m_tangoPosition;
transform.rotation = m_tangoRotation;
}
/// <summary>
/// Unity callback when application is paused.
/// </summary>
/// <param name="pauseStatus">The pauseStatus as reported by Unity.</param>
public void OnApplicationPause(bool pauseStatus)
{
m_poseCount = -1;
m_poseStatus = TangoEnums.TangoPoseStatusType.NA;
}
