New Feature: Control rotation during virtual move

It's now possible to rotate the playspace by turning the controller during a virtual move playspace drag. The feature is activated via a checkbox on the playspace tab. If X Y Z axis lock checkboxes are also checked only rotation will be applied. Offsets will still accumulate as the user swings the playspace around because rotation is now centered at head. This feature is intended for use with manual redirected walking (allowing the user to virtually walk in a straight line while walking circles in physical space). This distance walked is reflected in the X,Z offsets.

src/tabcontrollers/MoveCenterTabController.cpp

@@ -1,17 +1,18 @@
 #include "MoveCenterTabController.h"
 #include <QQuickWindow>
 #include "../overlaycontroller.h"
+#include "../utils/Matrix.h"
 
-void rotateCoordinates( float coordinates[3], float angle )
+void rotateCoordinates( double coordinates[3], double angle )
 {
     if ( angle == 0 )
     {
         return;
     }
-    float s = sin( angle );
-    float c = cos( angle );
-    float newX = coordinates[0] * c - coordinates[2] * s;
-    float newZ = coordinates[0] * s + coordinates[2] * c;
+    double s = sin( angle );
+    double c = cos( angle );
+    double newX = coordinates[0] * c - coordinates[2] * s;
+    double newZ = coordinates[0] * s + coordinates[2] * c;
     coordinates[0] = newX;
     coordinates[2] = newZ;
 }
@@ -33,6 +34,11 @@ void MoveCenterTabController::initStage1()
     {
         m_adjustChaperone = value.toBool();
     }
