/// <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) * motionSlider.value;
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>
/// Fix this! Doesn't output correct values. Input matrix probably not correct.
/// Generates the depth map using nearest neighbor upsampling.
/// </summary>
private void GenerateDepthMap_NearestNeighbor(ref TangoUnityDepth tangoUnityDepth)
{
TangoPoseData poseData = new TangoPoseData();
TangoCoordinateFramePair pair;
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(poseData, tangoUnityDepth.m_timestamp, pair);
if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
return;
}
Vector3 position;
Quaternion rotation;
TangoSupport.TangoPoseToWorldTransform(poseData, out position, out rotation);
Matrix4x4 ccWorld = Matrix4x4.TRS(position, rotation, Vector3.one);
bool isValid = false;
Vector3 colorCameraPoint = new Vector3();
for (int i = 0; i < _depthMapWidth; i++)
{
for (int j = 0; j < _depthMapHeight; j++)
{
if (TangoSupport.ScreenCoordinateToWorldNearestNeighbor(
_PointCloud.m_points, _PointCloud.m_pointsCount,
tangoUnityDepth.m_timestamp,
_ccIntrinsics,
ref ccWorld,
new Vector2(i / (float)_depthMapWidth, j / (float)_depthMapHeight),
out colorCameraPoint, out isValid) == Common.ErrorType.TANGO_INVALID)
{
_depthTexture.SetPixel(i, j, Color.red);
continue;
}
if (isValid)
{
float c = 1 - colorCameraPoint.z / 4.5f;
_depthTexture.SetPixel(i, j, new Color(c, c, c));
}
else
{
_depthTexture.SetPixel(i, j, Color.black);
}
}
}
_depthTexture.Apply();
_DepthMapQuad.sharedMaterial.mainTexture = _depthTexture;
//_debugMessage = "DepthAvailable: " + _waitingForDepth.ToString() + "\n" +
// " points: " + _PointCloud.m_pointsCount + "\n" +
// " timestamp: " + tangoUnityDepth.m_timestamp.ToString("0.00") + "\n" +
// " XYZ:" + colorCameraPoint.ToString();
}