From 74c163d04797ba9ecfa969c1b1402b95d919a960 Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Thu, 25 May 2017 10:25:29 -0700
Subject: [PATCH 01/20] snapshot of safeLanding. Works as long as entities
 don't intersect.

---
 interface/src/avatar/MyAvatar.cpp | 88 +++++++++++++++++++++++++++++++
 interface/src/avatar/MyAvatar.h   |  1 +
 2 files changed, 89 insertions(+)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 9996df2afc..bc8b4c33c3 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2180,6 +2180,94 @@ void MyAvatar::goToLocation(const glm::vec3& newPosition,
     emit transformChanged();
 }
 
+bool MyAvatar::safeLanding(const glm::vec3& position) {
+    // Considers all collision hull or non-collisionless primitive intersections on a vertical line through the point, regardless of "face direction".
+    // There to be a need for a "landing" if the closest above and the closest below are either:
+    // a) less than the avatar capsule height apart, or
+    // b) on the same entity (thus enclosing the point).
+    // If no landing is required, we go to that point directly and return false;
+    // When a landing is required, we the entity that has the nearest "above" intersection, and find the highest intersection on the same entity
+    // (which may be that same "nearest above intersection"). That highest intersection is the candidate landing point.
+    // We then recurse with that, and when it bottoms out, the last candidate point is where we place the avatar's feet, and return true;
+    // FIXME test: MyAvatar.safeLanding({x: 100, y: 100, z: 100})
+    //qDebug() << "FIXME avatar position:" << getPosition() << "halfHeight:" << _characterController.getCapsuleHalfHeight() << "height:" << _skeletonModel->getBoundingCapsuleHeight() << "offset:" << _characterController.getCapsuleLocalOffset();
+    auto direct = [&](QString label) {
+        qDebug() << "Already safe" << label << position;
+        goToLocation(position);
+        return false;
+    };
+    auto halfHeight = _characterController.getCapsuleHalfHeight();
+    if (halfHeight == 0) {
+        return direct("zero height avatar");
+    }   
+    auto entityTree = DependencyManager::get<EntityTreeRenderer>()->getTree();
+    if (!entityTree) {
+        return direct("no entity tree");
+    }
+    // FIXME: capsule has an offset from position!
+
+    QVector<EntityItemID> include{};
+    QVector<EntityItemID> ignore{};
+    OctreeElementPointer element;
+    EntityItemPointer intersectedEntity = NULL;
+    bool intersects;
+    float distance;
+    BoxFace face;
+    glm::vec3 surfaceNormal;
+    const bool visibleOnly = false;
+    const bool collidableOnly = true;
+    const bool precisionPicking = true;
+    const auto lockType = Octree::Lock; // Should we refactor to take a lock just once?
+    bool* accurateResult = NULL;
+    // FIXME DRY this, and get rid of the extra vector arithmetic.
+    intersects = entityTree->findRayIntersection(position, Vectors::UNIT_Y,
+        include, ignore, visibleOnly, collidableOnly, precisionPicking,
+        element, distance, face, surfaceNormal,
+        (void**)&intersectedEntity, lockType, accurateResult);
+    if (!intersects || !intersectedEntity) {
+        return direct("no above");
+    }
+    glm::vec3 upperIntersection = position + (Vectors::UNIT_Y * distance);
+    auto upperId = intersectedEntity->getEntityItemID();
+    auto upperY = surfaceNormal.y;
+
+    intersects = entityTree->findRayIntersection(position, Vectors::UNIT_NEG_Y,
+        include, ignore, visibleOnly, collidableOnly, precisionPicking,
+        element, distance, face, surfaceNormal,
+        (void**)&intersectedEntity, lockType, accurateResult);
+    if (!intersects || !intersectedEntity) {
+        return direct("no below");
+    }
+    glm::vec3 lowerIntersection = position + (Vectors::UNIT_NEG_Y * distance);
+    auto lowerId = intersectedEntity->getEntityItemID();
+    auto lowerY = surfaceNormal.y;
+ 
+    auto delta = glm::distance(upperIntersection, lowerIntersection);
+    //qDebug() << "FIXME position:" << position << "upper:" << upperId << upperIntersection << "lower:" << lowerId << lowerIntersection << "distance:" << delta << "height:" << (2 * _characterController.getCapsuleHalfHeight());
+    if ((upperId != lowerId) && (delta > (2 * halfHeight))) {
+        qDebug() << "FIXME upper:" << upperId << upperIntersection << " n:" << upperY << "lower:" << lowerId << lowerIntersection << " n:" << lowerY  << "delta:" << delta << "halfHeight:" << halfHeight;
+        return direct("enough room");
+    }
+    qDebug() << "FIXME need to compute safe landing for" << position;
+    const float big = (float)TREE_SCALE;
+    const auto skyHigh = Vectors::UNIT_Y * big;
+    include.push_back(upperId);
+    auto fromAbove = position + skyHigh;
+    intersects = entityTree->findRayIntersection(fromAbove, Vectors::UNIT_NEG_Y,
+        include, ignore, visibleOnly, collidableOnly, precisionPicking,
+        element, distance, face, surfaceNormal,
+        (void**)&intersectedEntity, lockType, accurateResult);
+    if (!intersects || !intersectedEntity) {
+        return direct("no landing");
+    }
+    // Bottom of capsule is at intersection, so capsule center is above that, which also avoids looping on the same intersection.
+    auto newFloor = fromAbove + (Vectors::UNIT_NEG_Y * distance);
+    auto newTarget = newFloor + (Vectors::UNIT_Y * halfHeight);
+    qDebug() << "FIXME newFloor:" << newFloor << "newTarget:" << newTarget;
+    safeLanding(newTarget);
+    return true;
+}
+
 void MyAvatar::updateMotionBehaviorFromMenu() {
 
     if (QThread::currentThread() != thread()) {
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index fde350a43e..b41483d761 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -506,6 +506,7 @@ public slots:
                       bool hasOrientation = false, const glm::quat& newOrientation = glm::quat(),
                       bool shouldFaceLocation = false);
     void goToLocation(const QVariant& properties);
+    bool safeLanding(const glm::vec3& position);
 
     void restrictScaleFromDomainSettings(const QJsonObject& domainSettingsObject);
     void clearScaleRestriction();

From 9d73d0c931d0db4397ca198e7cdcbccc99722259 Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Thu, 25 May 2017 12:09:22 -0700
Subject: [PATCH 02/20] works with nested enclosing entities

---
 interface/src/avatar/MyAvatar.cpp | 60 ++++++++++++++++++++++++++-----
 1 file changed, 52 insertions(+), 8 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index bc8b4c33c3..2a70a3dab3 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2199,12 +2199,16 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
     auto halfHeight = _characterController.getCapsuleHalfHeight();
     if (halfHeight == 0) {
         return direct("zero height avatar");
-    }   
+    }
     auto entityTree = DependencyManager::get<EntityTreeRenderer>()->getTree();
     if (!entityTree) {
         return direct("no entity tree");
     }
     // FIXME: capsule has an offset from position!
+    // We could repeat this whole test for each of the four corners of our bounding box, in case the surface is uneven.
+    // However:
+    // 1) This is only meant to cover the most important cases, and even the four corners won't handle random spikes.
+    // 2) My feeling is that this code is already at the limit of what can realistically be reviewed and maintained.
 
     QVector<EntityItemID> include{};
     QVector<EntityItemID> ignore{};
@@ -2225,8 +2229,12 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
         element, distance, face, surfaceNormal,
         (void**)&intersectedEntity, lockType, accurateResult);
     if (!intersects || !intersectedEntity) {
+        // We currently believe that physics will reliably push us out if our feet are embedded,
+        // as long as our capsule center is out and there's room above us. Here we have those
+        // conditions, so no need to check our feet below.
         return direct("no above");
     }
+    // FIXME: use _worldUpDirection and negative instead of UNIT_Y?  If we use normalMumble.y, will need to also dot against _worldUpDirection.
     glm::vec3 upperIntersection = position + (Vectors::UNIT_Y * distance);
     auto upperId = intersectedEntity->getEntityItemID();
     auto upperY = surfaceNormal.y;
@@ -2236,19 +2244,55 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
         element, distance, face, surfaceNormal,
         (void**)&intersectedEntity, lockType, accurateResult);
     if (!intersects || !intersectedEntity) {
+        // Our head may be embedded, but our center is out and there's room below. See corresponding comment above.
         return direct("no below");
     }
     glm::vec3 lowerIntersection = position + (Vectors::UNIT_NEG_Y * distance);
     auto lowerId = intersectedEntity->getEntityItemID();
     auto lowerY = surfaceNormal.y;
  
-    auto delta = glm::distance(upperIntersection, lowerIntersection);
-    //qDebug() << "FIXME position:" << position << "upper:" << upperId << upperIntersection << "lower:" << lowerId << lowerIntersection << "distance:" << delta << "height:" << (2 * _characterController.getCapsuleHalfHeight());
-    if ((upperId != lowerId) && (delta > (2 * halfHeight))) {
-        qDebug() << "FIXME upper:" << upperId << upperIntersection << " n:" << upperY << "lower:" << lowerId << lowerIntersection << " n:" << lowerY  << "delta:" << delta << "halfHeight:" << halfHeight;
-        return direct("enough room");
+    if ((upperY < 0) && (lowerY > 0)) {
+        // The surface above us is the bottom of something, and the surface below us it the top of something.
+        // I.e., we are in a clearing between two objects.
+        auto delta = glm::distance(upperIntersection, lowerIntersection);
+        //qDebug() << "FIXME position:" << position << "upper:" << upperId << upperIntersection << "lower:" << lowerId << lowerIntersection << "distance:" << delta << "height:" << (2 * _characterController.getCapsuleHalfHeight());
+        if (delta > (2 * halfHeight)) {
+            // There is room for us to fit in that clearing. If there wasn't, physics would oscilate us between the objects above and below.
+            if (upperId != lowerId) { // An optimization over what follows: the simplest case of not being inside an entity.
+                // We're going to iterate upwards through successive roofIntersections, testing to see if we're contained within the top surface of some entity.
+                // There will be one of two outcomes:
+                // a) We're not contained, so we have enough room and our position is good.
+                // b) We are contained, so we'll bail out of this but try again at a position above the containing entity.
+                auto entityAbove = upperId;
+                auto roofIntersection = upperIntersection;
+                while (1) {
+                    ignore.push_back(entityAbove);
+                    intersects = entityTree->findRayIntersection(roofIntersection, Vectors::UNIT_Y,
+                        include, ignore, visibleOnly, collidableOnly, precisionPicking,
+                        element, distance, face, surfaceNormal,
+                        (void**)&intersectedEntity, lockType, accurateResult);
+                    if (!intersects || !intersectedEntity) {
+                        // We're not inside an entity, and from the nested tests, we have room between what is above and below. So position is good!
+                        qDebug() << "FIXME upper:" << upperId << upperIntersection << " n:" << upperY << " lower:" << lowerId << lowerIntersection << " n:" << lowerY << " delta:" << delta << " halfHeight:" << halfHeight;
+                        return direct("enough room");
+                    }
+                    roofIntersection = roofIntersection + (Vectors::UNIT_Y * distance);
+                    if (surfaceNormal.y > 0) {
+                        // This new intersection is the top surface of an entity that we have not yet seen, which means we're contained within it.
+                        // We could break here and recurse from the top of the original ceiling, but since we've already done the work to find the top
+                        // of the enclosing entity, let's put our feet at roofIntersection and start over.
+                        safeLanding(roofIntersection + (Vectors::UNIT_Y * halfHeight));
+                        return true;
+                    }
+                    // We found a new bottom surface, which we're not interested in.
+                    // But there could still be a top surface above us for an entity we haven't seen, so keep looking upward.
+                    entityAbove = intersectedEntity->getEntityItemID();
+                }
+                ignore.clear(); // We didn't find anything, but in what happens below, don't ignore the entities we've encountered.
+            }
+        }
     }
-    qDebug() << "FIXME need to compute safe landing for" << position;
+    qDebug() << "FIXME need to compute safe landing for" << position << " based on " << upperIntersection << "@" << upperId << " and " << lowerIntersection << "@" << lowerId;
     const float big = (float)TREE_SCALE;
     const auto skyHigh = Vectors::UNIT_Y * big;
     include.push_back(upperId);
@@ -2263,7 +2307,7 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
     // Bottom of capsule is at intersection, so capsule center is above that, which also avoids looping on the same intersection.
     auto newFloor = fromAbove + (Vectors::UNIT_NEG_Y * distance);
     auto newTarget = newFloor + (Vectors::UNIT_Y * halfHeight);
-    qDebug() << "FIXME newFloor:" << newFloor << "newTarget:" << newTarget;
+    qDebug() << "FIXME newFloor:" << newFloor << " newTarget:" << newTarget;
     safeLanding(newTarget);
     return true;
 }

