Implementation of elbow model for the controller position and rotation.

device/vr/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 "device/vr/vr_math.h"
 
 #include <cmath>
 
 #include "base/logging.h"
 
@@ skipping 159 matching lines @@
   const float m31 = 2.0f * (xz - yw);
   const float m32 = 2.0f * (yz + xw);
   const float m33 = 1.0f - 2.0f * x2 - 2.0f * y2;
 
   *out = {{{{m11, m12, m13, 0.0f}},
            {{m21, m22, m23, 0.0f}},
            {{m31, m32, m33, 0.0f}},
            {{0.0f, 0.0f, 0.0f, 1.0f}}}};
 }
 
+vr::Quatf GetVectorRotation(const gfx::Vector3dF& from,
+                            const gfx::Vector3dF& to) {
+  float dot = gfx::DotProduct(from, to);
+  float norm = sqrt(from.LengthSquared() * to.LengthSquared());
+  float real = norm + dot;
+  gfx::Vector3dF w;
+  if (real < 1.e-6f * norm) {
+    real = 0.0f;
+    w = fabsf(from.x()) > fabsf(from.z())
+            ? gfx::Vector3dF{-from.y(), from.x(), 0.0f}
+            : gfx::Vector3dF{0.0f, -from.z(), from.y()};
+  } else {
+    w = gfx::CrossProduct(from, to);
+  }
+  vr::Quatf result{w.x(), w.y(), w.z(), real};
+  NormalizeQuat(&result);
+  return result;
+}
+
+vr::Quatf QuatSum(const vr::Quatf& a, const vr::Quatf& b) {
+  return {a.qx + b.qx, a.qy + b.qy, a.qz + b.qz, a.qw + b.qw};
+}
+
+vr::Quatf QuatProduct(const vr::Quatf& a, const vr::Quatf& b) {
+  return {a.qw * b.qx + a.qx * b.qw + a.qy * b.qz - a.qz * b.qy,
+          a.qw * b.qy - a.qx * b.qz + a.qy * b.qw + a.qz * b.qx,
+          a.qw * b.qz + a.qx * b.qy - a.qy * b.qx + a.qz * b.qw,
+          a.qw * b.qw - a.qx * b.qx - a.qy * b.qy - a.qz * b.qz};
+}
+
+vr::Quatf ScaleQuat(const vr::Quatf& q, float s) {
+  return {q.qx * s, q.qy * s, q.qz * s, q.qw * s};
+}
+
+vr::Quatf InvertQuat(const vr::Quatf& quat) {
+  return {-quat.qx, -quat.qy, -quat.qz, quat.qw};
+}
+
+float QuatAngleDegrees(const vr::Quatf& a, const vr::Quatf& b) {
+  return QuatProduct(b, InvertQuat(a)).qw;
+}
+
+vr::Quatf QuatLerp(const vr::Quatf& a, const vr::Quatf& b, float t) {
+  auto result = QuatSum(ScaleQuat(a, 1.0f - t), ScaleQuat(b, t));
+  NormalizeQuat(&result);
+  return result;
+}
+
+gfx::Vector3dF QuatSlerp(const gfx::Vector3dF& v_start,
+                         const gfx::Vector3dF& v_end,
+                         float percent) {
+  auto start = v_start;
+  auto end = v_end;
+  NormalizeVector(&start);
+  NormalizeVector(&end);
+  float dot = Clampf(gfx::DotProduct(start, end), -1.0f, 1.0f);
+  float theta = acos(dot) * percent;
+  auto relative_vec = end - gfx::ScaleVector3d(start, dot);
+  NormalizeVector(&relative_vec);
+  return gfx::ScaleVector3d(start, cos(theta)) +
+         gfx::ScaleVector3d(relative_vec, sin(theta));
+}
+
 gfx::Point3F GetRayPoint(const gfx::Point3F& rayOrigin,
                          const gfx::Vector3dF& rayVector,
                          float scale) {
   return rayOrigin + gfx::ScaleVector3d(rayVector, scale);
 }
 
 float Distance(const gfx::Point3F& p1, const gfx::Point3F& p2) {
   return std::sqrt(p1.SquaredDistanceTo(p2));
 }
 
 bool XZAngle(const gfx::Vector3dF& vec1,
              const gfx::Vector3dF& vec2,
              float* angle) {
   float len1 = vec1.Length();
   float len2 = vec2.Length();
   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;
 }
 
+gfx::Vector3dF ToVector(const gfx::Point3F& p) {
+  return {p.x(), p.y(), p.z()};
+}
+
+gfx::Point3F ToPoint(const gfx::Vector3dF& p) {
+  return {p.x(), p.y(), p.z()};
+}
+
+gfx::Point3F ScalePoint(const gfx::Point3F& p, const gfx::Vector3dF& s) {
+  return gfx::ScalePoint(p, s.x(), s.y(), s.z());
+}
+
+gfx::Vector3dF ScaleVector(const gfx::Vector3dF& v, const gfx::Vector3dF& s) {
+  return gfx::ScaleVector3d(v, s.x(), s.y(), s.z());
+}
+
+float Clampf(float value, float min, float max) {
+  if (value < min)
+    return min;
+  if (value > max)
+    return max;
+  return value;
+}
+
 }  // namespace vr