Merge pull request #11036 from AndrewMeadows/walkabout

fix bugs preventing avatar from walking on spherical planet

interface/src/Application.cpp

@@ -2323,7 +2323,7 @@ void Application::paintGL() {
                 }
             } else if (_myCamera.getMode() == CAMERA_MODE_MIRROR) {
                 if (isHMDMode()) {
-                    auto mirrorBodyOrientation = myAvatar->getWorldAlignedOrientation() * glm::quat(glm::vec3(0.0f, PI + _rotateMirror, 0.0f));
+                    auto mirrorBodyOrientation = myAvatar->getOrientation() * glm::quat(glm::vec3(0.0f, PI + _rotateMirror, 0.0f));
 
                     glm::quat hmdRotation = extractRotation(myAvatar->getHMDSensorMatrix());
                     // Mirror HMD yaw and roll
@@ -2345,7 +2345,7 @@ void Application::paintGL() {
                         + mirrorBodyOrientation * glm::vec3(0.0f, 0.0f, 1.0f) * MIRROR_FULLSCREEN_DISTANCE * _scaleMirror
                         + mirrorBodyOrientation * hmdOffset);
                 } else {
-                    _myCamera.setOrientation(myAvatar->getWorldAlignedOrientation()
+                    _myCamera.setOrientation(myAvatar->getOrientation()
                         * glm::quat(glm::vec3(0.0f, PI + _rotateMirror, 0.0f)));
                     _myCamera.setPosition(myAvatar->getDefaultEyePosition()
                         + glm::vec3(0, _raiseMirror * myAvatar->getUniformScale(), 0)

interface/src/avatar/MyAvatar.cpp

@@ -1299,7 +1299,7 @@ eyeContactTarget MyAvatar::getEyeContactTarget() {
 }
 
 glm::vec3 MyAvatar::getDefaultEyePosition() const {
-    return getPosition() + getWorldAlignedOrientation() * Quaternions::Y_180 * _skeletonModel->getDefaultEyeModelPosition();
+    return getPosition() + getOrientation() * Quaternions::Y_180 * _skeletonModel->getDefaultEyeModelPosition();
 }
 
 const float SCRIPT_PRIORITY = 1.0f + 1.0f;
@@ -1594,9 +1594,14 @@ void MyAvatar::updateMotors() {
             motorRotation = getMyHead()->getHeadOrientation();
         } else {
             // non-hovering = walking: follow camera twist about vertical but not lift
-            // so we decompose camera's rotation and store the twist part in motorRotation
+            // we decompose camera's rotation and store the twist part in motorRotation
+            // however, we need to perform the decomposition in the avatar-frame
+            // using the local UP axis and then transform back into world-frame
+            glm::quat orientation = getOrientation();
+            glm::quat headOrientation = glm::inverse(orientation) * getMyHead()->getHeadOrientation(); // avatar-frame
             glm::quat liftRotation;
-            swingTwistDecomposition(getMyHead()->getHeadOrientation(), _worldUpDirection, liftRotation, motorRotation);
+            swingTwistDecomposition(headOrientation, Vectors::UNIT_Y, liftRotation, motorRotation);
+            motorRotation = orientation * motorRotation;
         }
         const float DEFAULT_MOTOR_TIMESCALE = 0.2f;
         const float INVALID_MOTOR_TIMESCALE = 1.0e6f;
@@ -1650,11 +1655,31 @@ void MyAvatar::prepareForPhysicsSimulation() {
     _prePhysicsRoomPose = AnimPose(_sensorToWorldMatrix);
 }
 
+// There are a number of possible strategies for this set of tools through endRender, below.
+void MyAvatar::nextAttitude(glm::vec3 position, glm::quat orientation) {
+    bool success;
+    Transform trans = getTransform(success);
+    if (!success) {
+        qCWarning(interfaceapp) << "Warning -- MyAvatar::nextAttitude failed";
+        return;
+    }
+    trans.setTranslation(position);
+    trans.setRotation(orientation);
+    SpatiallyNestable::setTransform(trans, success);
+    if (!success) {
+        qCWarning(interfaceapp) << "Warning -- MyAvatar::nextAttitude failed";
+    }
+    updateAttitude(orientation);
+}
+
 void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
-    glm::vec3 position = getPosition();
+    glm::vec3 position;
-    glm::quat orientation = getOrientation();
+    glm::quat orientation;
     if (_characterController.isEnabledAndReady()) {
         _characterController.getPositionAndOrientation(position, orientation);
+    } else {
+        position = getPosition();
+        orientation = getOrientation();
     }
     nextAttitude(position, orientation);
     _bodySensorMatrix = _follow.postPhysicsUpdate(*this, _bodySensorMatrix);

interface/src/avatar/MyAvatar.h

@@ -438,6 +438,7 @@ public:
 
     void updateMotors();
     void prepareForPhysicsSimulation();
+    void nextAttitude(glm::vec3 position, glm::quat orientation); // Can be safely called at any time.
     void harvestResultsFromPhysicsSimulation(float deltaTime);
 
     const QString& getCollisionSoundURL() { return _collisionSoundURL; }
@@ -557,7 +558,6 @@ public:
     Q_INVOKABLE bool isUp(const glm::vec3& direction) { return glm::dot(direction, _worldUpDirection) > 0.0f; }; // true iff direction points up wrt avatar's definition of up.
     Q_INVOKABLE bool isDown(const glm::vec3& direction) { return glm::dot(direction, _worldUpDirection) < 0.0f; };
 
-
 public slots:
     void increaseSize();
     void decreaseSize();

interface/src/avatar/MyHead.cpp

@@ -29,7 +29,7 @@ MyHead::MyHead(MyAvatar* owningAvatar) : Head(owningAvatar) {
 glm::quat MyHead::getHeadOrientation() const {
     // NOTE: Head::getHeadOrientation() is not used for orienting the camera "view" while in Oculus mode, so
     // you may wonder why this code is here. This method will be called while in Oculus mode to determine how
-    // to change the driving direction while in Oculus mode. It is used to support driving toward where you're
+    // to change the driving direction while in Oculus mode. It is used to support driving toward where your
     // head is looking. Note that in oculus mode, your actual camera view and where your head is looking is not
     // always the same.
 
@@ -39,7 +39,7 @@ glm::quat MyHead::getHeadOrientation() const {
         return headPose.rotation * Quaternions::Y_180;
     }
 
-    return myAvatar->getWorldAlignedOrientation() * glm::quat(glm::radians(glm::vec3(_basePitch, 0.0f, 0.0f)));
+    return myAvatar->getOrientation() * glm::quat(glm::radians(glm::vec3(_basePitch, 0.0f, 0.0f)));
 }
 
 void MyHead::simulate(float deltaTime) {

libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp

@@ -151,11 +151,6 @@ glm::vec3 Avatar::getNeckPosition() const {
     return _skeletonModel->getNeckPosition(neckPosition) ? neckPosition : getPosition();
 }
 
-
-glm::quat Avatar::getWorldAlignedOrientation () const {
-    return computeRotationFromBodyToWorldUp() * getOrientation();
-}
-
 AABox Avatar::getBounds() const {
     if (!_skeletonModel->isRenderable() || _skeletonModel->needsFixupInScene()) {
         // approximately 2m tall, scaled to user request.
@@ -436,6 +431,11 @@ void Avatar::slamPosition(const glm::vec3& newPosition) {
     _lastVelocity = glm::vec3(0.0f);
 }
 
+void Avatar::updateAttitude(const glm::quat& orientation) {
+    _skeletonModel->updateAttitude(orientation);
+	_worldUpDirection = orientation * Vectors::UNIT_Y;
+}
+
 void Avatar::applyPositionDelta(const glm::vec3& delta) {
     setPosition(getPosition() + delta);
     _positionDeltaAccumulator += delta;
@@ -628,22 +628,6 @@ void Avatar::render(RenderArgs* renderArgs) {
     }
 }
 
-glm::quat Avatar::computeRotationFromBodyToWorldUp(float proportion) const {
-    glm::quat orientation = getOrientation();
-    glm::vec3 currentUp = orientation * IDENTITY_UP;
-    float angle = acosf(glm::clamp(glm::dot(currentUp, _worldUpDirection), -1.0f, 1.0f));
-    if (angle < EPSILON) {
-        return glm::quat();
-    }
-    glm::vec3 axis;
-    if (angle > 179.99f * RADIANS_PER_DEGREE) { // 180 degree rotation; must use another axis
-        axis = orientation * IDENTITY_RIGHT;
-    } else {
-        axis = glm::normalize(glm::cross(currentUp, _worldUpDirection));
-    }
-    return glm::angleAxis(angle * proportion, axis);
-}
-
 void Avatar::fixupModelsInScene(const render::ScenePointer& scene) {
     _attachmentsToDelete.clear();
 
@@ -1401,14 +1385,14 @@ glm::quat Avatar::getUncachedRightPalmRotation() const {
     return rightPalmRotation;
 }
 
-void Avatar::setPosition(const glm::vec3& position) {
+void Avatar::setPositionViaScript(const glm::vec3& position) {
-    AvatarData::setPosition(position);
+    setPosition(position);
-    updateAttitude();
+    updateAttitude(getOrientation());
 }
 
-void Avatar::setOrientation(const glm::quat& orientation) {
+void Avatar::setOrientationViaScript(const glm::quat& orientation) {
-    AvatarData::setOrientation(orientation);
+    setOrientation(orientation);
-    updateAttitude();
+    updateAttitude(orientation);
 }
 
 void Avatar::updatePalms() {

libraries/avatars-renderer/src/avatars-renderer/Avatar.h

@@ -112,8 +112,6 @@ public:
     const Head* getHead() const { return static_cast<const Head*>(_headData); }
     Head* getHead() { return static_cast<Head*>(_headData); }
 
-    glm::quat getWorldAlignedOrientation() const;
-
     AABox getBounds() const;
 
     /// Returns the distance to use as a LOD parameter.
@@ -184,7 +182,7 @@ public:
     void scaleVectorRelativeToPosition(glm::vec3 &positionToScale) const;
 
     void slamPosition(const glm::vec3& position);
-    virtual void updateAttitude() override { _skeletonModel->updateAttitude(); }
+    virtual void updateAttitude(const glm::quat& orientation) override;
 
     // Call this when updating Avatar position with a delta.  This will allow us to
     // _accurately_ measure position changes and compute the resulting velocity
@@ -197,10 +195,8 @@ public:
     void getCapsule(glm::vec3& start, glm::vec3& end, float& radius);
     float computeMass();
 
-    using SpatiallyNestable::setPosition;
+    void setPositionViaScript(const glm::vec3& position) override;
-    virtual void setPosition(const glm::vec3& position) override;
+    void setOrientationViaScript(const glm::quat& orientation) override;
-    using SpatiallyNestable::setOrientation;
-    virtual void setOrientation(const glm::quat& orientation) override;
 
     // these call through to the SpatiallyNestable versions, but they are here to expose these to javascript.
     Q_INVOKABLE virtual const QUuid getParentID() const override { return SpatiallyNestable::getParentID(); }
@@ -240,7 +236,7 @@ public:
     bool hasNewJointData() const { return _hasNewJointData; }
 
     float getBoundingRadius() const;
-    
+
     void addToScene(AvatarSharedPointer self, const render::ScenePointer& scene);
     void ensureInScene(AvatarSharedPointer self, const render::ScenePointer& scene);
     bool isInScene() const { return render::Item::isValidID(_renderItemID); }
@@ -303,7 +299,6 @@ protected:
 
     glm::vec3 getBodyRightDirection() const { return getOrientation() * IDENTITY_RIGHT; }
     glm::vec3 getBodyUpDirection() const { return getOrientation() * IDENTITY_UP; }
-    glm::quat computeRotationFromBodyToWorldUp(float proportion = 1.0f) const;
     void measureMotionDerivatives(float deltaTime);
 
     float getSkeletonHeight() const;

libraries/avatars-renderer/src/avatars-renderer/SkeletonModel.cpp

@@ -118,16 +118,16 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
     _rig.updateFromEyeParameters(eyeParams);
 }
 
-void SkeletonModel::updateAttitude() {
+void SkeletonModel::updateAttitude(const glm::quat& orientation) {
     setTranslation(_owningAvatar->getSkeletonPosition());
-    setRotation(_owningAvatar->getOrientation() * Quaternions::Y_180);
+    setRotation(orientation * Quaternions::Y_180);
     setScale(glm::vec3(1.0f, 1.0f, 1.0f) * _owningAvatar->getScale());
 }
 
 // Called by Avatar::simulate after it has set the joint states (fullUpdate true if changed),
 // but just before head has been simulated.
 void SkeletonModel::simulate(float deltaTime, bool fullUpdate) {
-    updateAttitude();
+    updateAttitude(_owningAvatar->getOrientation());
     if (fullUpdate) {
         setBlendshapeCoefficients(_owningAvatar->getHead()->getSummedBlendshapeCoefficients());
 

libraries/avatars-renderer/src/avatars-renderer/SkeletonModel.h

@@ -35,7 +35,7 @@ public:
 
     void simulate(float deltaTime, bool fullUpdate = true) override;
     void updateRig(float deltaTime, glm::mat4 parentTransform) override;
-    void updateAttitude();
+    void updateAttitude(const glm::quat& orientation);
 
     /// Returns the index of the left hand joint, or -1 if not found.
     int getLeftHandJointIndex() const { return isActive() ? getFBXGeometry().leftHandJointIndex : -1; }

libraries/avatars/src/AvatarData.cpp

@@ -91,9 +91,6 @@ AvatarData::AvatarData() :
     _targetVelocity(0.0f),
     _density(DEFAULT_AVATAR_DENSITY)
 {
-    setBodyPitch(0.0f);
-    setBodyYaw(-90.0f);
-    setBodyRoll(0.0f);
 }
 
 AvatarData::~AvatarData() {
@@ -110,23 +107,6 @@ const QUrl& AvatarData::defaultFullAvatarModelUrl() {
     return _defaultFullAvatarModelUrl;
 }
 
-// There are a number of possible strategies for this set of tools through endRender, below.
-void AvatarData::nextAttitude(glm::vec3 position, glm::quat orientation) {
-    bool success;
-    Transform trans = getTransform(success);
-    if (!success) {
-        qCWarning(avatars) << "Warning -- AvatarData::nextAttitude failed";
-        return;
-    }
-    trans.setTranslation(position);
-    trans.setRotation(orientation);
-    SpatiallyNestable::setTransform(trans, success);
-    if (!success) {
-        qCWarning(avatars) << "Warning -- AvatarData::nextAttitude failed";
-    }
-    updateAttitude();
-}
-
 void AvatarData::setTargetScale(float targetScale) {
     auto newValue = glm::clamp(targetScale, MIN_AVATAR_SCALE, MAX_AVATAR_SCALE);
     if (_targetScale != newValue) {
@@ -2100,6 +2080,7 @@ void AvatarData::fromJson(const QJsonObject& json, bool useFrameSkeleton) {
         currentBasis = std::make_shared<Transform>(Transform::fromJson(json[JSON_AVATAR_BASIS]));
     }
 
+    glm::quat orientation;
     if (json.contains(JSON_AVATAR_RELATIVE)) {
         // During playback you can either have the recording basis set to the avatar current state
         // meaning that all playback is relative to this avatars starting position, or
@@ -2111,12 +2092,14 @@ void AvatarData::fromJson(const QJsonObject& json, bool useFrameSkeleton) {
         auto relativeTransform = Transform::fromJson(json[JSON_AVATAR_RELATIVE]);
         auto worldTransform = currentBasis->worldTransform(relativeTransform);
         setPosition(worldTransform.getTranslation());
-        setOrientation(worldTransform.getRotation());
+        orientation = worldTransform.getRotation();
     } else {
         // We still set the position in the case that there is no movement.
         setPosition(currentBasis->getTranslation());
-        setOrientation(currentBasis->getRotation());
+        orientation = currentBasis->getRotation();
     }
+    setOrientation(orientation);
+    updateAttitude(orientation);
 
     // Do after avatar orientation because head look-at needs avatar orientation.
     if (json.contains(JSON_AVATAR_HEAD)) {
@@ -2234,11 +2217,11 @@ void AvatarData::setBodyRoll(float bodyRoll) {
     setOrientation(glm::quat(glm::radians(eulerAngles)));
 }
 
-void AvatarData::setPosition(const glm::vec3& position) {
+void AvatarData::setPositionViaScript(const glm::vec3& position) {
     SpatiallyNestable::setPosition(position);
 }
 
-void AvatarData::setOrientation(const glm::quat& orientation) {
+void AvatarData::setOrientationViaScript(const glm::quat& orientation) {
     SpatiallyNestable::setOrientation(orientation);
 }
 

libraries/avatars/src/AvatarData.h

@@ -351,14 +351,14 @@ public:
 class AvatarData : public QObject, public SpatiallyNestable {
     Q_OBJECT
 
-    Q_PROPERTY(glm::vec3 position READ getPosition WRITE setPosition)
+    Q_PROPERTY(glm::vec3 position READ getPosition WRITE setPositionViaScript)
     Q_PROPERTY(float scale READ getTargetScale WRITE setTargetScale)
     Q_PROPERTY(glm::vec3 handPosition READ getHandPosition WRITE setHandPosition)
     Q_PROPERTY(float bodyYaw READ getBodyYaw WRITE setBodyYaw)
     Q_PROPERTY(float bodyPitch READ getBodyPitch WRITE setBodyPitch)
     Q_PROPERTY(float bodyRoll READ getBodyRoll WRITE setBodyRoll)
 
-    Q_PROPERTY(glm::quat orientation READ getOrientation WRITE setOrientation)
+    Q_PROPERTY(glm::quat orientation READ getOrientation WRITE setOrientationViaScript)
     Q_PROPERTY(glm::quat headOrientation READ getHeadOrientation WRITE setHeadOrientation)
     Q_PROPERTY(float headPitch READ getHeadPitch WRITE setHeadPitch)
     Q_PROPERTY(float headYaw READ getHeadYaw WRITE setHeadYaw)
@@ -440,13 +440,10 @@ public:
     float getBodyRoll() const;
     void setBodyRoll(float bodyRoll);
 
-    using SpatiallyNestable::setPosition;
+    virtual void setPositionViaScript(const glm::vec3& position);
-    virtual void setPosition(const glm::vec3& position) override;
+    virtual void setOrientationViaScript(const glm::quat& orientation);
-    using SpatiallyNestable::setOrientation;
-    virtual void setOrientation(const glm::quat& orientation) override;
 
-    void nextAttitude(glm::vec3 position, glm::quat orientation); // Can be safely called at any time.
+    virtual void updateAttitude(const glm::quat& orientation) {}
-    virtual void updateAttitude() {} // Tell skeleton mesh about changes
 
     glm::quat getHeadOrientation() const {
         lazyInitHeadData();

libraries/physics/src/CharacterController.cpp

@@ -445,7 +445,7 @@ void CharacterController::handleChangedCollisionGroup() {
 
 void CharacterController::updateUpAxis(const glm::quat& rotation) {
     _currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS);
-    if (_state != State::Hover && _rigidBody) {
+    if (_rigidBody) {
         _rigidBody->setGravity(_gravity * _currentUp);
     }
 }

libraries/shared/src/shared/Camera.h

@@ -127,7 +127,7 @@ private:
     glm::mat4 _projection;
 
     // derived
-    glm::vec3 _position;
+    glm::vec3 _position { 0.0f, 0.0f, 0.0f };
     glm::quat _orientation;
     bool _isKeepLookingAt{ false };
     glm::vec3 _lookingAt;