From ac8cb6341928c1d5e728889b46ddb8af35f7ef97 Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Thu, 25 May 2017 14:34:24 -0700
Subject: [PATCH 03/20] clean and DRY

---
 interface/src/avatar/MyAvatar.cpp | 152 ++++++++++++++----------------
 1 file changed, 70 insertions(+), 82 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 2a70a3dab3..808d2730a9 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2181,19 +2181,24 @@ void MyAvatar::goToLocation(const glm::vec3& newPosition,
 }
 
 bool MyAvatar::safeLanding(const glm::vec3& position) {
-    // Considers all collision hull or non-collisionless primitive intersections on a vertical line through the point, regardless of "face direction".
-    // There to be a need for a "landing" if the closest above and the closest below are either:
-    // a) less than the avatar capsule height apart, or
-    // b) on the same entity (thus enclosing the point).
+    // Considers all collision hull or non-collisionless primitive intersections on a vertical line through the point.
+    // There needs to be a "landing" if:
+    // a) the closest above and the closest below are less than the avatar capsule height apart, or
+    // b) the above point is the top surface of an entity, indicating that we are inside it.
     // If no landing is required, we go to that point directly and return false;
-    // When a landing is required, we the entity that has the nearest "above" intersection, and find the highest intersection on the same entity
+    // When a landing is required by a, we find the highest intersection on that closest-above entity
     // (which may be that same "nearest above intersection"). That highest intersection is the candidate landing point.
-    // We then recurse with that, and when it bottoms out, the last candidate point is where we place the avatar's feet, and return true;
-    // FIXME test: MyAvatar.safeLanding({x: 100, y: 100, z: 100})
-    //qDebug() << "FIXME avatar position:" << getPosition() << "halfHeight:" << _characterController.getCapsuleHalfHeight() << "height:" << _skeletonModel->getBoundingCapsuleHeight() << "offset:" << _characterController.getCapsuleLocalOffset();
-    auto direct = [&](QString label) {
-        qDebug() << "Already safe" << label << position;
-        goToLocation(position);
+    // For b, use that top surface point.
+    // We then place our feet there, recurse with new capsule center point, and return true;
+
+    const auto offset = _characterController.getCapsuleLocalOffset();
+    const auto capsuleCenter = position + offset;
+    // We could repeat this whole test for each of the four corners of our bounding box, in case the surface is uneven. However:
+    // 1) This is only meant to cover the most important cases, and even the four corners won't handle random spikes in the surfaces or avatar.
+    // 2) My feeling is that this code is already at the limit of what can realistically be reviewed and maintained.
+    auto direct = [&](const char* label) {  // position is good to go, or at least, we cannot do better
+        //qDebug() << "Already safe" << label << position;
+        goToLocation(position);  // I.e., capsuleCenter - offset
         return false;
     };
     auto halfHeight = _characterController.getCapsuleHalfHeight();
@@ -2204,58 +2209,58 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
     if (!entityTree) {
         return direct("no entity tree");
     }
-    // FIXME: capsule has an offset from position!
-    // We could repeat this whole test for each of the four corners of our bounding box, in case the surface is uneven.
-    // However:
-    // 1) This is only meant to cover the most important cases, and even the four corners won't handle random spikes.
-    // 2) My feeling is that this code is already at the limit of what can realistically be reviewed and maintained.
 
+    glm::vec3 upperIntersection, upperNormal, lowerIntersection, lowerNormal;
+    EntityItemID upperId, lowerId;
+    const auto up = _worldUpDirection, down = -up;
     QVector<EntityItemID> include{};
     QVector<EntityItemID> ignore{};
-    OctreeElementPointer element;
-    EntityItemPointer intersectedEntity = NULL;
-    bool intersects;
-    float distance;
-    BoxFace face;
-    glm::vec3 surfaceNormal;
-    const bool visibleOnly = false;
-    const bool collidableOnly = true;
-    const bool precisionPicking = true;
-    const auto lockType = Octree::Lock; // Should we refactor to take a lock just once?
-    bool* accurateResult = NULL;
-    // FIXME DRY this, and get rid of the extra vector arithmetic.
-    intersects = entityTree->findRayIntersection(position, Vectors::UNIT_Y,
-        include, ignore, visibleOnly, collidableOnly, precisionPicking,
-        element, distance, face, surfaceNormal,
-        (void**)&intersectedEntity, lockType, accurateResult);
-    if (!intersects || !intersectedEntity) {
+    auto recurse = [&] {  // Place bottom of capsule at the upperIntersection, and check again based on the capsule center.
+        safeLanding(upperIntersection + (up * halfHeight) - offset);
+        return true;
+    };
+    auto isUp = [&](const glm::vec3& normal) { return glm::dot(normal, up) > 0.0f; }; // true iff normal points up
+    auto isDown = [&](const glm::vec3& normal) { return glm::dot(normal, up) < 0.0f; };
+    auto findIntersection = [&](const glm::vec3& startPointIn, const glm::vec3& directionIn, glm::vec3& intersectionOut, EntityItemID& entityIdOut, glm::vec3& normalOut) {
+        OctreeElementPointer element;
+        EntityItemPointer intersectedEntity = NULL;
+        float distance;
+        BoxFace face;
+        const bool visibleOnly = false;
+        const bool collidableOnly = true;
+        const bool precisionPicking = true;
+        const auto lockType = Octree::Lock; // Should we refactor to take a lock just once?
+        bool* accurateResult = NULL;
+
+        bool intersects = entityTree->findRayIntersection(startPointIn, directionIn, include, ignore, visibleOnly, collidableOnly, precisionPicking,
+            element, distance, face, normalOut, (void**)&intersectedEntity, lockType, accurateResult);
+        if (!intersects || !intersectedEntity) {
+             return false;
+        }
+        intersectionOut = startPointIn + (directionIn * distance);
+        entityIdOut = intersectedEntity->getEntityItemID();
+        return true;
+    };
+
+    // The Algorithm, in four parts:
+
+    if (!findIntersection(capsuleCenter, up, upperIntersection, upperId, upperNormal)) {
         // We currently believe that physics will reliably push us out if our feet are embedded,
         // as long as our capsule center is out and there's room above us. Here we have those
         // conditions, so no need to check our feet below.
-        return direct("no above");
+        return direct("nothing above");
     }
-    // FIXME: use _worldUpDirection and negative instead of UNIT_Y?  If we use normalMumble.y, will need to also dot against _worldUpDirection.
-    glm::vec3 upperIntersection = position + (Vectors::UNIT_Y * distance);
-    auto upperId = intersectedEntity->getEntityItemID();
-    auto upperY = surfaceNormal.y;
 
-    intersects = entityTree->findRayIntersection(position, Vectors::UNIT_NEG_Y,
-        include, ignore, visibleOnly, collidableOnly, precisionPicking,
-        element, distance, face, surfaceNormal,
-        (void**)&intersectedEntity, lockType, accurateResult);
-    if (!intersects || !intersectedEntity) {
+    if (!findIntersection(capsuleCenter, down, lowerIntersection, lowerId, lowerNormal)) {
         // Our head may be embedded, but our center is out and there's room below. See corresponding comment above.
-        return direct("no below");
+        return direct("nothing below");
     }
-    glm::vec3 lowerIntersection = position + (Vectors::UNIT_NEG_Y * distance);
-    auto lowerId = intersectedEntity->getEntityItemID();
-    auto lowerY = surfaceNormal.y;
  
-    if ((upperY < 0) && (lowerY > 0)) {
+    // See if we have room between entities above and below, but that we are not contained.
+    if (isDown(upperNormal) && isUp(lowerNormal)) {
         // The surface above us is the bottom of something, and the surface below us it the top of something.
         // I.e., we are in a clearing between two objects.
         auto delta = glm::distance(upperIntersection, lowerIntersection);
-        //qDebug() << "FIXME position:" << position << "upper:" << upperId << upperIntersection << "lower:" << lowerId << lowerIntersection << "distance:" << delta << "height:" << (2 * _characterController.getCapsuleHalfHeight());
         if (delta > (2 * halfHeight)) {
             // There is room for us to fit in that clearing. If there wasn't, physics would oscilate us between the objects above and below.
             if (upperId != lowerId) { // An optimization over what follows: the simplest case of not being inside an entity.
@@ -2263,53 +2268,36 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
                 // There will be one of two outcomes:
                 // a) We're not contained, so we have enough room and our position is good.
                 // b) We are contained, so we'll bail out of this but try again at a position above the containing entity.
-                auto entityAbove = upperId;
-                auto roofIntersection = upperIntersection;
-                while (1) {
-                    ignore.push_back(entityAbove);
-                    intersects = entityTree->findRayIntersection(roofIntersection, Vectors::UNIT_Y,
-                        include, ignore, visibleOnly, collidableOnly, precisionPicking,
-                        element, distance, face, surfaceNormal,
-                        (void**)&intersectedEntity, lockType, accurateResult);
-                    if (!intersects || !intersectedEntity) {
+                for (;;) {
+                    ignore.push_back(upperId);
+                    if (!findIntersection(upperIntersection, up, upperIntersection, upperId, upperNormal)) {
                         // We're not inside an entity, and from the nested tests, we have room between what is above and below. So position is good!
-                        qDebug() << "FIXME upper:" << upperId << upperIntersection << " n:" << upperY << " lower:" << lowerId << lowerIntersection << " n:" << lowerY << " delta:" << delta << " halfHeight:" << halfHeight;
+                        //qDebug() << "FIXME upper:" << upperId << upperIntersection << " n:" << upperNormal.y << " lower:" << lowerId << lowerIntersection << " n:" << lowerNormal.y << " delta:" << delta << " halfHeight:" << halfHeight;
                         return direct("enough room");
                     }
-                    roofIntersection = roofIntersection + (Vectors::UNIT_Y * distance);
-                    if (surfaceNormal.y > 0) {
+                    if (isUp(upperNormal)) {
                         // This new intersection is the top surface of an entity that we have not yet seen, which means we're contained within it.
                         // We could break here and recurse from the top of the original ceiling, but since we've already done the work to find the top
-                        // of the enclosing entity, let's put our feet at roofIntersection and start over.
-                        safeLanding(roofIntersection + (Vectors::UNIT_Y * halfHeight));
-                        return true;
+                        // of the enclosing entity, let's put our feet at upperIntersection and start over.
+                        return recurse();
                     }
                     // We found a new bottom surface, which we're not interested in.
                     // But there could still be a top surface above us for an entity we haven't seen, so keep looking upward.
-                    entityAbove = intersectedEntity->getEntityItemID();
                 }
-                ignore.clear(); // We didn't find anything, but in what happens below, don't ignore the entities we've encountered.
             }
         }
     }
-    qDebug() << "FIXME need to compute safe landing for" << position << " based on " << upperIntersection << "@" << upperId << " and " << lowerIntersection << "@" << lowerId;
+
+    //qDebug() << "FIXME need to compute safe landing for" << capsuleCenter << " based on " << upperIntersection << "@" << upperId << " and " << lowerIntersection << "@" << lowerId;
     const float big = (float)TREE_SCALE;
-    const auto skyHigh = Vectors::UNIT_Y * big;
-    include.push_back(upperId);
-    auto fromAbove = position + skyHigh;
-    intersects = entityTree->findRayIntersection(fromAbove, Vectors::UNIT_NEG_Y,
-        include, ignore, visibleOnly, collidableOnly, precisionPicking,
-        element, distance, face, surfaceNormal,
-        (void**)&intersectedEntity, lockType, accurateResult);
-    if (!intersects || !intersectedEntity) {
-        return direct("no landing");
+    const auto skyHigh = up * big;
+    auto fromAbove = capsuleCenter + skyHigh;
+    include.push_back(upperId); // We're looking for the intersection from above onto this entity.
+    if (!findIntersection(fromAbove, down, upperIntersection, upperId, upperNormal)) {
+        return direct("Unable to find a landing");
     }
-    // Bottom of capsule is at intersection, so capsule center is above that, which also avoids looping on the same intersection.
-    auto newFloor = fromAbove + (Vectors::UNIT_NEG_Y * distance);
-    auto newTarget = newFloor + (Vectors::UNIT_Y * halfHeight);
-    qDebug() << "FIXME newFloor:" << newFloor << " newTarget:" << newTarget;
-    safeLanding(newTarget);
-    return true;
+    // Our arbitrary rule is to always go up. There's no need to look down or sideways for a "closer" safe candidate.
+    return recurse();
 }
 
 void MyAvatar::updateMotionBehaviorFromMenu() {

From 905b5261d1a57bc1051b76d93735db487cabf58e Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Thu, 25 May 2017 16:32:44 -0700
Subject: [PATCH 04/20] If collisions are enabled, turn them off in
 safeLanding, and restore.

---
 interface/src/avatar/MyAvatar.cpp | 13 ++++++++++++-
 interface/src/avatar/MyAvatar.h   |  1 +
 2 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 808d2730a9..d72b96828a 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2180,6 +2180,10 @@ void MyAvatar::goToLocation(const glm::vec3& newPosition,
     emit transformChanged();
 }
 
+void MyAvatar::goToLocationAndEnableCollisions(const glm::vec3& position) { // See use case in safeLanding.
+    goToLocation(position);
+    QMetaObject::invokeMethod(this, "setCollisionsEnabled", Qt::QueuedConnection, Q_ARG(bool, true));
+}
 bool MyAvatar::safeLanding(const glm::vec3& position) {
     // Considers all collision hull or non-collisionless primitive intersections on a vertical line through the point.
     // There needs to be a "landing" if:
@@ -2198,7 +2202,14 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
     // 2) My feeling is that this code is already at the limit of what can realistically be reviewed and maintained.
     auto direct = [&](const char* label) {  // position is good to go, or at least, we cannot do better
         //qDebug() << "Already safe" << label << position;
-        goToLocation(position);  // I.e., capsuleCenter - offset
+        if (!getCollisionsEnabled()) {
+            goToLocation(position);
+        } else { // If you try to go while stuck, physics will keep you stuck.
+            setCollisionsEnabled(false);
+            // Don't goToLocation just yet. Yield so that physics can act on the above.
+            QMetaObject::invokeMethod(this, "goToLocationAndEnableCollisions", Qt::QueuedConnection, // The equivalent of javascript nextTick
+                Q_ARG(glm::vec3, position)); // I.e., capsuleCenter - offset
+         }
         return false;
     };
     auto halfHeight = _characterController.getCapsuleHalfHeight();
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index b41483d761..00f2cdf6bd 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -506,6 +506,7 @@ public slots:
                       bool hasOrientation = false, const glm::quat& newOrientation = glm::quat(),
                       bool shouldFaceLocation = false);
     void goToLocation(const QVariant& properties);
+    void goToLocationAndEnableCollisions(const glm::vec3& newPosition);
     bool safeLanding(const glm::vec3& position);
 
     void restrictScaleFromDomainSettings(const QJsonObject& domainSettingsObject);

From 4be31f1b1830c0de0acc521c8b1016f098a0d8e6 Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Fri, 26 May 2017 15:31:59 -0700
Subject: [PATCH 05/20] thread safety

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

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 43fffe7e0b..ee83975142 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2233,6 +2233,11 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
     // (which may be that same "nearest above intersection"). That highest intersection is the candidate landing point.
     // For b, use that top surface point.
     // We then place our feet there, recurse with new capsule center point, and return true;
+    if (QThread::currentThread() != thread()) {
+        bool result;
+        QMetaObject::invokeMethod(this, "safeLanding", Qt::BlockingQueuedConnection, Q_RETURN_ARG(bool, result), Q_ARG(const glm::vec3&, position));
+        return result;
+    }
 
     const auto offset = _characterController.getCapsuleLocalOffset();
     const auto capsuleCenter = position + offset;

From f694691efbc30a146fedf654cc7f266578dc53e0 Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Fri, 26 May 2017 16:15:18 -0700
Subject: [PATCH 06/20] add fixme comments and make isUp/isDown ordinary
 methods.

---
 interface/src/avatar/MyAvatar.cpp | 11 +++++++----
 interface/src/avatar/MyAvatar.h   |  4 ++++
 2 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index ee83975142..422053dfa5 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2220,6 +2220,7 @@ void MyAvatar::goToLocation(const glm::vec3& newPosition,
 }
 
 void MyAvatar::goToLocationAndEnableCollisions(const glm::vec3& position) { // See use case in safeLanding.
+    // FIXME: Doesn't work 100% of time. Need to figure out what isn't happening fast enough. E.g., don't goToLocation until confirmed removed from physics?
     goToLocation(position);
     QMetaObject::invokeMethod(this, "setCollisionsEnabled", Qt::QueuedConnection, Q_ARG(bool, true));
 }
@@ -2232,14 +2233,17 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
     // When a landing is required by a, we find the highest intersection on that closest-above entity
     // (which may be that same "nearest above intersection"). That highest intersection is the candidate landing point.
     // For b, use that top surface point.
-    // We then place our feet there, recurse with new capsule center point, and return true;
+    // We then place our feet there, recurse with new capsule center point, and return true.
+
+    // I'm not sure the true/false return is useful, and it does make things more complicated. Might get rid of it.
+    // E.g., Q_RETURN_ARG, and direct() could be a normal method.
     if (QThread::currentThread() != thread()) {
         bool result;
         QMetaObject::invokeMethod(this, "safeLanding", Qt::BlockingQueuedConnection, Q_RETURN_ARG(bool, result), Q_ARG(const glm::vec3&, position));
         return result;
     }
 
-    const auto offset = _characterController.getCapsuleLocalOffset();
+    const auto offset = _characterController.getCapsuleLocalOffset(); // FIXME: correct space.
     const auto capsuleCenter = position + offset;
     // We could repeat this whole test for each of the four corners of our bounding box, in case the surface is uneven. However:
     // 1) This is only meant to cover the most important cases, and even the four corners won't handle random spikes in the surfaces or avatar.
@@ -2274,14 +2278,13 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
         safeLanding(upperIntersection + (up * halfHeight) - offset);
         return true;
     };
-    auto isUp = [&](const glm::vec3& normal) { return glm::dot(normal, up) > 0.0f; }; // true iff normal points up
-    auto isDown = [&](const glm::vec3& normal) { return glm::dot(normal, up) < 0.0f; };
     auto findIntersection = [&](const glm::vec3& startPointIn, const glm::vec3& directionIn, glm::vec3& intersectionOut, EntityItemID& entityIdOut, glm::vec3& normalOut) {
         OctreeElementPointer element;
         EntityItemPointer intersectedEntity = NULL;
         float distance;
         BoxFace face;
         const bool visibleOnly = false;
+        // FIXME: this doesn't do what we want here. findRayIntersection always works on mesh, skipping entirely based on collidable. What we really want is to use the collision hull! See CharacterGhostObject::rayTest?
         const bool collidableOnly = true;
         const bool precisionPicking = true;
         const auto lockType = Octree::Lock; // Should we refactor to take a lock just once?
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index a458e36f0d..08c3d9c67c 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -509,6 +509,10 @@ public:
     // results are in HMD frame
     glm::mat4 deriveBodyFromHMDSensor() const;
 
+    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();

From 92aeadd70f4477077d89066840c8b6b7ad5f876e Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Fri, 26 May 2017 16:34:08 -0700
Subject: [PATCH 07/20] rotation of local capsule offset

---
 interface/src/avatar/MyAvatar.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 422053dfa5..1d9009e129 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -294,6 +294,7 @@ void MyAvatar::simulateAttachments(float deltaTime) {
 QByteArray MyAvatar::toByteArrayStateful(AvatarDataDetail dataDetail) {
     CameraMode mode = qApp->getCamera().getMode();
     _globalPosition = getPosition();
+    // This might not be right! Isn't the capsule local offset in avatar space? -HR 5/26/17
     _globalBoundingBoxDimensions.x = _characterController.getCapsuleRadius();
     _globalBoundingBoxDimensions.y = _characterController.getCapsuleHalfHeight();
     _globalBoundingBoxDimensions.z = _characterController.getCapsuleRadius();
@@ -448,6 +449,7 @@ void MyAvatar::update(float deltaTime) {
     setAudioAverageLoudness(audio->getAudioAverageInputLoudness());
 
     glm::vec3 halfBoundingBoxDimensions(_characterController.getCapsuleRadius(), _characterController.getCapsuleHalfHeight(), _characterController.getCapsuleRadius());
+    // This might not be right! Isn't the capsule local offset in avatar space? -HR 5/26/17
     halfBoundingBoxDimensions += _characterController.getCapsuleLocalOffset();
     QMetaObject::invokeMethod(audio.data(), "setAvatarBoundingBoxParameters",
         Q_ARG(glm::vec3, (getPosition() - halfBoundingBoxDimensions)),
@@ -2230,7 +2232,7 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
     // a) the closest above and the closest below are less than the avatar capsule height apart, or
     // b) the above point is the top surface of an entity, indicating that we are inside it.
     // If no landing is required, we go to that point directly and return false;
-    // When a landing is required by a, we find the highest intersection on that closest-above entity
+    // When a landing is required by a, we find the highest intersection on that closest-agbove entity
     // (which may be that same "nearest above intersection"). That highest intersection is the candidate landing point.
     // For b, use that top surface point.
     // We then place our feet there, recurse with new capsule center point, and return true.
@@ -2243,7 +2245,7 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
         return result;
     }
 
-    const auto offset = _characterController.getCapsuleLocalOffset(); // FIXME: correct space.
+    const auto offset = getOrientation() *_characterController.getCapsuleLocalOffset();
     const auto capsuleCenter = position + offset;
     // We could repeat this whole test for each of the four corners of our bounding box, in case the surface is uneven. However:
     // 1) This is only meant to cover the most important cases, and even the four corners won't handle random spikes in the surfaces or avatar.

From 07ea2e271bc1c0ca0e23b9768fe6b75a8b43fd78 Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Tue, 30 May 2017 14:59:19 -0700
Subject: [PATCH 08/20] hook into location changes

---
 interface/src/avatar/MyAvatar.cpp | 35 ++++++++++++++++++++++---------
 interface/src/avatar/MyAvatar.h   |  5 +++--
 2 files changed, 28 insertions(+), 12 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 1d9009e129..e10327b7c7 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -435,6 +435,11 @@ void MyAvatar::update(float deltaTime) {
         // so we update now. It's ok if it updates again in the normal way.
         updateSensorToWorldMatrix();
         emit positionGoneTo();
+        _physicsSafetyPending = true;
+    }
+    if (_physicsSafetyPending && qApp->isPhysicsEnabled()) {
+        _physicsSafetyPending = false;
+        safeLanding(_goToPosition); // no-op if already safe
     }
 
     Head* head = getHead();
@@ -2244,22 +2249,30 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
         QMetaObject::invokeMethod(this, "safeLanding", Qt::BlockingQueuedConnection, Q_RETURN_ARG(bool, result), Q_ARG(const glm::vec3&, position));
         return result;
     }
+    glm::vec3 better;
+    if (requiresSafeLanding(position, better)) {
+        qDebug() << "rechecking" << position << " => " << better << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
+        if (!getCollisionsEnabled()) {
+            goToLocation(better);  // recurses
+        } else { // If you try to go while stuck, physics will keep you stuck.
+            setCollisionsEnabled(false);
+            // Don't goToLocation just yet. Yield so that physics can act on the above.
+            QMetaObject::invokeMethod(this, "goToLocationAndEnableCollisions", Qt::QueuedConnection, // The equivalent of javascript nextTick
+                Q_ARG(glm::vec3, better)); // I.e., capsuleCenter - offset
+        }
+        return true;
+    }
+    return false;
+}
 
+bool MyAvatar::requiresSafeLanding(const glm::vec3& position, glm::vec3& betterPosition) {
     const auto offset = getOrientation() *_characterController.getCapsuleLocalOffset();
     const auto capsuleCenter = position + offset;
     // We could repeat this whole test for each of the four corners of our bounding box, in case the surface is uneven. However:
     // 1) This is only meant to cover the most important cases, and even the four corners won't handle random spikes in the surfaces or avatar.
     // 2) My feeling is that this code is already at the limit of what can realistically be reviewed and maintained.
     auto direct = [&](const char* label) {  // position is good to go, or at least, we cannot do better
-        //qDebug() << "Already safe" << label << position;
-        if (!getCollisionsEnabled()) {
-            goToLocation(position);
-        } else { // If you try to go while stuck, physics will keep you stuck.
-            setCollisionsEnabled(false);
-            // Don't goToLocation just yet. Yield so that physics can act on the above.
-            QMetaObject::invokeMethod(this, "goToLocationAndEnableCollisions", Qt::QueuedConnection, // The equivalent of javascript nextTick
-                Q_ARG(glm::vec3, position)); // I.e., capsuleCenter - offset
-         }
+        qDebug() << "Already safe" << label << position << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
         return false;
     };
     auto halfHeight = _characterController.getCapsuleHalfHeight();
@@ -2277,7 +2290,7 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
     QVector<EntityItemID> include{};
     QVector<EntityItemID> ignore{};
     auto recurse = [&] {  // Place bottom of capsule at the upperIntersection, and check again based on the capsule center.
-        safeLanding(upperIntersection + (up * halfHeight) - offset);
+        betterPosition = upperIntersection + (up * halfHeight) - offset;
         return true;
     };
     auto findIntersection = [&](const glm::vec3& startPointIn, const glm::vec3& directionIn, glm::vec3& intersectionOut, EntityItemID& entityIdOut, glm::vec3& normalOut) {
@@ -2287,6 +2300,7 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
         BoxFace face;
         const bool visibleOnly = false;
         // FIXME: this doesn't do what we want here. findRayIntersection always works on mesh, skipping entirely based on collidable. What we really want is to use the collision hull! See CharacterGhostObject::rayTest?
+        // See https://highfidelity.fogbugz.com/f/cases/5003/findRayIntersection-has-option-to-use-collidableOnly-but-doesn-t-actually-use-colliders
         const bool collidableOnly = true;
         const bool precisionPicking = true;
         const auto lockType = Octree::Lock; // Should we refactor to take a lock just once?
@@ -2378,6 +2392,7 @@ void MyAvatar::updateMotionBehaviorFromMenu() {
     } else {
         _motionBehaviors &= ~AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED;
     }
+    qDebug() << "FIXME updateMotionBehaviorFromMenu collisions:" << menu->isOptionChecked(MenuOption::EnableAvatarCollisions) << "physics:" << qApp->isPhysicsEnabled();
     setCollisionsEnabled(menu->isOptionChecked(MenuOption::EnableAvatarCollisions));
 }
 
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 08c3d9c67c..efab177a04 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -571,7 +571,7 @@ private:
 
     glm::vec3 getWorldBodyPosition() const;
     glm::quat getWorldBodyOrientation() const;
-
+    bool requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& positionOut);
 
     virtual QByteArray toByteArrayStateful(AvatarDataDetail dataDetail) override;
 
@@ -722,7 +722,8 @@ private:
     };
     FollowHelper _follow;
 
-    bool _goToPending;
+    bool _goToPending { false };
+    bool _physicsSafetyPending { false };
     glm::vec3 _goToPosition;
     glm::quat _goToOrientation;
 

From 1e1dd3a633aca6aa4657832c8eafdbcea9d27865 Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Wed, 31 May 2017 17:10:37 -0700
Subject: [PATCH 09/20] checkpoint with working detector

---
 interface/src/avatar/MyAvatar.cpp             | 71 ++++++++++---------
 libraries/physics/src/CharacterController.cpp | 18 +++--
 libraries/physics/src/CharacterController.h   |  2 +
 3 files changed, 53 insertions(+), 38 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index e10327b7c7..e8552df7e9 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -435,9 +435,9 @@ void MyAvatar::update(float deltaTime) {
         // so we update now. It's ok if it updates again in the normal way.
         updateSensorToWorldMatrix();
         emit positionGoneTo();
-        _physicsSafetyPending = true;
+        _physicsSafetyPending = getCollisionsEnabled();  // Run safety tests as soon as we can after goToLocation, or clear if we're not colliding.
     }
-    if (_physicsSafetyPending && qApp->isPhysicsEnabled()) {
+    if (_physicsSafetyPending && qApp->isPhysicsEnabled() && _characterController.isEnabledAndReady()) { // fix only when needed and ready
         _physicsSafetyPending = false;
         safeLanding(_goToPosition); // no-op if already safe
     }
@@ -1555,6 +1555,11 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
 
     if (_characterController.isEnabledAndReady()) {
         setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity());
+        if (_characterController.isStuck()) {
+            _physicsSafetyPending = true;
+            _goToPosition = getPosition();
+            qDebug() << "FIXME setting safety test at:" << _goToPosition;
+        }
     } else {
         setVelocity(getVelocity() + _characterController.getFollowVelocity());
     }
@@ -2242,55 +2247,53 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
     // For b, use that top surface point.
     // We then place our feet there, recurse with new capsule center point, and return true.
 
-    // I'm not sure the true/false return is useful, and it does make things more complicated. Might get rid of it.
-    // E.g., Q_RETURN_ARG, and direct() could be a normal method.
     if (QThread::currentThread() != thread()) {
         bool result;
         QMetaObject::invokeMethod(this, "safeLanding", Qt::BlockingQueuedConnection, Q_RETURN_ARG(bool, result), Q_ARG(const glm::vec3&, position));
         return result;
     }
     glm::vec3 better;
-    if (requiresSafeLanding(position, better)) {
-        qDebug() << "rechecking" << position << " => " << better << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
-        if (!getCollisionsEnabled()) {
-            goToLocation(better);  // recurses
-        } else { // If you try to go while stuck, physics will keep you stuck.
-            setCollisionsEnabled(false);
-            // Don't goToLocation just yet. Yield so that physics can act on the above.
-            QMetaObject::invokeMethod(this, "goToLocationAndEnableCollisions", Qt::QueuedConnection, // The equivalent of javascript nextTick
-                Q_ARG(glm::vec3, better)); // I.e., capsuleCenter - offset
-        }
-        return true;
+    if (!requiresSafeLanding(position, better)) {
+        return false;
     }
-    return false;
+    qDebug() << "rechecking" << position << " => " << better << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
+    if (!getCollisionsEnabled()) {
+        goToLocation(better);  // recurses
+     } else { // If you try to go while stuck, physics will keep you stuck.
+        setCollisionsEnabled(false);
+        // Don't goToLocation just yet. Yield so that physics can act on the above.
+        QMetaObject::invokeMethod(this, "goToLocationAndEnableCollisions", Qt::QueuedConnection, // The equivalent of javascript nextTick
+            Q_ARG(glm::vec3, better)); // I.e., capsuleCenter - offset
+     }
+     return true;
 }
 
-bool MyAvatar::requiresSafeLanding(const glm::vec3& position, glm::vec3& betterPosition) {
-    const auto offset = getOrientation() *_characterController.getCapsuleLocalOffset();
-    const auto capsuleCenter = position + offset;
+// If position is not reliably safe from being stuck by physics, answer true and place a candidate better position in betterPositionOut.
+bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& betterPositionOut) {
     // We could repeat this whole test for each of the four corners of our bounding box, in case the surface is uneven. However:
     // 1) This is only meant to cover the most important cases, and even the four corners won't handle random spikes in the surfaces or avatar.
     // 2) My feeling is that this code is already at the limit of what can realistically be reviewed and maintained.
-    auto direct = [&](const char* label) {  // position is good to go, or at least, we cannot do better
-        qDebug() << "Already safe" << label << position << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
+    auto ok = [&](const char* label) {  // position is good to go, or at least, we cannot do better
+        qDebug() << "Already safe" << label << positionIn << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
         return false;
     };
     auto halfHeight = _characterController.getCapsuleHalfHeight();
     if (halfHeight == 0) {
-        return direct("zero height avatar");
+        return ok("zero height avatar");
     }
     auto entityTree = DependencyManager::get<EntityTreeRenderer>()->getTree();
     if (!entityTree) {
-        return direct("no entity tree");
+        return ok("no entity tree");
     }
 
+    const auto offset = getOrientation() *_characterController.getCapsuleLocalOffset();
+    const auto capsuleCenter = positionIn + offset;
+    const auto up = _worldUpDirection, down = -up;
     glm::vec3 upperIntersection, upperNormal, lowerIntersection, lowerNormal;
     EntityItemID upperId, lowerId;
-    const auto up = _worldUpDirection, down = -up;
-    QVector<EntityItemID> include{};
-    QVector<EntityItemID> ignore{};
-    auto recurse = [&] {  // Place bottom of capsule at the upperIntersection, and check again based on the capsule center.
-        betterPosition = upperIntersection + (up * halfHeight) - offset;
+    QVector<EntityItemID> include{}, ignore{};
+    auto mustMove = [&] {  // Place bottom of capsule at the upperIntersection, and check again based on the capsule center.
+        betterPositionOut = upperIntersection + (up * halfHeight) - offset;
         return true;
     };
     auto findIntersection = [&](const glm::vec3& startPointIn, const glm::vec3& directionIn, glm::vec3& intersectionOut, EntityItemID& entityIdOut, glm::vec3& normalOut) {
@@ -2322,12 +2325,12 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& position, glm::vec3& betterP
         // We currently believe that physics will reliably push us out if our feet are embedded,
         // as long as our capsule center is out and there's room above us. Here we have those
         // conditions, so no need to check our feet below.
-        return direct("nothing above");
+        return ok("nothing above");
     }
 
     if (!findIntersection(capsuleCenter, down, lowerIntersection, lowerId, lowerNormal)) {
         // Our head may be embedded, but our center is out and there's room below. See corresponding comment above.
-        return direct("nothing below");
+        return ok("nothing below");
     }
  
     // See if we have room between entities above and below, but that we are not contained.
@@ -2347,13 +2350,13 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& position, glm::vec3& betterP
                     if (!findIntersection(upperIntersection, up, upperIntersection, upperId, upperNormal)) {
                         // We're not inside an entity, and from the nested tests, we have room between what is above and below. So position is good!
                         //qDebug() << "FIXME upper:" << upperId << upperIntersection << " n:" << upperNormal.y << " lower:" << lowerId << lowerIntersection << " n:" << lowerNormal.y << " delta:" << delta << " halfHeight:" << halfHeight;
-                        return direct("enough room");
+                        return ok("enough room");
                     }
                     if (isUp(upperNormal)) {
                         // This new intersection is the top surface of an entity that we have not yet seen, which means we're contained within it.
                         // We could break here and recurse from the top of the original ceiling, but since we've already done the work to find the top
                         // of the enclosing entity, let's put our feet at upperIntersection and start over.
-                        return recurse();
+                        return mustMove();
                     }
                     // We found a new bottom surface, which we're not interested in.
                     // But there could still be a top surface above us for an entity we haven't seen, so keep looking upward.
@@ -2368,10 +2371,10 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& position, glm::vec3& betterP
     auto fromAbove = capsuleCenter + skyHigh;
     include.push_back(upperId); // We're looking for the intersection from above onto this entity.
     if (!findIntersection(fromAbove, down, upperIntersection, upperId, upperNormal)) {
-        return direct("Unable to find a landing");
+        return ok("Unable to find a landing");
     }
     // Our arbitrary rule is to always go up. There's no need to look down or sideways for a "closer" safe candidate.
-    return recurse();
+    return mustMove();
 }
 
 void MyAvatar::updateMotionBehaviorFromMenu() {
diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index ee240a6aac..88eff1a0fc 100755
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -152,6 +152,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
     btDispatcher* dispatcher = collisionWorld->getDispatcher();
     int numManifolds = dispatcher->getNumManifolds();
     bool hasFloor = false;
+    bool isStuck = false;
 
     btTransform rotation = _rigidBody->getWorldTransform();
     rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); // clear translation part
@@ -169,10 +170,18 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
                 btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame
                 btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character
                 btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp);
+                // If there's non-trivial penetration with a big impulse for several steps, we're probably stuck.
+                // Note it here in the controller, and let MyAvatar figure out what to do about it.
+                const float STUCK_PENETRATION = -0.1f; // always negative into the object.
+                const float STUCK_IMPULSE = 500.0f;
+                const int STUCK_LIFETIME = 50;
+                if ((contact.getDistance() < -STUCK_PENETRATION) && (contact.getAppliedImpulse() > STUCK_IMPULSE) && (contact.getLifeTime() > STUCK_LIFETIME)) {
+                    isStuck = true; // latch on
+                }
                 if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > _minFloorNormalDotUp) {
                     hasFloor = true;
-                    if (!pushing) {
-                        // we're not pushing against anything so we can early exit
+                    if (!pushing && isStuck) {
+                        // we're not pushing against anything and we're stuck so we can early exit
                         // (all we need to know is that there is a floor)
                         break;
                     }
@@ -182,7 +191,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
                     if (!_stepUpEnabled || hitHeight > _maxStepHeight) {
                         // this manifold is invalidated by point that is too high
                         stepContactIndex = -1;
-                        break;
+                        qDebug() << "FIXME breaking early";  break;
                     } else if (hitHeight > highestStep && normal.dot(_targetVelocity) < 0.0f ) {
                         highestStep = hitHeight;
                         stepContactIndex = j;
@@ -198,12 +207,13 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
                 _stepHeight = highestStep;
                 _stepPoint = rotation * pointOnCharacter; // rotate into world-frame
             }
-            if (hasFloor && !(pushing && _stepUpEnabled)) {
+            if (hasFloor && isStuck && !(pushing && _stepUpEnabled)) {
                 // early exit since all we need to know is that we're on a floor
                 break;
             }
         }
     }
+    _isStuck = isStuck;
     return hasFloor;
 }
 
diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h
index 6790495ff8..bf84d318d4 100644
--- a/libraries/physics/src/CharacterController.h
+++ b/libraries/physics/src/CharacterController.h
@@ -110,6 +110,7 @@ public:
     void setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale);
 
     bool isEnabledAndReady() const { return _dynamicsWorld; }
+    bool isStuck() const { return _isStuck; }
 
     void setCollisionless(bool collisionless);
     int16_t computeCollisionGroup() const;
@@ -192,6 +193,7 @@ protected:
 
     State _state;
     bool _isPushingUp;
+    bool _isStuck { false };
 
     btDynamicsWorld* _dynamicsWorld { nullptr };
     btRigidBody* _rigidBody { nullptr };

From 1068a4a9bd2248823a14c3c35a5935659c86e26f Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Thu, 1 Jun 2017 13:21:24 -0700
Subject: [PATCH 10/20] remove the optimization for upperId == lowerId. Doesn't
 work in zaru tunnels.

---
 interface/src/avatar/MyAvatar.cpp | 41 +++++++++++++++----------------
 1 file changed, 20 insertions(+), 21 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index e8552df7e9..bbac271450 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2340,32 +2340,31 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
         auto delta = glm::distance(upperIntersection, lowerIntersection);
         if (delta > (2 * halfHeight)) {
             // There is room for us to fit in that clearing. If there wasn't, physics would oscilate us between the objects above and below.
-            if (upperId != lowerId) { // An optimization over what follows: the simplest case of not being inside an entity.
-                // We're going to iterate upwards through successive roofIntersections, testing to see if we're contained within the top surface of some entity.
-                // There will be one of two outcomes:
-                // a) We're not contained, so we have enough room and our position is good.
-                // b) We are contained, so we'll bail out of this but try again at a position above the containing entity.
-                for (;;) {
-                    ignore.push_back(upperId);
-                    if (!findIntersection(upperIntersection, up, upperIntersection, upperId, upperNormal)) {
-                        // We're not inside an entity, and from the nested tests, we have room between what is above and below. So position is good!
-                        //qDebug() << "FIXME upper:" << upperId << upperIntersection << " n:" << upperNormal.y << " lower:" << lowerId << lowerIntersection << " n:" << lowerNormal.y << " delta:" << delta << " halfHeight:" << halfHeight;
-                        return ok("enough room");
-                    }
-                    if (isUp(upperNormal)) {
-                        // This new intersection is the top surface of an entity that we have not yet seen, which means we're contained within it.
-                        // We could break here and recurse from the top of the original ceiling, but since we've already done the work to find the top
-                        // of the enclosing entity, let's put our feet at upperIntersection and start over.
-                        return mustMove();
-                    }
-                    // We found a new bottom surface, which we're not interested in.
-                    // But there could still be a top surface above us for an entity we haven't seen, so keep looking upward.
+            // We're now going to iterate upwards through successive upperIntersections, testing to see if we're contained within the top surface of some entity.
+            // There will be one of two outcomes:
+            // a) We're not contained, so we have enough room and our position is good.
+            // b) We are contained, so we'll bail out of this but try again at a position above the containing entity.
+            for (;;) {
+                ignore.push_back(upperId);
+                if (!findIntersection(upperIntersection, up, upperIntersection, upperId, upperNormal)) {
+                    // We're not inside an entity, and from the nested tests, we have room between what is above and below. So position is good!
+                    //qDebug() << "FIXME upper:" << upperId << upperIntersection << " n:" << upperNormal.y << " lower:" << lowerId << lowerIntersection << " n:" << lowerNormal.y << " delta:" << delta << " halfHeight:" << halfHeight;
+                    return ok("enough room");
                 }
+                if (isUp(upperNormal)) {
+                    // This new intersection is the top surface of an entity that we have not yet seen, which means we're contained within it.
+                    // We could break here and recurse from the top of the original ceiling, but since we've already done the work to find the top
+                    // of the enclosing entity, let's put our feet at upperIntersection and start over.
+                    qDebug() << "FIXME inside above:" << upperId << " below:" << lowerId;
+                    return mustMove();
+                }
+                // We found a new bottom surface, which we're not interested in.
+                // But there could still be a top surface above us for an entity we haven't seen, so keep looking upward.
             }
         }
     }
 
-    //qDebug() << "FIXME need to compute safe landing for" << capsuleCenter << " based on " << upperIntersection << "@" << upperId << " and " << lowerIntersection << "@" << lowerId;
+    qDebug() << "FIXME need to compute safe landing for" << capsuleCenter << " based on " << (isDown(upperNormal) ? "down " : "up ") << upperIntersection << "@" << upperId << " and " << (isUp(lowerNormal) ? "up " : "down ") << lowerIntersection << "@" << lowerId;
     const float big = (float)TREE_SCALE;
     const auto skyHigh = up * big;
     auto fromAbove = capsuleCenter + skyHigh;

From 6b2e4c5abca650a627d53462aabe6c04f030a869 Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Thu, 1 Jun 2017 17:08:22 -0700
Subject: [PATCH 11/20] zaru stuff

---
 interface/src/avatar/MyAvatar.cpp             | 14 +++++++++-----
 interface/src/avatar/MyAvatar.h               |  4 ++--
 libraries/physics/src/CharacterController.cpp |  8 +++++---
 3 files changed, 16 insertions(+), 10 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index bbac271450..d9b5309a77 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -439,7 +439,7 @@ void MyAvatar::update(float deltaTime) {
     }
     if (_physicsSafetyPending && qApp->isPhysicsEnabled() && _characterController.isEnabledAndReady()) { // fix only when needed and ready
         _physicsSafetyPending = false;
-        safeLanding(_goToPosition); // no-op if already safe
+        safeLanding(_goToPosition, _characterController.isStuck()); // no-op if already safe
     }
 
     Head* head = getHead();
@@ -2236,7 +2236,7 @@ void MyAvatar::goToLocationAndEnableCollisions(const glm::vec3& position) { // S
     goToLocation(position);
     QMetaObject::invokeMethod(this, "setCollisionsEnabled", Qt::QueuedConnection, Q_ARG(bool, true));
 }
-bool MyAvatar::safeLanding(const glm::vec3& position) {
+bool MyAvatar::safeLanding(const glm::vec3& position, bool force) {
     // Considers all collision hull or non-collisionless primitive intersections on a vertical line through the point.
     // There needs to be a "landing" if:
     // a) the closest above and the closest below are less than the avatar capsule height apart, or
@@ -2253,7 +2253,7 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
         return result;
     }
     glm::vec3 better;
-    if (!requiresSafeLanding(position, better)) {
+    if (!requiresSafeLanding(position, better, force)) {
         return false;
     }
     qDebug() << "rechecking" << position << " => " << better << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
@@ -2269,7 +2269,7 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
 }
 
 // If position is not reliably safe from being stuck by physics, answer true and place a candidate better position in betterPositionOut.
-bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& betterPositionOut) {
+bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& betterPositionOut, bool force) {
     // We could repeat this whole test for each of the four corners of our bounding box, in case the surface is uneven. However:
     // 1) This is only meant to cover the most important cases, and even the four corners won't handle random spikes in the surfaces or avatar.
     // 2) My feeling is that this code is already at the limit of what can realistically be reviewed and maintained.
@@ -2324,7 +2324,11 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
     if (!findIntersection(capsuleCenter, up, upperIntersection, upperId, upperNormal)) {
         // We currently believe that physics will reliably push us out if our feet are embedded,
         // as long as our capsule center is out and there's room above us. Here we have those
-        // conditions, so no need to check our feet below.
+        // conditions, so no need to check our feet below, unless forced.
+        if (force) {
+            upperIntersection = capsuleCenter;
+            return mustMove();
+        }
         return ok("nothing above");
     }
 
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index efab177a04..d9a2c03c38 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -523,7 +523,7 @@ public slots:
                       bool shouldFaceLocation = false);
     void goToLocation(const QVariant& properties);
     void goToLocationAndEnableCollisions(const glm::vec3& newPosition);
-    bool safeLanding(const glm::vec3& position);
+    bool safeLanding(const glm::vec3& position, bool force = false);
 
     void restrictScaleFromDomainSettings(const QJsonObject& domainSettingsObject);
     void clearScaleRestriction();
@@ -571,7 +571,7 @@ private:
 
     glm::vec3 getWorldBodyPosition() const;
     glm::quat getWorldBodyOrientation() const;
-    bool requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& positionOut);
+    bool requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& positionOut, bool force = false);
 
     virtual QByteArray toByteArrayStateful(AvatarDataDetail dataDetail) override;
 
diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index 88eff1a0fc..06c211e2f3 100755
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -172,10 +172,12 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
                 btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp);
                 // If there's non-trivial penetration with a big impulse for several steps, we're probably stuck.
                 // Note it here in the controller, and let MyAvatar figure out what to do about it.
-                const float STUCK_PENETRATION = -0.1f; // always negative into the object.
+                const float STUCK_PENETRATION = -0.05f; // always negative into the object.
                 const float STUCK_IMPULSE = 500.0f;
-                const int STUCK_LIFETIME = 50;
-                if ((contact.getDistance() < -STUCK_PENETRATION) && (contact.getAppliedImpulse() > STUCK_IMPULSE) && (contact.getLifeTime() > STUCK_LIFETIME)) {
+                const int STUCK_LIFETIME = 3;
+                //if (contact.getDistance() < STUCK_PENETRATION) qDebug() << "FIXME checking contact:" << contact.getDistance() << " impulse:" << contact.getAppliedImpulse() << " lifetime:" << contact.getLifeTime();
+                if ((contact.getDistance() < STUCK_PENETRATION) && (contact.getAppliedImpulse() > STUCK_IMPULSE) && (contact.getLifeTime() > STUCK_LIFETIME)) {
+                    qDebug() << "FIXME stuck contact:" << contact.getDistance() << " impulse:" << contact.getAppliedImpulse() << " lifetime:" << contact.getLifeTime();
                     isStuck = true; // latch on
                 }
                 if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > _minFloorNormalDotUp) {

From 02c46080f7fb3e624523c5f72caeca53f78c600a Mon Sep 17 00:00:00 2001
From: Howard Stearns <howard@highfidelity.io>
Date: Sat, 3 Jun 2017 15:43:14 -0700
Subject: [PATCH 12/20] we don't need to force, but we do need some extra room
 for confined spaces

---
 interface/src/avatar/MyAvatar.cpp | 28 +++++++++++++---------------
 interface/src/avatar/MyAvatar.h   |  4 ++--
 2 files changed, 15 insertions(+), 17 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 5a3c00b1ec..3c7f1e7d8d 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -439,7 +439,7 @@ void MyAvatar::update(float deltaTime) {
     }
     if (_physicsSafetyPending && qApp->isPhysicsEnabled() && _characterController.isEnabledAndReady()) { // fix only when needed and ready
         _physicsSafetyPending = false;
-        safeLanding(_goToPosition, _characterController.isStuck()); // no-op if already safe
+        safeLanding(_goToPosition); // no-op if already safe
     }
 
     Head* head = getHead();
@@ -1555,11 +1555,11 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
 
     if (_characterController.isEnabledAndReady()) {
         setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity());
-        if (_characterController.isStuck()) {
+        /*FIXME if (_characterController.isStuck()) {
             _physicsSafetyPending = true;
             _goToPosition = getPosition();
             qDebug() << "FIXME setting safety test at:" << _goToPosition;
-        }
+        }*/
     } else {
         setVelocity(getVelocity() + _characterController.getFollowVelocity());
     }
@@ -2236,7 +2236,7 @@ void MyAvatar::goToLocationAndEnableCollisions(const glm::vec3& position) { // S
     goToLocation(position);
     QMetaObject::invokeMethod(this, "setCollisionsEnabled", Qt::QueuedConnection, Q_ARG(bool, true));
 }
-bool MyAvatar::safeLanding(const glm::vec3& position, bool force) {
+bool MyAvatar::safeLanding(const glm::vec3& position) {
     // Considers all collision hull or non-collisionless primitive intersections on a vertical line through the point.
     // There needs to be a "landing" if:
     // a) the closest above and the closest below are less than the avatar capsule height apart, or
@@ -2253,7 +2253,7 @@ bool MyAvatar::safeLanding(const glm::vec3& position, bool force) {
         return result;
     }
     glm::vec3 better;
-    if (!requiresSafeLanding(position, better, force)) {
+    if (!requiresSafeLanding(position, better)) {
         return false;
     }
     qDebug() << "rechecking" << position << " => " << better << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
@@ -2269,7 +2269,7 @@ bool MyAvatar::safeLanding(const glm::vec3& position, bool force) {
 }
 
 // If position is not reliably safe from being stuck by physics, answer true and place a candidate better position in betterPositionOut.
-bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& betterPositionOut, bool force) {
+bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& betterPositionOut) {
     // We could repeat this whole test for each of the four corners of our bounding box, in case the surface is uneven. However:
     // 1) This is only meant to cover the most important cases, and even the four corners won't handle random spikes in the surfaces or avatar.
     // 2) My feeling is that this code is already at the limit of what can realistically be reviewed and maintained.
@@ -2302,7 +2302,8 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
         float distance;
         BoxFace face;
         const bool visibleOnly = false;
-        // FIXME: this doesn't do what we want here. findRayIntersection always works on mesh, skipping entirely based on collidable. What we really want is to use the collision hull! See CharacterGhostObject::rayTest?
+        // This isn't quite what we really want here. findRayIntersection always works on mesh, skipping entirely based on collidable.
+        // What we really want is to use the collision hull!
         // See https://highfidelity.fogbugz.com/f/cases/5003/findRayIntersection-has-option-to-use-collidableOnly-but-doesn-t-actually-use-colliders
         const bool collidableOnly = true;
         const bool precisionPicking = true;
@@ -2324,11 +2325,7 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
     if (!findIntersection(capsuleCenter, up, upperIntersection, upperId, upperNormal)) {
         // We currently believe that physics will reliably push us out if our feet are embedded,
         // as long as our capsule center is out and there's room above us. Here we have those
-        // conditions, so no need to check our feet below, unless forced.
-        if (force) {
-            upperIntersection = capsuleCenter;
-            return mustMove();
-        }
+        // conditions, so no need to check our feet below.
         return ok("nothing above");
     }
 
@@ -2342,7 +2339,8 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
         // The surface above us is the bottom of something, and the surface below us it the top of something.
         // I.e., we are in a clearing between two objects.
         auto delta = glm::distance(upperIntersection, lowerIntersection);
-        if (delta > (2 * halfHeight)) {
+        const float halfHeightFactor = 2.5f; // Until case 5003 is fixed (and maybe after?), we need a fudge factor. Also account for content modelers not being precise.
+        if (delta > (halfHeightFactor * halfHeight)) {
             // There is room for us to fit in that clearing. If there wasn't, physics would oscilate us between the objects above and below.
             // We're now going to iterate upwards through successive upperIntersections, testing to see if we're contained within the top surface of some entity.
             // There will be one of two outcomes:
@@ -2368,11 +2366,11 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
         }
     }
 
-    qDebug() << "FIXME need to compute safe landing for" << capsuleCenter << " based on " << (isDown(upperNormal) ? "down " : "up ") << upperIntersection << "@" << upperId << " and " << (isUp(lowerNormal) ? "up " : "down ") << lowerIntersection << "@" << lowerId;
+    include.push_back(upperId); // We're now looking for the intersection from above onto this entity.
     const float big = (float)TREE_SCALE;
     const auto skyHigh = up * big;
     auto fromAbove = capsuleCenter + skyHigh;
-    include.push_back(upperId); // We're looking for the intersection from above onto this entity.
+    qDebug() << "FIXME need to compute safe landing for" << capsuleCenter << " based on " << (isDown(upperNormal) ? "down " : "up ") << upperIntersection << "@" << upperId << " and " << (isUp(lowerNormal) ? "up " : "down ") << lowerIntersection << "@" << lowerId;
     if (!findIntersection(fromAbove, down, upperIntersection, upperId, upperNormal)) {
         return ok("Unable to find a landing");
     }
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index d9a2c03c38..efab177a04 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -523,7 +523,7 @@ public slots:
                       bool shouldFaceLocation = false);
     void goToLocation(const QVariant& properties);
     void goToLocationAndEnableCollisions(const glm::vec3& newPosition);
-    bool safeLanding(const glm::vec3& position, bool force = false);
+    bool safeLanding(const glm::vec3& position);
 
     void restrictScaleFromDomainSettings(const QJsonObject& domainSettingsObject);
     void clearScaleRestriction();
@@ -571,7 +571,7 @@ private:
 
     glm::vec3 getWorldBodyPosition() const;
     glm::quat getWorldBodyOrientation() const;
-    bool requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& positionOut, bool force = false);
+    bool requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& positionOut);
 
     virtual QByteArray toByteArrayStateful(AvatarDataDetail dataDetail) override;
 

From 556dcd69ac1bbb934ecc3eb77ddad4337cd00b29 Mon Sep 17 00:00:00 2001
From: Howard Stearns <howard@highfidelity.io>
Date: Sat, 3 Jun 2017 15:48:48 -0700
Subject: [PATCH 13/20] wording change regarding use of exact for floors

---
 interface/resources/qml/AssetServer.qml                    | 2 +-
 interface/resources/qml/hifi/dialogs/TabletAssetServer.qml | 2 +-
 interface/resources/qml/hifi/tablet/NewModelDialog.qml     | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/interface/resources/qml/AssetServer.qml b/interface/resources/qml/AssetServer.qml
index bf46ba121c..eb47afc951 100644
--- a/interface/resources/qml/AssetServer.qml
+++ b/interface/resources/qml/AssetServer.qml
@@ -177,7 +177,7 @@ ScrollingWindow {
                     SHAPE_TYPE_STATIC_MESH
                 ],
                 checkStateOnDisable: false,
-                warningOnDisable: "Models with 'Exact' automatic collisions cannot be dynamic"
+                warningOnDisable: "Models with 'Exact' automatic collisions cannot be dynamic, and should not be used as floors"
             }
         });
 
diff --git a/interface/resources/qml/hifi/dialogs/TabletAssetServer.qml b/interface/resources/qml/hifi/dialogs/TabletAssetServer.qml
index 85f8a2f59e..6e0263787b 100644
--- a/interface/resources/qml/hifi/dialogs/TabletAssetServer.qml
+++ b/interface/resources/qml/hifi/dialogs/TabletAssetServer.qml
@@ -179,7 +179,7 @@ Rectangle {
                                                            SHAPE_TYPE_STATIC_MESH
                                                        ],
                                                        checkStateOnDisable: false,
-                                                       warningOnDisable: "Models with 'Exact' automatic collisions cannot be dynamic"
+                                                       warningOnDisable: "Models with 'Exact' automatic collisions cannot be dynamic, and should not be used as floors"
                                                    }
                                                });
 
diff --git a/interface/resources/qml/hifi/tablet/NewModelDialog.qml b/interface/resources/qml/hifi/tablet/NewModelDialog.qml
index 5dbb733872..2d9d121209 100644
--- a/interface/resources/qml/hifi/tablet/NewModelDialog.qml
+++ b/interface/resources/qml/hifi/tablet/NewModelDialog.qml
@@ -118,7 +118,7 @@ Rectangle {
                         id: text2
                         width: 160
                         color: "#ffffff"
-                        text: qsTr("Models with automatic collisions set to 'Exact' cannot be dynamic")
+                        text: qsTr("Models with automatic collisions set to 'Exact' cannot be dynamic, and should not be used as floors")
                         wrapMode: Text.WordWrap
                         font.pixelSize: 12
                     }

From fd01258c76705bbd04cfb091db9e7a77689fa9d3 Mon Sep 17 00:00:00 2001
From: Howard Stearns <howard@highfidelity.io>
Date: Sat, 3 Jun 2017 16:02:58 -0700
Subject: [PATCH 14/20] cleanup

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

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 3c7f1e7d8d..a14fd71f9b 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -1555,11 +1555,11 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
 
     if (_characterController.isEnabledAndReady()) {
         setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity());
-        /*FIXME if (_characterController.isStuck()) {
+        if (_characterController.isStuck()) {
             _physicsSafetyPending = true;
             _goToPosition = getPosition();
             qDebug() << "FIXME setting safety test at:" << _goToPosition;
-        }*/
+        }
     } else {
         setVelocity(getVelocity() + _characterController.getFollowVelocity());
     }
