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; } }