-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathmain.cpp
More file actions
91 lines (80 loc) · 2.28 KB
/
Copy pathmain.cpp
File metadata and controls
91 lines (80 loc) · 2.28 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
#include <iostream>
#include <fstream>
#include <unordered_set>
#include <chrono>
#include <thread>
#include "../include/Dummy.hpp"
#include "../include/PathFinder.hpp"
struct IntCoord {
public:
IntCoord() : x{0}, y{0} { }
IntCoord(int x, int y) : x{x}, y{y} { }
int x, y;
bool operator==(const IntCoord &lhs) const {
return x == lhs.x && y == lhs.y;
}
};
namespace std {
template<>
struct hash<IntCoord> {
std::size_t operator()(const IntCoord &coord) const {
return std::hash<int>()(coord.x)
^ (std::hash<int>()(coord.y) << (sizeof(int) / 2));
}
};
}
// prints out a .pgm (portable grey map) image of the map and it's path
#define SCALE 4
void printMapWithPath(std::ostream &out, Map &map,
std::unordered_set<IntCoord> path) {
out << "P3" << std::endl;
out << map.sizeX * SCALE << " " << map.sizeY * SCALE << " 2" << std::endl;
for (int i1 = 0; i1 < map.sizeY * SCALE; i1++) {
for (int i2 = 0; i2 < map.sizeX * SCALE; i2++) {
int groundVal = 1 - map.map[i1 / SCALE][i2 / SCALE];
if (path.find(IntCoord(i1, i2)) != path.end()) {
out << "2 " << 0 << " " << 0 << " ";
} else {
out << groundVal * 2 << " " << groundVal * 2 << " " <<
groundVal * 2 << " ";
}
}
out << std::endl;
}
}
int main() {
// debugging code for visualisation of paths
int mapX = 300, mapY = 300, mapCount = 0;
bool done = false;
while (!done) {
Map map = {mapX, mapY, 0.3f};
PathFinder pathFinder = {map,
{0.5 * Length::METER,
0.5 * Length::METER,
0 * Length::METER}};
std::vector<Coordinate> path;
done = pathFinder.get_path_to_coordinate(
{5.5f * Length::METER,
5.5f * Length::METER,
0.0f * Length::METER},
{(mapX - 5.5f)* Length::METER,
(mapY - 5.5f) * Length::METER,
0.0f * Length::METER},
path);
mapCount++;
if (done) {
std::unordered_set<IntCoord> intPath;
for (Coordinate &coord : path) {
std::cout << coord << std::endl;
intPath.emplace(coord.get_x() / Length::METER * SCALE, coord.get_y() / Length::METER * SCALE);
}
std::cout.flush();
std::ofstream ofs{"path.pgm"};
printMapWithPath(ofs, map, intPath);
ofs.flush();
std::cout << "searched " << mapCount << " maps" << std::endl;
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
return 0;
}