vector<POINT> Exploration::planPath(POINT &dest, MAP &map) {
int cellSize = ROBOT_SIZE / map.getDelta();
int* grid = GridConverter::createAstar(map, cellSize);
int cols = map.getMapSizeX() / cellSize;
int rows = map.getMapSizeY() / cellSize;
int sourceX = _currentPos.x / cellSize;
int sourceY = _currentPos.y / cellSize;
int destX = dest.x / cellSize + 1;
int destY = dest.y / cellSize - 1;
vector<node> pathGrid = _planner.get_graph(grid, cols, rows, sourceX, sourceY, destX, destY);
delete grid;
//DEBUG
//cout << "Source is " << sourceX << "," << sourceY << endl;
//cout << "Dest is " << destX << "," << destY << endl;
//End DEBUG
vector<POINT> pathMap;
//TODO: need to fix 90 orientation and transform to map coordinations
for (int i = 0; i < (int) pathGrid.size(); ++i) {
int x = (double)pathGrid[i].x / cellSize + (double)cellSize / 2;
int y = (double)pathGrid[i].y / cellSize + (double)cellSize / 2;
POINT p(x, y);
pathMap.push_back(p);
}
//DEBUG
// int limitX, limitY;
// limitX = map.getMapSizeX() / cellSize;
// limitY = map.getMapSizeY() / cellSize;
// int* testGrid = GridConverter::createAstar(map, cellSize);
// for (int y = 270; y > 230; --y) {
// for (int x = 230; x < 270; ++x) {
// if (x == 250 && y == 250)
// cout << "X";
// else if (x == sourceX && y == sourceY)
// cout << "S";
// else if (x == destX && y == destY)
// cout << "T";
// else
// cout << testGrid[x + (y*limitX)];
// }
// cout << endl;
// }
// delete testGrid;
//End DEBUG
return pathMap;
}
