void Update()
{
// visualization for single markers
if (type != MarkerType.multi)
{
if (confidenceColoBox != null)
{
confidenceColoBox.color = new Color(0, confidence, 0, 1);
}
if (confidenceTextBox != null)
{
confidenceTextBox.text = string.Format("{0:0.000}", confidence);
}
}
// visulization for all
if (target != null && visible)
{
if (applyRotation)
{
target.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(transMatrix);
}
if (applyTranslation)
{
target.transform.localPosition = ARUWPUtils.PositionFromMatrix(transMatrix);
// Debug.Log(TAG + ": Current local position " + target.transform.localPosition);
}
}
}
/// <summary>
/// This function is called by the associated ARUWPMarker.cs to update the pose of this
/// virtual object. When smoothing is enabled, the new pose will be filtered with current
/// pose using lerp. Big sudden change of 6-DOF pose will be prohibited. [public use]
/// </summary>
public void SetMatrix4x4(Matrix4x4 localToWorldMatrix)
{
Vector3 previousPosition = transform.localPosition;
Quaternion previousRotation = transform.localRotation;
Vector3 targetPosition = ARUWPUtils.PositionFromMatrix(localToWorldMatrix);
Quaternion targetRotation = ARUWPUtils.QuaternionFromMatrix(localToWorldMatrix);
if (!smoothing)
{
transform.localRotation = targetRotation;
transform.localPosition = targetPosition;
}
else
{
float positionDiff = Vector3.Distance(targetPosition, previousPosition);
float rotationDiff = Quaternion.Angle(targetRotation, previousRotation);
if (Mathf.Abs(positionDiff) < positionJumpThreshold && Mathf.Abs(rotationDiff) < rotationJumpThreshold)
{
transform.localRotation = Quaternion.Slerp(previousRotation, targetRotation, lerp);
transform.localPosition = Vector3.Lerp(previousPosition, targetPosition, lerp);
pendingPositionList.Clear();
pendingRotationList.Clear();
}
else
{
// maybe there is a jump
pendingPositionList.Add(targetPosition);
pendingRotationList.Add(targetRotation);
bool confirmJump = true;
if (pendingPositionList.Count > maxPendingList)
{
for (int i = 0; i < maxPendingList - 1; i++)
{
float tempPositionDiff = Vector3.Distance(pendingPositionList[pendingPositionList.Count - i - 1], pendingPositionList[pendingPositionList.Count - i - 2]);
float tempRotationDiff = Quaternion.Angle(pendingRotationList[pendingRotationList.Count - i - 1], pendingRotationList[pendingRotationList.Count - i - 2]);
if (Mathf.Abs(tempPositionDiff) > positionRecoverThreshold || Mathf.Abs(tempRotationDiff) > rotationRecoverThreshold)
{
confirmJump = false;
break;
}
}
if (confirmJump)
{
transform.localRotation = targetRotation;
transform.localPosition = targetPosition;
pendingPositionList.Clear();
pendingRotationList.Clear();
}
}
}
}
}
// latestMarkerTransMatrix now contains the latest marker pose in static world frame
private void processMarkerTrackingInfo()
{
// Interlocked.Exchange(ref latestMarkerTransMatrix, aruwpMarker.GetMarkerTransformationMatrix());
latestMarkerTransMatrix = aruwpMarker.GetMarkerTransformationMatrix();
// convert the transformation in hololens frame
// to transformation in the global frame
// because hololens can move in the world, it's frame is not fixed
dummyGameObject.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestMarkerTransMatrix);
dummyGameObject.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestMarkerTransMatrix);
latestMarkerTransMatrix = dummyGameObject.transform.localToWorldMatrix;
// Debug.Log("marker to user" + ARUWPUtils.PositionFromMatrix(latestMarkerTransMatrix));
// Debug.Log("marker to user orientation" + ARUWPUtils.QuaternionFromMatrix(latestMarkerTransMatrix).eulerAngles);
}
/// <summary>
/// Unity Monobehavior function: update tracking information of the current marker.
/// If magic function is enabled, then a distortion transformation is applied on top
/// of the tracking result. [internal use]
/// </summary>
private void Update()
{
if (signalTrackingUpdated && id != -1)
{
Interlocked.Exchange(ref latestTrackingInfo, _info);
if (latestTrackingInfo.visible)
{
latestTransMatrix = ARUWPUtils.ConvertARUWPFloatArrayToMatrix4x4(latestTrackingInfo.trans);
if (performMagicFunction)
{
MagicFunction();
}
if (target != null)
{
if (!anchoredToWorld)
{
if (applyRotation)
{
target.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTransMatrix);
}
if (applyTranslation)
{
target.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTransMatrix);
}
}
else
{
if (applyRotation)
{
dummyGameObject.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTrackingInfo.locatableCameraToWorld * latestTransMatrix);
}
if (applyTranslation)
{
dummyGameObject.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTrackingInfo.locatableCameraToWorld * latestTransMatrix);
}
latestPoseInWorldCoordinates = dummyGameObject.transform.localToWorldMatrix;
ARUWPUtils.SetMatrix4x4ToGameObject(ref target, latestPoseInWorldCoordinates);
}
}
}
}
signalTrackingUpdated = false;
}
/// <summary>
/// Unity Monobehavior function: update tracking information of the current marker.
/// If magic function is enabled, then a distortion transformation is applied on top
/// of the tracking result. [internal use]
/// </summary>
private void LateUpdate()
{
if (signalTrackingUpdated && id != -1)
{
Interlocked.Exchange(ref latestTrackingInfo, _info);
if (latestTrackingInfo.visible)
{
latestTransMatrix = ARUWPUtils.ConvertARUWPFloatArrayToMatrix4x4(latestTrackingInfo.trans);
// ARssistSi
latestTransMatrix = calibrationMatrix * latestTransMatrix;
if (target != null)
{
if (!anchoredToWorld)
{
if (applyRotation)
{
target.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTransMatrix);
}
if (applyTranslation)
{
target.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTransMatrix);
}
}
else
{
//if (applyRotation)
//{
// dummyGameObject.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTransMatrix);
//}
//if (applyTranslation)
//{
// dummyGameObject.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTransMatrix);
//}
ARUWPUtils.SetMatrix4x4ToGameObject(ref target, latestTrackingInfo.locatableCameraToWorld * latestTransMatrix);
//ARUWPUtils.SetMatrix4x4ToGameObject(ref target, dummyGameObject.transform.localToWorldMatrix);
}
}
}
}
signalTrackingUpdated = false;
}
void Update()
{
// visualization for single markers
if (type != MarkerType.multi)
{
if (confidenceColoBox != null)
{
confidenceColoBox.color = new Color(0, confidence, 0, 1);
}
if (confidenceTextBox != null)
{
confidenceTextBox.text = string.Format("{0:0.000}", confidence);
}
}
// visulization for all
if (target != null && visible)
{
if (!anchoredToWorld)
{
if (applyRotation)
{
target.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(transMatrix);
}
if (applyTranslation)
{
target.transform.localPosition = ARUWPUtils.PositionFromMatrix(transMatrix);
}
}
else
{
if (applyRotation)
{
dummyGameObject.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(transMatrix);
}
if (applyTranslation)
{
dummyGameObject.transform.localPosition = ARUWPUtils.PositionFromMatrix(transMatrix);
}
ARUWPUtils.SetMatrix4x4ToGameObject(ref target, dummyGameObject.transform.localToWorldMatrix);
}
}
}
public void TryPublishing(TimeSpan currentTime, double elapsedTimeInSeconds)
{
if (elapsedTimeInSeconds >= nextPublishTime && marker != null)
{
nextPublishTime = nextPublishTime + publishPeriod;
if (marker.GetMarkerVisibility())
{
Matrix4x4 latestPoseMatrix = marker.GetMarkerPoseInWorldCoordinateFrame();
Quaternion currentRotation = ARUWPUtils.QuaternionFromMatrix(latestPoseMatrix);
Vector3 currentPosition = ARUWPUtils.PositionFromMatrix(latestPoseMatrix);
#if NETFX_CORE
ThreadPool.RunAsync((MarkerPoseSendWork) => { SendPose(currentTime.Add(Timer.GetOffsetUTC()), currentRotation, currentPosition); });
#endif
}
else
{
Debug.Log("Marker not detected.");
}
}
}
private void calculateForceVisualOrientation()
{
_forceVisualOrientation = ARUWPUtils.QuaternionFromMatrix(robotPoseToHololensMatrices[TCPManager.TCPPackageConstants.BASE]);
_forceVisualOrientation = toVisualizeForce * _forceVisualOrientation;
}
/// <summary>
/// Unity Monobehavior function: update tracking information of the current marker.
/// If magic function is enabled, then a distortion transformation is applied on top
/// of the tracking result. [internal use]
/// </summary>
private void Update()
{
if (signalTrackingUpdated && id != -1)
{
Interlocked.Exchange(ref latestTrackingInfo, _info);
if (latestTrackingInfo.visible)
{
latestTransMatrix = ARUWPUtils.ConvertARUWPFloatArrayToMatrix4x4(latestTrackingInfo.trans);
//if (performMagicFunction) { MagicFunction(); }
if (target != null)
{
if (!anchoredToWorld)
{
if (applyRotation)
{
target.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTransMatrix);
}
if (applyTranslation)
{
target.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTransMatrix);
}
if (performCustomMagic)
{
Matrix4x4 newPos = new Matrix4x4();
newPos.SetColumn(0, new Vector4(target.transform.localPosition.x, dummyGameObject.transform.localPosition.y, dummyGameObject.transform.localPosition.z, 1));
Matrix4x4 result = newMagic * newPos;
target.transform.localPosition = result.GetColumn(0);
}
}
else
{
if (applyRotation)
{
dummyGameObject.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTransMatrix);
}
if (applyTranslation)
{
dummyGameObject.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTransMatrix);
}
if (performCustomMagic)
{
Matrix4x4 newPos = new Matrix4x4();
newPos.SetColumn(0, new Vector4(dummyGameObject.transform.localPosition.x, dummyGameObject.transform.localPosition.y, dummyGameObject.transform.localPosition.z, 1));
Matrix4x4 result = newMagic * newPos;
dummyGameObject.transform.localPosition = result.GetColumn(0);
}
// Dummy Object is anchored to camera. And this line converts it to world coordinate.
ARUWPUtils.SetMatrix4x4ToGameObject(ref target, dummyGameObject.transform.localToWorldMatrix);
//if (performCustomMagic)
//{
// Matrix4x4 newPos = new Matrix4x4();
// newPos.SetColumn(0, new Vector4(target.transform.localPosition.x, target.transform.localPosition.y, target.transform.localPosition.z, 1));
// Matrix4x4 result = newMagic * newPos;
// target.transform.localPosition = result.GetColumn(0);
//}
}
}
}
}
signalTrackingUpdated = false;
}