From 867fc3ccbf5946f7c96e1941c0339a7cce252e31 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 11 Oct 2016 16:37:29 -0700
Subject: [PATCH] prevent avatar from recentering below ground

---
 interface/src/avatar/MyAvatar.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 19346e51db..3f906cfff7 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -247,6 +247,15 @@ void MyAvatar::centerBody() {
     auto worldBodyPos = extractTranslation(worldBodyMatrix);
     auto worldBodyRot = glm::normalize(glm::quat_cast(worldBodyMatrix));
 
+    if (_characterController.getState() == CharacterController::State::Ground) {
+        // the avatar's physical aspect thinks it is standing on something
+        // therefore need to be careful to not "center" the body below the floor
+        float downStep = glm::dot(worldBodyPos - getPosition(), _worldUpDirection);
+        if (downStep < -0.5f * _characterController.getCapsuleHalfHeight() + _characterController.getCapsuleRadius()) {
+            worldBodyPos -= downStep * _worldUpDirection;
+        }
+    }
+
     // this will become our new position.
     setPosition(worldBodyPos);
     setOrientation(worldBodyRot);