public override void receiveErrorPose(SimpleErrorPose newPose)
{
lock (thisLock)
{
m_pose = UbiMeasurementUtils.ubitrackToUnity(newPose);
}
}
// Use this for initialization
public override void utInit(SimpleFacade simpleFacade)
{
base.utInit(simpleFacade);
switch (ubitrackEvent)
{
case UbitrackEventType.Pull: {
m_posePull = simpleFacade.getSimplePullSinkErrorPose(patternID);
m_simplePose = new SimpleErrorPose();
if (m_posePull == null)
{
throw new Exception("SimpleApplicationPullSinkErrorPose not found for poseID:" + patternID);
}
break;
}
case UbitrackEventType.Push: {
m_poseReceiver = new UnityErrorPoseReceiver();
if (!simpleFacade.setErrorPoseCallback(patternID, m_poseReceiver))
{
throw new Exception("SimpleErrorPoseReceiver could not be set for poseID:" + patternID);
}
break;
}
default:
break;
}
}
public override void receiveErrorPose(SimpleErrorPose newPose)
{
lock (thisLock)
{
m_pose = UbiMeasurementUtils.ubitrackToUnity(newPose);
}
}
internal static Measurement <ErrorPose> ubitrackToUnity(SimpleErrorPose ubitrackPose)
{
Vector3 trans = new Vector3((float)ubitrackPose.tx, (float)ubitrackPose.ty, (float)ubitrackPose.tz);
Quaternion rot = new Quaternion((float)ubitrackPose.rx, (float)ubitrackPose.ry, (float)ubitrackPose.rz, (float)ubitrackPose.rw);
ErrorPose data = new ErrorPose();
trans = trans * UbiToUnityScaleFactor;
coordsysemChange(trans, ref data.pos);
coordsysemChange(rot, ref data.rot);
ubitrackCovarianceToFloatArray(ubitrackPose, ref data.covariance);
return(new Measurement <ErrorPose>(data, ubitrackPose.timestamp));
}
public static void ubitrackCovarianceToFloatArray(SimpleErrorPose ubitrackPose, ref float[,] covariance)
{
// todo needs rhs lhs transfrom
covariance[0, 0] = (float)ubitrackPose.co11;
covariance[0, 1] = (float)ubitrackPose.co12;
covariance[0, 2] = (float)ubitrackPose.co13;
covariance[0, 3] = (float)ubitrackPose.co14;
covariance[0, 4] = (float)ubitrackPose.co15;
covariance[0, 5] = (float)ubitrackPose.co16;
covariance[1, 0] = (float)ubitrackPose.co21;
covariance[1, 1] = (float)ubitrackPose.co22;
covariance[1, 2] = (float)ubitrackPose.co23;
covariance[1, 3] = (float)ubitrackPose.co24;
covariance[1, 4] = (float)ubitrackPose.co25;
covariance[1, 5] = (float)ubitrackPose.co26;
covariance[2, 0] = (float)ubitrackPose.co31;
covariance[2, 1] = (float)ubitrackPose.co32;
covariance[2, 2] = (float)ubitrackPose.co33;
covariance[2, 3] = (float)ubitrackPose.co34;
covariance[2, 4] = (float)ubitrackPose.co35;
covariance[2, 5] = (float)ubitrackPose.co36;
covariance[3, 0] = (float)ubitrackPose.co41;
covariance[3, 1] = (float)ubitrackPose.co42;
covariance[3, 2] = (float)ubitrackPose.co43;
covariance[3, 3] = (float)ubitrackPose.co44;
covariance[3, 4] = (float)ubitrackPose.co45;
covariance[3, 5] = (float)ubitrackPose.co46;
covariance[4, 0] = (float)ubitrackPose.co51;
covariance[4, 1] = (float)ubitrackPose.co52;
covariance[4, 2] = (float)ubitrackPose.co53;
covariance[4, 3] = (float)ubitrackPose.co54;
covariance[4, 4] = (float)ubitrackPose.co55;
covariance[4, 5] = (float)ubitrackPose.co56;
covariance[5, 0] = (float)ubitrackPose.co61;
covariance[5, 1] = (float)ubitrackPose.co62;
covariance[5, 2] = (float)ubitrackPose.co63;
covariance[5, 3] = (float)ubitrackPose.co64;
covariance[5, 4] = (float)ubitrackPose.co65;
covariance[5, 5] = (float)ubitrackPose.co66;
}
public static void ubitrackCovarianceToFloatArray(SimpleErrorPose ubitrackPose, ref float[,] covariance)
{
// todo needs rhs lhs transfrom
covariance[0, 0] = (float)ubitrackPose.co11;
covariance[0, 1] = (float)ubitrackPose.co12;
covariance[0, 2] = (float)ubitrackPose.co13;
covariance[0, 3] = (float)ubitrackPose.co14;
covariance[0, 4] = (float)ubitrackPose.co15;
covariance[0, 5] = (float)ubitrackPose.co16;
covariance[1, 0] = (float)ubitrackPose.co21;
covariance[1, 1] = (float)ubitrackPose.co22;
covariance[1, 2] = (float)ubitrackPose.co23;
covariance[1, 3] = (float)ubitrackPose.co24;
covariance[1, 4] = (float)ubitrackPose.co25;
covariance[1, 5] = (float)ubitrackPose.co26;
covariance[2, 0] = (float)ubitrackPose.co31;
covariance[2, 1] = (float)ubitrackPose.co32;
covariance[2, 2] = (float)ubitrackPose.co33;
covariance[2, 3] = (float)ubitrackPose.co34;
covariance[2, 4] = (float)ubitrackPose.co35;
covariance[2, 5] = (float)ubitrackPose.co36;
covariance[3, 0] = (float)ubitrackPose.co41;
covariance[3, 1] = (float)ubitrackPose.co42;
covariance[3, 2] = (float)ubitrackPose.co43;
covariance[3, 3] = (float)ubitrackPose.co44;
covariance[3, 4] = (float)ubitrackPose.co45;
covariance[3, 5] = (float)ubitrackPose.co46;
covariance[4, 0] = (float)ubitrackPose.co51;
covariance[4, 1] = (float)ubitrackPose.co52;
covariance[4, 2] = (float)ubitrackPose.co53;
covariance[4, 3] = (float)ubitrackPose.co54;
covariance[4, 4] = (float)ubitrackPose.co55;
covariance[4, 5] = (float)ubitrackPose.co56;
covariance[5, 0] = (float)ubitrackPose.co61;
covariance[5, 1] = (float)ubitrackPose.co62;
covariance[5, 2] = (float)ubitrackPose.co63;
covariance[5, 3] = (float)ubitrackPose.co64;
covariance[5, 4] = (float)ubitrackPose.co65;
covariance[5, 5] = (float)ubitrackPose.co66;
}
