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));
}
public static void ubitrack3x4MatrixToFloatArray(SimpleMatrix3x3 ubiMatrix, ref float[] matrix)
{
doubleArrayClass dac = doubleArrayClass.frompointer(ubiMatrix.values);
for (int i = 0; i < 9; i++)
{
matrix[i] = (float)dac.getitem(i);
}
}
public static void ubitrack4x4MatrixToMatrix(SimpleMatrix4x4 ubiMatrix, ref Matrix4x4 matrix)
{
doubleArrayClass da = doubleArrayClass.frompointer(ubiMatrix.values);
//for(int i=0;i<16;i++){
for (int row = 0; row < 4; row++)
{
for (int col = 0; col < 4; col++)
{
matrix[row, col] = (float)da.getitem(row * 4 + col);
}
}
}
