49 lines
1.3 KiB
C++
49 lines
1.3 KiB
C++
#ifdef WITH_TESTS
|
|
|
|
#include "../../Tests.h"
|
|
|
|
#include "../../../grid/Grid.h"
|
|
#include "../../../nav/a-star/AStar.h"
|
|
#include "../../grid/Plot.h"
|
|
|
|
TEST(AStar, 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);}
|
|
const std::vector<GP>& getAllNodes() const {return grid.getNodes();}
|
|
decltype(grid.neighbors(GP())) getNeighbors(const GP& node) const {return grid.neighbors(node);}
|
|
float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();}
|
|
float getHeuristic(const GP& n1, const GP& n2) const {return std::abs(n1.x_cm - n2.x_cm) + std::abs(n1.y_cm - n2.y_cm);}
|
|
} tmp(grid);
|
|
|
|
AStar<GP> nav;
|
|
std::vector<const GP*> vec = nav.get(&grid[0], &grid[4], tmp);
|
|
|
|
for (const GP* g : vec) {
|
|
std::cout << g << std::endl;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif
|