fix(simulator): fix angular and velocity calculations

Fix the calculation for angular velocity, now decently correct.
Tweak calculation of velocity to use fewer samples.

Author: virror

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

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