Added map

glnav.h

@@ -13,6 +13,7 @@
 #include "inc/glnav_heading.h"
 #include "inc/glnav_traveler.h"
 #include "inc/glnav_dispatch.h"
+#include "inc/glnav_map.h"
 #include "inc/glnav_dijkstra.h"
 // #include "inc/glnav_astar.h"
 

inc/glnav_map.h

@@ -0,0 +1,48 @@
+#ifndef GLNAV_MAP_H
+#define GLNAV_MAP_H
+
+#include "con3/con3.h"
+#include "glnav_network.h"
+#include "glnav_path.h"
+#include "glnav_obstacle.h"
+
+namespace glnav
+{
+    template<typename T>
+    class map : public con3::set<obstacle<T> *>
+    {
+    public:
+        map()
+            : con3::set<obstacle<T> *>()
+        { }
+
+        bool obstructs(const path<T> &input)
+        {
+            for(size_t i = 0; i < this->values().size(); i++)
+            {
+                if(this->at(i)->obstructs(input)) return true;
+            }
+            return false;
+        }
+
+        std::set<point<T> > corners()
+        {
+            std::set<point<T> > result;
+
+            // Iterate through the obstacles
+            for(size_t i = 0; i < this->values().size(); i++)
+            {
+                const point_group<T> corners = this->at(i)->corners();
+
+                // Iterate through the corners
+                for(size_t j = 0; j < corners.size(); j++)
+                {
+                    result.insert(corners.at(j));
+                }
+            }
+            return result;
+        }
+    };
+}
+
+#endif

test/map.cpp

@@ -0,0 +1,25 @@
+#include "glnav.h"
+#include "test.h"
+
+int main(void)
+{
+    glnav::map<int> test;
+    glnav::obstacle<int> test_obs = glnav::obstacle<int>::square(
+        glnav::point<int>(1, 1),
+        4
+    );
+    test.force_add(&test_obs);
+    TEST_EQUAL(test.values().size(), 1);
+
+    glnav::path<int> test_path(1, -10, 1, 10);
+    TEST_TRUE(test.obstructs(test_path));
+    test_path = glnav::path<int>(5, -10, 5, 10);
+    TEST_FALSE(test.obstructs(test_path));
+
+    std::set<glnav::point<int> > corners = test.corners();
+    TEST_EQUAL(corners.size(), 4);
+    TEST_TRUE(corners.find(glnav::point<int>(-1, -1)) != corners.end());
+    TEST_TRUE(corners.find(glnav::point<int>(-1, 3)) != corners.end());
+    TEST_TRUE(corners.find(glnav::point<int>(3, -1)) != corners.end());
+    TEST_TRUE(corners.find(glnav::point<int>(3, 3)) != corners.end());
+}