added missing code changes

started working on refactoring of sensors
new test-cases

tests/nav/dijkstra/TestDijkstra.cpp

@@ -48,4 +48,68 @@ TEST(Dijkstra, build) {
 
 }
 
+void build(Grid<GP>& grid) {
+
+	const int size = 10000;
+	const int gs = grid.getGridSize_cm();
+
+	for (int x = 0; x < size; x += gs) {
+		for (int y = 0; y < size; y += gs) {
+			grid.add(GP(x,y,0));
+		}
+	}
+
+	std::set<int> done;
+
+	for (int x = 0; x < size; x += gs) {
+		for (int y = 0; y < size; y += gs) {
+
+			const GridPoint gp1(x,y,0);
+			const int idx1 = grid.getNodeFor(gp1).getIdx();
+
+			for (int x1 = -gs; x1 <= +gs; x1 += gs) {
+				for (int y1 = -gs; y1 <= +gs; y1 += gs) {
+					const GridPoint gp2(x+x1, y+y1, 0);
+					if (grid.hasNodeFor(gp2)) {
+						int idx2 = grid.getNodeFor(gp2).getIdx();
+						if (done.find(idx2) != done.end()) {continue;}
+						grid.connectBiDir(idx1, idx2);
+					}
+
+				}
+			}
+
+			done.insert(idx1);
+
+		}
+	}
+
+}
+
+void dijkstra(Grid<GP>& grid) {
+
+	class TMP {
+		Grid<GP>& grid;
+	public:
+		TMP(Grid<GP>& grid) : grid(grid) {;}
+		int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
+		const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
+		float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();}
+	} tmp(grid);
+
+	Dijkstra<GP> d;
+	d.build(grid[0], grid[0], tmp);
+
+}
+
+TEST(Dijkstra, huge) {
+
+
+	Grid<GP> grid(10);
+	build(grid);
+	dijkstra(grid);
+
+}
+
+
 #endif