+    value = settings->value( "rotateHand", m_rotateHand );
+    if ( value.isValid() && !value.isNull() )
+    {
+        m_rotateHand = value.toBool();
+    }
     value = settings->value( "moveShortcutRight", m_moveShortcutRightEnabled );
     if ( value.isValid() && !value.isNull() )
     {
@@ -76,7 +82,7 @@ void MoveCenterTabController::initStage2( OverlayController* var_parent,
 
 int MoveCenterTabController::trackingUniverse() const
 {
-    return ( int ) m_trackingUniverse;
+    return static_cast<int>( m_trackingUniverse );
 }
 
 void MoveCenterTabController::setTrackingUniverse( int value, bool notify )
@@ -136,31 +142,111 @@ int MoveCenterTabController::rotation() const
     return m_rotation;
 }
 
+int MoveCenterTabController::tempRotation() const
+{
+    return m_tempRotation;
+}
+
 void MoveCenterTabController::setRotation( int value, bool notify )
 {
     if ( m_rotation != value )
     {
-        // float angle = (value - m_rotation) * 2 * M_PI / 360.0;
-        // parent->RotateUniverseCenter((vr::TrackingUniverseOrigin)m_trackingUniverse,
-        // angle, m_adjustChaperone);
+        double angle = ( value - m_rotation ) * 2 * M_PI / 360.0;
+
+        // Revert now because we don't commit in RotateUniverseCenter and
+        // AddOffsetToUniverseCenter. We do this so rotation and offset can
+        // happen in one go, avoiding positional judder.
+        vr::VRChaperoneSetup()->RevertWorkingCopy();
+
+        // Get hmd pose matrix.
+        vr::TrackedDevicePose_t
+            devicePosesForRot[vr::k_unMaxTrackedDeviceCount];
+        vr::VRSystem()->GetDeviceToAbsoluteTrackingPose(
+            vr::TrackingUniverseStanding,
+            0.0f,
+            devicePosesForRot,
+            vr::k_unMaxTrackedDeviceCount );
+        vr::HmdMatrix34_t oldHmdPos
+            = devicePosesForRot[0].mDeviceToAbsoluteTracking;
+
+        // Set up xyz coordinate values from pose matrix.
+        double oldHmdXyz[3]
+            = { oldHmdPos.m[0][3], oldHmdPos.m[1][3], oldHmdPos.m[2][3] };
+        double newHmdXyz[3]
+            = { oldHmdPos.m[0][3], oldHmdPos.m[1][3], oldHmdPos.m[2][3] };
+
+        // Convert oldHmdXyz into un-rotated coordinates.
+        double oldAngle = -m_rotation * 2 * M_PI / 360.0;
+        rotateCoordinates( oldHmdXyz, oldAngle );
+
+        // Set newHmdXyz to have additional rotation from incoming angle change.
+        rotateCoordinates( newHmdXyz, oldAngle - angle );
+
+        // find difference in x,z offset due to incoming angle change
+        // (coordinates are in un-rotated axis).
+        double hmdRotDiff[3]
+            = { oldHmdXyz[0] - newHmdXyz[0], 0, oldHmdXyz[2] - newHmdXyz[2] };
+
+        // Rotate the tracking univese center without committing.
+        parent->RotateUniverseCenter(
+            vr::TrackingUniverseOrigin( m_trackingUniverse ),
+            static_cast<float>( angle ),
+            m_adjustChaperone,
+            false );
+
         m_rotation = value;
         if ( notify )
         {
             emit rotationChanged( m_rotation );
         }
+
+        // Get rotated offset to apply to universe center.
+        // We use rotated coordinates here because we have already applied
+        // RotateUniverseCenter. This will be the final offset ready to apply,
+        // so it must match the current universe axis rotation.
+        double finalAngle = m_rotation * 2 * M_PI / 360.0;
+        double finalHmdRotDiff[3] = { hmdRotDiff[0], 0, hmdRotDiff[2] };
+        rotateCoordinates( finalHmdRotDiff, finalAngle );
+
+        // We're done with calculations now so we can down-cast the double
+        // values to float for compatilibilty with openvr format
+        float finalHmdRotDiffFloat[3]
+            = { static_cast<float>( finalHmdRotDiff[0] ),
+                static_cast<float>( finalHmdRotDiff[1] ),
+                static_cast<float>( finalHmdRotDiff[2] ) };
+
+        // Apply the offset (in rotated coordinates) without commit.
+        // We still can't commit yet because it would call
+        // vr::VRChaperoneSetup()->RevertWorkingCopy() and we'd lose our
+        // uncommitted RotateUniverseCenter.
+        parent->AddOffsetToUniverseCenter(
+            vr::TrackingUniverseOrigin( m_trackingUniverse ),
+            finalHmdRotDiffFloat,
+            m_adjustChaperone,
+            false );
+
+        // Commit here because we didn't in RotateUniverseCenter and
+        // AddOffsetToUniverseCenter to combine into one go.
+        vr::VRChaperoneSetup()->CommitWorkingCopy(
+            vr::EChaperoneConfigFile_Live );
+
+        // Update UI offsets.
+        m_offsetX += hmdRotDiff[0];
+        m_offsetZ += hmdRotDiff[2];
+        if ( notify )
+        {
+            emit offsetXChanged( m_offsetX );
+            emit offsetZChanged( m_offsetZ );
+        }
     }
 }
 
-void MoveCenterTabController::applyRotation()
+void MoveCenterTabController::setTempRotation( int value, bool notify )
 {
-    if ( m_rotation != m_rotationOld )
+    m_tempRotation = value;
+    if ( notify )
     {
-        float angle = ( m_rotationOld - m_rotation ) * 2 * M_PI / 360.0;
-        parent->RotateUniverseCenter(
-            ( vr::TrackingUniverseOrigin ) m_trackingUniverse,
-            angle,
-            m_adjustChaperone );
-        m_rotationOld = m_rotation;
+        emit tempRotationChanged( m_tempRotation );
     }
 }
 
@@ -176,14 +262,21 @@ void MoveCenterTabController::setAdjustChaperone( bool value, bool notify )
         m_adjustChaperone = value;
         if ( m_trackingUniverse == vr::TrackingUniverseStanding )
         {
-            auto angle = m_rotation * 2 * M_PI / 360.0;
-            float offsetdir = m_adjustChaperone ? -1.0 : 1.0;
-            float offset[3] = { offsetdir * m_offsetX,
-                                offsetdir * m_offsetY,
-                                offsetdir * m_offsetZ };
+            double angle = m_rotation * 2 * M_PI / 360.0;
+            double offsetdir = m_adjustChaperone ? -1.0 : 1.0;
+            double offset[3] = { offsetdir * m_offsetX,
+                                 offsetdir * m_offsetY,
+                                 offsetdir * m_offsetZ };
             rotateCoordinates( offset, angle );
 
-            parent->AddOffsetToCollisionBounds( offset );
+            // We're done with calculations so down-cast to float for
+            // compatibility with openvr format
+
+            float offsetFloat[3] = { static_cast<float>( offset[0] ),
+                                     static_cast<float>( offset[1] ),
+                                     static_cast<float>( offset[2] ) };
+
+            parent->AddOffsetToCollisionBounds( offsetFloat );
         }
         auto settings = OverlayController::appSettings();
         settings->beginGroup( "playspaceSettings" );
@@ -197,6 +290,25 @@ void MoveCenterTabController::setAdjustChaperone( bool value, bool notify )
     }
 }
 
