void Update()
{
if (acquiring)
{
writeData();
}
///////////////////////////////////////////////////////////////////////
// Update the tool POSITION
///////////////////////////////////////////////////////////////////////
Vector pr = PositionTracker.pr.Clone();
Vector pd = Vector.Build.DenseOfArray(new float[] { 0, 0, 400 });
Matrix Ad = Matrix.Build.DenseOfArray(new float[, ] {
{ -1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
});
Vector prpr = pd - Ad * pr;
Vector3 prprUnity = new Vector3(prpr[0], prpr[1], prpr[2]);
prprUnity = 0.001f * prprUnity;
ThreeDTool.transform.position = Cam.transform.position + prprUnity;
///////////////////////////////////////////////////////////////////////
// Update the tool ORIENTATION
///////////////////////////////////////////////////////////////////////
Vector qt = OrientationTracker.qt.Clone(); // tool orientation, from MEKF
Matrix As = OrientationTracker.AMat(qt); // sensor attitude
Matrix sRb = Matrix.Build.DenseOfArray(new float[, ] {
{ 0, 0, -1 }, { 0, 1, 0 }, { 1, 0, 0 }
}); // body to sensor rotation matrix
Matrix wRv = Matrix.Build.DenseOfArray(new float[, ] {
{ 1, 0, 0 }, { 0, 0, -1 }, { 0, -1, 0 }
}); // rotation from virtual to world coords
Vector qs = OrientationTracker.rotm2quat(sRb * As * wRv);
// Graphics update
Quaternion qinv = new Quaternion(qs[0], -qs[1], qs[2], qs[3]); // body to world
ThreeDTool.transform.rotation = qinv;
ThreeDTool.transform.rotation = Quaternion.Euler(0, OrientationTracker.AlignYaw, 0) * ThreeDTool.transform.rotation;
///////////////////////////////////////////////////////////////////////
// Update the sample rate counter and clock
///////////////////////////////////////////////////////////////////////
++frames;
float timeNow = Time.realtimeSinceStartup;
if (timeNow > lastInterval + updateInterval)
{
fps = (float)(frames / (timeNow - lastInterval));
frames = 0;
lastInterval = timeNow;
}
}
