Interesting pose modes

Pose source experiments from earlier, adapted to work in
PoseService::UpdatePoseMode(). Might be fun to test things on.

Bug: None
Test: adb shell pose --mode=<motion_sickness OR flying>
Change-Id: If488fcb250c35b6ab7df27890f705efde3e0e3cd

diff --git a/cmds/vr/pose/pose.cpp b/cmds/vr/pose/pose.cpp
index 2288a86..faceb67 100644 (file)
--- a/cmds/vr/pose/pose.cpp
+++ b/cmds/vr/pose/pose.cpp
@@ -28,7 +28,7 @@ void PrintUsage(const char* executable_name) {
                " px, py, pz as position (0,0,0 if omitted).\n"
             << "  --mode=mode: sets mode to one of normal, head_turn:slow, "
                "head_turn:fast, rotate:slow, rotate:medium, rotate:fast, "
-               "circle_strafe.\n"
+               "circle_strafe, float, motion_sickness.\n"
             << "  --unfreeze: sets the mode to normal.\n"
             << "  --log_controller=[true|false]: starts and stops controller"
                " logs\n"
@@ -150,6 +150,12 @@ bool ParseSetMode(const std::string& arg, DvrPoseMode* mode) {
   } else if (value == "circle_strafe") {
     *mode = DVR_POSE_MODE_MOCK_CIRCLE_STRAFE;
     return true;
+  } else if (value == "float") {
+    *mode = DVR_POSE_MODE_FLOAT;
+    return true;
+  } else if (value == "motion_sickness") {
+    *mode = DVR_POSE_MODE_MOCK_MOTION_SICKNESS;
+    return true;
   } else {
     return false;
   }

diff --git a/libs/vr/libvrsensor/include/dvr/pose_client.h b/libs/vr/libvrsensor/include/dvr/pose_client.h
index ed75f84..6802fa9 100644 (file)
--- a/libs/vr/libvrsensor/include/dvr/pose_client.h
+++ b/libs/vr/libvrsensor/include/dvr/pose_client.h
@@ -105,6 +105,8 @@ typedef enum DvrPoseMode {
   DVR_POSE_MODE_MOCK_ROTATE_MEDIUM,
   DVR_POSE_MODE_MOCK_ROTATE_FAST,
   DVR_POSE_MODE_MOCK_CIRCLE_STRAFE,
+  DVR_POSE_MODE_FLOAT,
+  DVR_POSE_MODE_MOCK_MOTION_SICKNESS,
 
   // Always last.
   DVR_POSE_MODE_COUNT,

diff --git a/services/vr/sensord/pose_service.cpp b/services/vr/sensord/pose_service.cpp
index 32a2160..f9c7462 100644 (file)
--- a/services/vr/sensord/pose_service.cpp
+++ b/services/vr/sensord/pose_service.cpp
@@ -91,6 +91,10 @@ std::string GetPoseModeString(DvrPoseMode mode) {
       return "DVR_POSE_MODE_MOCK_ROTATE_FAST";
     case DVR_POSE_MODE_MOCK_CIRCLE_STRAFE:
       return "DVR_POSE_MODE_MOCK_CIRCLE_STRAFE";
+    case DVR_POSE_MODE_FLOAT:
+      return "DVR_POSE_MODE_FLOAT";
+    case DVR_POSE_MODE_MOCK_MOTION_SICKNESS:
+      return "DVR_POSE_MODE_MOCK_MOTION_SICKNESS";
     default:
       return "Unknown pose mode";
   }
@@ -418,7 +422,8 @@ void PoseService::UpdatePoseMode() {
                       current_time_ns);
       break;
     }
-    case DVR_POSE_MODE_3DOF: {
+    case DVR_POSE_MODE_3DOF:
+    case DVR_POSE_MODE_FLOAT: {
       // Sensor fusion provides IMU-space data, transform to world space.
 
       // Constants to perform IMU orientation adjustments. Note that these
@@ -446,13 +451,25 @@ void PoseService::UpdatePoseMode() {
       }
       start_from_head_rotation.normalize();
 
-      // Neck / head model code procedure for when no 6dof is available.
-      // To apply the neck model, first translate the head pose to the new
-      // center of eyes, then rotate around the origin (the original head
-      // pos).
-      Vector3d position =
-          start_from_head_rotation * Vector3d(0.0, kDefaultNeckVerticalOffset,
-                                              -kDefaultNeckHorizontalOffset);
+      Vector3d position;
+      switch (pose_mode_) {
+        default:
+        case DVR_POSE_MODE_3DOF:
+          // Neck / head model code procedure for when no 6dof is available.
+          // To apply the neck model, first translate the head pose to the new
+          // center of eyes, then rotate around the origin (the original head
+          // pos).
+          position = start_from_head_rotation *
+                     Vector3d(0.0, kDefaultNeckVerticalOffset,
+                              -kDefaultNeckHorizontalOffset);
+          break;
+        case DVR_POSE_MODE_FLOAT:
+          // Change position a bit in facing direction.
+          mock_pos_offset_ += start_from_head_rotation.toRotationMatrix() * Vector3d(0, 0, -0.01);
+          ResetMockDeviatedPosition();
+          position = mock_pos_offset_;
+          break;
+      }
 
       // IMU driver gives timestamps on its own clock, but we need monotonic
       // clock. Subtract 5ms to account for estimated IMU sample latency.
@@ -460,6 +477,26 @@ void PoseService::UpdatePoseMode() {
                       pose_state.timestamp_ns + 5000000);
       break;
     }
+    case DVR_POSE_MODE_MOCK_MOTION_SICKNESS: {
+      double phase = std::sin(current_time_ns / 1e9) + 1;
+      // Randomize 3rd order rotation axis on phase minimum.
+      if (phase > mock_prev_phase_ && mock_diff_phase_ < 0)
+        mock_rot_axis_2_ = RandVector();
+      mock_diff_phase_ = phase - mock_prev_phase_;
+      mock_prev_phase_ = phase;
+
+      // Rotate axes all the way down.
+      mock_rot_axis_2_ = AngleAxisd(0.004 * phase, mock_rot_axis_3_) * mock_rot_axis_2_;
+      mock_rot_axis_1_ = AngleAxisd(0.002 * (std::sin(current_time_ns / 5e8 + M_PI / 2) + 1), mock_rot_axis_2_) * mock_rot_axis_1_;
+      Rotationd rotation = Rotationd(AngleAxisd(fmod(current_time_ns / 2e9, kTwoPi), mock_rot_axis_1_));
+
+      // Change position a bit.
+      mock_pos_offset_ += rotation.toRotationMatrix() * Vector3d(0, 0, 0.003 * (std::sin(current_time_ns / 6e8) + 1));
+      ResetMockDeviatedPosition();
+
+      WriteAsyncPoses(mock_pos_offset_, rotation, current_time_ns);
+      break;
+    }
     default:
     case DVR_POSE_MODE_6DOF:
       ALOGE("ERROR: invalid pose mode");
@@ -467,6 +504,13 @@ void PoseService::UpdatePoseMode() {
   }
 }
 
+void PoseService::ResetMockDeviatedPosition() {
+  if (mock_pos_offset_[1] < -1) mock_pos_offset_[1] = 2;
+  if (mock_pos_offset_[1] > 30) mock_pos_offset_[1] = 2;
+  if (abs(mock_pos_offset_[0]) > 30) mock_pos_offset_[0] = mock_pos_offset_[2] = 0;
+  if (abs(mock_pos_offset_[2]) > 30) mock_pos_offset_[0] = mock_pos_offset_[2] = 0;
+}
+
 int PoseService::HandleMessage(pdx::Message& msg) {
   int ret = 0;
   const pdx::MessageInfo& info = msg.GetInfo();
@@ -646,6 +690,10 @@ void PoseService::SetPoseMode(DvrPoseMode mode) {
   if (mode == DVR_POSE_MODE_6DOF) {
     // Only 3DoF is currently supported.
     mode = DVR_POSE_MODE_3DOF;
+  } else if (mode == DVR_POSE_MODE_MOCK_MOTION_SICKNESS) {
+    mock_rot_axis_1_ = RandVector();
+    mock_rot_axis_2_ = RandVector();
+    mock_rot_axis_3_ = RandVector();
   }
 
   pose_mode_ = mode;

diff --git a/services/vr/sensord/pose_service.h b/services/vr/sensord/pose_service.h
index 899d5fb..517334a 100644 (file)
--- a/services/vr/sensord/pose_service.h
+++ b/services/vr/sensord/pose_service.h
@@ -99,6 +99,25 @@ class PoseService : public pdx::ServiceBase<PoseService> {
   // Last known pose.
   DvrPoseAsync last_known_pose_;
 
+  // Position offset for use in pose modes.
+  Eigen::Vector3d mock_pos_offset_;
+
+  // Phase data for DVR_POSE_MODE_MOCK_MOTION_SICKNESS.
+  double mock_prev_phase_, mock_diff_phase_;
+
+  // Axis data for DVR_POSE_MODE_MOCK_MOTION_SICKNESS.
+  Eigen::Vector3d mock_rot_axis_1_, mock_rot_axis_2_, mock_rot_axis_3_;
+
+  // Return a random normalized 3d vector.
+  static Eigen::Vector3d RandVector() {
+    Eigen::Vector3d vec = Eigen::Vector3d::Random();
+    vec.normalize();
+    return vec;
+  }
+
+  // Reset mock_pos_offset_ if strayed too far
+  void ResetMockDeviatedPosition();
+
   // If this flag is true, the pose published includes a small prediction of
   // where it'll be when it's consumed.
   bool enable_pose_prediction_;