116 lines
2.7 KiB
C++
116 lines
2.7 KiB
C++
#ifdef WITH_TESTS
|
|
|
|
#include "../../Tests.h"
|
|
|
|
#include "../../../grid/Grid.h"
|
|
#include "../../../nav/dijkstra/Dijkstra.h"
|
|
#include "../../grid/Plot.h"
|
|
|
|
TEST(Dijkstra, build) {
|
|
|
|
Grid<GP> grid(1);
|
|
|
|
int idx1 = grid.add(GP( 0, 0, 0));
|
|
int idx2 = grid.add(GP( 0, 1, 0));
|
|
int idx3 = grid.add(GP( 0,-1, 0));
|
|
int idx4 = grid.add(GP( 1, 0, 0));
|
|
int idx5 = grid.add(GP(-1, 0, 0));
|
|
|
|
grid.connectBiDir(idx1, idx2);
|
|
grid.connectBiDir(idx1, idx3);
|
|
grid.connectBiDir(idx1, idx4);
|
|
grid.connectBiDir(idx1, idx5);
|
|
|
|
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 (n1.inCentimeter() - n2.inCentimeter()).length();}
|
|
} tmp(grid);
|
|
|
|
Dijkstra<GP> d;
|
|
d.build(&grid[idx5], &grid[idx3], tmp, 99999);
|
|
|
|
// start node must be "idx5"
|
|
DijkstraNode<GP>* n = d.getNode(grid[idx5]);
|
|
ASSERT_EQ(&grid[idx5], n->element); ASSERT_EQ(nullptr, n->previous); ASSERT_EQ(0, n->cumWeight);
|
|
|
|
// "idx1" (the center) is reached via idx5
|
|
DijkstraNode<GP>* n2 = d.getNode(grid[idx1]);
|
|
ASSERT_EQ(&grid[idx1], n2->element); ASSERT_EQ(&grid[idx5], n2->previous->element);
|
|
|
|
// "idx3" (the target) is reached via idx1 (the center)
|
|
DijkstraNode<GP>* n3 = d.getNode(grid[idx3]);
|
|
ASSERT_EQ(&grid[idx3], n3->element); ASSERT_EQ(&grid[idx1], n3->previous->element);
|
|
|
|
|
|
}
|
|
|
|
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 (n1.inCentimeter() - n2.inCentimeter()).length();}
|
|
} tmp(grid);
|
|
|
|
Dijkstra<GP> d;
|
|
d.build(&grid[0], tmp);
|
|
|
|
}
|
|
|
|
TEST(Dijkstra, huge) {
|
|
|
|
|
|
Grid<GP> grid(10);
|
|
build(grid);
|
|
dijkstra(grid);
|
|
|
|
}
|
|
|
|
|
|
#endif
|