fix motor direction when walking upside down

also maintain worldUp and remove unnecessary cruft

interface/src/Application.cpp

@@ -2314,7 +2314,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
@@ -2336,7 +2336,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

@@ -1291,7 +1291,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;
@@ -1588,7 +1588,7 @@ void MyAvatar::updateMotors() {
             // non-hovering = walking: follow camera twist about vertical but not lift
             // so we decompose camera's rotation and store the twist part in motorRotation
             glm::quat liftRotation;
-            swingTwistDecomposition(getMyHead()->getHeadOrientation(), _worldUpDirection, liftRotation, motorRotation);
+            motorRotation = getOrientation();
         }
         const float DEFAULT_MOTOR_TIMESCALE = 0.2f;
         const float INVALID_MOTOR_TIMESCALE = 1.0e6f;
@@ -1656,7 +1656,7 @@ void MyAvatar::nextAttitude(glm::vec3 position, glm::quat orientation) {
     if (!success) {
         qCWarning(interfaceapp) << "Warning -- MyAvatar::nextAttitude failed";
     }
-    updateAttitude();
+    updateAttitude(orientation);
 }
 
 void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {

interface/src/avatar/MyAvatar.h

@@ -552,7 +552,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();

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) {
-    AvatarData::setPosition(position);
-    updateAttitude();
+void Avatar::setPositionViaScript(const glm::vec3& position) {
+    setPosition(position);
+    updateAttitude(getOrientation());
 }
 
-void Avatar::setOrientation(const glm::quat& orientation) {
-    AvatarData::setOrientation(orientation);
-    updateAttitude();
+void Avatar::setOrientationViaScript(const glm::quat& orientation) {
+    setOrientation(orientation);
+    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() { _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;
-    virtual void setPosition(const glm::vec3& position) override;
-    using SpatiallyNestable::setOrientation;
-    virtual void setOrientation(const glm::quat& orientation) override;
+    void setPositionViaScript(const glm::vec3& position) override;
+    void setOrientationViaScript(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() {
@@ -2083,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
@@ -2094,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)) {
@@ -2217,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,10 +440,10 @@ public:
     float getBodyRoll() const;
     void setBodyRoll(float bodyRoll);
 
-    using SpatiallyNestable::setPosition;
-    virtual void setPosition(const glm::vec3& position) override;
-    using SpatiallyNestable::setOrientation;
-    virtual void setOrientation(const glm::quat& orientation) override;
+    virtual void setPositionViaScript(const glm::vec3& position);
+    virtual void setOrientationViaScript(const glm::quat& orientation);
+
+    virtual void updateAttitude(const glm::quat& orientation) {}
 
     glm::quat getHeadOrientation() const {
         lazyInitHeadData();

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;