From 57731e7bd8b7502878d03fdceefee910be6dca93 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 13 Jul 2015 13:36:55 -0700 Subject: [PATCH] More refactoring of MyAvatar HMD Sensor math. Moved compuation of avatar postion using head/body joints int a helper method. Also we compute this once per frame in setHMDSensorMatrix instead of inside of every setPostion/setOrientation call. --- interface/src/avatar/MyAvatar.cpp | 56 ++++++++++++++++++------------- interface/src/avatar/MyAvatar.h | 18 +++++++--- 2 files changed, 47 insertions(+), 27 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index a001bfae01..289d62b299 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -105,8 +105,10 @@ MyAvatar::MyAvatar() : _firstPersonSkeletonModel(this), _prevShouldDrawHead(true), _hmdSensorMatrix(), - _hmdSensorPosition(), _hmdSensorOrientation(), + _hmdSensorPosition(), + _bodySensorOrientation(), + _bodySensorPosition(), _sensorToWorldMatrix() { _firstPersonSkeletonModel.setIsFirstPerson(true); @@ -253,6 +255,8 @@ void MyAvatar::setHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { _hmdSensorMatrix = hmdSensorMatrix; _hmdSensorPosition = extractTranslation(hmdSensorMatrix); _hmdSensorOrientation = glm::quat_cast(hmdSensorMatrix); + + deriveBodyFromHMDSensor(_bodySensorOrientation, _bodySensorPosition); } // Update avatar head rotation with sensor data @@ -1302,8 +1306,8 @@ void MyAvatar::updateOrientation(float deltaTime) { // AJT: FIXME move this into a method. also, why doesn't plain ole getOrientation, setOrientation work here? // update sensor mat, so that rotation will take effect when room tracking. - glm::vec3 bodyPosition = calcBodyPositionFromSensors(); - glm::quat bodyOrientation = calcBodyOrientationFromSensors(); + glm::vec3 bodyPosition = getWorldBodyPosition(); + glm::quat bodyOrientation = getWorldBodyOrientation(); glm::mat4 bodyMat = createMatFromQuatAndPos(bodyOrientation, bodyPosition); glm::mat4 sensorOffset = bodyMat * glm::mat4_cast(twist) * glm::inverse(bodyMat); _sensorToWorldMatrix = sensorOffset * _sensorToWorldMatrix; @@ -1323,7 +1327,7 @@ void MyAvatar::updateOrientation(float deltaTime) { if (qApp->isHMDMode()) { // these angles will be in radians - glm::quat orientation = glm::quat_cast(getSensorToWorldMatrix() * getHMDSensorMatrix()); + glm::quat orientation = glm::quat_cast(getSensorToWorldMatrix()) * getHMDSensorOrientation(); glm::quat localOrientation = glm::inverse(bodyOrientation) * orientation; // ... so they need to be converted to degrees before we do math... glm::vec3 euler = glm::eulerAngles(localOrientation) * DEGREES_PER_RADIAN; @@ -1339,8 +1343,8 @@ void MyAvatar::updateOrientation(float deltaTime) { head->setBasePitch(PITCH(euler)); head->setBaseRoll(ROLL(euler)); - glm::quat q = calcBodyOrientationFromSensors(); - setAvatarOrientation(q); + // AJT: FIXME, I might be able to do just a setOrientation here right? + setAvatarOrientation(getWorldBodyOrientation()); } } @@ -1501,7 +1505,7 @@ void MyAvatar::updatePosition(float deltaTime) { _moving = speed > MOVING_SPEED_THRESHOLD; if (qApp->isHMDMode() && isRoomTracking) { - setAvatarPosition(calcBodyPositionFromSensors()); + setAvatarPosition(getWorldBodyPosition()); } } @@ -1706,7 +1710,7 @@ void MyAvatar::setAvatarOrientation(glm::quat quat) { void MyAvatar::setPosition(const glm::vec3 position, bool overideReferential) { // update the sensor mat so that the body position will end up in the desired // position when driven from the head. - glm::vec3 bodyPos = calcBodyPositionFromSensors(); + glm::vec3 bodyPos = getWorldBodyPosition(); glm::vec3 desiredPos = position; glm::vec3 sensorPos = extractTranslation(_sensorToWorldMatrix); _sensorToWorldMatrix[3] = glm::vec4(desiredPos - bodyPos + sensorPos, 1); @@ -1718,8 +1722,8 @@ void MyAvatar::setPosition(const glm::vec3 position, bool overideReferential) { void MyAvatar::setOrientation(const glm::quat& orientation, bool overideReferential) { // update the sensor mat so that the body orientation will end up in the desired // location when driven from the head. - glm::vec3 bodyPos = calcBodyPositionFromSensors(); - glm::quat bodyOrientation = calcBodyOrientationFromSensors(); + glm::vec3 bodyPos = getWorldBodyPosition(); + glm::quat bodyOrientation = getWorldBodyOrientation(); glm::mat4 bodyMat = createMatFromQuatAndPos(bodyOrientation, bodyPos); glm::mat4 desiredMat = createMatFromQuatAndPos(orientation, bodyPos); _sensorToWorldMatrix = desiredMat * glm::inverse(bodyMat) * _sensorToWorldMatrix; @@ -1727,8 +1731,19 @@ void MyAvatar::setOrientation(const glm::quat& orientation, bool overideReferent setAvatarOrientation(orientation); } -glm::vec3 MyAvatar::calcBodyPositionFromSensors() const { - // hmd is in sensor space. +glm::vec3 MyAvatar::getWorldBodyPosition() const { + return transformPoint(_sensorToWorldMatrix, _bodySensorPosition); +} + +glm::quat MyAvatar::getWorldBodyOrientation() const { + return glm::quat_cast(_sensorToWorldMatrix) * _bodySensorOrientation; +} + +// derive avatar body position and orientation from the current HMD Sensor location. +// results are in sensor space +void MyAvatar::deriveBodyFromHMDSensor(glm::quat& bodyOrientationOut, glm::vec3& bodyPositionOut) const { + + // HMD is in sensor space. const glm::vec3 hmdPosition = getHMDSensorPosition(); const glm::quat hmdOrientation = getHMDSensorOrientation(); const glm::quat hmdOrientationYawOnly = cancelOutRollAndPitch(hmdOrientation); @@ -1736,21 +1751,16 @@ glm::vec3 MyAvatar::calcBodyPositionFromSensors() const { // In sensor space, figure out where the avatar body should be, // by applying offsets from the avatar's neck & head joints. vec3 localEyes = _skeletonModel.getDefaultEyeModelPosition(); - vec3 localNeck(0.0f, 0.6f, 0.0f); // start with some kind of guess if the skeletonModel is not loaded yet. + vec3 localNeck(0.0f, 0.48f, 0.0f); // start with some kind of guess if the skeletonModel is not loaded yet. _skeletonModel.getLocalNeckPosition(localNeck); + // apply simplistic head/neck model + // eyeToNeck offset is relative full HMD orientation. + // while neckToRoot offset is only relative to HMDs yaw. glm::vec3 eyeToNeck = hmdOrientation * (localNeck - localEyes); glm::vec3 neckToRoot = hmdOrientationYawOnly * -localNeck; glm::vec3 roomBodyPos = hmdPosition + eyeToNeck + neckToRoot; - // now convert from sensor space into world coordinates - return transformPoint(_sensorToWorldMatrix, roomBodyPos); -} - -glm::quat MyAvatar::calcBodyOrientationFromSensors() const { - const glm::quat hmdOrientation = getHMDSensorOrientation(); - const glm::quat hmdOrientationYawOnly = cancelOutRollAndPitch(hmdOrientation); - - // TODO: do a beter calculation of bodyOrientation for now just use hmd facing. - return glm::quat_cast(_sensorToWorldMatrix) * hmdOrientationYawOnly; + bodyOrientationOut = hmdOrientationYawOnly; + bodyPositionOut = roomBodyPos; } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index aeafb1c119..4d3416273a 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -227,8 +227,8 @@ private: void setAvatarPosition(glm::vec3 pos); void setAvatarOrientation(glm::quat quat); - glm::vec3 calcBodyPositionFromSensors() const; - glm::quat calcBodyOrientationFromSensors() const; + glm::vec3 getWorldBodyPosition() const; + glm::quat getWorldBodyOrientation() const; bool cameraInsideHead() const; @@ -238,6 +238,10 @@ private: void setVisibleInSceneIfReady(Model* model, render::ScenePointer scene, bool visiblity); + // derive avatar body position and orientation from the current HMD Sensor location. + // results are in sensor space + void deriveBodyFromHMDSensor(glm::quat& bodyOrientationOut, glm::vec3& bodyPositionOut) const; + glm::vec3 _gravity; float _driveKeys[MAX_DRIVE_KEYS]; @@ -299,10 +303,16 @@ private: SkeletonModel _firstPersonSkeletonModel; bool _prevShouldDrawHead; - // cache of the current HMD sensor position and orientation, in sensor space. + // cache of the current HMD sensor position and orientation + // in sensor space. glm::mat4 _hmdSensorMatrix; - glm::vec3 _hmdSensorPosition; glm::quat _hmdSensorOrientation; + glm::vec3 _hmdSensorPosition; + + // cache of the current body position and orientation of the avatar's body, + // in sensor space. + glm::quat _bodySensorOrientation; + glm::vec3 _bodySensorPosition; // used to transform any sensor into world space, including the _hmdSensorMat, or hand controllers. glm::mat4 _sensorToWorldMatrix;