Implementation of elbow model for the controller position and rotation.

chrome/browser/android/vr_shell/vr_controller.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_controller.h"
 
 #include <algorithm>
 #include <cmath>
 #include <utility>
 
 #include "base/logging.h"
+#include "base/memory/ptr_util.h"
 #include "base/time/time.h"
+#include "chrome/browser/android/vr_shell/elbow_model.h"
 #include "device/vr/vr_math.h"
 #include "third_party/gvr-android-sdk/src/libraries/headers/vr/gvr/capi/include/gvr.h"
 #include "third_party/gvr-android-sdk/src/libraries/headers/vr/gvr/capi/include/gvr_controller.h"
 
 namespace vr_shell {
 
 namespace {
 
 constexpr float kDisplacementScaleFactor = 300.0f;
 
@@ skipping 11 matching lines @@
 // not equal. Also, minimum time distance needed to call two timestamps
 // not equal.
 constexpr float kDelta = 1.0e-7f;
 
 constexpr float kCutoffHz = 10.0f;
 constexpr float kRC = static_cast<float>(1.0 / (2.0 * M_PI * kCutoffHz));
 constexpr float kNanoSecondsPerSecond = 1.0e9f;
 
 constexpr int kMaxNumOfExtrapolations = 2;
 
-static constexpr gfx::Point3F kControllerPosition = {0.2f, -0.5f, -0.15f};
+// Distance from the center of the controller to start rendering the laser.
+constexpr float kLaserStartDisplacement = 0.045;
 
 void ClampTouchpadPosition(gfx::Vector2dF* position) {
-  position->set_x(std::min(std::max(0.0f, position->x()), 1.0f));
-  position->set_y(std::min(std::max(0.0f, position->y()), 1.0f));
+  position->set_x(vr::Clampf(position->x(), 0.0f, 1.0f));
+  position->set_y(vr::Clampf(position->y(), 0.0f, 1.0f));
+}
+
+float DeltaTimeSeconds(int64_t last_timestamp_nanos) {
+  return (gvr::GvrApi::GetTimePointNow().monotonic_system_time_nanos -
+          last_timestamp_nanos) /
+         kNanoSecondsPerSecond;
 }
 
 }  // namespace
 
 VrController::VrController(gvr_context* vr_context) {
   DVLOG(1) << __FUNCTION__ << "=" << this;
   Initialize(vr_context);
+  elbow_model_ = base::MakeUnique<ElbowModel>(handedness_);
   Reset();
+  last_timestamp_nanos_ =
+      gvr::GvrApi::GetTimePointNow().monotonic_system_time_nanos;
 }
 
 VrController::~VrController() {
   DVLOG(1) << __FUNCTION__ << "=" << this;
 }
 
 void VrController::OnResume() {
   if (controller_api_)
     controller_api_->Resume();
 }
@@ skipping 40 matching lines @@
 float VrController::TouchPosY() {
   return controller_state_->GetTouchPos().y;
 }
 
 vr::Quatf VrController::Orientation() const {
   const gvr::Quatf& orientation = controller_state_->GetOrientation();
   return *reinterpret_cast<vr::Quatf*>(const_cast<gvr::Quatf*>(&orientation));
 }
 
 void VrController::GetTransform(vr::Mat4f* out) const {
-  // TODO(acondor): Position and orientation needs to be obtained
-  // from an elbow model.
-  // Placing the controller in a fixed position for now.
-  vr::SetIdentityM(out);
-  // Changing rotation point.
-  vr::TranslateM(*out, gfx::Vector3dF(0, 0, 0.05), out);
-  vr::Mat4f quat_to_matrix;
-  vr::QuatToMatrix(Orientation(), &quat_to_matrix);
-  vr::MatrixMul(quat_to_matrix, *out, out);
-  gfx::Vector3dF translation(kControllerPosition.x(), kControllerPosition.y(),
-                             kControllerPosition.z() - 0.05);
-  vr::TranslateM(*out, translation, out);
+  QuatToMatrix(elbow_model_->GetControllerRotation(), out);
+  auto position = elbow_model_->GetControllerPosition();
+  vr::TranslateM(*out, vr::ToVector(position), out);
+}
+
+float VrController::GetOpacity() const {
+  return elbow_model_->GetAlphaValue();
+}
+
+gfx::Point3F VrController::GetPointerStart() const {
+  auto controller_position = elbow_model_->GetControllerPosition();
+  gfx::Vector3dF pointer_direction{0.0f, -sin(kErgoAngleOffset),
+                                   -cos(kErgoAngleOffset)};
+  vr::Mat4f rotation_mat;
+  vr::QuatToMatrix(Orientation(), &rotation_mat);
+  pointer_direction = vr::MatrixVectorRotate(rotation_mat, pointer_direction);
+  return controller_position +
+         gfx::ScaleVector3d(pointer_direction, kLaserStartDisplacement);
 }
 
 VrControllerModel::State VrController::GetModelState() const {
   if (ButtonState(gvr::ControllerButton::GVR_CONTROLLER_BUTTON_CLICK))
     return VrControllerModel::TOUCHPAD;
   if (ButtonState(gvr::ControllerButton::GVR_CONTROLLER_BUTTON_APP))
     return VrControllerModel::APP;
   if (ButtonState(gvr::ControllerButton::GVR_CONTROLLER_BUTTON_HOME))
     return VrControllerModel::SYSTEM;
   return VrControllerModel::IDLE;
@@ skipping 16 matching lines @@
 }
 
 bool VrController::ButtonState(gvr::ControllerButton button) const {
   return controller_state_->GetButtonState(button);
 }
 
 bool VrController::IsConnected() {
   return controller_state_->GetConnectionState() == gvr::kControllerConnected;
 }
 
-void VrController::UpdateState() {
+void VrController::UpdateState(const gfx::Vector3dF& head_direction) {
   const int32_t old_status = controller_state_->GetApiStatus();
   const int32_t old_connection_state = controller_state_->GetConnectionState();
   // Read current controller state.
   controller_state_->Update(*controller_api_);
   // Print new API status and connection state, if they changed.
   if (controller_state_->GetApiStatus() != old_status ||
       controller_state_->GetConnectionState() != old_connection_state) {
     VLOG(1) << "Controller Connection status: "
             << gvr_controller_connection_state_to_string(
                    controller_state_->GetConnectionState());
   }
+
+  const gvr::Vec3f& gvr_gyro = controller_state_->GetGyro();
+  elbow_model_->Update({IsConnected(), Orientation(),
+                        gfx::Vector3dF(gvr_gyro.x, gvr_gyro.y, gvr_gyro.z),
+                        head_direction,
+                        DeltaTimeSeconds(last_timestamp_nanos_)});
 }
 
 void VrController::UpdateTouchInfo() {
   CHECK(touch_info_ != nullptr) << "touch_info_ not initialized properly.";
   if (IsTouching() && state_ == SCROLLING &&
       (controller_state_->GetLastTouchTimestamp() == last_touch_timestamp_ ||
        (cur_touch_point_->position == prev_touch_point_->position &&
         extrapolated_touch_ < kMaxNumOfExtrapolations))) {
     extrapolated_touch_++;
     touch_position_changed_ = true;
     // Fill the touch_info
-    float duration =
-        (gvr::GvrApi::GetTimePointNow().monotonic_system_time_nanos -
-         last_timestamp_nanos_) /
-        kNanoSecondsPerSecond;
+    float duration = DeltaTimeSeconds(last_timestamp_nanos_);
     touch_info_->touch_point.position.set_x(cur_touch_point_->position.x() +
                                             overall_velocity_.x() * duration);
     touch_info_->touch_point.position.set_y(cur_touch_point_->position.y() +
                                             overall_velocity_.y() * duration);
   } else {
     if (extrapolated_touch_ == kMaxNumOfExtrapolations) {
       overall_velocity_ = {0, 0};
     }
     extrapolated_touch_ = 0;
   }
@@ skipping 218 matching lines @@
 
   const gfx::Vector2dF& velocity = ScaleVector2d(displacement, (1 / duration));
 
   float weight = duration / (kRC + duration);
 
   overall_velocity_ = ScaleVector2d(overall_velocity_, (1 - weight)) +
                       ScaleVector2d(velocity, weight);
 }
 
 }  // namespace vr_shell