@@ -2258,12 +2258,12 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
     }
     qDebug() << "rechecking" << position << " => " << better << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
     if (!getCollisionsEnabled()) {
-        goToLocation(better);  // recurses
+        goToLocation(better);  // recurses on next update
      } else { // If you try to go while stuck, physics will keep you stuck.
         setCollisionsEnabled(false);
         // Don't goToLocation just yet. Yield so that physics can act on the above.
         QMetaObject::invokeMethod(this, "goToLocationAndEnableCollisions", Qt::QueuedConnection, // The equivalent of javascript nextTick
-            Q_ARG(glm::vec3, better)); // I.e., capsuleCenter - offset
+            Q_ARG(glm::vec3, better));
      }
      return true;
 }

From 3d62900daf5840ec5ede6c6c6f16a4c0d247875d Mon Sep 17 00:00:00 2001
From: Howard Stearns <howard@highfidelity.io>
Date: Sat, 3 Jun 2017 16:49:16 -0700
Subject: [PATCH 15/20] When we do precision detailed picking on a model, check
 the back faces, too. In other words, precision picking from inside a model
 works.

---
 .../src/RenderableModelEntityItem.cpp                  |  2 +-
 libraries/render-utils/src/Model.cpp                   |  4 ++--
 libraries/render-utils/src/Model.h                     |  2 +-
 libraries/shared/src/GeometryUtil.cpp                  |  4 ++--
 libraries/shared/src/GeometryUtil.h                    |  6 +++---
 libraries/shared/src/TriangleSet.cpp                   | 10 +++++-----
 libraries/shared/src/TriangleSet.h                     |  6 +++---
 7 files changed, 17 insertions(+), 17 deletions(-)

