public override void receiveErrorPosition3D(SimpleErrorPosition3D position3d)
{
lock (thisLock)
{
m_data = UbiMeasurementUtils.ubitrackToUnity(position3d);
}
}
// Use this for initialization
public override void utInit(SimpleFacade simpleFacade)
{
base.utInit(simpleFacade);
switch (ubitrackEvent)
{
case UbitrackEventType.Pull:
{
m_posePull = simpleFacade.getPullSinkErrorPosition3D(patternID);
m_simplePose = new SimpleErrorPosition3D();
if (m_posePull == null)
{
throw new Exception("SimpleApplicationPullSinkErrorPose not found for poseID:" + patternID);
}
break;
}
case UbitrackEventType.Push:
{
m_poseReceiver = new UnityErrorPositionReceiver();
if (!simpleFacade.set3DErrorPositionCallback(patternID, m_poseReceiver))
{
throw new Exception("SimpleErrorPoseReceiver could not be set for poseID:" + patternID);
}
break;
}
default:
break;
}
}
internal static Measurement <ErrorVector3D> ubitrackToUnity(SimpleErrorPosition3D ubitrackPose)
{
Vector3 trans = new Vector3((float)ubitrackPose.x, (float)ubitrackPose.y, (float)ubitrackPose.z);
//Quaternion rot = new Quaternion((float)ubitrackPose.rx, (float)ubitrackPose.ry, (float)ubitrackPose.rz, (float)ubitrackPose.rw);
trans = trans * UbiToUnityScaleFactor;
ErrorVector3D data = new ErrorVector3D();
coordsysemChange(trans, ref data.position);
doubleArrayClass da = doubleArrayClass.frompointer(ubitrackPose.covariance);
//for(int i=0;i<16;i++){
for (int row = 0; row < 3; row++)
{
for (int col = 0; col < 3; col++)
{
data.covariance[row, col] = (float)da.getitem(row * 3 + col);
}
}
//coordsysemChange(rot, ref data.rot);
//ubitrackCovarianceToFloatArray(ubitrackPose, ref data.covariance);
/* TODO: Define ubitrackCovarianceToFloatArray()
* with SimpleErrorPosition3D covariance type
*/
return(new Measurement <ErrorVector3D>(data, ubitrackPose.timestamp));
}
