205 lines
5.9 KiB
C++
205 lines
5.9 KiB
C++
#ifdef WITH_TESTS
|
|
|
|
#include "../Tests.h"
|
|
|
|
#include "../../navMesh/NavMeshFactory.h"
|
|
#include "../../navMesh/walk/NavMeshSub.h"
|
|
#include "../../navMesh/meta/NavMeshDijkstra.h"
|
|
#include "../../navMesh/NavMeshDebug.h"
|
|
|
|
#include "../../floorplan/v2/FloorplanReader.h"
|
|
|
|
|
|
using namespace NM;
|
|
|
|
struct MyNMT1231902345 : public NM::NavMeshTriangle, public NM::NavMeshTriangleDijkstra {
|
|
|
|
MyNMT1231902345(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) : NavMeshTriangle(p1, p2, p3, type) {;}
|
|
|
|
};
|
|
|
|
TEST(NavMeshDijkstra, build) {
|
|
|
|
NavMeshSettings set;
|
|
NavMesh<MyNMT1231902345> nm;
|
|
|
|
|
|
|
|
Floorplan::IndoorMap map;
|
|
Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
|
|
Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
|
|
|
|
// circle (many triangles)
|
|
int i = 0;
|
|
for (float f = 0; f < M_PI*2; f += 0.8) {
|
|
const float x = std::cos(f) * 10;
|
|
const float y = std::sin(f) * 10;
|
|
outline.poly.points.push_back(Point2(x,y));
|
|
++i;
|
|
}
|
|
|
|
outline.outdoor = false;
|
|
outline.method = Floorplan::OutlineMethod::ADD;
|
|
|
|
Floorplan::FloorOutlinePolygon remove; floor.outline.push_back(&remove);
|
|
remove.outdoor = false;
|
|
remove.method = Floorplan::OutlineMethod::REMOVE;
|
|
remove.poly.points.push_back(Point2(+4,-3));
|
|
remove.poly.points.push_back(Point2(+11,-3));
|
|
remove.poly.points.push_back(Point2(+11,+1));
|
|
remove.poly.points.push_back(Point2(+4,+1));
|
|
|
|
Floorplan::FloorOutlinePolygon remove2; floor.outline.push_back(&remove2);
|
|
remove2.outdoor = false;
|
|
remove2.method = Floorplan::OutlineMethod::REMOVE;
|
|
remove2.poly.points.push_back(Point2(-11,-2));
|
|
remove2.poly.points.push_back(Point2(-2,-2));
|
|
remove2.poly.points.push_back(Point2(-2,+2));
|
|
remove2.poly.points.push_back(Point2(-11,+2));
|
|
|
|
NavMeshFactory<MyNMT1231902345> fac(&nm, set);
|
|
fac.build(&map);
|
|
|
|
// nm.add(Point3(0,0,0), Point3(10,10,0), Point3(0,10,0), 1);
|
|
// nm.add(Point3(0,10,0), Point3(10,10,0), Point3(0,20,0), 1);
|
|
// nm.add(Point3(0,20,0), Point3(10,10,0), Point3(10,20,0), 1);
|
|
|
|
|
|
NavMeshDebug dbg;
|
|
dbg.addMesh(nm);
|
|
dbg.draw();
|
|
|
|
NM::NavMeshDijkstra::stamp(nm, Point3(4,4,0));
|
|
|
|
dbg.addDijkstra(nm);
|
|
dbg.draw();
|
|
|
|
int xxx = 0; (void) xxx;
|
|
|
|
}
|
|
|
|
|
|
TEST(NavMeshDijkstra, museum) {
|
|
|
|
NavMeshSettings set;
|
|
NavMesh<MyNMT1231902345> nm;
|
|
|
|
//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/map_stair1.xml");
|
|
//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/SHL41_nm.xml");
|
|
//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/museum31.xml");
|
|
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/home/toni/Documents/programme/localization/maps/museum/map48_ap_path3_smallfixes.xml");
|
|
|
|
NavMeshFactory<MyNMT1231902345> fac(&nm, set);
|
|
fac.build(map);
|
|
|
|
NavMeshDebug dbg;
|
|
dbg.addMesh(nm);
|
|
dbg.draw();
|
|
|
|
NM::NavMeshRandom<MyNMT1231902345> rnd = nm.getRandom();
|
|
const Point3 dst = Point3(24,24,1.7f);
|
|
|
|
NM::NavMeshDijkstra::stamp(nm, dst);
|
|
dbg.addDijkstra(nm);
|
|
dbg.draw();
|
|
|
|
for (int i = 0; i < 1000; ++i) {
|
|
|
|
NM::NavMeshLocation<MyNMT1231902345> start = rnd.draw();
|
|
start.tria->getDistanceToDestination<MyNMT1231902345>(start.pos); // just a compiler-test
|
|
|
|
NM::NavMeshDijkstra::stamp(nm, dst);
|
|
|
|
//NM::NavMeshLocation<MyNMT1231902345> start = start;//nm.getLocation(Point3(0,-6,0));
|
|
std::vector<NM::NavMeshLocation<MyNMT1231902345>> path = start.tria->getPathToDestination<MyNMT1231902345>(start.pos);
|
|
std::cout << path.size() << std::endl;
|
|
|
|
dbg.addDijkstra(nm);
|
|
dbg.addDijkstra(path);
|
|
dbg.draw();
|
|
|
|
sleep(1);
|
|
|
|
}
|
|
|
|
int xxx = 0; (void) xxx;
|
|
|
|
}
|
|
|
|
TEST(NavMeshDijkstra, path) {
|
|
|
|
NavMeshSettings set;
|
|
NavMesh<MyNMT1231902345> nm;
|
|
|
|
// Floorplan::IndoorMap map;
|
|
// Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
|
|
// Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);
|
|
|
|
// // circle (many triangles)
|
|
// int i = 0;
|
|
// for (float f = 0; f < M_PI*2; f += 0.8) {
|
|
// const float x = std::cos(f) * 10;
|
|
// const float y = std::sin(f) * 10;
|
|
// outline.poly.points.push_back(Point2(x,y));
|
|
// ++i;
|
|
// }
|
|
|
|
// outline.outdoor = false;
|
|
// outline.method = Floorplan::OutlineMethod::ADD;
|
|
|
|
// Floorplan::FloorOutlinePolygon remove; floor.outline.push_back(&remove);
|
|
// remove.outdoor = false;
|
|
// remove.method = Floorplan::OutlineMethod::REMOVE;
|
|
// remove.poly.points.push_back(Point2(+4,-3));
|
|
// remove.poly.points.push_back(Point2(+11,-3));
|
|
// remove.poly.points.push_back(Point2(+11,+1));
|
|
// remove.poly.points.push_back(Point2(+4,+1));
|
|
|
|
// Floorplan::FloorOutlinePolygon remove2; floor.outline.push_back(&remove2);
|
|
// remove2.outdoor = false;
|
|
// remove2.method = Floorplan::OutlineMethod::REMOVE;
|
|
// remove2.poly.points.push_back(Point2(-11,-2));
|
|
// remove2.poly.points.push_back(Point2(-2,-2));
|
|
// remove2.poly.points.push_back(Point2(-2,+2));
|
|
// remove2.poly.points.push_back(Point2(-11,+2));
|
|
|
|
//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/mnt/vm/paper/diss/data/maps/map_stair1.xml");
|
|
//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/map_stair1.xml");
|
|
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/home/toni/Documents/programme/localization/maps/test/map_stair1.xml");
|
|
|
|
|
|
NavMeshFactory<MyNMT1231902345> fac(&nm, set);
|
|
fac.build(map);
|
|
|
|
|
|
NavMeshDebug dbg;
|
|
dbg.addMesh(nm);
|
|
dbg.draw();
|
|
|
|
NM::NavMeshRandom<MyNMT1231902345> rnd = nm.getRandom();
|
|
const Point3 dst = rnd.draw().pos;
|
|
|
|
for (int i = 0; i < 1000; ++i) {
|
|
|
|
|
|
NM::NavMeshLocation<MyNMT1231902345> start = rnd.draw();
|
|
start.tria->getDistanceToDestination<MyNMT1231902345>(start.pos); // just a compiler-test
|
|
|
|
NM::NavMeshDijkstra::stamp(nm, dst);
|
|
|
|
//NM::NavMeshLocation<MyNMT1231902345> start = start;//nm.getLocation(Point3(0,-6,0));
|
|
std::vector<NM::NavMeshLocation<MyNMT1231902345>> path = start.tria->getPathToDestination<MyNMT1231902345>(start.pos);
|
|
std::cout << path.size() << std::endl;
|
|
|
|
dbg.addDijkstra(nm);
|
|
dbg.addDijkstra(path);
|
|
dbg.draw();
|
|
|
|
sleep(1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|