diff --git a/libraries/entities-renderer/src/RenderableModelEntityItem.cpp b/libraries/entities-renderer/src/RenderableModelEntityItem.cpp
index 36273c1f07..eab0e3011e 100644
--- a/libraries/entities-renderer/src/RenderableModelEntityItem.cpp
+++ b/libraries/entities-renderer/src/RenderableModelEntityItem.cpp
@@ -605,7 +605,7 @@ bool RenderableModelEntityItem::findDetailedRayIntersection(const glm::vec3& ori
 
     QString extraInfo;
     return _model->findRayIntersectionAgainstSubMeshes(origin, direction, distance,
-                                                       face, surfaceNormal, extraInfo, precisionPicking);
+                                                       face, surfaceNormal, extraInfo, precisionPicking, precisionPicking);
 }
 
 void RenderableModelEntityItem::getCollisionGeometryResource() {
diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp
index e5a25d733e..447d0e37bd 100644
--- a/libraries/render-utils/src/Model.cpp
+++ b/libraries/render-utils/src/Model.cpp
@@ -332,7 +332,7 @@ void Model::initJointStates() {
 
 bool Model::findRayIntersectionAgainstSubMeshes(const glm::vec3& origin, const glm::vec3& direction, float& distance,
                                                     BoxFace& face, glm::vec3& surfaceNormal,
-                                                    QString& extraInfo, bool pickAgainstTriangles) {
+                                                    QString& extraInfo, bool pickAgainstTriangles, bool allowBackface) {
 
     bool intersectedSomething = false;
 
@@ -381,7 +381,7 @@ bool Model::findRayIntersectionAgainstSubMeshes(const glm::vec3& origin, const g
             float triangleSetDistance = 0.0f;
             BoxFace triangleSetFace;
             glm::vec3 triangleSetNormal;
-            if (triangleSet.findRayIntersection(meshFrameOrigin, meshFrameDirection, triangleSetDistance, triangleSetFace, triangleSetNormal, pickAgainstTriangles)) {
+            if (triangleSet.findRayIntersection(meshFrameOrigin, meshFrameDirection, triangleSetDistance, triangleSetFace, triangleSetNormal, pickAgainstTriangles, allowBackface)) {
 
                 glm::vec3 meshIntersectionPoint = meshFrameOrigin + (meshFrameDirection * triangleSetDistance);
                 glm::vec3 worldIntersectionPoint = glm::vec3(meshToWorldMatrix * glm::vec4(meshIntersectionPoint, 1.0f));
diff --git a/libraries/render-utils/src/Model.h b/libraries/render-utils/src/Model.h
index d718145d66..53d446d306 100644
--- a/libraries/render-utils/src/Model.h
+++ b/libraries/render-utils/src/Model.h
@@ -156,7 +156,7 @@ public:
 
     bool findRayIntersectionAgainstSubMeshes(const glm::vec3& origin, const glm::vec3& direction, float& distance,
                                              BoxFace& face, glm::vec3& surfaceNormal, 
-                                             QString& extraInfo, bool pickAgainstTriangles = false);
+                                             QString& extraInfo, bool pickAgainstTriangles = false, bool allowBackface = false);
 
     void setOffset(const glm::vec3& offset);
     const glm::vec3& getOffset() const { return _offset; }
diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp
index c137ebd438..f853240fe3 100644
--- a/libraries/shared/src/GeometryUtil.cpp
+++ b/libraries/shared/src/GeometryUtil.cpp
@@ -290,12 +290,12 @@ glm::vec3 Triangle::getNormal() const {
 }
 
 bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,
-        const glm::vec3& v0, const glm::vec3& v1, const glm::vec3& v2, float& distance) {
+        const glm::vec3& v0, const glm::vec3& v1, const glm::vec3& v2, float& distance, bool allowBackface) {
     glm::vec3 firstSide = v0 - v1;
     glm::vec3 secondSide = v2 - v1;
     glm::vec3 normal = glm::cross(secondSide, firstSide);
     float dividend = glm::dot(normal, v1) - glm::dot(origin, normal);
-    if (dividend > 0.0f) {
+    if (!allowBackface && dividend > 0.0f) {
         return false; // origin below plane
     }
     float divisor = glm::dot(normal, direction);
diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h
index 2fdc1aa25f..857d423896 100644
--- a/libraries/shared/src/GeometryUtil.h
+++ b/libraries/shared/src/GeometryUtil.h
@@ -83,7 +83,7 @@ bool findRayRectangleIntersection(const glm::vec3& origin, const glm::vec3& dire
         const glm::vec3& position, const glm::vec2& dimensions, float& distance);
 
 bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,
-                                    const glm::vec3& v0, const glm::vec3& v1, const glm::vec3& v2, float& distance);
+                                    const glm::vec3& v0, const glm::vec3& v1, const glm::vec3& v2, float& distance, bool allowBackface = false);
 
 /// \brief decomposes rotation into its components such that: rotation = swing * twist
 /// \param rotation[in] rotation to decompose
@@ -104,8 +104,8 @@ public:
 };
 
 inline bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,
-                                    const Triangle& triangle, float& distance) {
-    return findRayTriangleIntersection(origin, direction, triangle.v0, triangle.v1, triangle.v2, distance);
+                                    const Triangle& triangle, float& distance, bool allowBackface = false) {
+    return findRayTriangleIntersection(origin, direction, triangle.v0, triangle.v1, triangle.v2, distance, allowBackface);
 }
 
 
diff --git a/libraries/shared/src/TriangleSet.cpp b/libraries/shared/src/TriangleSet.cpp
index aa21aa5cc0..61ad811a4d 100644
--- a/libraries/shared/src/TriangleSet.cpp
+++ b/libraries/shared/src/TriangleSet.cpp
@@ -31,7 +31,7 @@ void TriangleSet::clear() {
 }
 
 bool TriangleSet::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
-    float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision) {
+    float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, bool allowBackface) {
 
     // reset our distance to be the max possible, lower level tests will store best distance here
     distance = std::numeric_limits<float>::max();
@@ -95,7 +95,7 @@ void TriangleSet::balanceOctree() {
 // Determine of the given ray (origin/direction) in model space intersects with any triangles
 // in the set. If an intersection occurs, the distance and surface normal will be provided.
 bool TriangleSet::TriangleOctreeCell::findRayIntersectionInternal(const glm::vec3& origin, const glm::vec3& direction,
-            float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched) {
+            float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched, bool allowBackface) {
 
     bool intersectedSomething = false;
     float boxDistance = distance;
@@ -114,7 +114,7 @@ bool TriangleSet::TriangleOctreeCell::findRayIntersectionInternal(const glm::vec
                 const auto& triangle = _allTriangles[triangleIndex];
                 float thisTriangleDistance;
                 trianglesTouched++;
-                if (findRayTriangleIntersection(origin, direction, triangle, thisTriangleDistance)) {
+                if (findRayTriangleIntersection(origin, direction, triangle, thisTriangleDistance, allowBackface)) {
                     if (thisTriangleDistance < bestDistance) {
                         bestDistance = thisTriangleDistance;
                         intersectedSomething = true;
@@ -203,7 +203,7 @@ void TriangleSet::TriangleOctreeCell::insert(size_t triangleIndex) {
 }
 
 bool TriangleSet::TriangleOctreeCell::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
-                float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched) {
+                float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched, bool allowBackface) {
 
     if (_population < 1) {
         return false; // no triangles below here, so we can't intersect
@@ -247,7 +247,7 @@ bool TriangleSet::TriangleOctreeCell::findRayIntersection(const glm::vec3& origi
             }
         }
         // also check our local triangle set
-        if (findRayIntersectionInternal(origin, direction, childDistance, childFace, childNormal, precision, trianglesTouched)) {
+        if (findRayIntersectionInternal(origin, direction, childDistance, childFace, childNormal, precision, trianglesTouched, allowBackface)) {
             if (childDistance < bestLocalDistance) {
                 bestLocalDistance = childDistance;
                 bestLocalFace = childFace;
diff --git a/libraries/shared/src/TriangleSet.h b/libraries/shared/src/TriangleSet.h
index 6cedc4da7e..3b0b33d7d5 100644
--- a/libraries/shared/src/TriangleSet.h
+++ b/libraries/shared/src/TriangleSet.h
@@ -27,7 +27,7 @@ class TriangleSet {
         void clear();
 
         bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
-            float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched);
+            float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched, bool allowBackface = false);
 
         const AABox& getBounds() const { return _bounds; }
 
@@ -38,7 +38,7 @@ class TriangleSet {
 
         // checks our internal list of triangles
         bool findRayIntersectionInternal(const glm::vec3& origin, const glm::vec3& direction,
-            float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched);
+            float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched, bool allowBackface = false);
 
         std::vector<Triangle>& _allTriangles;
         std::map<AABox::OctreeChild, TriangleOctreeCell> _children;
@@ -60,7 +60,7 @@ public:
     void insert(const Triangle& t);
 
     bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
-        float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision);
+        float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, bool allowBackface = false);
 
     void balanceOctree();
 

From a0ea306aedb88c9a3d10d51fb4487922ffd51028 Mon Sep 17 00:00:00 2001
From: Howard Stearns <howard@highfidelity.io>
Date: Sun, 4 Jun 2017 07:55:23 -0700
Subject: [PATCH 16/20] cleanup, and include missing allowBackface parameter

---
 interface/src/avatar/MyAvatar.cpp             | 7 +------
 libraries/physics/src/CharacterController.cpp | 4 +---
 libraries/shared/src/TriangleSet.cpp          | 2 +-
 3 files changed, 3 insertions(+), 10 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index a14fd71f9b..c12b6ebdee 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -1558,7 +1558,6 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
         if (_characterController.isStuck()) {
             _physicsSafetyPending = true;
             _goToPosition = getPosition();
-            qDebug() << "FIXME setting safety test at:" << _goToPosition;
         }
     } else {
         setVelocity(getVelocity() + _characterController.getFollowVelocity());
@@ -2232,7 +2231,6 @@ void MyAvatar::goToLocation(const glm::vec3& newPosition,
 }
 
 void MyAvatar::goToLocationAndEnableCollisions(const glm::vec3& position) { // See use case in safeLanding.
-    // FIXME: Doesn't work 100% of time. Need to figure out what isn't happening fast enough. E.g., don't goToLocation until confirmed removed from physics?
     goToLocation(position);
     QMetaObject::invokeMethod(this, "setCollisionsEnabled", Qt::QueuedConnection, Q_ARG(bool, true));
 }
@@ -2274,7 +2272,7 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
     // 1) This is only meant to cover the most important cases, and even the four corners won't handle random spikes in the surfaces or avatar.
     // 2) My feeling is that this code is already at the limit of what can realistically be reviewed and maintained.
     auto ok = [&](const char* label) {  // position is good to go, or at least, we cannot do better
-        qDebug() << "Already safe" << label << positionIn << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
+        //qDebug() << "Already safe" << label << positionIn << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
         return false;
     };
     auto halfHeight = _characterController.getCapsuleHalfHeight();
@@ -2350,14 +2348,12 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
                 ignore.push_back(upperId);
                 if (!findIntersection(upperIntersection, up, upperIntersection, upperId, upperNormal)) {
                     // We're not inside an entity, and from the nested tests, we have room between what is above and below. So position is good!
-                    //qDebug() << "FIXME upper:" << upperId << upperIntersection << " n:" << upperNormal.y << " lower:" << lowerId << lowerIntersection << " n:" << lowerNormal.y << " delta:" << delta << " halfHeight:" << halfHeight;
                     return ok("enough room");
                 }
                 if (isUp(upperNormal)) {
                     // This new intersection is the top surface of an entity that we have not yet seen, which means we're contained within it.
                     // We could break here and recurse from the top of the original ceiling, but since we've already done the work to find the top
                     // of the enclosing entity, let's put our feet at upperIntersection and start over.
-                    qDebug() << "FIXME inside above:" << upperId << " below:" << lowerId;
                     return mustMove();
                 }
                 // We found a new bottom surface, which we're not interested in.
@@ -2370,7 +2366,6 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
     const float big = (float)TREE_SCALE;
     const auto skyHigh = up * big;
     auto fromAbove = capsuleCenter + skyHigh;
-    qDebug() << "FIXME need to compute safe landing for" << capsuleCenter << " based on " << (isDown(upperNormal) ? "down " : "up ") << upperIntersection << "@" << upperId << " and " << (isUp(lowerNormal) ? "up " : "down ") << lowerIntersection << "@" << lowerId;
     if (!findIntersection(fromAbove, down, upperIntersection, upperId, upperNormal)) {
         return ok("Unable to find a landing");
     }
diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index 06c211e2f3..bd4d1201c7 100755
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -175,9 +175,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
                 const float STUCK_PENETRATION = -0.05f; // always negative into the object.
                 const float STUCK_IMPULSE = 500.0f;
                 const int STUCK_LIFETIME = 3;
-                //if (contact.getDistance() < STUCK_PENETRATION) qDebug() << "FIXME checking contact:" << contact.getDistance() << " impulse:" << contact.getAppliedImpulse() << " lifetime:" << contact.getLifeTime();
                 if ((contact.getDistance() < STUCK_PENETRATION) && (contact.getAppliedImpulse() > STUCK_IMPULSE) && (contact.getLifeTime() > STUCK_LIFETIME)) {
-                    qDebug() << "FIXME stuck contact:" << contact.getDistance() << " impulse:" << contact.getAppliedImpulse() << " lifetime:" << contact.getLifeTime();
                     isStuck = true; // latch on
                 }
                 if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > _minFloorNormalDotUp) {
@@ -193,7 +191,7 @@ bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) {
                     if (!_stepUpEnabled || hitHeight > _maxStepHeight) {
                         // this manifold is invalidated by point that is too high
                         stepContactIndex = -1;
-                        qDebug() << "FIXME breaking early";  break;
+                        break;
                     } else if (hitHeight > highestStep && normal.dot(_targetVelocity) < 0.0f ) {
                         highestStep = hitHeight;
                         stepContactIndex = j;
diff --git a/libraries/shared/src/TriangleSet.cpp b/libraries/shared/src/TriangleSet.cpp
index 61ad811a4d..68c99a9753 100644
--- a/libraries/shared/src/TriangleSet.cpp
+++ b/libraries/shared/src/TriangleSet.cpp
@@ -41,7 +41,7 @@ bool TriangleSet::findRayIntersection(const glm::vec3& origin, const glm::vec3&
     }
 
     int trianglesTouched = 0;
-    auto result = _triangleOctree.findRayIntersection(origin, direction, distance, face, surfaceNormal, precision, trianglesTouched);
+    auto result = _triangleOctree.findRayIntersection(origin, direction, distance, face, surfaceNormal, precision, trianglesTouched, allowBackface);
 
     #if WANT_DEBUGGING
     if (precision) {

From 05f41fb4f81c65db3687f4a518b720afc769e5c9 Mon Sep 17 00:00:00 2001
From: Howard Stearns <howard@highfidelity.io>
Date: Sun, 4 Jun 2017 08:13:12 -0700
Subject: [PATCH 17/20] cleanup

---
 interface/src/avatar/MyAvatar.cpp | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index c12b6ebdee..3ee43def87 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -435,9 +435,11 @@ void MyAvatar::update(float deltaTime) {
         // so we update now. It's ok if it updates again in the normal way.
         updateSensorToWorldMatrix();
         emit positionGoneTo();
-        _physicsSafetyPending = getCollisionsEnabled();  // Run safety tests as soon as we can after goToLocation, or clear if we're not colliding.
+        // Run safety tests as soon as we can after goToLocation, or clear if we're not colliding.
+        _physicsSafetyPending = getCollisionsEnabled();
     }
-    if (_physicsSafetyPending && qApp->isPhysicsEnabled() && _characterController.isEnabledAndReady()) { // fix only when needed and ready
+    if (_physicsSafetyPending && qApp->isPhysicsEnabled() && _characterController.isEnabledAndReady()) {
+        // When needed and ready, arrange to check and fix.
         _physicsSafetyPending = false;
         safeLanding(_goToPosition); // no-op if already safe
     }
@@ -2268,9 +2270,7 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
 
 // If position is not reliably safe from being stuck by physics, answer true and place a candidate better position in betterPositionOut.
 bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& betterPositionOut) {
-    // We could repeat this whole test for each of the four corners of our bounding box, in case the surface is uneven. However:
-    // 1) This is only meant to cover the most important cases, and even the four corners won't handle random spikes in the surfaces or avatar.
-    // 2) My feeling is that this code is already at the limit of what can realistically be reviewed and maintained.
+    // We begin with utilities and tests. The Algorithm in four parts is below.
     auto ok = [&](const char* label) {  // position is good to go, or at least, we cannot do better
         //qDebug() << "Already safe" << label << positionIn << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
         return false;
@@ -2283,7 +2283,7 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
     if (!entityTree) {
         return ok("no entity tree");
     }
-
+    // More utilities.
     const auto offset = getOrientation() *_characterController.getCapsuleLocalOffset();
     const auto capsuleCenter = positionIn + offset;
     const auto up = _worldUpDirection, down = -up;
@@ -2391,7 +2391,6 @@ void MyAvatar::updateMotionBehaviorFromMenu() {
     } else {
         _motionBehaviors &= ~AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED;
     }
-    qDebug() << "FIXME updateMotionBehaviorFromMenu collisions:" << menu->isOptionChecked(MenuOption::EnableAvatarCollisions) << "physics:" << qApp->isPhysicsEnabled();
     setCollisionsEnabled(menu->isOptionChecked(MenuOption::EnableAvatarCollisions));
 }
 

From ca526e07765cd316d91d7e50a87d1a9544a257de Mon Sep 17 00:00:00 2001
From: Howard Stearns <howard@highfidelity.io>
Date: Mon, 5 Jun 2017 07:11:25 -0700
Subject: [PATCH 18/20] git rid of a lambda

---
 interface/src/avatar/MyAvatar.cpp | 16 ++++++----------
 1 file changed, 6 insertions(+), 10 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 3ee43def87..0370acaf05 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2271,17 +2271,13 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
 // If position is not reliably safe from being stuck by physics, answer true and place a candidate better position in betterPositionOut.
 bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& betterPositionOut) {
     // We begin with utilities and tests. The Algorithm in four parts is below.
-    auto ok = [&](const char* label) {  // position is good to go, or at least, we cannot do better
-        //qDebug() << "Already safe" << label << positionIn << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
-        return false;
-    };
     auto halfHeight = _characterController.getCapsuleHalfHeight();
     if (halfHeight == 0) {
-        return ok("zero height avatar");
+        return false; // zero height avatar
     }
     auto entityTree = DependencyManager::get<EntityTreeRenderer>()->getTree();
     if (!entityTree) {
-        return ok("no entity tree");
+        return false; // no entity tree
     }
     // More utilities.
     const auto offset = getOrientation() *_characterController.getCapsuleLocalOffset();
@@ -2324,12 +2320,12 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
         // We currently believe that physics will reliably push us out if our feet are embedded,
         // as long as our capsule center is out and there's room above us. Here we have those
         // conditions, so no need to check our feet below.
-        return ok("nothing above");
+        return false; // nothing above
     }
 
     if (!findIntersection(capsuleCenter, down, lowerIntersection, lowerId, lowerNormal)) {
         // Our head may be embedded, but our center is out and there's room below. See corresponding comment above.
-        return ok("nothing below");
+        return false; // nothing below
     }
  
     // See if we have room between entities above and below, but that we are not contained.
@@ -2348,7 +2344,7 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
                 ignore.push_back(upperId);
                 if (!findIntersection(upperIntersection, up, upperIntersection, upperId, upperNormal)) {
                     // We're not inside an entity, and from the nested tests, we have room between what is above and below. So position is good!
-                    return ok("enough room");
+                    return false; // enough room
                 }
                 if (isUp(upperNormal)) {
                     // This new intersection is the top surface of an entity that we have not yet seen, which means we're contained within it.
@@ -2367,7 +2363,7 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
     const auto skyHigh = up * big;
     auto fromAbove = capsuleCenter + skyHigh;
     if (!findIntersection(fromAbove, down, upperIntersection, upperId, upperNormal)) {
-        return ok("Unable to find a landing");
+        return false; // Unable to find a landing
     }
     // Our arbitrary rule is to always go up. There's no need to look down or sideways for a "closer" safe candidate.
     return mustMove();

From 3de243b093dd370fb2fdf00909bbb3f603590f32 Mon Sep 17 00:00:00 2001
From: Howard Stearns <howard@highfidelity.io>
Date: Mon, 5 Jun 2017 18:48:11 -0700
Subject: [PATCH 19/20] Include the radius of the sphere at the avatar's feet.

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

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 0370acaf05..a79aa2a50d 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -294,7 +294,7 @@ void MyAvatar::simulateAttachments(float deltaTime) {
 QByteArray MyAvatar::toByteArrayStateful(AvatarDataDetail dataDetail) {
     CameraMode mode = qApp->getCamera().getMode();
     _globalPosition = getPosition();
-    // This might not be right! Isn't the capsule local offset in avatar space? -HR 5/26/17
+    // This might not be right! Isn't the capsule local offset in avatar space, and don't we need to add the radius to the y as well? -HRS 5/26/17
     _globalBoundingBoxDimensions.x = _characterController.getCapsuleRadius();
     _globalBoundingBoxDimensions.y = _characterController.getCapsuleHalfHeight();
     _globalBoundingBoxDimensions.z = _characterController.getCapsuleRadius();
@@ -456,7 +456,7 @@ void MyAvatar::update(float deltaTime) {
     setAudioAverageLoudness(audio->getAudioAverageInputLoudness());
 
     glm::vec3 halfBoundingBoxDimensions(_characterController.getCapsuleRadius(), _characterController.getCapsuleHalfHeight(), _characterController.getCapsuleRadius());
-    // This might not be right! Isn't the capsule local offset in avatar space? -HR 5/26/17
+    // This might not be right! Isn't the capsule local offset in avatar space? -HRS 5/26/17
     halfBoundingBoxDimensions += _characterController.getCapsuleLocalOffset();
     QMetaObject::invokeMethod(audio.data(), "setAvatarBoundingBoxParameters",
         Q_ARG(glm::vec3, (getPosition() - halfBoundingBoxDimensions)),
@@ -2271,7 +2271,7 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
 // If position is not reliably safe from being stuck by physics, answer true and place a candidate better position in betterPositionOut.
 bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& betterPositionOut) {
     // We begin with utilities and tests. The Algorithm in four parts is below.
-    auto halfHeight = _characterController.getCapsuleHalfHeight();
+    auto halfHeight = _characterController.getCapsuleHalfHeight() + _characterController.getCapsuleRadius();
     if (halfHeight == 0) {
         return false; // zero height avatar
     }

From 9cf027a68c3facf258f6fea15123f7675aae312e Mon Sep 17 00:00:00 2001
From: howard-stearns <howard.stearns@gmail.com>
Date: Wed, 7 Jun 2017 16:44:13 -0700
Subject: [PATCH 20/20] pr feedback

---
 interface/src/avatar/MyAvatar.cpp | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 811c0ae55e..bc621543e3 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2261,7 +2261,6 @@ bool MyAvatar::safeLanding(const glm::vec3& position) {
     if (!requiresSafeLanding(position, better)) {
         return false;
     }
-    qDebug() << "rechecking" << position << " => " << better << " collisions:" << getCollisionsEnabled() << " physics:" << qApp->isPhysicsEnabled();
     if (!getCollisionsEnabled()) {
         goToLocation(better);  // recurses on next update
      } else { // If you try to go while stuck, physics will keep you stuck.
@@ -2334,18 +2333,19 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
     }
  
     // See if we have room between entities above and below, but that we are not contained.
+    // First check if the surface above us is the bottom of something, and the surface below us it the top of something.
+    // I.e., we are in a clearing between two objects.
     if (isDown(upperNormal) && isUp(lowerNormal)) {
-        // The surface above us is the bottom of something, and the surface below us it the top of something.
-        // I.e., we are in a clearing between two objects.
-        auto delta = glm::distance(upperIntersection, lowerIntersection);
+        auto spaceBetween = glm::distance(upperIntersection, lowerIntersection);
         const float halfHeightFactor = 2.5f; // Until case 5003 is fixed (and maybe after?), we need a fudge factor. Also account for content modelers not being precise.
-        if (delta > (halfHeightFactor * halfHeight)) {
+        if (spaceBetween > (halfHeightFactor * halfHeight)) {
             // There is room for us to fit in that clearing. If there wasn't, physics would oscilate us between the objects above and below.
             // We're now going to iterate upwards through successive upperIntersections, testing to see if we're contained within the top surface of some entity.
             // There will be one of two outcomes:
             // a) We're not contained, so we have enough room and our position is good.
             // b) We are contained, so we'll bail out of this but try again at a position above the containing entity.
-            for (;;) {
+            const int iterationLimit = 1000;
+            for (int counter = 0; counter < iterationLimit; counter++) {
                 ignore.push_back(upperId);
                 if (!findIntersection(upperIntersection, up, upperIntersection, upperId, upperNormal)) {
                     // We're not inside an entity, and from the nested tests, we have room between what is above and below. So position is good!
@@ -2360,6 +2360,7 @@ bool MyAvatar::requiresSafeLanding(const glm::vec3& positionIn, glm::vec3& bette
                 // We found a new bottom surface, which we're not interested in.
                 // But there could still be a top surface above us for an entity we haven't seen, so keep looking upward.
             }
+            qCDebug(interfaceapp) << "Loop in requiresSafeLanding. Floor/ceiling do not make sense.";
         }
     }