From de460f172af08b02b72a9466e4302559e66fd6c6 Mon Sep 17 00:00:00 2001
From: Chelsea Jaggi <feilen1000@gmail.com>
Date: Fri, 17 Apr 2020 00:36:12 -0700
Subject: [PATCH] Fix logic errors in redirected-walking (#413)

* Resolve a logic issue for Redirected Walking

* Resolve issue with arc magnitudes being less than expected

* warnings

Co-authored-by: ykeara <ykeara1@gmail.com>
---
 src/tabcontrollers/RotationTabController.cpp | 34 +++++++++++---------
 1 file changed, 19 insertions(+), 15 deletions(-)

diff --git a/src/tabcontrollers/RotationTabController.cpp b/src/tabcontrollers/RotationTabController.cpp
index 4739a2e..5c38e02 100644
--- a/src/tabcontrollers/RotationTabController.cpp
+++ b/src/tabcontrollers/RotationTabController.cpp
@@ -97,10 +97,10 @@ void RotationTabController::doVestibularMotion(
                   itr++ )
             {
                 itrLast++;
-                if ( itr->distance > itrLast->distance )
-                {
-                    continue;
-                }
+                // if ( itr->distance > itrLast->distance )
+                //{
+                //    continue;
+                //}
 
                 if ( nearestWall == chaperoneDistances.end()
                      || itr->distance < nearestWall->distance )
@@ -135,19 +135,23 @@ void RotationTabController::doVestibularMotion(
 
             // Get the distance between previous and current
             // position
-            double distanceChange = std::sqrt(
-                std::pow( poseHmd.mDeviceToAbsoluteTracking.m[0][3]
-                              - m_autoTurnLastHmdUpdate.m[0][3],
-                          2.0 )
-                + std::pow( poseHmd.mDeviceToAbsoluteTracking.m[0][3]
+            double distanceChange = static_cast<double>(
+                std::hypot( poseHmd.mDeviceToAbsoluteTracking.m[0][3]
                                 - m_autoTurnLastHmdUpdate.m[0][3],
-                            2.0 ) );
+                            poseHmd.mDeviceToAbsoluteTracking.m[2][3]
+                                - m_autoTurnLastHmdUpdate.m[2][3] ) );
 
-            double rotationAmount
-                = ( distanceChange
-                    / ( 2.0 * M_PI
-                        * RotationTabController::vestibularMotionRadius() ) )
-                  * ( turnLeft ? 1 : -1 );
+            // Get the arc length between the previous point and current point
+            // 2 sin-1( (d/2)/r ) (in radians)
+            double arcLength = 2
+                               * std::asin( ( distanceChange / 2 )
+                                            / vestibularMotionRadius() );
+            if ( std::isnan( arcLength ) )
+            {
+                break;
+            }
+
+            double rotationAmount = arcLength * ( turnLeft ? 1 : -1 );
 
             double newRotationAngleDeg
                 = std::fmod( parent->m_moveCenterTabController.rotation()