avatar stands on lowest toe

interface/src/avatar/Avatar.cpp

@@ -775,7 +775,10 @@ void Avatar::setSkeletonOffset(const glm::vec3& offset) {
 }
 
 glm::vec3 Avatar::getSkeletonPosition() const { 
-    return _position + _skeletonOffset; 
+    // The avatar is rotated PI about the yAxis, so we have to correct for it 
+    // to get the skeleton offset contribution in the world-frame.
+    const glm::quat FLIP = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f));
+    return _position + getOrientation() * FLIP * _skeletonOffset; 
 }
 
 QVector<glm::quat> Avatar::getJointRotations() const {

interface/src/avatar/MyAvatar.cpp

@@ -89,6 +89,7 @@ MyAvatar::MyAvatar() :
     _billboardValid(false),
     _physicsSimulation(),
     _voxelShapeManager(),
+    _feetTouchFloor(true),
     _isLookingAtLeftEye(true)
 {
     ShapeCollider::initDispatchTable();
@@ -115,7 +116,7 @@ QByteArray MyAvatar::toByteArray() {
     if (mode == CAMERA_MODE_THIRD_PERSON || mode == CAMERA_MODE_INDEPENDENT) {
         // fake the avatar position that is sent up to the AvatarMixer
         glm::vec3 oldPosition = _position;
-        _position += _skeletonOffset;
+        _position = getSkeletonPosition();
         QByteArray array = AvatarData::toByteArray();
         // copy the correct position back
         _position = oldPosition;
@@ -155,6 +156,9 @@ void MyAvatar::update(float deltaTime) {
     }
 
     simulate(deltaTime);
+    if (_feetTouchFloor) {
+        _skeletonModel.updateStandingFoot();
+    }
 }
 
 void MyAvatar::simulate(float deltaTime) {
@@ -1034,7 +1038,14 @@ void MyAvatar::setAttachmentData(const QVector<AttachmentData>& attachmentData)
 glm::vec3 MyAvatar::getSkeletonPosition() const {
     CameraMode mode = Application::getInstance()->getCamera()->getMode();
     if (mode == CAMERA_MODE_THIRD_PERSON || mode == CAMERA_MODE_INDEPENDENT) {
-        return Avatar::getSkeletonPosition();
+        // The avatar is rotated PI about the yAxis, so we have to correct for it 
+        // to get the skeleton offset contribution in the world-frame.
+        const glm::quat FLIP = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f));
+        glm::vec3 skeletonOffset = _skeletonOffset;
+        if (_feetTouchFloor) {
+            skeletonOffset += _skeletonModel.getStandingOffset();
+        }
+        return _position + getOrientation() * FLIP * skeletonOffset;
     }
     return Avatar::getPosition();
 }

interface/src/avatar/MyAvatar.h

@@ -235,6 +235,7 @@ private:
     PhysicsSimulation _physicsSimulation;
     VoxelShapeManager _voxelShapeManager;
     
+    bool _feetTouchFloor;
     bool _isLookingAtLeftEye;
 
     RecorderPointer _recorder;

interface/src/avatar/SkeletonModel.cpp

@@ -22,13 +22,22 @@
 #include "SkeletonModel.h"
 #include "SkeletonRagdoll.h"
 
+enum StandingFootState {
+    LEFT_FOOT,
+    RIGHT_FOOT,
+    NO_FOOT
+};
+
 SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent) : 
     Model(parent),
     _owningAvatar(owningAvatar),
     _boundingShape(),
     _boundingShapeLocalOffset(0.0f),
     _ragdoll(NULL),
-    _defaultEyeModelPosition(glm::vec3(0.f, 0.f, 0.f)) {
+    _defaultEyeModelPosition(glm::vec3(0.f, 0.f, 0.f)),
+    _standingFoot(NO_FOOT),
+    _standingOffset(0.0f),
+    _clampedFootPosition(0.0f) {
 }
 
 SkeletonModel::~SkeletonModel() {
@@ -607,6 +616,62 @@ void SkeletonModel::updateVisibleJointStates() {
     }
 }
 
+/// \return offset of hips after foot animation
+void SkeletonModel::updateStandingFoot() {
+    glm::vec3 offset(0.0f);
+    int leftFootIndex = _geometry->getFBXGeometry().leftToeJointIndex;
+    int rightFootIndex = _geometry->getFBXGeometry().rightToeJointIndex;
+
+    if (leftFootIndex != -1 && rightFootIndex != -1) {
+        glm::vec3 leftPosition, rightPosition;
+        getJointPosition(leftFootIndex, leftPosition);
+        getJointPosition(rightFootIndex, rightPosition);
+
+        int lowestFoot = (leftPosition.y < rightPosition.y) ? LEFT_FOOT : RIGHT_FOOT;
+        const float MIN_STEP_HEIGHT_THRESHOLD = 0.05f;
+        bool oneFoot = fabsf(leftPosition.y - rightPosition.y) > MIN_STEP_HEIGHT_THRESHOLD;
+        int currentFoot = oneFoot ? lowestFoot : _standingFoot;
+
+        if (_standingFoot == NO_FOOT) {
+            currentFoot = lowestFoot;
+        }
+        if (currentFoot != _standingFoot) {
+            if (_standingFoot == NO_FOOT) {
+                // pick the lowest foot
+                glm::vec3 lowestPosition = (currentFoot == LEFT_FOOT) ? leftPosition : rightPosition;
+                // we ignore zero length positions which can happen for a few frames until skeleton is fully loaded
+                if (glm::length(lowestPosition) > 0.0f) {
+                    _standingFoot = currentFoot;
+                    _clampedFootPosition = lowestPosition;
+                }
+            } else {
+                // swap feet
+                _standingFoot = currentFoot;
+                glm::vec3 nextPosition = leftPosition;
+                glm::vec3 prevPosition = rightPosition;
+                if (_standingFoot == RIGHT_FOOT) {
+                    nextPosition = rightPosition;
+                    prevPosition = leftPosition;
+                }
+                glm::vec3 oldOffset = _clampedFootPosition - prevPosition;
+                _clampedFootPosition = oldOffset + nextPosition;
+                offset = _clampedFootPosition - nextPosition;
+            }
+        } else {
+            glm::vec3 nextPosition = (_standingFoot == LEFT_FOOT) ? leftPosition : rightPosition;
+            offset = _clampedFootPosition - nextPosition;
+        }
+
+        // clamp the offset to not exceed some max distance
+        const float MAX_STEP_OFFSET = 1.0f;
+        float stepDistance = glm::length(offset);
+        if (stepDistance > MAX_STEP_OFFSET) {
+            offset *= (MAX_STEP_OFFSET / stepDistance);
+        }
+    }
+    _standingOffset = offset;
+}
+
 SkeletonRagdoll* SkeletonModel::buildRagdoll() {
     if (!_ragdoll) {
         _ragdoll = new SkeletonRagdoll(this);

interface/src/avatar/SkeletonModel.h

@@ -101,6 +101,10 @@ public:
     /// \return whether or not the head was found.
     glm::vec3 getDefaultEyeModelPosition() const;
 
+    /// skeleton offset caused by moving feet
+    void updateStandingFoot();
+    const glm::vec3& getStandingOffset() const { return _standingOffset; }
+
     virtual void updateVisibleJointStates();
 
     SkeletonRagdoll* buildRagdoll();
@@ -154,6 +158,9 @@ private:
     SkeletonRagdoll* _ragdoll;
 
     glm::vec3 _defaultEyeModelPosition;
+    int _standingFoot;
+    glm::vec3 _standingOffset;
+    glm::vec3 _clampedFootPosition;
 };
 
 #endif // hifi_SkeletonModel_h