Implementation of elbow model for the controller position and rotation.

chrome/browser/android/vr_shell/vr_math.cc

Index: chrome/browser/android/vr_shell/vr_math.cc
diff --git a/chrome/browser/android/vr_shell/vr_math.cc b/chrome/browser/android/vr_shell/vr_math.cc
index 289a1458af4a8bfa8f7438baf6918bb58a14d872..13ba07a5d895a16e9e9ed9d7888b42fd17c328f1 100644
--- a/chrome/browser/android/vr_shell/vr_math.cc
+++ b/chrome/browser/android/vr_shell/vr_math.cc
@@ -34,7 +34,6 @@ void SetIdentityM(gvr::Mat4f& mat) {
   }
 }
 
-// Left multiply a translation matrix.
 void TranslateM(gvr::Mat4f& tmat, gvr::Mat4f& mat, float x, float y, float z) {
   if (&tmat != &mat) {
     for (int i = 0; i < 4; ++i) {
@@ -48,7 +47,6 @@ void TranslateM(gvr::Mat4f& tmat, gvr::Mat4f& mat, float x, float y, float z) {
   tmat.m[2][3] += z;
 }
 
-// Left multiply a scale matrix.
 void ScaleM(gvr::Mat4f& tmat,
             const gvr::Mat4f& mat,
             float x,
@@ -69,6 +67,29 @@ void ScaleM(gvr::Mat4f& tmat,
   }
 }
 
+float Clampf(float value, float min, float max) {

[cjgrant, 2017/04/03 19:25:00]

std::clamp is a thing, maybe we should use it instead.

[acondor_, 2017/04/19 18:44:27]

I checked, and it is a C++17 feature.

+  if (value < min)
+    return min;
+  if (value > max)
+    return max;
+  return value;
+}
+
+float ToDegrees(float radians) {
+  return (radians * 180.f) / M_PI;

[cjgrant, 2017/04/03 19:24:59]

No braces needed here.

[acondor_, 2017/04/19 18:44:27]

Done.

+}
+
+gvr::Vec3f QuatSlerp(gvr::Vec3f start, gvr::Vec3f end, float percent) {
+  NormalizeVector(start);
+  NormalizeVector(end);
+  float dot = Clampf(VectorDot(start, end), -1.f, 1.f);

[cjgrant, 2017/04/03 19:25:00]

1.0f etc?

[acondor_, 2017/04/19 18:44:27]

Done.

+  float theta = acos(dot) * percent;
+  auto relative_vec = VectorSubtract(end, VectorScalarMul(start, dot));
+  NormalizeVector(relative_vec);
+  return VectorAdd(VectorScalarMul(start, cos(theta)),
+                   VectorScalarMul(relative_vec, sin(theta)));
+}
+
 gvr::Vec3f MatrixVectorMul(const gvr::Mat4f& m, const gvr::Vec3f& v) {
   gvr::Vec3f res;
   res.x = m.m[0][0] * v.x + m.m[0][1] * v.y + m.m[0][2] * v.z + m.m[0][3];
@@ -77,7 +98,6 @@ gvr::Vec3f MatrixVectorMul(const gvr::Mat4f& m, const gvr::Vec3f& v) {
   return res;
 }
 
-// Rotation only, ignore translation components.
 gvr::Vec3f MatrixVectorRotate(const gvr::Mat4f& m, const gvr::Vec3f& v) {
   gvr::Vec3f res;
   res.x = m.m[0][0] * v.x + m.m[0][1] * v.y + m.m[0][2] * v.z;
@@ -143,19 +163,29 @@ gvr::Vec3f GetTranslation(const gvr::Mat4f& matrix) {
   return {matrix.m[0][3], matrix.m[1][3], matrix.m[2][3]};
 }
 
+float VectorLengthSquared(const gvr::Vec3f& vec) {
+  return vec.x * vec.x + vec.y * vec.y + vec.z * vec.z;
+}
+
 float VectorLength(const gvr::Vec3f& vec) {
-  return sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
+  return sqrt(VectorLengthSquared(vec));
 }
 
 gvr::Vec3f VectorSubtract(const gvr::Vec3f& a, const gvr::Vec3f& b) {
   return {a.x - b.x, a.y - b.y, a.z - b.z};
 }
 
+gvr::Vec3f VectorAdd(const gvr::Vec3f& a, const gvr::Vec3f& b) {
+  return {a.x + b.x, a.y + b.y, a.z + b.z};
+}
+
 float NormalizeVector(gvr::Vec3f& vec) {
   float len = VectorLength(vec);
-  vec.x /= len;
-  vec.y /= len;
-  vec.z /= len;
+  if (len != 0.0f)
+    len = 1.f / len;
+  vec.x *= len;
+  vec.y *= len;
+  vec.z *= len;
   return len;
 }
 
@@ -163,6 +193,23 @@ float VectorDot(const gvr::Vec3f& a, const gvr::Vec3f& b) {
   return a.x * b.x + a.y * b.y + a.z * b.z;
 }
 
+gvr::Vec3f VectorCross(const gvr::Vec3f& a, const gvr::Vec3f& b) {
+  return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x};
+}
+
+gvr::Vec3f PointwiseVectorMul(const gvr::Vec3f& a, const gvr::Vec3f& b) {
+  return {a.x * b.x, a.y * b.y, a.z * b.z};
+}
+
+gvr::Vec3f VectorScalarMul(const gvr::Vec3f& a, float s) {
+  return {a.x * s, a.y * s, a.z * s};
+}
+
+float VectorAngleDegrees(const gvr::Vec3f& a, const gvr::Vec3f& b) {
+  float angle_radians = acos(VectorDot(a, b));
+  return ToDegrees(angle_radians);

[cjgrant, 2017/04/03 19:24:59]

Could drop the intermediate variable here.

[acondor_, 2017/04/19 18:44:27]

Done.

+}
+
 void NormalizeQuat(gvr::Quatf& quat) {
   float len = sqrt(quat.qx * quat.qx + quat.qy * quat.qy + quat.qz * quat.qz +
                    quat.qw * quat.qw);
@@ -172,6 +219,20 @@ void NormalizeQuat(gvr::Quatf& quat) {
   quat.qw /= len;
 }
 
+gvr::Quatf QuatInverted(const gvr::Quatf& quat) {
+  return {-quat.qx, -quat.qy, -quat.qz, quat.qw};
+}
+
+float QuatAngleDegrees(const gvr::Quatf& a, const gvr::Quatf& b) {
+  return QuatMultiply(b, QuatInverted(a)).qw;
+}
+
+gvr::Quatf QuatLerp(const gvr::Quatf& a, const gvr::Quatf& b, float t) {
+  auto result = QuatAdd(QuatScalarMul(a, 1.f - t), QuatScalarMul(b, t));
+  NormalizeQuat(result);

[cjgrant, 2017/04/03 19:24:59]

With the quantity of math changes here, it might be a good time to fix our
style.  Methods that modify parameters should accept pointers, but we inherited
code that uses references instead.  It makes lines like this look misleading. 
The alternative is of course returning new values based on const inputs.

Michael, how do you feel about this?

[acondor_, 2017/04/19 18:44:27]

The change was already done. I just need to rebase.

+  return result;
+}
+
 gvr::Quatf QuatFromAxisAngle(const gvr::Vec3f& axis, float angle) {
   // Rotation angle is the product of |angle| and the magnitude of |axis|.
   gvr::Vec3f normal = axis;
@@ -187,6 +248,23 @@ gvr::Quatf QuatFromAxisAngle(const gvr::Vec3f& axis, float angle) {
   return res;
 }
 
+gvr::Quatf FromToRotation(const gvr::Vec3f& from, const gvr::Vec3f& to) {
+  float dot = VectorDot(from, to);
+  float norm = sqrt(VectorLengthSquared(from) * VectorLengthSquared(to));
+  float real = norm + dot;
+  gvr::Vec3f w;
+  if (real < 1.e-6f * norm) {
+    real = 0.f;
+    w = fabsf(from.x) > fabsf(from.z) ? gvr::Vec3f{-from.y, from.x, 0.f}
+                                      : gvr::Vec3f{0.f, -from.z, from.y};
+  } else {
+    w = VectorCross(from, to);
+  }
+  gvr::Quatf result{w.x, w.y, w.z, real};
+  NormalizeQuat(result);
+  return result;
+}
+
 gvr::Quatf QuatMultiply(const gvr::Quatf& a, const gvr::Quatf& b) {
   gvr::Quatf res;
   res.qw = a.qw * b.qw - a.qx * b.qx - a.qy * b.qy - a.qz * b.qz;
@@ -196,6 +274,14 @@ gvr::Quatf QuatMultiply(const gvr::Quatf& a, const gvr::Quatf& b) {
   return res;
 }
 
+gvr::Quatf QuatAdd(const gvr::Quatf& a, const gvr::Quatf& b) {
+  return {a.qx + b.qx, a.qy + b.qy, a.qz + b.qz, a.qw + b.qw};
+}
+
+gvr::Quatf QuatScalarMul(const gvr::Quatf& quat, float s) {
+  return {quat.qx * s, quat.qy * s, quat.qz * s, quat.qw * s};
+}
+
 gvr::Mat4f QuatToMatrix(const gvr::Quatf& quat) {
   const float x2 = quat.qx * quat.qx;
   const float y2 = quat.qy * quat.qy;