Turn and drive the HMD with linear velocity

interface/src/avatar/MyAvatar.cpp

@@ -1300,6 +1300,11 @@ void MyAvatar::updateMotors() {
         }
         const float DEFAULT_MOTOR_TIMESCALE = 0.2f;
         const float INVALID_MOTOR_TIMESCALE = 1.0e6f;
+
+        if (!qApp->isHMDMode()) {
+            // OUTOFBODY_HACK: add default zero velocity motor to the characterController
+            _characterController.addMotor(glm::vec3(), motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE);
+        } else {
             if (_isPushing || _isBraking || !_isBeingPushed) {
                 _characterController.addMotor(_actionMotorVelocity, motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE);
             } else {
@@ -1308,6 +1313,7 @@ void MyAvatar::updateMotors() {
                 _characterController.addMotor(_actionMotorVelocity, motorRotation, INVALID_MOTOR_TIMESCALE);
             }
         }
+    }
     if (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED) {
         if (_scriptedMotorFrame == SCRIPTED_MOTOR_CAMERA_FRAME) {
             motorRotation = getHead()->getCameraOrientation() * glm::angleAxis(PI, Vectors::UNIT_Y);
@@ -1692,11 +1698,20 @@ void MyAvatar::updateOrientation(float deltaTime) {
 
 
     // update body orientation by movement inputs
-    setOrientation(getOrientation() * glm::quat(glm::radians(glm::vec3(0.0f, totalBodyYaw, 0.0f))));
+    glm::quat deltaRotation = glm::quat(glm::radians(glm::vec3(0.0f, totalBodyYaw, 0.0f)));
+    setOrientation(getOrientation() * deltaRotation);
 
     getHead()->setBasePitch(getHead()->getBasePitch() + _driveKeys[PITCH] * _pitchSpeed * deltaTime);
 
     if (qApp->isHMDMode()) {
+
+        // rotate the sensorToWorldMatrix about the HMD!
+        glm::vec3 hmdOffset = extractTranslation(getHMDSensorMatrix());
+        _sensorToWorldMatrix = (_sensorToWorldMatrix *
+                                createMatFromQuatAndPos(glm::quat(), hmdOffset) *
+                                createMatFromQuatAndPos(deltaRotation, glm::vec3()) *
+                                createMatFromQuatAndPos(glm::quat(), -hmdOffset));
+
         glm::quat orientation = glm::quat_cast(getSensorToWorldMatrix()) * getHMDSensorOrientation();
         glm::quat bodyOrientation = getWorldBodyOrientation();
         glm::quat localOrientation = glm::inverse(bodyOrientation) * orientation;
@@ -1713,6 +1728,33 @@ void MyAvatar::updateOrientation(float deltaTime) {
 }
 
 void MyAvatar::updateActionMotor(float deltaTime) {
+
+    if (qApp->isHMDMode()) {
+        // actions are constant velocity, for your comfort
+        // OUTOFBODY_HACK TODO: what about flying?!?!
+
+        // compute action input
+        glm::vec3 front = (_driveKeys[TRANSLATE_Z]) * IDENTITY_FRONT;
+        glm::vec3 right = (_driveKeys[TRANSLATE_X]) * IDENTITY_RIGHT;
+        glm::vec3 direction = front + right;
+
+        _isPushing = false;
+        _isBeingPushed = false;
+
+        _wasPushing = _isPushing;
+        float directionLength = glm::length(direction);
+        _isPushing = directionLength > EPSILON;
+
+        // normalize direction
+        if (_isPushing) {
+            direction /= directionLength;
+        } else {
+            direction = Vectors::ZERO;
+        }
+
+        _actionMotorVelocity = MAX_WALKING_SPEED * direction;
+
+    } else {
         bool thrustIsPushing = (glm::length2(_thrust) > EPSILON);
         bool scriptedMotorIsPushing = (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED)
             && _scriptedMotorTimescale < MAX_CHARACTER_MOTOR_TIMESCALE;
@@ -1773,6 +1815,7 @@ void MyAvatar::updateActionMotor(float deltaTime) {
             // we're interacting with a floor --> simple horizontal speed and exponential decay
             _actionMotorVelocity = MAX_WALKING_SPEED * direction;
         }
+    }
 
     float boomChange = _driveKeys[ZOOM];
     _boomLength += 2.0f * _boomLength * boomChange + boomChange * boomChange;
@@ -1799,9 +1842,23 @@ void MyAvatar::updatePosition(float deltaTime) {
         measureMotionDerivatives(deltaTime);
         _moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED;
     } else {
-        // physics physics simulation updated elsewhere
         float speed2 = glm::length2(velocity);
         _moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED;
+
+        // OUTOFBODY_HACK, apply _actionMotorVelocity directly to the sensorToWorld matrix!
+        glm::quat motorRotation;
+        glm::quat liftRotation;
+        swingTwistDecomposition(glmExtractRotation(_sensorToWorldMatrix * getHMDSensorMatrix()), _worldUpDirection, liftRotation, motorRotation);
+
+        if (qApp->isHMDMode()) {
+            float speed2 = glm::length2(_actionMotorVelocity);
+            if (speed2 > MIN_AVATAR_SPEED_SQUARED) {
+                glm::vec3 worldVelocity = motorRotation * _actionMotorVelocity;
+                // update sensor to world position ourselves
+                glm::vec3 position = extractTranslation(_sensorToWorldMatrix) + deltaTime * worldVelocity;
+                _sensorToWorldMatrix[3] = glm::vec4(position, 1);
+            }
+        }
     }
 
     // capture the head rotation, in sensor space, when the user first indicates they would like to move/fly.

libraries/physics/src/CharacterController.cpp

@@ -222,9 +222,10 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
         _rigidBody->setLinearVelocity(velocity + _parentVelocity);
         if (_following) {
             // OUTOFBODY_HACK -- these consts were copied from elsewhere, and then tuned
-            const float NORMAL_WALKING_SPEED = 0.5f;
+            const float NORMAL_WALKING_SPEED = 1.5f; // actual walk speed is 2.5 m/sec
             const float FOLLOW_TIME = 0.8f;
             const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f);
+            const float FOLLOW_FACTOR = 0.5f;
 
             const float MAX_ANGULAR_SPEED = FOLLOW_ROTATION_THRESHOLD / FOLLOW_TIME;
 
@@ -232,7 +233,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
 
             btVector3 startPos = bodyTransform.getOrigin();
             btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos;
-            btVector3 vel = deltaPos * (0.5f / dt);
+            btVector3 vel = deltaPos * (FOLLOW_FACTOR / dt);
             btScalar speed = vel.length();
             if (speed > NORMAL_WALKING_SPEED) {
                 vel *= NORMAL_WALKING_SPEED / speed;