ordered ray/parabola intersection code

interface/src/avatar/AvatarManager.cpp

@@ -553,8 +553,6 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
         return result;
     }
 
-    glm::vec3 normDirection = glm::normalize(ray.direction);
-
     auto avatarHashCopy = getHashCopy();
     for (auto avatarData : avatarHashCopy) {
         auto avatar = std::static_pointer_cast<Avatar>(avatarData);
@@ -587,14 +585,14 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
         glm::vec3 end;
         float radius;
         avatar->getCapsule(start, end, radius);
-        bool intersects = findRayCapsuleIntersection(ray.origin, normDirection, start, end, radius, distance);
+        bool intersects = findRayCapsuleIntersection(ray.origin, ray.direction, start, end, radius, distance);
         if (!intersects) {
             // ray doesn't intersect avatar's capsule
             continue;
         }
 
         QVariantMap extraInfo;
-        intersects = avatarModel->findRayIntersectionAgainstSubMeshes(ray.origin, normDirection,
+        intersects = avatarModel->findRayIntersectionAgainstSubMeshes(ray.origin, ray.direction,
                                                                       distance, face, surfaceNormal, extraInfo, true);
 
         if (intersects && (!result.intersects || distance < result.distance)) {
@@ -608,7 +606,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
     }
 
     if (result.intersects) {
-        result.intersection = ray.origin + normDirection * result.distance;
+        result.intersection = ray.origin + ray.direction * result.distance;
     }
 
     return result;

interface/src/raypick/LaserPointer.cpp

@@ -42,6 +42,9 @@ glm::vec3 LaserPointer::getPickOrigin(const PickResultPointer& pickResult) const
 
 glm::vec3 LaserPointer::getPickEnd(const PickResultPointer& pickResult, float distance) const {
     auto rayPickResult = std::static_pointer_cast<RayPickResult>(pickResult);
+    if (!rayPickResult) {
+        return glm::vec3(0.0f);
+    }
     if (distance > 0.0f) {
         PickRay pick = PickRay(rayPickResult->pickVariant);
         return pick.origin + distance * pick.direction;

interface/src/raypick/ParabolaPointer.cpp

@@ -67,6 +67,9 @@ glm::vec3 ParabolaPointer::getPickOrigin(const PickResultPointer& pickResult) co
 
 glm::vec3 ParabolaPointer::getPickEnd(const PickResultPointer& pickResult, float distance) const {
     auto parabolaPickResult = std::static_pointer_cast<ParabolaPickResult>(pickResult);
+    if (!parabolaPickResult) {
+        return glm::vec3(0.0f);
+    }
     if (distance > 0.0f) {
         PickParabola pick = PickParabola(parabolaPickResult->pickVariant);
         return pick.origin + pick.velocity * distance + 0.5f * pick.acceleration * distance * distance;

libraries/entities-renderer/src/RenderablePolyVoxEntityItem.cpp

@@ -584,8 +584,6 @@ bool RenderablePolyVoxEntityItem::findDetailedRayIntersection(const glm::vec3& o
     glm::vec4 originInVoxel = wtvMatrix * glm::vec4(origin, 1.0f);
     glm::vec4 farInVoxel = wtvMatrix * glm::vec4(farPoint, 1.0f);
 
-    glm::vec4 directionInVoxel = glm::normalize(farInVoxel - originInVoxel);
-
     glm::vec4 result = glm::vec4(0.0f, 0.0f, 0.0f, 0.0f);
     PolyVox::RaycastResult raycastResult = doRayCast(originInVoxel, farInVoxel, result);
     if (raycastResult == PolyVox::RaycastResults::Completed) {
@@ -599,14 +597,9 @@ bool RenderablePolyVoxEntityItem::findDetailedRayIntersection(const glm::vec3& o
     voxelBox += result3 - Vectors::HALF;
     voxelBox += result3 + Vectors::HALF;
 
-    float voxelDistance;
+    glm::vec4 directionInVoxel = wtvMatrix * glm::vec4(direction, 0.0f);
-    bool hit = voxelBox.findRayIntersection(glm::vec3(originInVoxel), glm::vec3(directionInVoxel),
+    return voxelBox.findRayIntersection(glm::vec3(originInVoxel), glm::vec3(directionInVoxel),
-                                            voxelDistance, face, surfaceNormal);
+                                        distance, face, surfaceNormal);
-
-    glm::vec4 voxelIntersectionPoint = glm::vec4(glm::vec3(originInVoxel) + glm::vec3(directionInVoxel) * voxelDistance, 1.0);
-    glm::vec4 intersectionPoint = vtwMatrix * voxelIntersectionPoint;
-    distance = glm::distance(origin, glm::vec3(intersectionPoint));
-    return hit;
 }
 
 bool RenderablePolyVoxEntityItem::findDetailedParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity,

libraries/entities/src/EntityTree.cpp

@@ -825,15 +825,38 @@ bool findRayIntersectionOp(const OctreeElementPointer& element, void* extraData)
     RayArgs* args = static_cast<RayArgs*>(extraData);
     bool keepSearching = true;
     EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast<EntityTreeElement>(element);
-    EntityItemID entityID = entityTreeElementPointer->findRayIntersection(args->origin, args->direction, keepSearching,
+    EntityItemID entityID = entityTreeElementPointer->findRayIntersection(args->origin, args->direction,
         args->element, args->distance, args->face, args->surfaceNormal, args->entityIdsToInclude,
         args->entityIdsToDiscard, args->visibleOnly, args->collidableOnly, args->extraInfo, args->precisionPicking);
     if (!entityID.isNull()) {
         args->entityID = entityID;
+        // We recurse OctreeElements in order, so if we hit something, we can stop immediately
+        keepSearching = false;
     }
     return keepSearching;
 }
 
+float findRayIntersectionSortingOp(const OctreeElementPointer& element, void* extraData) {
+    RayArgs* args = static_cast<RayArgs*>(extraData);
+    EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast<EntityTreeElement>(element);
+    float distance = FLT_MAX;
+    // If origin is inside the cube, always check this element first
+    if (entityTreeElementPointer->getAACube().contains(args->origin)) {
+        distance = 0.0f;
+    } else {
+        float boundDistance = FLT_MAX;
+        BoxFace face;
+        glm::vec3 surfaceNormal;
+        if (entityTreeElementPointer->getAACube().findRayIntersection(args->origin, args->direction, boundDistance, face, surfaceNormal)) {
+            // Don't add this cell if it's already farther than our best distance so far
+            if (boundDistance < args->distance) {
+                distance = boundDistance;
+            }
+        }
+    }
+    return distance;
+}
+
 EntityItemID EntityTree::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
                                     QVector<EntityItemID> entityIdsToInclude, QVector<EntityItemID> entityIdsToDiscard,
                                     bool visibleOnly, bool collidableOnly, bool precisionPicking, 
@@ -846,7 +869,7 @@ EntityItemID EntityTree::findRayIntersection(const glm::vec3& origin, const glm:
 
     bool requireLock = lockType == Octree::Lock;
     bool lockResult = withReadLock([&]{
-        recurseTreeWithOperation(findRayIntersectionOp, &args);
+        recurseTreeWithOperationSorted(findRayIntersectionOp, findRayIntersectionSortingOp, &args);
     }, requireLock);
 
     if (accurateResult) {
@@ -860,15 +883,38 @@ bool findParabolaIntersectionOp(const OctreeElementPointer& element, void* extra
     ParabolaArgs* args = static_cast<ParabolaArgs*>(extraData);
     bool keepSearching = true;
     EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast<EntityTreeElement>(element);
-    EntityItemID entityID = entityTreeElementPointer->findParabolaIntersection(args->origin, args->velocity, args->acceleration, keepSearching,
+    EntityItemID entityID = entityTreeElementPointer->findParabolaIntersection(args->origin, args->velocity, args->acceleration,
         args->element, args->parabolicDistance, args->face, args->surfaceNormal, args->entityIdsToInclude,
         args->entityIdsToDiscard, args->visibleOnly, args->collidableOnly, args->extraInfo, args->precisionPicking);
     if (!entityID.isNull()) {
         args->entityID = entityID;
+        // We recurse OctreeElements in order, so if we hit something, we can stop immediately
+        keepSearching = false;
     }
     return keepSearching;
 }
 
+float findParabolaIntersectionSortingOp(const OctreeElementPointer& element, void* extraData) {
+    ParabolaArgs* args = static_cast<ParabolaArgs*>(extraData);
+    EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast<EntityTreeElement>(element);
+    float distance = FLT_MAX;
+    // If origin is inside the cube, always check this element first
+    if (entityTreeElementPointer->getAACube().contains(args->origin)) {
+        distance = 0.0f;
+    } else {
+        float boundDistance = FLT_MAX;
+        BoxFace face;
+        glm::vec3 surfaceNormal;
+        if (entityTreeElementPointer->getAACube().findParabolaIntersection(args->origin, args->velocity, args->acceleration, boundDistance, face, surfaceNormal)) {
+            // Don't add this cell if it's already farther than our best distance so far
+            if (boundDistance < args->parabolicDistance) {
+                distance = boundDistance;
+            }
+        }
+    }
+    return distance;
+}
+
 EntityItemID EntityTree::findParabolaIntersection(const PickParabola& parabola,
                                     QVector<EntityItemID> entityIdsToInclude, QVector<EntityItemID> entityIdsToDiscard,
                                     bool visibleOnly, bool collidableOnly, bool precisionPicking,
@@ -882,7 +928,7 @@ EntityItemID EntityTree::findParabolaIntersection(const PickParabola& parabola,
 
     bool requireLock = lockType == Octree::Lock;
     bool lockResult = withReadLock([&] {
-        recurseTreeWithOperation(findParabolaIntersectionOp, &args);
+        recurseTreeWithOperationSorted(findParabolaIntersectionOp, findParabolaIntersectionSortingOp, &args);
     }, requireLock);
 
     if (accurateResult) {

libraries/entities/src/EntityTreeElement.cpp

@@ -140,27 +140,17 @@ bool EntityTreeElement::bestFitBounds(const glm::vec3& minPoint, const glm::vec3
 }
 
 EntityItemID EntityTreeElement::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
-    bool& keepSearching, OctreeElementPointer& element, float& distance,
+    OctreeElementPointer& element, float& distance, BoxFace& face, glm::vec3& surfaceNormal,
-    BoxFace& face, glm::vec3& surfaceNormal, const QVector<EntityItemID>& entityIdsToInclude,
+    const QVector<EntityItemID>& entityIdsToInclude, const QVector<EntityItemID>& entityIdsToDiscard,
-    const QVector<EntityItemID>& entityIdsToDiscard, bool visibleOnly, bool collidableOnly,
+    bool visibleOnly, bool collidableOnly, QVariantMap& extraInfo, bool precisionPicking) {
-    QVariantMap& extraInfo, bool precisionPicking) {
 
     EntityItemID result;
-    float distanceToElementCube = std::numeric_limits<float>::max();
+    float distanceToElementCube = FLT_MAX;
     BoxFace localFace;
     glm::vec3 localSurfaceNormal;
 
-    // if the ray doesn't intersect with our cube OR the distance to element is less than current best distance
-    // we can stop searching!
-    bool hit = _cube.findRayIntersection(origin, direction, distanceToElementCube, localFace, localSurfaceNormal);
-    if (!hit || (!_cube.contains(origin) && distanceToElementCube > distance)) {
-        keepSearching = false; // no point in continuing to search
-        return result; // we did not intersect
-    }
-
-    // by default, we only allow intersections with leaves with content
     if (!canPickIntersect()) {
-        return result; // we don't intersect with non-leaves, and we keep searching
+        return result;
     }
 
     // if the distance to the element cube is not less than the current best distance, then it's not possible
@@ -289,7 +279,7 @@ bool EntityTreeElement::findSpherePenetration(const glm::vec3& center, float rad
 }
 
 EntityItemID EntityTreeElement::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity,
-    const glm::vec3& acceleration, bool& keepSearching, OctreeElementPointer& element, float& parabolicDistance,
+    const glm::vec3& acceleration, OctreeElementPointer& element, float& parabolicDistance,
     BoxFace& face, glm::vec3& surfaceNormal, const QVector<EntityItemID>& entityIdsToInclude,
     const QVector<EntityItemID>& entityIdsToDiscard, bool visibleOnly, bool collidableOnly,
     QVariantMap& extraInfo, bool precisionPicking) {
@@ -299,17 +289,8 @@ EntityItemID EntityTreeElement::findParabolaIntersection(const glm::vec3& origin
     BoxFace localFace;
     glm::vec3 localSurfaceNormal;
 
-    // if the parabola doesn't intersect with our cube OR the distance to element is less than current best distance
-    // we can stop searching!
-    bool hit = _cube.findParabolaIntersection(origin, velocity, acceleration, distanceToElementCube, localFace, localSurfaceNormal);
-    if (!hit || (!_cube.contains(origin) && distanceToElementCube > parabolicDistance)) {
-        keepSearching = false; // no point in continuing to search
-        return result; // we did not intersect
-    }
-
-    // by default, we only allow intersections with leaves with content
     if (!canPickIntersect()) {
-        return result; // we don't intersect with non-leaves, and we keep searching
+        return result;
     }
 
     // if the distance to the element cube is not less than the current best distance, then it's not possible

libraries/entities/src/EntityTreeElement.h

@@ -136,10 +136,9 @@ public:
 
     virtual bool canPickIntersect() const override { return hasEntities(); }
     virtual EntityItemID findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
-        bool& keepSearching, OctreeElementPointer& element, float& distance,
+        OctreeElementPointer& element, float& distance, BoxFace& face, glm::vec3& surfaceNormal,
-        BoxFace& face, glm::vec3& surfaceNormal, const QVector<EntityItemID>& entityIdsToInclude,
+        const QVector<EntityItemID>& entityIdsToInclude, const QVector<EntityItemID>& entityIdsToDiscard,
-        const QVector<EntityItemID>& entityIdsToDiscard, bool visibleOnly, bool collidableOnly,
+        bool visibleOnly, bool collidableOnly, QVariantMap& extraInfo, bool precisionPicking = false);
-        QVariantMap& extraInfo, bool precisionPicking = false);
     virtual EntityItemID findDetailedRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
                          OctreeElementPointer& element, float& distance,
                          BoxFace& face, glm::vec3& surfaceNormal, const QVector<EntityItemID>& entityIdsToInclude,
@@ -149,7 +148,7 @@ public:
                         glm::vec3& penetration, void** penetratedObject) const override;
 
     virtual EntityItemID findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity,
-        const glm::vec3& acceleration, bool& keepSearching, OctreeElementPointer& element, float& parabolicDistance,
+        const glm::vec3& acceleration, OctreeElementPointer& element, float& parabolicDistance,
         BoxFace& face, glm::vec3& surfaceNormal, const QVector<EntityItemID>& entityIdsToInclude,
         const QVector<EntityItemID>& entityIdsToDiscard, bool visibleOnly, bool collidableOnly,
         QVariantMap& extraInfo, bool precisionPicking = false);

libraries/entities/src/ShapeEntityItem.cpp

@@ -262,20 +262,18 @@ bool ShapeEntityItem::findDetailedRayIntersection(const glm::vec3& origin, const
     glm::mat4 entityToWorldMatrix = getEntityToWorldMatrix();
     glm::mat4 worldToEntityMatrix = glm::inverse(entityToWorldMatrix);
     glm::vec3 entityFrameOrigin = glm::vec3(worldToEntityMatrix * glm::vec4(origin, 1.0f));
-    glm::vec3 entityFrameDirection = glm::normalize(glm::vec3(worldToEntityMatrix * glm::vec4(direction, 0.0f)));
+    glm::vec3 entityFrameDirection = glm::vec3(worldToEntityMatrix * glm::vec4(direction, 0.0f));
 
-    float localDistance;
     // NOTE: unit sphere has center of 0,0,0 and radius of 0.5
-    if (findRaySphereIntersection(entityFrameOrigin, entityFrameDirection, glm::vec3(0.0f), 0.5f, localDistance)) {
+    if (findRaySphereIntersection(entityFrameOrigin, entityFrameDirection, glm::vec3(0.0f), 0.5f, distance)) {
-        // determine where on the unit sphere the hit point occured
-        glm::vec3 entityFrameHitAt = entityFrameOrigin + (entityFrameDirection * localDistance);
-        // then translate back to work coordinates
-        glm::vec3 hitAt = glm::vec3(entityToWorldMatrix * glm::vec4(entityFrameHitAt, 1.0f));
-        distance = glm::distance(origin, hitAt);
         bool success;
-        // FIXME: this is only correct for uniformly scaled spheres
+        glm::vec3 center = getCenterPosition(success);
-        surfaceNormal = glm::normalize(hitAt - getCenterPosition(success));
+        if (success) {
-        if (!success) {
+            // FIXME: this is only correct for uniformly scaled spheres
+            // determine where on the unit sphere the hit point occured
+            glm::vec3 hitAt = origin + (direction * distance);
+            surfaceNormal = glm::normalize(hitAt - center);
+        } else {
             return false;
         }
         return true;
@@ -297,9 +295,11 @@ bool ShapeEntityItem::findDetailedParabolaIntersection(const glm::vec3& origin,
     // NOTE: unit sphere has center of 0,0,0 and radius of 0.5
     if (findParabolaSphereIntersection(entityFrameOrigin, entityFrameVelocity, entityFrameAcceleration, glm::vec3(0.0f), 0.5f, parabolicDistance)) {
         bool success;
-        // FIXME: this is only correct for uniformly scaled spheres
+        glm::vec3 center = getCenterPosition(success);
-        surfaceNormal = glm::normalize((origin + velocity * parabolicDistance + 0.5f * acceleration * parabolicDistance * parabolicDistance) - getCenterPosition(success));
+        if (success) {
-        if (!success) {
+            // FIXME: this is only correct for uniformly scaled spheres
+            surfaceNormal = glm::normalize((origin + velocity * parabolicDistance + 0.5f * acceleration * parabolicDistance * parabolicDistance) - center);
+        } else {
             return false;
         }
         return true;

libraries/octree/src/Octree.cpp

@@ -68,55 +68,12 @@ Octree::~Octree() {
     eraseAllOctreeElements(false);
 }
 
-
-// Inserts the value and key into three arrays sorted by the key array, the first array is the value,
-// the second array is a sorted key for the value, the third array is the index for the value in it original
-// non-sorted array
-// returns -1 if size exceeded
-// originalIndexArray is optional
-int insertOctreeElementIntoSortedArrays(const OctreeElementPointer& value, float key, int originalIndex,
-                                        OctreeElementPointer* valueArray, float* keyArray, int* originalIndexArray,
-                                        int currentCount, int maxCount) {
-
-    if (currentCount < maxCount) {
-        int i = 0;
-        if (currentCount > 0) {
-            while (i < currentCount && key > keyArray[i]) {
-                i++;
-            }
-            // i is our desired location
-            // shift array elements to the right
-            if (i < currentCount && i+1 < maxCount) {
-                for (int j = currentCount - 1; j > i; j--) {
-                    valueArray[j] = valueArray[j - 1];
-                    keyArray[j] = keyArray[j - 1];
-                }
-            }
-        }
-        // place new element at i
-        valueArray[i] = value;
-        keyArray[i] = key;
-        if (originalIndexArray) {
-            originalIndexArray[i] = originalIndex;
-        }
-        return currentCount + 1;
-    }
-    return -1; // error case
-}
-
-
-
 // Recurses voxel tree calling the RecurseOctreeOperation function for each element.
 // stops recursion if operation function returns false.
 void Octree::recurseTreeWithOperation(const RecurseOctreeOperation& operation, void* extraData) {
     recurseElementWithOperation(_rootElement, operation, extraData);
 }
 
-// Recurses voxel tree calling the RecurseOctreePostFixOperation function for each element in post-fix order.
-void Octree::recurseTreeWithPostOperation(const RecurseOctreeOperation& operation, void* extraData) {
-    recurseElementWithPostOperation(_rootElement, operation, extraData);
-}
-
 // Recurses voxel element with an operation function
 void Octree::recurseElementWithOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation, void* extraData,
                         int recursionCount) {
@@ -129,71 +86,53 @@ void Octree::recurseElementWithOperation(const OctreeElementPointer& element, co
         for (int i = 0; i < NUMBER_OF_CHILDREN; i++) {
             OctreeElementPointer child = element->getChildAtIndex(i);
             if (child) {
-                recurseElementWithOperation(child, operation, extraData, recursionCount+1);
+                recurseElementWithOperation(child, operation, extraData, recursionCount + 1);
             }
         }
     }
 }
 
-// Recurses voxel element with an operation function
+void Octree::recurseTreeWithOperationSorted(const RecurseOctreeOperation& operation, const RecurseOctreeSortingOperation& sortingOperation, void* extraData) {
-void Octree::recurseElementWithPostOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
+    recurseElementWithOperationSorted(_rootElement, operation, sortingOperation, extraData);
-                                             void* extraData, int recursionCount) {
+}
+
+// Recurses voxel element with an operation function, calling operation on its children in a specific order
+bool Octree::recurseElementWithOperationSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
+                                               const RecurseOctreeSortingOperation& sortingOperation, void* extraData, int recursionCount) {
     if (recursionCount > DANGEROUSLY_DEEP_RECURSION) {
-        HIFI_FCDEBUG(octree(), "Octree::recurseElementWithPostOperation() reached DANGEROUSLY_DEEP_RECURSION, bailing!");
+        HIFI_FCDEBUG(octree(), "Octree::recurseElementWithOperationSorted() reached DANGEROUSLY_DEEP_RECURSION, bailing!");
-        return;
+        // If we go too deep, we want to keep searching other paths
+        return true;
     }
 
+    bool keepSearching = operation(element, extraData);
+
+    std::vector<SortedChild> sortedChildren;
     for (int i = 0; i < NUMBER_OF_CHILDREN; i++) {
         OctreeElementPointer child = element->getChildAtIndex(i);
         if (child) {
-            recurseElementWithPostOperation(child, operation, extraData, recursionCount+1);
+            float priority = sortingOperation(child, extraData);
-        }
+            if (priority < FLT_MAX) {
-    }
+                sortedChildren.emplace_back(priority, child);
-    operation(element, extraData);
-}
-
-// Recurses voxel tree calling the RecurseOctreeOperation function for each element.
-// stops recursion if operation function returns false.
-void Octree::recurseTreeWithOperationDistanceSorted(const RecurseOctreeOperation& operation,
-                                                       const glm::vec3& point, void* extraData) {
-
-    recurseElementWithOperationDistanceSorted(_rootElement, operation, point, extraData);
-}
-
-// Recurses voxel element with an operation function
-void Octree::recurseElementWithOperationDistanceSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
-                                                       const glm::vec3& point, void* extraData, int recursionCount) {
-
-    if (recursionCount > DANGEROUSLY_DEEP_RECURSION) {
-        HIFI_FCDEBUG(octree(), "Octree::recurseElementWithOperationDistanceSorted() reached DANGEROUSLY_DEEP_RECURSION, bailing!");
-        return;
-    }
-
-    if (operation(element, extraData)) {
-        // determine the distance sorted order of our children
-        OctreeElementPointer sortedChildren[NUMBER_OF_CHILDREN] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL };
-        float distancesToChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-        int indexOfChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-        int currentCount = 0;
-
-        for (int i = 0; i < NUMBER_OF_CHILDREN; i++) {
-            OctreeElementPointer childElement = element->getChildAtIndex(i);
-            if (childElement) {
-                // chance to optimize, doesn't need to be actual distance!! Could be distance squared
-                float distanceSquared = childElement->distanceSquareToPoint(point);
-                currentCount = insertOctreeElementIntoSortedArrays(childElement, distanceSquared, i,
-                                                                   sortedChildren, (float*)&distancesToChildren,
-                                                                   (int*)&indexOfChildren, currentCount, NUMBER_OF_CHILDREN);
-            }
-        }
-
-        for (int i = 0; i < currentCount; i++) {
-            OctreeElementPointer childElement = sortedChildren[i];
-            if (childElement) {
-                recurseElementWithOperationDistanceSorted(childElement, operation, point, extraData);
             }
         }
     }
+
+    if (sortedChildren.size() > 1) {
+        static auto comparator = [](const SortedChild& left, const SortedChild& right) { return left.first > right.first; };
+        std::sort(sortedChildren.begin(), sortedChildren.end(), comparator);
+    }
+
+    for (auto it = sortedChildren.begin(); it != sortedChildren.end(); ++it) {
+        const SortedChild& sortedChild = *it;
+        // Our children were sorted, so if one hits something, we don't need to check the others
+        if (!recurseElementWithOperationSorted(sortedChild.second, operation, sortingOperation, extraData, recursionCount + 1)) {
+            return false;
+        }
+    }
+    // We checked all our children and didn't find anything.
+    // Stop if we hit something in this element.  Continue if we didn't.
+    return keepSearching;
 }
 
 void Octree::recurseTreeWithOperator(RecurseOctreeOperator* operatorObject) {

libraries/octree/src/Octree.h

@@ -49,6 +49,9 @@ public:
 
 // Callback function, for recuseTreeWithOperation
 using RecurseOctreeOperation = std::function<bool(const OctreeElementPointer&, void*)>;
+// Function for sorting octree children during recursion.  If return value == FLT_MAX, child is discarded
+using RecurseOctreeSortingOperation = std::function<float(const OctreeElementPointer&, void*)>;
+using SortedChild = std::pair<float, OctreeElementPointer>;
 typedef QHash<uint, AACube> CubeList;
 
 const bool NO_EXISTS_BITS         = false;
@@ -163,17 +166,10 @@ public:
     OctreeElementPointer getOrCreateChildElementContaining(const AACube& box);
 
     void recurseTreeWithOperation(const RecurseOctreeOperation& operation, void* extraData = NULL);
-    void recurseTreeWithPostOperation(const RecurseOctreeOperation& operation, void* extraData = NULL);
+    void recurseTreeWithOperationSorted(const RecurseOctreeOperation& operation, const RecurseOctreeSortingOperation& sortingOperation, void* extraData = NULL);
-
-    /// \param operation type of operation
-    /// \param point point in world-frame (meters)
-    /// \param extraData hook for user data to be interpreted by special context
-    void recurseTreeWithOperationDistanceSorted(const RecurseOctreeOperation& operation,
-                                                const glm::vec3& point, void* extraData = NULL);
 
     void recurseTreeWithOperator(RecurseOctreeOperator* operatorObject);
 
-
     bool isDirty() const { return _isDirty; }
     void clearDirtyBit() { _isDirty = false; }
     void setDirtyBit() { _isDirty = true; }
@@ -227,14 +223,8 @@ public:
 
     void recurseElementWithOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
                 void* extraData, int recursionCount = 0);
-
+    bool recurseElementWithOperationSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
-    /// Traverse child nodes of node applying operation in post-fix order
+        const RecurseOctreeSortingOperation& sortingOperation, void* extraData, int recursionCount = 0);
-    ///
-    void recurseElementWithPostOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
-                void* extraData, int recursionCount = 0);
-
-    void recurseElementWithOperationDistanceSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
-                const glm::vec3& point, void* extraData, int recursionCount = 0);
 
     bool recurseElementWithOperator(const OctreeElementPointer& element, RecurseOctreeOperator* operatorObject, int recursionCount = 0);
 

libraries/render-utils/src/Model.cpp

@@ -379,14 +379,17 @@ bool Model::findRayIntersectionAgainstSubMeshes(const glm::vec3& origin, const g
     if (modelFrameBox.findRayIntersection(modelFrameOrigin, modelFrameDirection, distance, face, surfaceNormal)) {
         QMutexLocker locker(&_mutex);
 
-        float bestDistance = std::numeric_limits<float>::max();
+        float bestDistance = FLT_MAX;
+        BoxFace bestFace;
         Triangle bestModelTriangle;
         Triangle bestWorldTriangle;
-        int bestSubMeshIndex = 0;
+        glm::vec3 bestWorldIntersectionPoint;
+        glm::vec3 bestMeshIntersectionPoint;
+        int bestPartIndex;
+        int bestShapeID;
+        int bestSubMeshIndex;
 
-        int subMeshIndex = 0;
         const FBXGeometry& geometry = getFBXGeometry();
-
         if (!_triangleSetsValid) {
             calculateTriangleSets(geometry);
         }
@@ -399,39 +402,75 @@ bool Model::findRayIntersectionAgainstSubMeshes(const glm::vec3& origin, const g
         glm::vec3 meshFrameDirection = glm::vec3(worldToMeshMatrix * glm::vec4(direction, 0.0f));
 
         int shapeID = 0;
+        int subMeshIndex = 0;
+
+        std::vector<SortedTriangleSet> sortedTriangleSets;
         for (auto& meshTriangleSets : _modelSpaceMeshTriangleSets) {
             int partIndex = 0;
-            for (auto &partTriangleSet : meshTriangleSets) {
+            for (auto& partTriangleSet : meshTriangleSets) {
-                float triangleSetDistance;
+                float priority = FLT_MAX;
-                BoxFace triangleSetFace;
+                if (partTriangleSet.getBounds().contains(meshFrameOrigin)) {
-                Triangle triangleSetTriangle;
+                    priority = 0.0f;
-                if (partTriangleSet.findRayIntersection(meshFrameOrigin, meshFrameDirection, triangleSetDistance, triangleSetFace, triangleSetTriangle, pickAgainstTriangles, allowBackface)) {
+                } else {
-                    glm::vec3 meshIntersectionPoint = meshFrameOrigin + (meshFrameDirection * triangleSetDistance);
+                    float partBoundDistance = FLT_MAX;
-                    glm::vec3 worldIntersectionPoint = glm::vec3(meshToWorldMatrix * glm::vec4(meshIntersectionPoint, 1.0f));
+                    BoxFace partBoundFace;
-                    float worldDistance = glm::distance(origin, worldIntersectionPoint);
+                    glm::vec3 partBoundNormal;
-
+                    if (partTriangleSet.getBounds().findRayIntersection(meshFrameOrigin, meshFrameDirection, partBoundDistance,
-                    if (worldDistance < bestDistance) {
+                                                                        partBoundFace, partBoundNormal)) {
-                        bestDistance = worldDistance;
+                        priority = partBoundDistance;
-                        intersectedSomething = true;
-                        face = triangleSetFace;
-                        bestModelTriangle = triangleSetTriangle;
-                        bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix;
-                        extraInfo["worldIntersectionPoint"] = vec3toVariant(worldIntersectionPoint);
-                        extraInfo["meshIntersectionPoint"] = vec3toVariant(meshIntersectionPoint);
-                        extraInfo["partIndex"] = partIndex;
-                        extraInfo["shapeID"] = shapeID;
-                        bestSubMeshIndex = subMeshIndex;
                     }
                 }
+
+                if (priority < FLT_MAX) {
+                    sortedTriangleSets.emplace_back(priority, &partTriangleSet, partIndex, shapeID, subMeshIndex);
+                }
                 partIndex++;
                 shapeID++;
             }
             subMeshIndex++;
         }
 
+        if (sortedTriangleSets.size() > 1) {
+            static auto comparator = [](const SortedTriangleSet& left, const SortedTriangleSet& right) { return left.distance > right.distance; };
+            std::sort(sortedTriangleSets.begin(), sortedTriangleSets.end(), comparator);
+        }
+
+        for (auto it = sortedTriangleSets.begin(); it != sortedTriangleSets.end(); ++it) {
+            const SortedTriangleSet& sortedTriangleSet = *it;
+            // We can exit once triangleSetDistance > bestDistance
+            if (sortedTriangleSet.distance > bestDistance) {
+                break;
+            }
+            float triangleSetDistance = FLT_MAX;
+            BoxFace triangleSetFace;
+            Triangle triangleSetTriangle;
+            if (sortedTriangleSet.triangleSet->findRayIntersection(meshFrameOrigin, meshFrameDirection, triangleSetDistance, triangleSetFace,
+                                                                   triangleSetTriangle, pickAgainstTriangles, allowBackface)) {
+                if (triangleSetDistance < bestDistance) {
+                    bestDistance = triangleSetDistance;
+                    intersectedSomething = true;
+                    bestFace = triangleSetFace;
+                    bestModelTriangle = triangleSetTriangle;
+                    bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix;
+                    glm::vec3 meshIntersectionPoint = meshFrameOrigin + (meshFrameDirection * triangleSetDistance);
+                    glm::vec3 worldIntersectionPoint = glm::vec3(meshToWorldMatrix * glm::vec4(meshIntersectionPoint, 1.0f));
+                    bestWorldIntersectionPoint = worldIntersectionPoint;
+                    bestMeshIntersectionPoint = meshIntersectionPoint;
+                    bestPartIndex = sortedTriangleSet.partIndex;
+                    bestShapeID = sortedTriangleSet.shapeID;
+                    bestSubMeshIndex = sortedTriangleSet.subMeshIndex;
+                }
+            }
+        }
+
         if (intersectedSomething) {
             distance = bestDistance;
+            face = bestFace;
             surfaceNormal = bestWorldTriangle.getNormal();
+            extraInfo["worldIntersectionPoint"] = vec3toVariant(bestWorldIntersectionPoint);
+            extraInfo["meshIntersectionPoint"] = vec3toVariant(bestMeshIntersectionPoint);
+            extraInfo["partIndex"] = bestPartIndex;
+            extraInfo["shapeID"] = bestShapeID;
             if (pickAgainstTriangles) {
                 extraInfo["subMeshIndex"] = bestSubMeshIndex;
                 extraInfo["subMeshName"] = geometry.getModelNameOfMesh(bestSubMeshIndex);
@@ -483,13 +522,16 @@ bool Model::findParabolaIntersectionAgainstSubMeshes(const glm::vec3& origin, co
         QMutexLocker locker(&_mutex);
 
         float bestDistance = FLT_MAX;
+        BoxFace bestFace;
         Triangle bestModelTriangle;
         Triangle bestWorldTriangle;
-        int bestSubMeshIndex = 0;
+        glm::vec3 bestWorldIntersectionPoint;
+        glm::vec3 bestMeshIntersectionPoint;
+        int bestPartIndex;
+        int bestShapeID;
+        int bestSubMeshIndex;
 
-        int subMeshIndex = 0;
         const FBXGeometry& geometry = getFBXGeometry();
-
         if (!_triangleSetsValid) {
             calculateTriangleSets(geometry);
         }
@@ -503,40 +545,79 @@ bool Model::findParabolaIntersectionAgainstSubMeshes(const glm::vec3& origin, co
         glm::vec3 meshFrameAcceleration = glm::vec3(worldToMeshMatrix * glm::vec4(acceleration, 0.0f));
 
         int shapeID = 0;
+        int subMeshIndex = 0;
+
+        std::vector<SortedTriangleSet> sortedTriangleSets;
         for (auto& meshTriangleSets : _modelSpaceMeshTriangleSets) {
             int partIndex = 0;
-            for (auto &partTriangleSet : meshTriangleSets) {
+            for (auto& partTriangleSet : meshTriangleSets) {
-                float triangleSetDistance;
+                float priority = FLT_MAX;
-                BoxFace triangleSetFace;
+                if (partTriangleSet.getBounds().contains(meshFrameOrigin)) {
-                Triangle triangleSetTriangle;
+                    priority = 0.0f;
-                if (partTriangleSet.findParabolaIntersection(meshFrameOrigin, meshFrameVelocity, meshFrameAcceleration,
+                } else {
-                    triangleSetDistance, triangleSetFace, triangleSetTriangle, pickAgainstTriangles, allowBackface)) {
+                    float partBoundDistance = FLT_MAX;
-                    if (triangleSetDistance < bestDistance) {
+                    BoxFace partBoundFace;
-                        bestDistance = triangleSetDistance;
+                    glm::vec3 partBoundNormal;
-                        intersectedSomething = true;
+                    if (partTriangleSet.getBounds().findParabolaIntersection(meshFrameOrigin, meshFrameVelocity, meshFrameAcceleration,
-                        face = triangleSetFace;
+                                                                             partBoundDistance, partBoundFace, partBoundNormal)) {
-                        bestModelTriangle = triangleSetTriangle;
+                        priority = partBoundDistance;
-                        bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix;
-                        glm::vec3 meshIntersectionPoint = meshFrameOrigin + meshFrameVelocity * triangleSetDistance +
-                            0.5f * meshFrameAcceleration * triangleSetDistance * triangleSetDistance;
-                        glm::vec3 worldIntersectionPoint = origin + velocity * triangleSetDistance +
-                            0.5f * acceleration * triangleSetDistance * triangleSetDistance;
-                        extraInfo["worldIntersectionPoint"] = vec3toVariant(worldIntersectionPoint);
-                        extraInfo["meshIntersectionPoint"] = vec3toVariant(meshIntersectionPoint);
-                        extraInfo["partIndex"] = partIndex;
-                        extraInfo["shapeID"] = shapeID;
-                        bestSubMeshIndex = subMeshIndex;
                     }
                 }
+
+                if (priority < FLT_MAX) {
+                    sortedTriangleSets.emplace_back(priority, &partTriangleSet, partIndex, shapeID, subMeshIndex);
+                }
                 partIndex++;
                 shapeID++;
             }
             subMeshIndex++;
         }
 
+        if (sortedTriangleSets.size() > 1) {
+            static auto comparator = [](const SortedTriangleSet& left, const SortedTriangleSet& right) { return left.distance > right.distance; };
+            std::sort(sortedTriangleSets.begin(), sortedTriangleSets.end(), comparator);
+        }
+
+        for (auto it = sortedTriangleSets.begin(); it != sortedTriangleSets.end(); ++it) {
+            const SortedTriangleSet& sortedTriangleSet = *it;
+            // We can exit once triangleSetDistance > bestDistance
+            if (sortedTriangleSet.distance > bestDistance) {
+                break;
+            }
+            float triangleSetDistance = FLT_MAX;
+            BoxFace triangleSetFace;
+            Triangle triangleSetTriangle;
+            if (sortedTriangleSet.triangleSet->findParabolaIntersection(meshFrameOrigin, meshFrameVelocity, meshFrameAcceleration,
+                                                                        triangleSetDistance, triangleSetFace, triangleSetTriangle,
+                                                                        pickAgainstTriangles, allowBackface)) {
+                if (triangleSetDistance < bestDistance) {
+                    bestDistance = triangleSetDistance;
+                    intersectedSomething = true;
+                    bestFace = triangleSetFace;
+                    bestModelTriangle = triangleSetTriangle;
+                    bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix;
+                    glm::vec3 meshIntersectionPoint = meshFrameOrigin + meshFrameVelocity * triangleSetDistance +
+                        0.5f * meshFrameAcceleration * triangleSetDistance * triangleSetDistance;
+                    glm::vec3 worldIntersectionPoint = origin + velocity * triangleSetDistance +
+                        0.5f * acceleration * triangleSetDistance * triangleSetDistance;
+                    bestWorldIntersectionPoint = worldIntersectionPoint;
+                    bestMeshIntersectionPoint = meshIntersectionPoint;
+                    bestPartIndex = sortedTriangleSet.partIndex;
+                    bestShapeID = sortedTriangleSet.shapeID;
+                    bestSubMeshIndex = sortedTriangleSet.subMeshIndex;
+                    // These sets can overlap, so we can't exit early if we find something
+                }
+            }
+        }
+
         if (intersectedSomething) {
             parabolicDistance = bestDistance;
+            face = bestFace;
             surfaceNormal = bestWorldTriangle.getNormal();
+            extraInfo["worldIntersectionPoint"] = vec3toVariant(bestWorldIntersectionPoint);
+            extraInfo["meshIntersectionPoint"] = vec3toVariant(bestMeshIntersectionPoint);
+            extraInfo["partIndex"] = bestPartIndex;
+            extraInfo["shapeID"] = bestShapeID;
             if (pickAgainstTriangles) {
                 extraInfo["subMeshIndex"] = bestSubMeshIndex;
                 extraInfo["subMeshName"] = geometry.getModelNameOfMesh(bestSubMeshIndex);

libraries/render-utils/src/Model.h

@@ -64,6 +64,16 @@ class Model;
 using ModelPointer = std::shared_ptr<Model>;
 using ModelWeakPointer = std::weak_ptr<Model>;
 
+struct SortedTriangleSet {
+    SortedTriangleSet(float distance, TriangleSet* triangleSet, int partIndex, int shapeID, int subMeshIndex) :
+        distance(distance), triangleSet(triangleSet), partIndex(partIndex), shapeID(shapeID), subMeshIndex(subMeshIndex) {}
+
+    float distance;
+    TriangleSet* triangleSet;
+    int partIndex;
+    int shapeID;
+    int subMeshIndex;
+};
 
 /// A generic 3D model displaying geometry loaded from a URL.
 class Model : public QObject, public std::enable_shared_from_this<Model>, public scriptable::ModelProvider {

libraries/shared/src/GeometryUtil.cpp

@@ -286,12 +286,13 @@ bool findRaySphereIntersection(const glm::vec3& origin, const glm::vec3& directi
         distance = 0.0f;
         return true; // starts inside the sphere
     }
-    float b = glm::dot(direction, relativeOrigin);
+    float b = 2.0f * glm::dot(direction, relativeOrigin);
-    float radicand = b * b - c;
+    float a = glm::dot(direction, direction);
+    float radicand = b * b - 4.0f * a * c;
     if (radicand < 0.0f) {
         return false; // doesn't hit the sphere
     }
-    float t = -b - sqrtf(radicand);
+    float t = 0.5f * (-b - sqrtf(radicand)) / a;
     if (t < 0.0f) {
         return false; // doesn't hit the sphere
     }

libraries/shared/src/TriangleSet.cpp

@@ -13,6 +13,8 @@
 
 #include "GLMHelpers.h"
 
+#include <list>
+
 void TriangleSet::insert(const Triangle& t) {
     _isBalanced = false;
 
@@ -30,47 +32,6 @@ void TriangleSet::clear() {
     _triangleOctree.clear();
 }
 
-bool TriangleSet::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
-         float& distance, BoxFace& face, Triangle& triangle, 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();
-
-    if (!_isBalanced) {
-        balanceOctree();
-    }
-
-    int trianglesTouched = 0;
-    auto result = _triangleOctree.findRayIntersection(origin, direction, distance, face, triangle, precision, trianglesTouched, allowBackface);
-
-    #if WANT_DEBUGGING
-    if (precision) {
-        qDebug() << "trianglesTouched :" << trianglesTouched << "out of:" << _triangleOctree._population << "_triangles.size:" << _triangles.size();
-    }
-    #endif
-    return result;
-}
-
-bool TriangleSet::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration,
-    float& parabolicDistance, BoxFace& face, Triangle& triangle, bool precision, bool allowBackface) {
-    // reset our distance to be the max possible, lower level tests will store best distance here
-    parabolicDistance = FLT_MAX;
-
-    if (!_isBalanced) {
-        balanceOctree();
-    }
-
-    int trianglesTouched = 0;
-    auto result = _triangleOctree.findParabolaIntersection(origin, velocity, acceleration, parabolicDistance, face, triangle, precision, trianglesTouched, allowBackface);
-
-#if WANT_DEBUGGING
-    if (precision) {
-        qDebug() << "trianglesTouched :" << trianglesTouched << "out of:" << _triangleOctree._population << "_triangles.size:" << _triangles.size();
-    }
-#endif
-    return result;
-}
-
 bool TriangleSet::convexHullContains(const glm::vec3& point) const {
     if (!_bounds.contains(point)) {
         return false;
@@ -97,7 +58,7 @@ void TriangleSet::debugDump() {
 }
 
 void TriangleSet::balanceOctree() {
-    _triangleOctree.reset(_bounds, 0);
+    _triangleOctree.reset(_bounds);
 
     // insert all the triangles
     for (size_t i = 0; i < _triangles.size(); i++) {
@@ -106,79 +67,15 @@ void TriangleSet::balanceOctree() {
 
     _isBalanced = true;
 
-    #if WANT_DEBUGGING
+#if WANT_DEBUGGING
     debugDump();
-    #endif
+#endif
 }
 
-
+// With an octree: 8 ^ MAX_DEPTH = 4096 leaves
-// Determine of the given ray (origin/direction) in model space intersects with any triangles
+//static const int MAX_DEPTH = 4;
-// in the set. If an intersection occurs, the distance and surface normal will be provided.
+// With a k-d tree: 2 ^ MAX_DEPTH = 4096 leaves
-bool TriangleSet::TriangleOctreeCell::findRayIntersectionInternal(const glm::vec3& origin, const glm::vec3& direction,
+static const int MAX_DEPTH = 12;
-                                                                  float& distance, BoxFace& face, Triangle& triangle, bool precision,
-                                                                  int& trianglesTouched, bool allowBackface) {
-    bool intersectedSomething = false;
-    float bestDistance = FLT_MAX;
-
-    if (precision) {
-        for (const auto& triangleIndex : _triangleIndices) {
-            const auto& thisTriangle = _allTriangles[triangleIndex];
-            float thisTriangleDistance;
-            trianglesTouched++;
-            if (findRayTriangleIntersection(origin, direction, thisTriangle, thisTriangleDistance, allowBackface)) {
-                if (thisTriangleDistance < bestDistance) {
-                    bestDistance = thisTriangleDistance;
-                    intersectedSomething = true;
-                    triangle = thisTriangle;
-                }
-            }
-        }
-    } else {
-        intersectedSomething = true;
-        bestDistance = distance;
-    }
-
-    if (intersectedSomething) {
-        distance = bestDistance;
-    }
-
-    return intersectedSomething;
-}
-
-bool TriangleSet::TriangleOctreeCell::findParabolaIntersectionInternal(const glm::vec3& origin, const glm::vec3& velocity,
-                                                                       const glm::vec3& acceleration, float& parabolicDistance,
-                                                                       BoxFace& face, Triangle& triangle, bool precision,
-                                                                       int& trianglesTouched, bool allowBackface) {
-    bool intersectedSomething = false;
-    float bestDistance = FLT_MAX;
-
-    if (precision) {
-        for (const auto& triangleIndex : _triangleIndices) {
-            const auto& thisTriangle = _allTriangles[triangleIndex];
-            float thisTriangleDistance;
-            trianglesTouched++;
-            if (findParabolaTriangleIntersection(origin, velocity, acceleration, thisTriangle, thisTriangleDistance, allowBackface)) {
-                if (thisTriangleDistance < bestDistance) {
-                    bestDistance = thisTriangleDistance;
-                    intersectedSomething = true;
-                    triangle = thisTriangle;
-                }
-            }
-        }
-    } else {
-        intersectedSomething = true;
-        bestDistance = parabolicDistance;
-    }
-
-    if (intersectedSomething) {
-        parabolicDistance = bestDistance;
-    }
-
-    return intersectedSomething;
-}
-
-static const int MAX_DEPTH = 4; // for now
-static const int MAX_CHILDREN = 8;
 
 TriangleSet::TriangleOctreeCell::TriangleOctreeCell(std::vector<Triangle>& allTriangles, const AABox& bounds, int depth) :
     _allTriangles(allTriangles)
@@ -190,7 +87,8 @@ void TriangleSet::TriangleOctreeCell::clear() {
     _population = 0;
     _triangleIndices.clear();
     _bounds.clear();
-    _children.clear();
+    _children.first.reset();
+    _children.second.reset();
 }
 
 void TriangleSet::TriangleOctreeCell::reset(const AABox& bounds, int depth) {
@@ -201,45 +99,76 @@ void TriangleSet::TriangleOctreeCell::reset(const AABox& bounds, int depth) {
 
 void TriangleSet::TriangleOctreeCell::debugDump() {
     qDebug() << __FUNCTION__;
-    qDebug() << "bounds:" << getBounds();
+    qDebug() << "               bounds:" << getBounds();
-    qDebug() << "depth:" << _depth;
+    qDebug() << "                depth:" << _depth;
-    qDebug() << "population:" << _population << "this level or below" 
+    qDebug() << "           population:" << _population << "this level or below"
              << " ---- triangleIndices:" << _triangleIndices.size() << "in this cell";
 
-    qDebug() << "child cells:" << _children.size();
+    int numChildren = 0;
+    if (_children.first) {
+        numChildren++;
+    } else if (_children.second) {
+        numChildren++;
+    }
+    qDebug() << "child cells:" << numChildren;
     if (_depth < MAX_DEPTH) {
-        int childNum = 0;
+        if (_children.first) {
-        for (auto& child : _children) {
+            qDebug() << "child: 0";
-            qDebug() << "child:" << childNum;
+            _children.first->debugDump();
-            child.second.debugDump();
+        }
-            childNum++;
+        if (_children.second) {
+            qDebug() << "child: 1";
+            _children.second->debugDump();
         }
     }
 }
 
+std::pair<AABox, AABox> TriangleSet::TriangleOctreeCell::getTriangleOctreeCellChildBounds() {
+    std::pair<AABox, AABox> toReturn;
+    int axis = 0;
+    // find biggest axis
+    glm::vec3 dimensions = _bounds.getDimensions();
+    for (int i = 0; i < 3; i++) {
+        if (dimensions[i] >= dimensions[(i + 1) % 3] && dimensions[i] >= dimensions[(i + 2) % 3]) {
+            axis = i;
+            break;
+        }
+    }
+
+    // The new boxes are half the size in the largest dimension
+    glm::vec3 newDimensions = dimensions;
+    newDimensions[axis] *= 0.5f;
+    toReturn.first.setBox(_bounds.getCorner(), newDimensions);
+    glm::vec3 offset = glm::vec3(0.0f);
+    offset[axis] = newDimensions[axis];
+    toReturn.second.setBox(_bounds.getCorner() + offset, newDimensions);
+    return toReturn;
+}
+
 void TriangleSet::TriangleOctreeCell::insert(size_t triangleIndex) {
-    const Triangle& triangle = _allTriangles[triangleIndex];
     _population++;
+
     // if we're not yet at the max depth, then check which child the triangle fits in
     if (_depth < MAX_DEPTH) {
+        const Triangle& triangle = _allTriangles[triangleIndex];
+        auto childBounds = getTriangleOctreeCellChildBounds();
 
-        for (int child = 0; child < MAX_CHILDREN; child++) {
+        auto insertOperator = [&](const AABox& childBound, std::shared_ptr<TriangleOctreeCell>& child) {
-            AABox childBounds = getBounds().getOctreeChild((AABox::OctreeChild)child);
-
-
             // if the child AABox would contain the triangle...
-            if (childBounds.contains(triangle)) {
+            if (childBound.contains(triangle)) {
                 // if the child cell doesn't yet exist, create it...
-                if (_children.find((AABox::OctreeChild)child) == _children.end()) {
+                if (!child) {
-                    _children.insert(
+                    child = std::make_shared<TriangleOctreeCell>(_allTriangles, childBound, _depth + 1);
-                        std::pair<AABox::OctreeChild, TriangleOctreeCell>
-                        ((AABox::OctreeChild)child, TriangleOctreeCell(_allTriangles, childBounds, _depth + 1)));
                 }
 
                 // insert the triangleIndex in the child cell
-                _children.at((AABox::OctreeChild)child).insert(triangleIndex);
+                child->insert(triangleIndex);
-                return;
+                return true;
             }
+            return false;
+        };
+        if (insertOperator(childBounds.first, _children.first) || insertOperator(childBounds.second, _children.second)) {
+            return;
         }
     }
     // either we're at max depth, or the triangle doesn't fit in one of our
@@ -247,6 +176,62 @@ void TriangleSet::TriangleOctreeCell::insert(size_t triangleIndex) {
     _triangleIndices.push_back(triangleIndex);
 }
 
+bool TriangleSet::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance,
+                                      BoxFace& face, Triangle& triangle, bool precision, bool allowBackface) {
+    if (!_isBalanced) {
+        balanceOctree();
+    }
+
+    float localDistance = distance;
+    int trianglesTouched = 0;
+    bool hit = _triangleOctree.findRayIntersection(origin, direction, localDistance, face, triangle, precision, trianglesTouched, allowBackface);
+    if (hit) {
+        distance = localDistance;
+    }
+
+#if WANT_DEBUGGING
+    if (precision) {
+        qDebug() << "trianglesTouched :" << trianglesTouched << "out of:" << _triangleOctree._population << "_triangles.size:" << _triangles.size();
+    }
+#endif
+    return hit;
+}
+
+// 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, Triangle& triangle, bool precision,
+                                                                  int& trianglesTouched, bool allowBackface) {
+    bool intersectedSomething = false;
+    float bestDistance = FLT_MAX;
+    Triangle bestTriangle;
+
+    if (precision) {
+        for (const auto& triangleIndex : _triangleIndices) {
+            const auto& thisTriangle = _allTriangles[triangleIndex];
+            float thisTriangleDistance;
+            trianglesTouched++;
+            if (findRayTriangleIntersection(origin, direction, thisTriangle, thisTriangleDistance, allowBackface)) {
+                if (thisTriangleDistance < bestDistance) {
+                    bestDistance = thisTriangleDistance;
+                    bestTriangle = thisTriangle;
+                    intersectedSomething = true;
+                }
+            }
+        }
+    } else {
+        intersectedSomething = true;
+        bestDistance = distance;
+    }
+
+    if (intersectedSomething) {
+        distance = bestDistance;
+        triangle = bestTriangle;
+    }
+
+    return intersectedSomething;
+}
+
 bool TriangleSet::TriangleOctreeCell::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance,
                                                           BoxFace& face, Triangle& triangle, bool precision, int& trianglesTouched,
                                                           bool allowBackface) {
@@ -257,52 +242,81 @@ bool TriangleSet::TriangleOctreeCell::findRayIntersection(const glm::vec3& origi
     float bestLocalDistance = FLT_MAX;
     BoxFace bestLocalFace;
     Triangle bestLocalTriangle;
-    glm::vec3 bestLocalNormal;
     bool intersects = false;
 
-    float boxDistance = FLT_MAX;
+    // Check our local triangle set first
-    // if the pick intersects our bounding box, then continue
+    // The distance passed in here is the distance to our bounding box.  If !precision, that distance is used
-    if (getBounds().findRayIntersection(origin, direction, boxDistance, bestLocalFace, bestLocalNormal)) {
+    {
-        // if the intersection with our bounding box, is greater than the current best distance (the distance passed in)
+        float internalDistance = distance;
-        // then we know that none of our triangles can represent a better intersection and we can return
-        if (boxDistance > distance) {
-            return false;
-        }
-
-        // if we're not yet at the max depth, then check which child the triangle fits in
-        if (_depth < MAX_DEPTH) {
-            float bestChildDistance = FLT_MAX;
-            for (auto& child : _children) {
-                // check each child, if there's an intersection, it will return some distance that we need
-                // to compare against the other results, because there might be multiple intersections and
-                // we will always choose the best (shortest) intersection
-                float childDistance = bestChildDistance;
-                BoxFace childFace;
-                Triangle childTriangle;
-                if (child.second.findRayIntersection(origin, direction, childDistance, childFace, childTriangle, precision, trianglesTouched)) {
-                    if (childDistance < bestLocalDistance) {
-                        bestLocalDistance = childDistance;
-                        bestChildDistance = childDistance;
-                        bestLocalFace = childFace;
-                        bestLocalTriangle = childTriangle;
-                        intersects = true;
-                    }
-                }
-            }
-        }
-        // also check our local triangle set
-        float internalDistance = boxDistance;
         BoxFace internalFace;
         Triangle internalTriangle;
         if (findRayIntersectionInternal(origin, direction, internalDistance, internalFace, internalTriangle, precision, trianglesTouched, allowBackface)) {
-            if (internalDistance < bestLocalDistance) {
+            bestLocalDistance = internalDistance;
-                bestLocalDistance = internalDistance;
+            bestLocalFace = internalFace;
-                bestLocalFace = internalFace;
+            bestLocalTriangle = internalTriangle;
-                bestLocalTriangle = internalTriangle;
+            intersects = true;
-                intersects = true;
+        }
+    }
+
+    // if we're not yet at the max depth, then check our children
+    if (_depth < MAX_DEPTH) {
+        std::list<SortedTriangleCell> sortedTriangleCells;
+        auto sortingOperator = [&](std::shared_ptr<TriangleOctreeCell>& child) {
+            if (child) {
+                float priority = FLT_MAX;
+                if (child->getBounds().contains(origin)) {
+                    priority = 0.0f;
+                } else {
+                    float childBoundDistance = FLT_MAX;
+                    BoxFace childBoundFace;
+                    glm::vec3 childBoundNormal;
+                    if (child->getBounds().findRayIntersection(origin, direction, childBoundDistance, childBoundFace, childBoundNormal)) {
+                        // We only need to add this cell if it's closer than the local triangle set intersection (if there was one)
+                        if (childBoundDistance < bestLocalDistance) {
+                            priority = childBoundDistance;
+                        }
+                    }
+                }
+
+                if (priority < FLT_MAX) {
+                    if (sortedTriangleCells.size() > 0 && priority < sortedTriangleCells.front().first) {
+                        sortedTriangleCells.emplace_front(priority, child);
+                    } else {
+                        sortedTriangleCells.emplace_back(priority, child);
+                    }
+                }
+            }
+        };
+        sortingOperator(_children.first);
+        sortingOperator(_children.second);
+
+        for (auto it = sortedTriangleCells.begin(); it != sortedTriangleCells.end(); ++it) {
+            const SortedTriangleCell& sortedTriangleCell = *it;
+            float childDistance = sortedTriangleCell.first;
+            // We can exit once childDistance > bestLocalDistance
+            if (childDistance > bestLocalDistance) {
+                break;
+            }
+            // If we're inside the child cell and !precision, we need the actual distance to the cell bounds
+            if (!precision && childDistance < EPSILON) {
+                BoxFace childBoundFace;
+                glm::vec3 childBoundNormal;
+                sortedTriangleCell.second->getBounds().findRayIntersection(origin, direction, childDistance, childBoundFace, childBoundNormal);
+            }
+            BoxFace childFace;
+            Triangle childTriangle;
+            if (sortedTriangleCell.second->findRayIntersection(origin, direction, childDistance, childFace, childTriangle, precision, trianglesTouched)) {
+                if (childDistance < bestLocalDistance) {
+                    bestLocalDistance = childDistance;
+                    bestLocalFace = childFace;
+                    bestLocalTriangle = childTriangle;
+                    intersects = true;
+                    break;
+                }
             }
         }
     }
+
     if (intersects) {
         distance = bestLocalDistance;
         face = bestLocalFace;
@@ -311,6 +325,61 @@ bool TriangleSet::TriangleOctreeCell::findRayIntersection(const glm::vec3& origi
     return intersects;
 }
 
+bool TriangleSet::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration,
+                                           float& parabolicDistance, BoxFace& face, Triangle& triangle, bool precision, bool allowBackface) {
+    if (!_isBalanced) {
+        balanceOctree();
+    }
+
+    float localDistance = parabolicDistance;
+    int trianglesTouched = 0;
+    bool hit = _triangleOctree.findParabolaIntersection(origin, velocity, acceleration, localDistance, face, triangle, precision, trianglesTouched, allowBackface);
+    if (hit) {
+        parabolicDistance = localDistance;
+    }
+
+#if WANT_DEBUGGING
+    if (precision) {
+        qDebug() << "trianglesTouched :" << trianglesTouched << "out of:" << _triangleOctree._population << "_triangles.size:" << _triangles.size();
+    }
+#endif
+    return hit;
+}
+
+bool TriangleSet::TriangleOctreeCell::findParabolaIntersectionInternal(const glm::vec3& origin, const glm::vec3& velocity,
+                                                                       const glm::vec3& acceleration, float& parabolicDistance,
+                                                                       BoxFace& face, Triangle& triangle, bool precision,
+                                                                       int& trianglesTouched, bool allowBackface) {
+    bool intersectedSomething = false;
+    float bestDistance = FLT_MAX;
+    Triangle bestTriangle;
+
+    if (precision) {
+        for (const auto& triangleIndex : _triangleIndices) {
+            const auto& thisTriangle = _allTriangles[triangleIndex];
+            float thisTriangleDistance;
+            trianglesTouched++;
+            if (findParabolaTriangleIntersection(origin, velocity, acceleration, thisTriangle, thisTriangleDistance, allowBackface)) {
+                if (thisTriangleDistance < bestDistance) {
+                    bestDistance = thisTriangleDistance;
+                    bestTriangle = thisTriangle;
+                    intersectedSomething = true;
+                }
+            }
+        }
+    } else {
+        intersectedSomething = true;
+        bestDistance = parabolicDistance;
+    }
+
+    if (intersectedSomething) {
+        parabolicDistance = bestDistance;
+        triangle = bestTriangle;
+    }
+
+    return intersectedSomething;
+}
+
 bool TriangleSet::TriangleOctreeCell::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity,
                                                                const glm::vec3& acceleration, float& parabolicDistance,
                                                                BoxFace& face, Triangle& triangle, bool precision,
@@ -322,52 +391,81 @@ bool TriangleSet::TriangleOctreeCell::findParabolaIntersection(const glm::vec3&
     float bestLocalDistance = FLT_MAX;
     BoxFace bestLocalFace;
     Triangle bestLocalTriangle;
-    glm::vec3 bestLocalNormal;
     bool intersects = false;
 
-    float boxDistance = FLT_MAX;
+    // Check our local triangle set first
-    // if the pick intersects our bounding box, then continue
+    // The distance passed in here is the distance to our bounding box.  If !precision, that distance is used
-    if (getBounds().findParabolaIntersection(origin, velocity, acceleration, boxDistance, bestLocalFace, bestLocalNormal)) {
+    {
-        // if the intersection with our bounding box, is greater than the current best distance (the distance passed in)
+        float internalDistance = parabolicDistance;
-        // then we know that none of our triangles can represent a better intersection and we can return
-        if (boxDistance > parabolicDistance) {
-            return false;
-        }
-
-        // if we're not yet at the max depth, then check which child the triangle fits in
-        if (_depth < MAX_DEPTH) {
-            float bestChildDistance = FLT_MAX;
-            for (auto& child : _children) {
-                // check each child, if there's an intersection, it will return some distance that we need
-                // to compare against the other results, because there might be multiple intersections and
-                // we will always choose the best (shortest) intersection
-                float childDistance = bestChildDistance;
-                BoxFace childFace;
-                Triangle childTriangle;
-                if (child.second.findParabolaIntersection(origin, velocity, acceleration, childDistance, childFace, childTriangle, precision, trianglesTouched)) {
-                    if (childDistance < bestLocalDistance) {
-                        bestLocalDistance = childDistance;
-                        bestChildDistance = childDistance;
-                        bestLocalFace = childFace;
-                        bestLocalTriangle = childTriangle;
-                        intersects = true;
-                    }
-                }
-            }
-        }
-        // also check our local triangle set
-        float internalDistance = boxDistance;
         BoxFace internalFace;
         Triangle internalTriangle;
         if (findParabolaIntersectionInternal(origin, velocity, acceleration, internalDistance, internalFace, internalTriangle, precision, trianglesTouched, allowBackface)) {
-            if (internalDistance < bestLocalDistance) {
+            bestLocalDistance = internalDistance;
-                bestLocalDistance = internalDistance;
+            bestLocalFace = internalFace;
-                bestLocalFace = internalFace;
+            bestLocalTriangle = internalTriangle;
-                bestLocalTriangle = internalTriangle;
+            intersects = true;
-                intersects = true;
+        }
+    }
+
+    // if we're not yet at the max depth, then check our children
+    if (_depth < MAX_DEPTH) {
+        std::list<SortedTriangleCell> sortedTriangleCells;
+        auto sortingOperator = [&](std::shared_ptr<TriangleOctreeCell>& child) {
+            if (child) {
+                float priority = FLT_MAX;
+                if (child->getBounds().contains(origin)) {
+                    priority = 0.0f;
+                } else {
+                    float childBoundDistance = FLT_MAX;
+                    BoxFace childBoundFace;
+                    glm::vec3 childBoundNormal;
+                    if (child->getBounds().findParabolaIntersection(origin, velocity, acceleration, childBoundDistance, childBoundFace, childBoundNormal)) {
+                        // We only need to add this cell if it's closer than the local triangle set intersection (if there was one)
+                        if (childBoundDistance < bestLocalDistance) {
+                            priority = childBoundDistance;
+                        }
+                    }
+                }
+
+                if (priority < FLT_MAX) {
+                    if (sortedTriangleCells.size() > 0 && priority < sortedTriangleCells.front().first) {
+                        sortedTriangleCells.emplace_front(priority, child);
+                    } else {
+                        sortedTriangleCells.emplace_back(priority, child);
+                    }
+                }
+            }
+        };
+        sortingOperator(_children.first);
+        sortingOperator(_children.second);
+
+        for (auto it = sortedTriangleCells.begin(); it != sortedTriangleCells.end(); ++it) {
+            const SortedTriangleCell& sortedTriangleCell = *it;
+            float childDistance = sortedTriangleCell.first;
+            // We can exit once childDistance > bestLocalDistance
+            if (childDistance > bestLocalDistance) {
+                break;
+            }
+            // If we're inside the child cell and !precision, we need the actual distance to the cell bounds
+            if (!precision && childDistance < EPSILON) {
+                BoxFace childBoundFace;
+                glm::vec3 childBoundNormal;
+                sortedTriangleCell.second->getBounds().findParabolaIntersection(origin, velocity, acceleration, childDistance, childBoundFace, childBoundNormal);
+            }
+            BoxFace childFace;
+            Triangle childTriangle;
+            if (sortedTriangleCell.second->findParabolaIntersection(origin, velocity, acceleration, childDistance, childFace, childTriangle, precision, trianglesTouched)) {
+                if (childDistance < bestLocalDistance) {
+                    bestLocalDistance = childDistance;
+                    bestLocalFace = childFace;
+                    bestLocalTriangle = childTriangle;
+                    intersects = true;
+                    break;
+                }
             }
         }
     }
+
     if (intersects) {
         parabolicDistance = bestLocalDistance;
         face = bestLocalFace;

libraries/shared/src/TriangleSet.h

@@ -12,6 +12,7 @@
 #pragma once
 
 #include <vector>
+#include <memory>
 
 #include "AABox.h"
 #include "GeometryUtil.h"
@@ -20,9 +21,8 @@ class TriangleSet {
 
     class TriangleOctreeCell {
     public:
-        TriangleOctreeCell(std::vector<Triangle>& allTriangles) :
+        TriangleOctreeCell(std::vector<Triangle>& allTriangles) : _allTriangles(allTriangles) {}
-            _allTriangles(allTriangles)
+        TriangleOctreeCell(std::vector<Triangle>& allTriangles, const AABox& bounds, int depth);
-        { }
 
         void insert(size_t triangleIndex);
         void reset(const AABox& bounds, int depth = 0);
@@ -40,8 +40,6 @@ class TriangleSet {
         void debugDump();
 
     protected:
-        TriangleOctreeCell(std::vector<Triangle>& allTriangles, const AABox& bounds, int depth);
-
         // checks our internal list of triangles
         bool findRayIntersectionInternal(const glm::vec3& origin, const glm::vec3& direction,
             float& distance, BoxFace& face, Triangle& triangle, bool precision, int& trianglesTouched,
@@ -50,20 +48,22 @@ class TriangleSet {
             float& parabolicDistance, BoxFace& face, Triangle& triangle, bool precision, int& trianglesTouched,
             bool allowBackface = false);
 
+        std::pair<AABox, AABox> getTriangleOctreeCellChildBounds();
+
         std::vector<Triangle>& _allTriangles;
-        std::map<AABox::OctreeChild, TriangleOctreeCell> _children;
+        std::pair<std::shared_ptr<TriangleOctreeCell>, std::shared_ptr<TriangleOctreeCell>> _children;
-        int _depth{ 0 };
+        int _depth { 0 };
-        int _population{ 0 };
+        int _population { 0 };
         AABox _bounds;
         std::vector<size_t> _triangleIndices;
 
         friend class TriangleSet;
     };
 
+    using SortedTriangleCell = std::pair<float, std::shared_ptr<TriangleOctreeCell>>;
+
 public:
-    TriangleSet() :
+    TriangleSet() : _triangleOctree(_triangles) {}
-        _triangleOctree(_triangles)
-    {}
 
     void debugDump();
 
@@ -87,8 +87,7 @@ public:
     const AABox& getBounds() const { return _bounds; }
 
 protected:
-
+    bool _isBalanced { false };
-    bool _isBalanced{ false };
     std::vector<Triangle> _triangles;
     TriangleOctreeCell _triangleOctree;
     AABox _bounds;