fix(simulator): fix angular and velocity calculations

Fix the calculation for angular velocity, now decently correct. Tweak calculation of velocity to use fewer samples.
ExtendRealityLtd · May 20, 2017 · cb3c49f · cb3c49f
1 parent b5759f1
commit cb3c49f
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 12 deletions.
diff --git a/Assets/VRTK/SDK/Simulator/SDK_ControllerSim.cs b/Assets/VRTK/SDK/Simulator/SDK_ControllerSim.cs
@@ -6,10 +6,12 @@
     public class SDK_ControllerSim : MonoBehaviour
     {
         private Vector3 lastPos;
-        private Vector3 lastRot;
+        private Quaternion lastRot;
         private List<Vector3> posList;
         private List<Vector3> rotList;
         private bool selected;
+        private float magnitude;
+        private Vector3 axis;
 
         public bool Selected
         {
@@ -50,23 +52,25 @@ private void Awake()
             posList = new List<Vector3>();
             rotList = new List<Vector3>();
             lastPos = transform.position;
-            lastRot = transform.rotation.eulerAngles;
+            lastRot = transform.rotation;
         }
 
         private void Update()
         {
             posList.Add((transform.position - lastPos) / Time.deltaTime);
-            if (posList.Count > 10)
+            if (posList.Count > 4)
             {
                 posList.RemoveAt(0);
             }
-            rotList.Add((Quaternion.FromToRotation(lastRot, transform.rotation.eulerAngles)).eulerAngles / Time.deltaTime);
-            if (rotList.Count > 10)
+            Quaternion deltaRotation = transform.rotation * Quaternion.Inverse (lastRot);
+            deltaRotation.ToAngleAxis(out magnitude, out axis);
+            rotList.Add((axis * magnitude));
+            if (rotList.Count > 4)
             {
                 rotList.RemoveAt(0);
             }
             lastPos = transform.position;
-            lastRot = transform.rotation.eulerAngles;
+            lastRot = transform.rotation;
         }
     }
 }
diff --git a/Assets/VRTK/SDK/Simulator/SDK_SimHeadset.cs b/Assets/VRTK/SDK/Simulator/SDK_SimHeadset.cs
@@ -12,9 +12,11 @@ public class SDK_SimHeadset : SDK_BaseHeadset
     {
         private Transform camera;
         private Vector3 lastPos;
-        private Vector3 lastRot;
+        private Quaternion lastRot;
         private List<Vector3> posList;
         private List<Vector3> rotList;
+        private float magnitude;
+        private Vector3 axis;
 
         /// <summary>
         /// The ProcessUpdate method enables an SDK to run logic for every Unity Update
@@ -23,17 +25,19 @@ public class SDK_SimHeadset : SDK_BaseHeadset
         public override void ProcessUpdate(Dictionary<string, object> options)
         {
             posList.Add((camera.position - lastPos) / Time.deltaTime);
-            if (posList.Count > 10)
+            if (posList.Count > 4)
             {
                 posList.RemoveAt(0);
             }
-            rotList.Add((Quaternion.FromToRotation(lastRot, camera.rotation.eulerAngles)).eulerAngles / Time.deltaTime);
-            if (rotList.Count > 10)
+            Quaternion deltaRotation = camera.rotation * Quaternion.Inverse (lastRot);
+            deltaRotation.ToAngleAxis(out magnitude, out axis);
+            rotList.Add((axis * magnitude));
+            if (rotList.Count > 4)
             {
                 rotList.RemoveAt(0);
             }
             lastPos = camera.position;
-            lastRot = camera.rotation.eulerAngles;
+            lastRot = camera.rotation;
         }
 
         /// <summary>
@@ -143,7 +147,7 @@ private void Awake()
             if (headset != null)
             {
                 lastPos = headset.position;
-                lastRot = headset.rotation.eulerAngles;
+                lastRot = headset.rotation;
             }
         }
     }