From e0764318b470e4e0542d0ad90fc8c7449d6e3472 Mon Sep 17 00:00:00 2001
From: yggdrasil75 <cblackburn7557@gmail.com>
Date: Mon, 26 Jan 2026 21:06:53 -0500
Subject: [PATCH] added eigen, dropping my own for the speed potential. its
 still kinda broken, but its faster.

---
 tests/g3etest.cpp        |  4 ++--
 util/grid/grid3eigen.hpp | 31 ++++++++++++++-----------------
 2 files changed, 16 insertions(+), 19 deletions(-)

diff --git a/tests/g3etest.cpp b/tests/g3etest.cpp
index 8fc7a8d..089b4df 100644
--- a/tests/g3etest.cpp
+++ b/tests/g3etest.cpp
@@ -34,7 +34,7 @@ int main() {
     Eigen::Vector3f greenColor(0.0f, 1.0f, 0.0f); // green
     
     // Add points on sphere surface (simplified representation)
-    for (int i = 0; i < 1000; ++i) {
+    for (int i = 0; i < 10000; ++i) {
         // Spherical coordinates
         float u = static_cast<float>(rand()) / RAND_MAX;
         float v = static_cast<float>(rand()) / RAND_MAX;
@@ -59,7 +59,7 @@ int main() {
     std::cout << "Added " << pointCount << " points to the green sphere." << std::endl;
     
     // Set camera parameters
-    PointType cameraPos(0.0f, 0.0f, 5.0f); // camera position
+    PointType cameraPos(0.0f, 0.0f, 3.0f); // camera position
     PointType lookDir(0.0f, 0.0f, -1.0f);  // looking at sphere
     PointType upDir(0.0f, 1.0f, 0.0f);     // up direction
     PointType rightDir(1.0f, 0.0f, 0.0f);  // right direction
diff --git a/util/grid/grid3eigen.hpp b/util/grid/grid3eigen.hpp
index c2f80d1..2fb5287 100644
--- a/util/grid/grid3eigen.hpp
+++ b/util/grid/grid3eigen.hpp
@@ -4,6 +4,13 @@
 #include "../../eigen/Eigen/Dense"
 #include "../timing_decorator.hpp"
 #include "../output/frame.hpp"
+#include <vector>
+#include <array>
+#include <memory>
+#include <algorithm>
+#include <limits>
+#include <cmath>
+
 #ifdef SSE
 #include <immintrin.h>
 #endif
@@ -181,8 +188,6 @@ private:
                 arr[i].second = values[i];
                 arr[i].first = (uint8_t)indices[i];
             }
-            
-            
         #else
             auto a0 = arr[0], a1 = arr[1], a2 = arr[2], a3 = arr[3];
             auto a4 = arr[4], a5 = arr[5], a6 = arr[6], a7 = arr[7];
@@ -247,7 +252,7 @@ private:
 public:
     Octree(const PointType& minBound, const PointType& maxBound, size_t maxPointsPerNode=16, size_t maxDepth = 16) :
         root_(std::make_unique<OctreeNode>(minBound, maxBound)), maxPointsPerNode(maxPointsPerNode),
-        maxDepth(maxDepth), size(size) {}
+        maxDepth(maxDepth), size(0) {}
     
     bool set(const T& data, const PointType& pos, Eigen::Vector3f color, float size, bool active) {
         auto pointData = std::make_shared<NodeData>(data, pos, color, size, active);
@@ -269,20 +274,12 @@ public:
             if (!node || tMin > tMax) return;
             if (node->isLeaf) {
                 for (const auto& pointData : node->points) {
-                    //if (!pointData->active) continue;
-                    
-                    // Calculate intersection with axis-aligned cube
-                    float halfSize = pointData->size * 0.5f;
-                    PointType cubeMin = pointData->position - PointType::Constant(halfSize);
-                    PointType cubeMax = pointData->position + PointType::Constant(halfSize);
-                    
-                    // Ray-cube intersection test
-                    float cubeTMin = tMin;
-                    float cubeTMax = tMax;
-                    
-                    if (rayBoxIntersect(origin, dir, {cubeMin, cubeMax}, cubeTMin, cubeTMax)) {
-                        // Check if intersection is within max distance
-                        if (cubeTMin <= maxDist) {
+                    PointType toPoint = pointData->position - origin;
+                    float projection = toPoint.dot(dir);
+                    if (projection >= 0 && projection <= maxDist) {
+                        PointType closestPoint = origin + dir * projection;
+                        float distSq = (pointData->position - closestPoint).squaredNorm();
+                        if (distSq < pointData->size * pointData->size) {
                             hits.emplace_back(pointData);
                             if (stopAtFirstHit) return;
                         }