templated PlaneStateSpace

Author: justbuchanan

CMakeLists.txt

@@ -50,7 +50,6 @@ set(lib_SRC
     "src/rrt/2dplane/2dplane.cpp"
     "src/rrt/2dplane/GridStateSpace.cpp"
     "src/rrt/2dplane/ObstacleGrid.cpp"
-    "src/rrt/2dplane/PlaneStateSpace.cpp"
 )
 
 add_library("RRT" STATIC ${lib_SRC})

src/rrt/2dplane/GridStateSpace.hpp

@@ -10,7 +10,7 @@ namespace RRT {
  * @brief A 2d plane with continuous states and discretized obstacles.
  * @details The state space is broken up into a grid with the given discrete height and widths.
  */
-class GridStateSpace : public PlaneStateSpace {
+class GridStateSpace : public PlaneStateSpace<Eigen::Vector2f> {
 public:
     GridStateSpace(float width, float height, int discretizedWidth, int discretizedHeight);
 

src/rrt/2dplane/PlaneStateSpace.cpp

@@ -8,23 +8,41 @@ namespace RRT {
 /**
  * @brief A 2d plane with continuous states and no obstacles.
  */
-class PlaneStateSpace : public StateSpace<Eigen::Vector2f> {
+template<class POINT_CLASS = Eigen::Vector2f>
+class PlaneStateSpace : public StateSpace<POINT_CLASS> {
 public:
-    PlaneStateSpace(float width, float height);
+    PlaneStateSpace(float width, float height) : _width(width), _height(height) {}
 
-    Eigen::Vector2f randomState() const;
+    POINT_CLASS randomState() const {
+        return POINT_CLASS(drand48() * width(), drand48() * height());
+    }
 
-    Eigen::Vector2f intermediateState(const Eigen::Vector2f &source, const Eigen::Vector2f &target, float stepSize) const;
+    POINT_CLASS intermediateState(const POINT_CLASS &source, const POINT_CLASS &target, float stepSize) const {
+        POINT_CLASS delta = target - source;
+        delta = delta / delta.norm();   //  unit vector
+        
+        POINT_CLASS val = source + delta * stepSize;
+        return val;
+    }
 
-    double distance(const Eigen::Vector2f &from, const Eigen::Vector2f &to) const;
+
+    double distance(const POINT_CLASS &from, const POINT_CLASS &to) const {
+        POINT_CLASS delta = from - to;
+        return sqrtf(powf(delta.x(), 2) + powf(delta.y(), 2));
+    }
 
     /**
      * Returns a boolean indicating whether the given point is within bounds.
      */
-    bool stateValid(const Eigen::Vector2f &pt) const;
-
-    float width() const;
-    float height() const;
+    bool stateValid(const POINT_CLASS &pt) const {
+        return pt.x() >= 0
+            && pt.y() >= 0
+            && pt.x() < width()
+            && pt.y() < height();
+    }
+
+    float width() const { return _width; }
+    float height() const { return _height; }
 
 
 private:

src/rrt/2dplane/PlaneStateSpace.hpp

@@ -0,0 +1 @@
+Subproject commit 90cc111f04dc1baee0a99f75a5c850282b54bcde