Merge pull request #10455 from ZappoMan/TriangleSetOptimizations

TriangleSet optimizations - ~80% improvement in Ray picking against models

libraries/render-utils/src/Model.cpp

@@ -376,7 +376,7 @@ bool Model::findRayIntersectionAgainstSubMeshes(const glm::vec3& origin, const g
         glm::vec3 meshFrameOrigin = glm::vec3(worldToMeshMatrix * glm::vec4(origin, 1.0f));
         glm::vec3 meshFrameDirection = glm::vec3(worldToMeshMatrix * glm::vec4(direction, 0.0f));
 
-        for (const auto& triangleSet : _modelSpaceMeshTriangleSets) {
+        for (auto& triangleSet : _modelSpaceMeshTriangleSets) {
             float triangleSetDistance = 0.0f;
             BoxFace triangleSetFace;
             glm::vec3 triangleSetNormal;

libraries/shared/src/AABox.cpp

@@ -114,6 +114,10 @@ static bool isWithin(float value, float corner, float size) {
     return value >= corner && value <= corner + size;
 }
 
+bool AABox::contains(const Triangle& triangle) const {
+    return contains(triangle.v0) && contains(triangle.v1) && contains(triangle.v2);
+}
+
 bool AABox::contains(const glm::vec3& point) const {
     return isWithin(point.x, _corner.x, _scale.x) &&
         isWithin(point.y, _corner.y, _scale.y) &&
@@ -622,3 +626,40 @@ void AABox::transform(const glm::mat4& matrix) {
     _corner = newCenter - newDir;
     _scale = newDir * 2.0f;
 }
+
+AABox AABox::getOctreeChild(OctreeChild child) const {
+    AABox result(*this); // self
+    switch (child) {
+        case topLeftNear:
+            result._corner.y += _scale.y / 2.0f;
+            break;
+        case topLeftFar:
+            result._corner.y += _scale.y / 2.0f;
+            result._corner.z += _scale.z / 2.0f;
+            break;
+        case topRightNear:
+            result._corner.y += _scale.y / 2.0f;
+            result._corner.x += _scale.x / 2.0f;
+            break;
+        case topRightFar:
+            result._corner.y += _scale.y / 2.0f;
+            result._corner.x += _scale.x / 2.0f;
+            result._corner.z += _scale.z / 2.0f;
+            break;
+        case bottomLeftNear:
+            // _corner = same as parent
+            break;
+        case bottomLeftFar:
+            result._corner.z += _scale.z / 2.0f;
+            break;
+        case bottomRightNear:
+            result._corner.x += _scale.x / 2.0f;
+            break;
+        case bottomRightFar:
+            result._corner.x += _scale.x / 2.0f;
+            result._corner.z += _scale.z / 2.0f;
+            break;
+    }
+    result._scale /= 2.0f; // everything is half the scale
+    return result;
+}

libraries/shared/src/AABox.h

@@ -20,6 +20,7 @@
 #include <QDebug>
 
 #include "BoxBase.h"
+#include "GeometryUtil.h"
 #include "StreamUtils.h"
 
 class AACube;
@@ -58,6 +59,7 @@ public:
     const glm::vec3& getMinimumPoint() const { return _corner; }
     glm::vec3 getMaximumPoint() const { return calcTopFarLeft(); }
 
+    bool contains(const Triangle& triangle) const;
     bool contains(const glm::vec3& point) const;
     bool contains(const AABox& otherBox) const;
     bool touches(const AABox& otherBox) const;
@@ -112,6 +114,19 @@ public:
 
     void clear() { _corner = INFINITY_VECTOR; _scale = glm::vec3(0.0f); }
 
+    typedef enum {
+        topLeftNear,
+        topLeftFar,
+        topRightNear,
+        topRightFar,
+        bottomLeftNear,
+        bottomLeftFar,
+        bottomRightNear,
+        bottomRightFar
+    } OctreeChild;
+
+    AABox getOctreeChild(OctreeChild child) const; // returns the AABox of the would be octree child of this AABox
+
 private:
     glm::vec3 getClosestPointOnFace(const glm::vec3& point, BoxFace face) const;
     glm::vec3 getClosestPointOnFace(const glm::vec4& origin, const glm::vec4& direction, BoxFace face) const;

libraries/shared/src/TriangleSet.cpp

@@ -12,9 +12,11 @@
 #include "GLMHelpers.h"
 #include "TriangleSet.h"
 
-void TriangleSet::insert(const Triangle& t) {
-    _triangles.push_back(t);
 
+void TriangleSet::insert(const Triangle& t) {
+    _isBalanced = false;
+
+    _triangles.push_back(t);
     _bounds += t.v0;
     _bounds += t.v1;
     _bounds += t.v2;
@@ -23,39 +25,31 @@ void TriangleSet::insert(const Triangle& t) {
 void TriangleSet::clear() {
     _triangles.clear();
     _bounds.clear();
+    _isBalanced = false;
+
+    _triangleOctree.clear();
 }
 
-// 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::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
-                    float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision) const {
+    float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision) {
 
-    bool intersectedSomething = false;
-    float boxDistance = std::numeric_limits<float>::max();
-    float bestDistance = std::numeric_limits<float>::max();
+    // reset our distance to be the max possible, lower level tests will store best distance here
+    distance = std::numeric_limits<float>::max();
 
-    if (_bounds.findRayIntersection(origin, direction, boxDistance, face, surfaceNormal)) {
-        if (precision) {
-            for (const auto& triangle : _triangles) {
-                float thisTriangleDistance;
-                if (findRayTriangleIntersection(origin, direction, triangle, thisTriangleDistance)) {
-                    if (thisTriangleDistance < bestDistance) {
-                        bestDistance = thisTriangleDistance;
-                        intersectedSomething = true;
-                        surfaceNormal = triangle.getNormal();
-                        distance = bestDistance;
-                    }
-                }
-            }
-        } else {
-            intersectedSomething = true;
-            distance = boxDistance;
-        }
+    if (!_isBalanced) {
+        balanceOctree();
     }
 
-    return intersectedSomething;
-}
+    int trianglesTouched = 0;
+    auto result = _triangleOctree.findRayIntersection(origin, direction, distance, face, surfaceNormal, precision, trianglesTouched);
 
+    #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)) {
@@ -74,3 +68,198 @@ bool TriangleSet::convexHullContains(const glm::vec3& point) const {
     return insideMesh;
 }
 
+void TriangleSet::debugDump() {
+    qDebug() << __FUNCTION__;
+    qDebug() << "bounds:" << getBounds();
+    qDebug() << "triangles:" << size() << "at top level....";
+    qDebug() << "----- _triangleOctree -----";
+    _triangleOctree.debugDump();
+}
+
+void TriangleSet::balanceOctree() {
+    _triangleOctree.reset(_bounds, 0);
+
+    // insert all the triangles
+    for (size_t i = 0; i < _triangles.size(); i++) {
+        _triangleOctree.insert(i);
+    }
+
+    _isBalanced = true;
+
+    #if WANT_DEBUGGING
+    debugDump();
+    #endif
+}
+
+
+// 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) {
+
+    bool intersectedSomething = false;
+    float boxDistance = distance;
+    float bestDistance = distance;
+
+    if (_bounds.findRayIntersection(origin, direction, boxDistance, face, surfaceNormal)) {
+
+        // if our bounding box intersects at a distance greater than the current known
+        // best distance, than we can safely not check any of our triangles
+        if (boxDistance > bestDistance) {
+            return false;
+        }
+
+        if (precision) {
+            for (const auto& triangleIndex : _triangleIndices) {
+                const auto& triangle = _allTriangles[triangleIndex];
+                float thisTriangleDistance;
+                trianglesTouched++;
+                if (findRayTriangleIntersection(origin, direction, triangle, thisTriangleDistance)) {
+                    if (thisTriangleDistance < bestDistance) {
+                        bestDistance = thisTriangleDistance;
+                        intersectedSomething = true;
+                        surfaceNormal = triangle.getNormal();
+                        distance = bestDistance;
+                    }
+                }
+            }
+        } else {
+            intersectedSomething = true;
+            distance = boxDistance;
+        }
+    }
+
+    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)
+{
+    reset(bounds, depth);
+}
+
+void TriangleSet::TriangleOctreeCell::clear() {
+    _population = 0;
+    _triangleIndices.clear();
+    _bounds.clear();
+    _children.clear();
+}
+
+void TriangleSet::TriangleOctreeCell::reset(const AABox& bounds, int depth) {
+    clear();
+    _bounds = bounds;
+    _depth = depth;
+}
+
+void TriangleSet::TriangleOctreeCell::debugDump() {
+    qDebug() << __FUNCTION__;
+    qDebug() << "bounds:" << getBounds();
+    qDebug() << "depth:" << _depth;
+    qDebug() << "population:" << _population << "this level or below" 
+             << " ---- triangleIndices:" << _triangleIndices.size() << "in this cell";
+
+    qDebug() << "child cells:" << _children.size();
+    if (_depth < MAX_DEPTH) {
+        int childNum = 0;
+        for (auto& child : _children) {
+            qDebug() << "child:" << childNum;
+            child.second.debugDump();
+            childNum++;
+        }
+    }
+}
+
+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) {
+
+        for (int child = 0; child < MAX_CHILDREN; child++) {
+            AABox childBounds = getBounds().getOctreeChild((AABox::OctreeChild)child);
+
+
+            // if the child AABox would contain the triangle...
+            if (childBounds.contains(triangle)) {
+                // if the child cell doesn't yet exist, create it...
+                if (_children.find((AABox::OctreeChild)child) == _children.end()) {
+                    _children.insert(
+                        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);
+                return;
+            }
+        }
+    }
+    // either we're at max depth, or the triangle doesn't fit in one of our
+    // children and so we want to just record it here
+    _triangleIndices.push_back(triangleIndex);
+}
+
+bool TriangleSet::TriangleOctreeCell::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
+                float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched) {
+
+    if (_population < 1) {
+        return false; // no triangles below here, so we can't intersect
+    }
+
+    float bestLocalDistance = distance;
+    BoxFace bestLocalFace;
+    glm::vec3 bestLocalNormal;
+    bool intersects = false;
+
+    // if the ray intersects our bounding box, then continue
+    if (getBounds().findRayIntersection(origin, direction, bestLocalDistance, bestLocalFace, bestLocalNormal)) {
+
+        // if the intersection with our bounding box, is greater than the current best distance (the distance passed in)
+        // then we know that none of our triangles can represent a better intersection and we can return
+
+        if (bestLocalDistance > distance) {
+            return false;
+        }
+
+        bestLocalDistance = distance;
+
+        float childDistance = distance;
+        BoxFace childFace;
+        glm::vec3 childNormal;
+
+        // if we're not yet at the max depth, then check which child the triangle fits in
+        if (_depth < MAX_DEPTH) {
+            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
+                if (child.second.findRayIntersection(origin, direction, childDistance, childFace, childNormal, precision, trianglesTouched)) {
+                    if (childDistance < bestLocalDistance) {
+                        bestLocalDistance = childDistance;
+                        bestLocalFace = childFace;
+                        bestLocalNormal = childNormal;
+                        intersects = true;
+                    }
+                }
+            }
+        }
+        // also check our local triangle set
+        if (findRayIntersectionInternal(origin, direction, childDistance, childFace, childNormal, precision, trianglesTouched)) {
+            if (childDistance < bestLocalDistance) {
+                bestLocalDistance = childDistance;
+                bestLocalFace = childFace;
+                bestLocalNormal = childNormal;
+                intersects = true;
+            }
+        }
+    }
+    if (intersects) {
+        distance = bestLocalDistance;
+        face = bestLocalFace;
+        surfaceNormal = bestLocalNormal;
+    }
+    return intersects;
+}

libraries/shared/src/TriangleSet.h

@@ -15,19 +15,64 @@
 #include "GeometryUtil.h"
 
 class TriangleSet {
-public:
-    void reserve(size_t size) { _triangles.reserve(size); } // reserve space in the datastructure for size number of triangles
-    size_t size() const { return _triangles.size(); } 
 
-    const Triangle& getTriangle(size_t t) const { return _triangles[t]; }
+    class TriangleOctreeCell {
+    public:
+        TriangleOctreeCell(std::vector<Triangle>& allTriangles) :
+            _allTriangles(allTriangles)
+        { }
+
+        void insert(size_t triangleIndex);
+        void reset(const AABox& bounds, int depth = 0);
+        void clear();
+
+        bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
+            float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched);
+
+        const AABox& getBounds() const { return _bounds; }
+
+        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, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched);
+
+        std::vector<Triangle>& _allTriangles;
+        std::map<AABox::OctreeChild, TriangleOctreeCell> _children;
+        int _depth{ 0 };
+        int _population{ 0 };
+        AABox _bounds;
+        std::vector<size_t> _triangleIndices;
+
+        friend class TriangleSet;
+    };
+
+public:
+    TriangleSet() :
+        _triangleOctree(_triangles)
+    {}
+
+    void debugDump();
 
     void insert(const Triangle& t);
+
+    bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
+        float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision);
+
+    void balanceOctree();
+
+    void reserve(size_t size) { _triangles.reserve(size); } // reserve space in the datastructure for size number of triangles
+    size_t size() const { return _triangles.size(); }
     void clear();
 
     // Determine if 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 findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, 
-        float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision) const;
+    // note: this might side-effect internal structures
+    bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
+        float& distance, BoxFace& face, glm::vec3& surfaceNormal, bool precision, int& trianglesTouched);
 
     // Determine if a point is "inside" all the triangles of a convex hull. It is the responsibility of the caller to
     // determine that the triangle set is indeed a convex hull. If the triangles added to this set are not in fact a 
