From 9cef922b684c84fa2a8c74468d9db0216d24daac Mon Sep 17 00:00:00 2001
From: Nick Stathas <nistath@mit.edu>
Date: Tue, 29 Oct 2019 09:51:49 -0400
Subject: [PATCH] Eigen is probably bad at this.

---
 fast_raycast/Makefile     |  2 +-
 fast_raycast/src/main.cpp | 11 ++++++-----
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/fast_raycast/Makefile b/fast_raycast/Makefile
index 50fd6c6..a751394 100644
--- a/fast_raycast/Makefile
+++ b/fast_raycast/Makefile
@@ -27,7 +27,7 @@ build:
 	@mkdir -p $(APP_DIR)
 	@mkdir -p $(OBJ_DIR)
 
-debug: CXXFLAGS += -DDEBUG -g -Og
+debug: CXXFLAGS += -DEIGEN_NO_DEBUG -DDEBUG -g -Og
 debug: all
 
 release: CXXFLAGS += -O3 -ffast-math
diff --git a/fast_raycast/src/main.cpp b/fast_raycast/src/main.cpp
index 14bc407..1ed3764 100644
--- a/fast_raycast/src/main.cpp
+++ b/fast_raycast/src/main.cpp
@@ -160,11 +160,12 @@ class Cone {
     auto sol = ((-c1 - dis) / c2).min((-c1 + dis) / c2);
 
     if (heightLimit) {
-      auto hit_height = (L.transpose() * direction_).array() +
-                        (sol * (U.transpose() * direction_).array());
+      Coeffs sol2 = sol;
+      Coeffs hit_height = (L.transpose() * direction_).array() +
+                          (sol2 * (U.transpose() * direction_).array());
 
       solutions =
-          ((0 <= hit_height) && (hit_height <= height_)).select(sol, nan());
+          ((0 <= hit_height) && (hit_height <= height_)).select(sol2, nan());
     } else {
       solutions = sol;
     }
@@ -189,8 +190,8 @@ int main() {
   constexpr el_t VFOV = M_PI / 6;
   constexpr el_t VBIAS = -M_PI / 2;
 
-  constexpr int NRings = 20;
-  constexpr int NPoints = 20;
+  constexpr int NRings = 20000;
+  constexpr int NPoints = 2000;
   constexpr int NRays = NPoints * NRings;
   Rays<Dynamic> rays = Rays<NRays>::Zero();
   rays.origins().col(2) = decltype(rays.origins().col(2))::Ones(NRays, 1);