terrible branch.

util/grid/grid3eigen.hpp

@@ -89,6 +89,13 @@ public:
         }
     };
 
+    struct RaycastHit {
+        std::shared_ptr<NodeData> node;
+        float distance;
+        PointType normal;
+        PointType hitPoint;
+    };
+
     struct OctreeNode {
         BoundingBox bounds;
         std::vector<std::shared_ptr<NodeData>> points;
@@ -634,6 +641,89 @@ private:
         }
     }
 
+    void insertHit(std::vector<RaycastHit>& hits, size_t maxHits,  const std::shared_ptr<NodeData>& node, 
+                   float t, const PointType& normal, const PointType& hitPoint, float& maxDist) const {
+        for (const auto& h : hits) {
+            if (h.node == node) return;
+        }
+
+        auto it = std::lower_bound(hits.begin(), hits.end(), t, 
+            [](const RaycastHit& a, float val) {
+                return a.distance < val;
+            });
+            
+        hits.insert(it, {node, t, normal, hitPoint});
+        
+        if (hits.size() > maxHits) {
+            hits.pop_back();
+        }
+        
+        if (hits.size() == maxHits) {
+            maxDist = std::min(maxDist, hits.back().distance);
+        }
+    }
+
+    void voxelTraverseMultipleRecursive(OctreeNode* node, float tMin, float tMax, float& maxDist, bool enableLOD,
+                                        const Ray& ray, std::vector<RaycastHit>& hits, size_t maxHits, float invLodf) const {
+        if (enableLOD && !node->isLeaf) {
+            float dist = (node->center - ray.origin).norm();
+            float ratio = dist / node->nodeSize;
+            
+            if (dist > lodMinDistance_ && ratio > invLodf && node->lodData) {
+                float t;
+                PointType n;
+                PointType h;
+                if (rayCubeIntersect(ray, node->lodData.get(), t, n, h)) {
+                    if (t >= 0 && t <= maxDist) {
+                        insertHit(hits, maxHits, node->lodData, t, n, h, maxDist);
+                    }
+                }
+                return;
+            }
+        }
+
+        for (const auto& pointData : node->points) {
+            if (!pointData->active) continue;
+            
+            float t;
+            PointType normal, hitPoint;
+            if (rayCubeIntersect(ray, pointData.get(), t, normal, hitPoint)) {
+                if (t >= 0 && t <= maxDist && t <= tMax + 0.001f) {
+                    insertHit(hits, maxHits, pointData, t, normal, hitPoint, maxDist);
+                }
+            }
+        }
+
+        if (node->isLeaf) return;
+
+        // DDA Traversal
+        PointType center = node->center;
+        Eigen::Vector3f ttt = (center - ray.origin).cwiseProduct(ray.invDir);
+
+        int currIdx = 0;
+        currIdx = ((tMin >= ttt.x()) ? 1 : 0) | ((tMin >= ttt.y()) ? 2 : 0) | ((tMin >= ttt.z()) ? 4 : 0);
+        
+        float tNext;
+
+        while(tMin < tMax && tMin <= maxDist) {
+            Eigen::Vector3f next_t;
+            next_t[0] = (currIdx & 1) ? tMax : ttt[0];
+            next_t[1] = (currIdx & 2) ? tMax : ttt[1];
+            next_t[2] = (currIdx & 4) ? tMax : ttt[2];
+
+            tNext = next_t.minCoeff();
+
+            int physIdx = currIdx ^ ray.signMask;
+
+            if (node->children[physIdx]) {
+                voxelTraverseMultipleRecursive(node->children[physIdx].get(), tMin, tNext, maxDist, enableLOD, ray, hits, maxHits, invLodf);
+            }
+
+            tMin = tNext;
+            currIdx |= ((next_t[0] <= tNext) ? 1 : 0) | ((next_t[1] <= tNext) ? 2 : 0) | ((next_t[2] <= tNext) ? 4 : 0);
+        }
+    }
+
     PointType sampleGGX(const PointType& n, float roughness, uint32_t& state) const {
         float alpha = std::max(EPSILON, roughness * roughness);
         float r1 = float(rand_r(&state)) / float(RAND_MAX);
@@ -2280,6 +2370,11 @@ public:
         
         size = 0;
     }
+
+    void collectNodesByObjectId(int id, std::vector<std::shared_ptr<NodeData>>& results) const {
+        std::unordered_set<std::shared_ptr<NodeData>> seen;
+        collectNodesByObjectIdRecursive(root_.get(), id, results, seen);
+    }
 };
 
 #endif