maxDist adjustments

Author: joshhting

src/2dplane/ObstacleGrid.cpp

@@ -2,8 +2,6 @@
 #include <stdlib.h>
 #include <iostream>
 
-#define SQRT2 1.41421356237
-
 using namespace Eigen;
 using namespace std;
 
@@ -32,8 +30,8 @@ float ObstacleGrid::nearestObstacleDist(const Vector2f &state, float maxDist) co
     //x and y are the indices of the cell that state is located in
     float x = (state.x() / (_width / _discretizedWidth));
     float y = (state.y() / (_height / _discretizedHeight));
-    int xSearchRad = maxDist * _discretizedWidth / (_width * SQRT2);
-    int ySearchRad = maxDist * _discretizedHeight / (_height * SQRT2);
+    int xSearchRad = maxDist * _discretizedWidth / _width;
+    int ySearchRad = maxDist * _discretizedHeight / _height;
     //here we loop through the cells around (x,y) to find the minimum distance of the point to the nearest obstacle
     for (int i = x - xSearchRad; i <= x + xSearchRad; i++) {
         for (int j = y - ySearchRad; j <= y + ySearchRad; j++) {

src/2dplane/ObstacleGrid.hpp

@@ -14,8 +14,13 @@ class ObstacleGrid {
 
     Eigen::Vector2i gridSquareForLocation(const Eigen::Vector2f &loc) const;
 
-    // finds the distance from state to its neareset obstacle. Only searches up to maxDist around
-    // state so as to not waste time checking far away and irrelevant obstacles.
+    /** 
+     * Finds the distance from state to its neareset obstacle. Only searches up to maxDist around
+     * state so as to not waste time checking far away and irrelevant obstacles.
+     *
+     * @param state The location to search with respect to for the nearest obstacle dist
+     * @param maxDist The maximum vertical and horizontal distance from state to search for obstacles
+     */
     float nearestObstacleDist(const Eigen::Vector2f &state, float maxDist) const;
     void clear();
     bool &obstacleAt(int x, int y);