@@ -35,7 +80,10 @@ public:
     bool convexHullContains(const glm::vec3& point) const;
     const AABox& getBounds() const { return _bounds; }
 
-private:
+protected:
+
+    bool _isBalanced{ false };
+    TriangleOctreeCell _triangleOctree;
     std::vector<Triangle> _triangles;
     AABox _bounds;
 };

scripts/developer/tests/performance/rayPickPerformance.js

@@ -0,0 +1,131 @@
+//
+//  rayPickingPerformance.js
+//  examples
+//
+//  Created by Brad Hefta-Gaub on 5/13/2017
+//  Copyright 2017 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+
+
+var MIN_RANGE = -3;
+var MAX_RANGE = 3;
+var RANGE_DELTA = 0.5;
+var OUTER_LOOPS = 10;
+
+// NOTE: These expected results depend completely on the model, and the range settings above
+var EXPECTED_TESTS = 1385 * OUTER_LOOPS;
+var EXPECTED_INTERSECTIONS = 1286 * OUTER_LOOPS;
+
+
+var center = Vec3.sum(MyAvatar.position, Vec3.multiply(3, Quat.getFront(Camera.getOrientation())));
+var model_url = "http://hifi-content.s3.amazonaws.com/caitlyn/production/Scansite/buddhaReduced.fbx";
+
+var rayPickOverlays = Array();
+
+var modelEntity = Entities.addEntity({
+    type: "Model",
+    modelURL: model_url,
+    dimensions: {
+        x: 0.671,
+        y: 1.21,
+        z: 0.938
+    },
+    position: center
+});
+
+function rayCastTest() {
+    var tests = 0;
+    var intersections = 0;
+
+    var testStart = Date.now();
+    for (var t = 0; t < OUTER_LOOPS; t++) {
+        print("beginning loop:" + t);
+        for (var x = MIN_RANGE; x < MAX_RANGE; x += RANGE_DELTA) {
+            for (var y = MIN_RANGE; y < MAX_RANGE; y += RANGE_DELTA) {
+                for (var z = MIN_RANGE; z < MAX_RANGE; z += RANGE_DELTA) {
+                    if ((x <= -2 || x >= 2) || 
+                        (y <= -2 || y >= 2) || 
+                        (z <= -2 || z >= 2)) {
+
+                        tests++;
+
+                        var origin = { x: center.x + x,
+                                       y: center.y + y,
+                                       z: center.z + z };
+                        var direction = Vec3.subtract(center, origin);
+
+                        var pickRay = { 
+                            origin: origin,
+                            direction: direction
+                        };
+
+                        var pickResults = Entities.findRayIntersection(pickRay, true);
+
+                        var color;
+                        var visible;
+
+                        if (pickResults.intersects && pickResults.entityID == modelEntity) {
+                            intersections++;
+                            color = {
+                                red: 0,
+                                green: 255,
+                                blue: 0
+                            };
+                            visible = false;
+
+                        } else {
+                            /*
+                            print("NO INTERSECTION?");
+                            Vec3.print("origin:", origin);
+                            Vec3.print("direction:", direction);
+                            */
+
+                            color = {
+                                red: 255,
+                                green: 0,
+                                blue: 0
+                            };
+                            visible = true;
+                        }
+
+                        var overlayID = Overlays.addOverlay("line3d", {
+                            color: color,
+                            alpha: 1,
+                            visible: visible,
+                            lineWidth: 2,
+                            start: origin,
+                            end: Vec3.sum(origin,Vec3.multiply(5,direction))
+                        });
+
+                        rayPickOverlays.push(overlayID);
+
+                    }
+                }
+            }
+        }
+        print("ending loop:" + t);
+    }
+    var testEnd = Date.now();
+    var testElapsed = testEnd - testStart;
+
+
+    print("EXPECTED tests:" + EXPECTED_TESTS + " intersections:" + EXPECTED_INTERSECTIONS);
+    print("ACTUAL   tests:" + tests + " intersections:" + intersections);
+    print("ELAPSED TIME:" + testElapsed + " ms");
+
+}
+
+function cleanup() {
+    Entities.deleteEntity(modelEntity);
+    rayPickOverlays.forEach(function(item){
+        Overlays.deleteOverlay(item);
+    });
+}
+
+Script.scriptEnding.connect(cleanup);
+
+rayCastTest(); // run ray cast test immediately