From bc9621baabdce000adb77181cd22c45b13fd670c Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Mon, 25 Jan 2016 21:54:35 -0800
Subject: [PATCH] CharacterController: Improve ground support detection

In addition to the existing line probe ground check, also check if the bottom sphere of the capsule is contact with any geometry before going into hover mode.
This should prevent going into the fly animation when standing or walking on collision shapes with small gaps between elements.
---
 libraries/physics/src/CharacterController.cpp | 29 +++++++++++++++++--
 libraries/physics/src/CharacterController.h   |  2 ++
 2 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index 432b89ba31..65432e2f65 100644
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -60,6 +60,7 @@ CharacterController::CharacterController() {
     _followTime = 0.0f;
     _followLinearDisplacement = btVector3(0, 0, 0);
     _followAngularDisplacement = btQuaternion::getIdentity();
+    _hasSupport = false;
 
     _pendingFlags = PENDING_FLAG_UPDATE_SHAPE;
 
@@ -106,6 +107,28 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
     }
 }
 
+bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) const {
+    int numManifolds = collisionWorld->getDispatcher()->getNumManifolds();
+    for (int i = 0; i < numManifolds; i++) {
+        btPersistentManifold* contactManifold = collisionWorld->getDispatcher()->getManifoldByIndexInternal(i);
+        const btCollisionObject* obA = static_cast<const btCollisionObject*>(contactManifold->getBody0());
+        const btCollisionObject* obB = static_cast<const btCollisionObject*>(contactManifold->getBody1());
+        if (obA == _rigidBody || obB == _rigidBody) {
+            int numContacts = contactManifold->getNumContacts();
+            for (int j = 0; j < numContacts; j++) {
+                btManifoldPoint& pt = contactManifold->getContactPoint(j);
+
+                // check to see if contact point is touching the bottom sphere of the capsule.
+                float contactPointY = (obA == _rigidBody) ? pt.m_localPointA.getY() : pt.m_localPointB.getY();
+                if (contactPointY < -_halfHeight) {
+                    return true;
+                }
+            }
+        }
+    }
+    return false;
+}
+
 void CharacterController::preStep(btCollisionWorld* collisionWorld) {
     // trace a ray straight down to see if we're standing on the ground
     const btTransform& xform = _rigidBody->getWorldTransform();
@@ -125,6 +148,8 @@ void CharacterController::preStep(btCollisionWorld* collisionWorld) {
     if (rayCallback.hasHit()) {
         _floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius;
     }
+
+    _hasSupport = checkForSupport(collisionWorld);
 }
 
 void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
@@ -248,7 +273,7 @@ void CharacterController::jump() {
 
 bool CharacterController::onGround() const {
     const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius;
-    return _floorDistance < FLOOR_PROXIMITY_THRESHOLD;
+    return _floorDistance < FLOOR_PROXIMITY_THRESHOLD || _hasSupport;
 }
 
 void CharacterController::setHovering(bool hover) {
@@ -400,7 +425,7 @@ void CharacterController::preSimulation() {
             if (_floorDistance < JUMP_PROXIMITY_THRESHOLD) {
                 _isJumping = false;
             }
-        } else {
+        } else if (!_hasSupport) {
             _floorDistance = FLT_MAX;
             setHovering(true);
         }
diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h
index a32425e3e7..224632ea6b 100644
--- a/libraries/physics/src/CharacterController.h
+++ b/libraries/physics/src/CharacterController.h
@@ -86,6 +86,7 @@ public:
 
 protected:
     void updateUpAxis(const glm::quat& rotation);
+    bool checkForSupport(btCollisionWorld* collisionWorld) const;
 
 protected:
     btVector3 _currentUp;
@@ -104,6 +105,7 @@ protected:
     btScalar _radius;
 
     btScalar _floorDistance;
+    bool _hasSupport;
 
     btScalar _gravity;