Implementation of elbow model for the controller position and rotation.

chrome/browser/android/vr_shell/vr_math.cc

 // Copyright 2016 The Chromium Authors. All rights reserved.
 // Use of this source code is governed by a BSD-style license that can be
 // found in the LICENSE file.
 
 #include "chrome/browser/android/vr_shell/vr_math.h"
 
 #include <cmath>
 
 #include "base/logging.h"
 
@@ skipping 16 matching lines @@
 void SetIdentityM(gvr::Mat4f& mat) {
   float* m = reinterpret_cast<float*>(mat.m);
   for (int i = 0; i < 16; i++) {
     m[i] = 0;
   }
   for (int i = 0; i < 16; i += 5) {
     m[i] = 1.0f;
   }
 }
 
-// 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) {
       for (int j = 0; j < 4; ++j) {
         tmat.m[i][j] = mat.m[i][j];
       }
     }
   }
   tmat.m[0][3] += x;
   tmat.m[1][3] += y;
   tmat.m[2][3] += z;
 }
 
-// Left multiply a scale matrix.
 void ScaleM(gvr::Mat4f& tmat,
             const gvr::Mat4f& mat,
             float x,
             float y,
             float z) {
   if (&tmat != &mat) {
     for (int i = 0; i < 4; ++i) {
       for (int j = 0; j < 3; ++j) {
         tmat.m[i][j] = mat.m[i][j];
       }
     }
   }
   // Multiply all rows including translation components.
   for (int j = 0; j < 4; ++j) {
     tmat.m[0][j] *= x;
     tmat.m[1][j] *= y;
     tmat.m[2][j] *= z;
   }
 }
 
+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];
   res.y = m.m[1][0] * v.x + m.m[1][1] * v.y + m.m[1][2] * v.z + m.m[1][3];
   res.z = m.m[2][0] * v.x + m.m[2][1] * v.y + m.m[2][2] * v.z + m.m[2][3];
   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;
   res.y = m.m[1][0] * v.x + m.m[1][1] * v.y + m.m[1][2] * v.z;
   res.z = m.m[2][0] * v.x + m.m[2][1] * v.y + m.m[2][2] * v.z;
   return res;
 }
 
 gvr::Mat4f MatrixMul(const gvr::Mat4f& matrix1, const gvr::Mat4f& matrix2) {
   gvr::Mat4f result;
@@ skipping 45 matching lines @@
 gvr::Vec3f GetForwardVector(const gvr::Mat4f& matrix) {
   // Same as multiplying the inverse of the rotation component of the matrix by
   // (0, 0, -1, 0).
   return {-matrix.m[2][0], -matrix.m[2][1], -matrix.m[2][2]};
 }
 
 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;
 }
 
 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);
   quat.qx /= len;
   quat.qy /= len;
   quat.qz /= len;
   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;
   float length = NormalizeVector(normal);
   angle *= length;
 
   gvr::Quatf res;
   float s = sin(angle / 2);
   res.qx = normal.x * s;
   res.qy = normal.y * s;
   res.qz = normal.z * s;
   res.qw = cos(angle / 2);
   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;
   res.qx = a.qw * b.qx + a.qx * b.qw + a.qy * b.qz - a.qz * b.qy;
   res.qy = a.qw * b.qy - a.qx * b.qz + a.qy * b.qw + a.qz * b.qx;
   res.qz = a.qw * b.qz + a.qx * b.qy - a.qy * b.qx + a.qz * b.qw;
   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;
   const float z2 = quat.qz * quat.qz;
   const float xy = quat.qx * quat.qy;
   const float xz = quat.qx * quat.qz;
   const float xw = quat.qx * quat.qw;
   const float yz = quat.qy * quat.qz;
   const float yw = quat.qy * quat.qw;
   const float zw = quat.qz * quat.qw;
@@ skipping 32 matching lines @@
   float len1 = VectorLength(vec1);
   float len2 = VectorLength(vec2);
   if (len1 == 0 || len2 == 0)
     return false;
   float cross_p = vec1.x * vec2.z - vec1.z * vec2.x;
   *angle = asin(cross_p / (len1 * len2));
   return true;
 }
 
 }  // namespace vr_shell