diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 029aefcac4..eca39a0a44 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -232,30 +232,27 @@ void MyAvatar::reset(bool andReload) { setThrust(glm::vec3(0.0f)); if (andReload) { - // Get fresh data, in case we're really slow and out of wack. - _hmdSensorMatrix = qApp->getHMDSensorPose(); - _hmdSensorPosition = extractTranslation(_hmdSensorMatrix); - _hmdSensorOrientation = glm::quat_cast(_hmdSensorMatrix); - - // Reset body position/orientation under the head. + // derive the desired body orientation from the *old* hmd orientation, before the sensor reset. auto newBodySensorMatrix = deriveBodyFromHMDSensor(); // Based on current cached HMD position/rotation.. + + // transform this body into world space auto worldBodyMatrix = _sensorToWorldMatrix * newBodySensorMatrix; - glm::vec3 worldBodyPos = extractTranslation(worldBodyMatrix); - glm::quat worldBodyRot = glm::normalize(glm::quat_cast(worldBodyMatrix)); - - // FIXME: Hack to retain the previous behavior wrt height. - // I'd like to make the body match head height, but that will have to wait for separate PR. - worldBodyPos.y = getPosition().y; + auto worldBodyPos = extractTranslation(worldBodyMatrix); + auto worldBodyRot = glm::normalize(glm::quat_cast(worldBodyMatrix)); + // this will become our new position. setPosition(worldBodyPos); setOrientation(worldBodyRot); - // If there is any discrepency between positioning and the head (as there is in initial deriveBodyFromHMDSensor), - // we can make that right by setting _bodySensorMatrix = newBodySensorMatrix. - // However, doing so will make the head want to point to the previous body orientation, as cached above. - //_bodySensorMatrix = newBodySensorMatrix; - //updateSensorToWorldMatrix(); // Uses updated position/orientation and _bodySensorMatrix changes - qApp->setRawAvatarUpdateThreading(); + // now sample the new hmd orientation AFTER sensor reset. + updateFromHMDSensorMatrix(qApp->getHMDSensorPose()); + + // update the body in sensor space using the new hmd sensor sample + _bodySensorMatrix = deriveBodyFromHMDSensor(); + + // rebuild the sensor to world matrix such that, the HMD will point in the desired orientation. + // i.e. the along avatar's current position and orientation. + updateSensorToWorldMatrix(); } }