Merge pull request #8595 from hyperlogic/out-of-body-experience

ramp support with comfort mode option

interface/src/Menu.cpp

@@ -511,6 +511,9 @@ Menu::Menu() {
         avatar, SLOT(updateMotionBehaviorFromMenu()),
         UNSPECIFIED_POSITION, "Developer");
 
+    addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::EnableVerticalComfortMode, 0, false,
+        avatar, SLOT(setEnableVerticalComfortMode(bool)));
+
     // Developer > Hands >>>
     MenuWrapper* handOptionsMenu = developerMenu->addMenu("Hands");
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false,

interface/src/Menu.h

@@ -99,6 +99,7 @@ namespace MenuOption {
     const QString EchoServerAudio = "Echo Server Audio";
     const QString EnableAvatarCollisions = "Enable Avatar Collisions";
     const QString EnableInverseKinematics = "Enable Inverse Kinematics";
+    const QString EnableVerticalComfortMode = "Enable Vertical Comfort Mode";
     const QString ExpandMyAvatarSimulateTiming = "Expand /myAvatar/simulation";
     const QString ExpandMyAvatarTiming = "Expand /myAvatar";
     const QString ExpandOtherAvatarTiming = "Expand /otherAvatar";

interface/src/avatar/MyAvatar.cpp

@@ -422,6 +422,11 @@ void MyAvatar::simulate(float deltaTime) {
         updatePosition(deltaTime);
     }
 
+    // update sensorToWorldMatrix for camera and hand controllers
+    // before we perform rig animations and IK.
+
+    updateSensorToWorldMatrix(_enableVerticalComfortMode ? SensorToWorldUpdateMode::VerticalComfort : SensorToWorldUpdateMode::Vertical);
+
     {
         PerformanceTimer perfTimer("skeleton");
         _skeletonModel->simulate(deltaTime);
@@ -548,11 +553,23 @@ void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeV
 // best called at end of main loop, after physics.
 // update sensor to world matrix from current body position and hmd sensor.
 // This is so the correct camera can be used for rendering.
-void MyAvatar::updateSensorToWorldMatrix() {
+void MyAvatar::updateSensorToWorldMatrix(SensorToWorldUpdateMode mode) {
-    // update the sensor mat so that the body position will end up in the desired
+    if (mode == SensorToWorldUpdateMode::Full) {
-    // position when driven from the head.
+        glm::mat4 bodyToWorld = createMatFromQuatAndPos(getOrientation(), getPosition());
-    glm::mat4 bodyToWorld = createMatFromQuatAndPos(getOrientation(), getPosition());
+        setSensorToWorldMatrix(bodyToWorld * glm::inverse(_bodySensorMatrix));
-    setSensorToWorldMatrix(bodyToWorld * glm::inverse(_bodySensorMatrix));
+    } else if (mode == SensorToWorldUpdateMode::Vertical ||
+               mode == SensorToWorldUpdateMode::VerticalComfort) {
+        glm::mat4 bodyToWorld = createMatFromQuatAndPos(getOrientation(), getPosition());
+        glm::mat4 newSensorToWorldMat = bodyToWorld * glm::inverse(_bodySensorMatrix);
+        glm::mat4 sensorToWorldMat = _sensorToWorldMatrix;
+        sensorToWorldMat[3][1] = newSensorToWorldMat[3][1];
+        if (mode == SensorToWorldUpdateMode::VerticalComfort &&
+            fabsf(_sensorToWorldMatrix[3][1] - newSensorToWorldMat[3][1]) > 0.1f) {
+            setSensorToWorldMatrix(sensorToWorldMat);
+        } else if (mode == SensorToWorldUpdateMode::Vertical) {
+            setSensorToWorldMatrix(sensorToWorldMat);
+        }
+    }
 }
 
 void MyAvatar::setSensorToWorldMatrix(const glm::mat4& sensorToWorld) {
@@ -861,6 +878,10 @@ void MyAvatar::setEnableInverseKinematics(bool isEnabled) {
     _rig->setEnableInverseKinematics(isEnabled);
 }
 
+void MyAvatar::setEnableVerticalComfortMode(bool isEnabled) {
+    _enableVerticalComfortMode = isEnabled;
+}
+
 void MyAvatar::loadData() {
     Settings settings;
     settings.beginGroup("Avatar");
@@ -1318,9 +1339,14 @@ void MyAvatar::prepareForPhysicsSimulation() {
     _characterController.handleChangedCollisionGroup();
     _characterController.setParentVelocity(parentVelocity);
 
-    _characterController.setPositionAndOrientation(getPosition(), getOrientation());
+    glm::vec3 position = getPosition();
+    glm::quat orientation = getOrientation();
+
+    _characterController.setPositionAndOrientation(position, orientation);
     if (qApp->isHMDMode()) {
-        _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix);
+        glm::mat4 bodyToWorldMatrix = createMatFromQuatAndPos(orientation, position);
+        glm::mat4 currentBodySensorMatrix = glm::inverse(_sensorToWorldMatrix) * bodyToWorldMatrix;
+        _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), currentBodySensorMatrix);
     } else {
         _follow.deactivate();
         getCharacterController()->disableFollow();
@@ -1338,8 +1364,7 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
     nextAttitude(position, orientation);
 
     // compute new _bodyToSensorMatrix
-    glm::mat4 bodyToWorldMatrix = createMatFromQuatAndPos(orientation, position);
+    //_bodySensorMatrix = deriveBodyFromHMDSensor();
-    _bodySensorMatrix = glm::inverse(_sensorToWorldMatrix) * bodyToWorldMatrix;
 
     if (_characterController.isEnabledAndReady()) {
         setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity());

interface/src/avatar/MyAvatar.h

@@ -124,7 +124,12 @@ public:
     // best called at end of main loop, just before rendering.
     // update sensor to world matrix from current body position and hmd sensor.
     // This is so the correct camera can be used for rendering.
-    void updateSensorToWorldMatrix();
+    enum class SensorToWorldUpdateMode {
+        Full = 0,
+        Vertical,
+        VerticalComfort
+    };
+    void updateSensorToWorldMatrix(SensorToWorldUpdateMode mode = SensorToWorldUpdateMode::Full);
 
     void setSensorToWorldMatrix(const glm::mat4& sensorToWorld);
 
@@ -306,6 +311,7 @@ public slots:
     void setEnableMeshVisible(bool isEnabled);
     void setUseAnimPreAndPostRotations(bool isEnabled);
     void setEnableInverseKinematics(bool isEnabled);
+    void setEnableVerticalComfortMode(bool isEnabled);
 
     QUrl getAnimGraphOverrideUrl() const;  // thread-safe
     void setAnimGraphOverrideUrl(QUrl value);  // thread-safe
@@ -477,6 +483,7 @@ private:
     bool _enableDebugDrawHandControllers { false };
     bool _enableDebugDrawSensorToWorldMatrix { false };
     bool _enableDebugDrawIKTargets { false };
+    bool _enableVerticalComfortMode { false };
 
     AudioListenerMode _audioListenerMode;
     glm::vec3 _customListenPosition;

interface/src/avatar/SkeletonModel.cpp

@@ -25,7 +25,7 @@
 
 glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f);
 float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f;
-float TRUNCATE_IK_CAPSULE_RADIUS = 0.5f;
+float TRUNCATE_IK_CAPSULE_RADIUS = 0.25f;
 
 SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent, RigPointer rig) :
     Model(rig, parent),