WebVR: add angular velocity estimate to pose

chrome/browser/android/vr_shell/vr_shell.cc

Index: chrome/browser/android/vr_shell/vr_shell.cc
diff --git a/chrome/browser/android/vr_shell/vr_shell.cc b/chrome/browser/android/vr_shell/vr_shell.cc
index edcbb68f0fe05e2c0f75e96229e281b9e33a5b32..c9b38bf58480e1b017b17a2792e954f4486c69ea 100644
--- a/chrome/browser/android/vr_shell/vr_shell.cc
+++ b/chrome/browser/android/vr_shell/vr_shell.cc
@@ -22,6 +22,7 @@
 #include "chrome/browser/android/vr_shell/vr_compositor.h"
 #include "chrome/browser/android/vr_shell/vr_gl_thread.h"
 #include "chrome/browser/android/vr_shell/vr_input_manager.h"
+#include "chrome/browser/android/vr_shell/vr_math.h"
 #include "chrome/browser/android/vr_shell/vr_shell_delegate.h"
 #include "chrome/browser/android/vr_shell/vr_shell_gl.h"
 #include "chrome/browser/android/vr_shell/vr_usage_monitor.h"
@@ -655,7 +656,9 @@ void VrShell::RegisterGamepadDataFetcher(
 }
 
 /* static */
-device::mojom::VRPosePtr VrShell::VRPosePtrFromGvrPose(gvr::Mat4f head_mat) {
+device::mojom::VRPosePtr VrShell::VRPosePtrFromGvrPose(gvr::Mat4f head_mat,
+                                                       gvr::Mat4f head_mat_2,
+                                                       int64_t epsilon_nanos) {
   device::mojom::VRPosePtr pose = device::mojom::VRPose::New();
 
   pose->orientation.emplace(4);
@@ -682,6 +685,51 @@ device::mojom::VRPosePtr VrShell::VRPosePtrFromGvrPose(gvr::Mat4f head_mat) {
     pose->position.value()[2] = decomposed_transform.translate[2];
   }
 
+  // The angular velocity is a 3-element vector pointing along the rotation
+  // axis with magnitude equal to rotation speed in radians/second, expressed
+  // in the seated frame of reference.
+  //
+  // The spec isn't very clear on details, clarification requested in
+  // https://github.com/w3c/webvr/issues/212 .
+  //
+  // Assuming that pose prediction is simply based on adding a time * angular
+  // velocity rotation to the pose, we can approximate the angular velocity
+  // from the difference between two successive poses. This is a first order
+  // estimate that assumes small enough rotations so that we can do linear
+  // approximation.
+  //
+  // See:
+  // https://en.wikipedia.org/wiki/Angular_velocity#Calculation_from_the_orientation_matrix
+  pose->angularVelocity.emplace(3);
+  // Assume that epsilon is nonzero since it's a compile-time constant
+  // provided by the caller.
+  double epsilon_seconds = epsilon_nanos * 1e-9;
+  gvr::Mat4f delta_mat;
+  gvr::Mat4f inverse_head_mat;
+  // Calculate difference matrix, and inverse head matrix rotation.
+  // For the inverse rotation, just transpose the 3x3 subsection.
+  for (int j = 0; j < 3; ++j) {
+    for (int i = 0; i < 3; ++i) {
+      delta_mat.m[j][i] =
+          (head_mat_2.m[j][i] - head_mat.m[j][i]) / epsilon_seconds;
+      inverse_head_mat.m[j][i] = head_mat.m[i][j];
+    }
+    delta_mat.m[j][3] = delta_mat.m[3][j] = 0.0;
+    inverse_head_mat.m[j][3] = inverse_head_mat.m[3][j] = 0.0;
+  }
+  delta_mat.m[3][3] = 1.0;
+  inverse_head_mat.m[3][3] = 1.0;
+  gvr::Mat4f omega_mat = MatrixMul(delta_mat, inverse_head_mat);
+  gvr::Vec3f omega_vec;
+  omega_vec.x = -omega_mat.m[2][1];
+  omega_vec.y = omega_mat.m[2][0];
+  omega_vec.z = -omega_mat.m[1][0];
+
+  // Rotate by inverse head matrix to bring into seated space.
+  gvr::Vec3f angular_velocity = MatrixVectorRotate(inverse_head_mat, omega_vec);
+  pose->angularVelocity.value()[0] = angular_velocity.x;
+  pose->angularVelocity.value()[1] = angular_velocity.y;
+  pose->angularVelocity.value()[2] = angular_velocity.z;
   return pose;
 }