+bool MoveCenterTabController::rotateHand() const
+{
+    return m_rotateHand;
+}
+
+void MoveCenterTabController::setRotateHand( bool value, bool notify )
+{
+    m_rotateHand = value;
+    auto settings = OverlayController::appSettings();
+    settings->beginGroup( "playspaceSettings" );
+    settings->setValue( "rotateHand", m_rotateHand );
+    settings->endGroup();
+    settings->sync();
+    if ( notify )
+    {
+        emit rotateHandChanged( m_rotateHand );
+    }
+}
+
 bool MoveCenterTabController::moveShortcutRight() const
 {
     return m_moveShortcutRightEnabled;
@@ -316,12 +428,15 @@ void MoveCenterTabController::modOffsetX( float value, bool notify )
     // TODO ? possible issue with locking position this way
     if ( !m_lockXToggle )
     {
-        auto angle = m_rotation * 2 * M_PI / 360.0;
-        float offset[3] = { value, 0, 0 };
+        double angle = m_rotation * 2 * M_PI / 360.0;
+        double offset[3] = { value, 0, 0 };
         rotateCoordinates( offset, angle );
+        float offsetFloat[3] = { static_cast<float>( offset[0] ),
+                                 static_cast<float>( offset[1] ),
+                                 static_cast<float>( offset[2] ) };
         parent->AddOffsetToUniverseCenter(
-            ( vr::TrackingUniverseOrigin ) m_trackingUniverse,
-            offset,
+            vr::TrackingUniverseOrigin( m_trackingUniverse ),
+            offsetFloat,
             m_adjustChaperone );
         m_offsetX += value;
         if ( notify )
@@ -336,7 +451,7 @@ void MoveCenterTabController::modOffsetY( float value, bool notify )
     if ( !m_lockYToggle )
     {
         parent->AddOffsetToUniverseCenter(
-            ( vr::TrackingUniverseOrigin ) m_trackingUniverse,
+            vr::TrackingUniverseOrigin( m_trackingUniverse ),
             1,
             value,
             m_adjustChaperone );
@@ -352,12 +467,15 @@ void MoveCenterTabController::modOffsetZ( float value, bool notify )
 {
     if ( !m_lockZToggle )
     {
-        auto angle = m_rotation * 2 * M_PI / 360.0;
-        float offset[3] = { 0, 0, value };
+        double angle = m_rotation * 2 * M_PI / 360.0;
+        double offset[3] = { 0, 0, value };
         rotateCoordinates( offset, angle );
+        float offsetFloat[3] = { static_cast<float>( offset[0] ),
+                                 static_cast<float>( offset[1] ),
+                                 static_cast<float>( offset[2] ) };
         parent->AddOffsetToUniverseCenter(
-            ( vr::TrackingUniverseOrigin ) m_trackingUniverse,
-            offset,
+            vr::TrackingUniverseOrigin( m_trackingUniverse ),
+            offsetFloat,
             m_adjustChaperone );
         m_offsetZ += value;
         if ( notify )
@@ -371,13 +489,13 @@ void MoveCenterTabController::reset()
 {
     vr::VRChaperoneSetup()->RevertWorkingCopy();
     parent->RotateUniverseCenter(
-        ( vr::TrackingUniverseOrigin ) m_trackingUniverse,
+        vr::TrackingUniverseOrigin( m_trackingUniverse ),
         -m_rotation * 2 * M_PI / 360.0,
         m_adjustChaperone,
         false );
     float offset[3] = { -m_offsetX, -m_offsetY, -m_offsetZ };
     parent->AddOffsetToUniverseCenter(
-        ( vr::TrackingUniverseOrigin ) m_trackingUniverse,
+        vr::TrackingUniverseOrigin( m_trackingUniverse ),
         offset,
         m_adjustChaperone,
         false );
@@ -486,104 +604,179 @@ void MoveCenterTabController::eventLoopTick(
     vr::ETrackingUniverseOrigin universe,
     vr::TrackedDevicePose_t* devicePoses )
 {
-    if ( settingsUpdateCounter >= 50 )
+    if ( settingsUpdateCounter >= 100 )
     {
-        setTrackingUniverse( ( int ) universe );
+        setTrackingUniverse( int( universe ) );
         settingsUpdateCounter = 0;
     }
     else
     {
         settingsUpdateCounter++;
-        auto oldMoveHand = m_activeMoveController;
-        auto newMoveHand = getMoveShortcutHand();
-        if ( newMoveHand == vr::TrackedControllerRole_Invalid )
+    }
+    auto oldMoveHand = m_activeMoveController;
+    auto newMoveHand = getMoveShortcutHand();
+    if ( newMoveHand == vr::TrackedControllerRole_Invalid )
+    {
+        emit offsetXChanged( m_offsetX );
+        emit offsetYChanged( m_offsetY );
+        emit offsetZChanged( m_offsetZ );
+
+        // Set lastHandYaw to placeholder value for invalid (-10.0) when we
+        // release move shortcut.
+        if ( m_rotateHand )
+        {
+            lastHandYaw = -10.0;
+        }
+
+        return;
+    }
+    auto handId
+        = vr::VRSystem()->GetTrackedDeviceIndexForControllerRole( newMoveHand );
+    if ( newMoveHand == vr::TrackedControllerRole_Invalid
+         || handId == vr::k_unTrackedDeviceIndexInvalid
+         || handId >= vr::k_unMaxTrackedDeviceCount )
+    {
+        if ( oldMoveHand != vr::TrackedControllerRole_Invalid )
         {
             emit offsetXChanged( m_offsetX );
             emit offsetYChanged( m_offsetY );
             emit offsetZChanged( m_offsetZ );
-            return;
-        }
-        auto handId = vr::VRSystem()->GetTrackedDeviceIndexForControllerRole(
-            newMoveHand );
-        if ( newMoveHand == vr::TrackedControllerRole_Invalid
-             || handId == vr::k_unTrackedDeviceIndexInvalid
-             || handId >= vr::k_unMaxTrackedDeviceCount )
-        {
-            if ( oldMoveHand != vr::TrackedControllerRole_Invalid )
-            {
-                emit offsetXChanged( m_offsetX );
-                emit offsetYChanged( m_offsetY );
-                emit offsetZChanged( m_offsetZ );
-            }
-            return;
-        }
-        vr::TrackedDevicePose_t* pose = devicePoses + handId;
-        if ( !pose->bPoseIsValid || !pose->bDeviceIsConnected
-             || pose->eTrackingResult != vr::TrackingResult_Running_OK )
-        {
-            return;
-        }
-        float relativeControllerPosition[]
-            = { pose->mDeviceToAbsoluteTracking.m[0][3],
-                pose->mDeviceToAbsoluteTracking.m[1][3],
-                pose->mDeviceToAbsoluteTracking.m[2][3] };
 
-        auto angle = m_rotation * 2 * M_PI / 360.0;
-        rotateCoordinates( relativeControllerPosition, -angle );
-        float absoluteControllerPosition[] = {
-            relativeControllerPosition[0] + m_offsetX,
-            relativeControllerPosition[1] + m_offsetY,
-            relativeControllerPosition[2] + m_offsetZ,
+            if ( m_rotateHand )
+            {
+                lastHandYaw = -10.0;
+            }
+        }
+        return;
+    }
+    vr::TrackedDevicePose_t* pose = devicePoses + handId;
+    if ( !pose->bPoseIsValid || !pose->bDeviceIsConnected
+         || pose->eTrackingResult != vr::TrackingResult_Running_OK )
+    {
+        return;
+    }
+    double relativeControllerPosition[]
+        = { pose->mDeviceToAbsoluteTracking.m[0][3],
+            pose->mDeviceToAbsoluteTracking.m[1][3],
+            pose->mDeviceToAbsoluteTracking.m[2][3] };
+
+    double angle = m_rotation * 2 * M_PI / 360.0;
+    rotateCoordinates( relativeControllerPosition, -angle );
+    float absoluteControllerPosition[] = {
+        static_cast<float>( relativeControllerPosition[0] ) + m_offsetX,
+        static_cast<float>( relativeControllerPosition[1] ) + m_offsetY,
+        static_cast<float>( relativeControllerPosition[2] ) + m_offsetZ,
+    };
+
+    // Get hand's rotation.
+    if ( m_rotateHand )
+    {
+        // handMatrix is in rotated coordinates.
+        vr::HmdMatrix34_t handMatrix = pose->mDeviceToAbsoluteTracking;
+
+        // We need un-rotated coordinates for valid comparison between
+        // handYaw and lastHandYaw. Set up (un)rotation matrix.
+        vr::HmdMatrix34_t handMatrixRotMat;
+        vr::HmdMatrix34_t handMatrixAbsolute;
+        utils::initRotationMatrix(
+            handMatrixRotMat, 1, static_cast<float>( angle ) );
+
+        // Get handMatrixAbsolute in un-rotated coordinates.
+        utils::matMul33( handMatrixAbsolute, handMatrixRotMat, handMatrix );
+
+        // Convert pose matrix to yaw.
+        handYaw = std::atan2( handMatrixAbsolute.m[0][2],
+                              handMatrixAbsolute.m[2][2] );
+    }
+
+    if ( oldMoveHand == newMoveHand )
+    {
+        double diff[3] = {
+            absoluteControllerPosition[0] - m_lastControllerPosition[0],
+            absoluteControllerPosition[1] - m_lastControllerPosition[1],
+            absoluteControllerPosition[2] - m_lastControllerPosition[2],
         };
 
-        if ( oldMoveHand == newMoveHand )
+        // offset is un-rotated coordinates
+
+        // prevents UI from updating if axis movement is locked
+        if ( !m_lockXToggle )
         {
-            float diff[3] = {
-                absoluteControllerPosition[0] - m_lastControllerPosition[0],
-                absoluteControllerPosition[1] - m_lastControllerPosition[1],
-                absoluteControllerPosition[2] - m_lastControllerPosition[2],
-            };
+            m_offsetX += diff[0];
+        }
+        if ( !m_lockYToggle )
+        {
+            m_offsetY += diff[1];
+        }
+        if ( !m_lockZToggle )
+        {
+            m_offsetZ += diff[2];
+        }
 
-            // offset is un-rotated coordinates
+        rotateCoordinates( diff, angle );
 
-            // prevents UI from updating if axis movement is locked
-            if ( !m_lockXToggle )
-            {
-                m_offsetX += diff[0];
-            }
-            if ( !m_lockYToggle )
-            {
-                m_offsetY += diff[1];
-            }
-            if ( !m_lockZToggle )
-            {
-                m_offsetZ += diff[2];
-            }
-
-            rotateCoordinates( diff, angle );
-            // If locked removes movement
-            if ( m_lockXToggle )
-            {
-                diff[0] = 0;
-            }
-            if ( m_lockYToggle )
-            {
-                diff[1] = 0;
-            }
-            if ( m_lockZToggle )
-            {
-                diff[2] = 0;
-            }
+        // Done calculating rotation so we down-cast double to float for openvr
+        // format
+        float diffFloat[3] = { static_cast<float>( diff[0] ),
+                               static_cast<float>( diff[1] ),
+                               static_cast<float>( diff[2] ) };
+        // If locked removes movement
+        if ( m_lockXToggle )
+        {
+            diffFloat[0] = 0;
+        }
+        if ( m_lockYToggle )
+        {
+            diffFloat[1] = 0;
+        }
+        if ( m_lockZToggle )
+        {
+            diffFloat[2] = 0;
+        }
 
+        // Check if diffFloat is anything before comitting.
+        if ( diffFloat[0] != 0 || diffFloat[1] != 0 || diffFloat[2] != 0 )
+        {
             parent->AddOffsetToUniverseCenter(
-                ( vr::TrackingUniverseOrigin ) m_trackingUniverse,
-                diff,
+                vr::TrackingUniverseOrigin( m_trackingUniverse ),
+                diffFloat,
                 m_adjustChaperone );
         }
-        m_lastControllerPosition[0] = absoluteControllerPosition[0];
-        m_lastControllerPosition[1] = absoluteControllerPosition[1];
-        m_lastControllerPosition[2] = absoluteControllerPosition[2];
-    }
-}
 
+        // Get rotation change of hand.
+        if ( m_rotateHand )
+        {
+            // Checking if set to -10.0 placeholder for invalid hand.
+            if ( lastHandYaw < -9.0 )
+            {
+                lastHandYaw = handYaw;
+            }
+
+            else
+            {
+                double handYawDiff = handYaw - lastHandYaw;
+
+                // TODO: Increase angular granularity beyond 1 degree.
+                int newRotationAngleDeg = static_cast<int>(
+                    round( handYawDiff * 180.0 / M_PI ) + m_rotation );
+
+                // Keep angle within -180 ~ 180
+                if ( newRotationAngleDeg > 180 )
+                {
+                    newRotationAngleDeg -= 360;
+                }
+                else if ( newRotationAngleDeg < -180 )
+                {
+                    newRotationAngleDeg += 360;
+                }
+
+                setRotation( newRotationAngleDeg );
+            }
+        }
+    }
+    m_lastControllerPosition[0] = absoluteControllerPosition[0];
+    m_lastControllerPosition[1] = absoluteControllerPosition[1];
+    m_lastControllerPosition[2] = absoluteControllerPosition[2];
+    lastHandYaw = handYaw;
+}
 } // namespace advsettings