I implemented A* and JPS(Jump Point Search) using VS2008.
Then i tried comparing time cost of these code.
On debug mode, (my) JPS is faster than A* about 2.0~50 times .
But on release mode, JPS is faster than A* about 0.6~3.0 times.
Especially, almost cases of test on release mode, JPS slower than A*.
Why results are so different?
In the paper( "Online Graph Pruning for Pathfinding on Grid Maps", 2011 ),
JPS is faster than A* about 20~30 times.
If i want to get a similar results in the paper, what should i do?
I just call map1.A_star() and map2.JPS() in main.cpp.
and I used prioiry_queue(STL) for A* and JPS.
↓ pathfinding.cpp
#include "util.h"
using namespace std;
int DIR_X[8] = { 0, 1, 1, 1, 0, -1, -1, -1 };
int DIR_Y[8] = { -1, -1, 0, 1, 1, 1, 0, -1 };
// diagonal index {1, 3, 5, 7}
template<class T>
void vector_clear(vector<T>& vecObj)
{
vector<T> tempObj;
tempObj.swap(vecObj);
}
bool operator<(const Node& a, const Node& b)
{
return a.getPriority() > b.getPriority();
}
void read_scenario(char* path, char(*scenarios)[256], int& total) {
ifstream scen_file(path);
char buffer[256];
int num = 0;
scen_file.getline(buffer, 256);
while (!scen_file.eof()) {
scen_file.getline(buffer, 256);
num++;
int index1;
int index2;
int count = 0;
for (int i = 0; i<256; i++) {
if (buffer[i] == ' ')
count++;
if (buffer[i] == ' ' && count == 4)
index1 = i;
if (buffer[i] == ' ' && count == 8)
index2 = i;
}
for (int i = index1 + 1; i <= index2 - 1; i++) {
scenarios[num][i - (index1 + 1)] = buffer[i];
}
scenarios[num][index2] = NULL;
}
std::cout << num << " 개의 시나리오가 있습니다." << endl;
total = num;
scen_file.close();
}
Map::Map(int* START_GOAL, char* IN_PATH, char* OUT_PATH, string MODE) {
sx = START_GOAL[0];
sy = START_GOAL[1];
gx = START_GOAL[2];
gy = START_GOAL[3];
mode = MODE;
in_path = IN_PATH;
out_path = OUT_PATH;
ifstream map_file(in_path);
if (!map_file.is_open()) {
std::cout << "there is no map_file" << endl;
}
char buffer[128];
char ch[4];
char cw[4];
map_file.getline(buffer, 128);
map_file.getline(buffer, 128);
for (int i = 7; i < strlen(buffer); i++) {
ch[i - 7] = buffer[i];
}
h = atoi(ch);
std::cout << "height = " << h << endl;;
map_file.getline(buffer, 128);
for (int i = 6; i < strlen(buffer); i++) {
cw[i - 6] = buffer[i];
}
w = atoi(cw);
std::cout << "width = " << w << endl;;
map_file.getline(buffer, 128);
std::cout << "Start at (" << sx << " " << sy << ")" << endl;
std::cout << "Goal is (" << gx << " " << gy << ")" << endl;
std::cout << endl;
in_map = new char*[h];
direction_map = new int*[h];
visit_map = new bool*[h];
out_map = new char*[h];
parent_map = new int*[h];
open_node_map = new double*[h];
for (int j = 0; j<h; j++) {
in_map[j] = new char[w];
direction_map[j] = new int[w];
visit_map[j] = new bool[w];
out_map[j] = new char[w];
parent_map[j] = new int[w];
open_node_map[j] = new double[w];
for (int i = 0; i <= w; i++) {
char tmp;
map_file.get(tmp);
if (w == i)
continue;
in_map[j][i] = tmp;
direction_map[j][i] = -1;
visit_map[j][i] = false;
out_map[j][i] = tmp;
open_node_map[j][i] = 0.0;
}
}
map_file.close();
}
Map::~Map() {
for (int i = 0; i<h; i++) {
delete[] parent_map[i];
delete[] in_map[i];
delete[] direction_map[i];
delete[] visit_map[i];
delete[] out_map[i];
delete[] open_node_map[i];
}
delete[] parent_map;
delete[] in_map;
delete[] direction_map;
delete[] visit_map;
delete[] out_map;
delete[] open_node_map;
}
int Map::getGx() const { return gx; }
int Map::getGy() const { return gy; }
int Map::getSx() const { return sx; }
int Map::getSy() const { return sy; }
int Map::getHeight() const { return h; }
int Map::getWidth() const { return w; }
double Map::getOptimalLength() const { return optimal_length; }
char Map::getInMapData(int x, int y) { return in_map[y][x]; }
int Map::getDirectionData(int x, int y) { return direction_map[y][x]; }
bool Map::getVisitMapData(int x, int y) { return visit_map[y][x]; }
int Map::getParentMapData(int x, int y) { return parent_map[y][x]; }
double Map::getOpen_NodeData(int x, int y) { return open_node_map[y][x]; }
char Map::getOutMapData(int x, int y) const { return out_map[y][x]; }
void Map::setVisitMap(int x, int y, bool data) { visit_map[y][x] = data; }
void Map::setDirectionMap(int x, int y, int data) { direction_map[y][x] = data; }
void Map::setOutMap(int x, int y, char data) { out_map[y][x] = data; }
void Map::setParentMap(int x, int y, int data) { parent_map[y][x] = data; }
void Map::setOpen_NodeMap(int x, int y, double data) { open_node_map[y][x] = data; }
void Map::initialize() {}
void Map::draw_map() {
ofstream out_file(out_path);
for (int j = 0; j<h; j++) {
for (int i = 0; i<w; i++) {
if (j == sy && i == sx)
out_map[j][i] = 'S';
if (j == gy && i == gx)
out_map[j][i] = 'G';
out_file << out_map[j][i];
}
out_file << "\r\n";
}
out_file.close();
}
void Map::A_star() {
priority_queue<Node> search_q[2];
Node startPoint(sx, sy, gx, gy, -1, 0, mode);
int pqi = 0;
search_q[pqi].push(startPoint);
Map::setOpen_NodeMap(sx, sy, startPoint.getPriority());
while (!search_q[pqi].empty()) {
int cx = search_q[pqi].top().getX(); // current x, y
int cy = search_q[pqi].top().getY();
double passedLength_c = search_q[pqi].top().getPassedLength();
Map::setVisitMap(cx, cy, true);
Map::setOpen_NodeMap(cx, cy, search_q[pqi].top().getPriority());
search_q[pqi].pop();
if (cx == gx && cy == gy) {
double shortestLength = 0;
while (1) {
if ((cx == sx) && (cy == sy)) break;
int tmp_x, tmp_y, tmp_dir;
tmp_x = cx;
tmp_y = cy;
tmp_dir = getDirectionData(tmp_x, tmp_y);
cx -= DIR_X[tmp_dir];
cy -= DIR_Y[tmp_dir];
setOutMap(cx, cy, '#');
if (tmp_dir % 2 == 1)
shortestLength += sqrt(2.0);
else
shortestLength += 1.0;
}
cout << "A_star find!" << endl;
cout << "Path Length = " << shortestLength << endl;
optimal_length = shortestLength;
while (!search_q[pqi].empty()) {
search_q[pqi].pop();
}
return;
}
for (int dir = 0; dir<8; dir++) {
// next_node
int nx = cx + DIR_X[dir];
int ny = cy + DIR_Y[dir];
if (!(nx >(w - 1) || nx < 0 || ny >(h - 1) || ny < 0 || getInMapData(nx, ny) == '#' || getVisitMapData(nx, ny) == true)) {
Node next_node(nx, ny, gx, gy, passedLength_c, dir, mode, 1);
if (Map::getOpen_NodeData(nx, ny) == 0) {
Map::setOutMap(nx, ny, 'I');
Map::setOpen_NodeMap(nx, ny, next_node.getPriority());
search_q[pqi].push(next_node);
Map::setDirectionMap(nx, ny, dir);
}
else if (Map::getOpen_NodeData(nx, ny) > next_node.getPriority()) {
Map::setOpen_NodeMap(nx, ny, next_node.getPriority());
Map::setDirectionMap(nx, ny, dir);
search_q[pqi].push(next_node);
/*
while (!(search_q[pqi].top().getX() == nx && search_q[pqi].top().getY() == ny))
search_q[1 - pqi].push(search_q[pqi].top());
search_q[pqi].pop();
}
search_q[pqi].pop();
if (search_q[pqi].size() > search_q[1 - pqi].size()) {
pqi = 1 - pqi;
}
while (!search_q[pqi].empty()) {
search_q[1 - pqi].push(search_q[pqi].top());
search_q[pqi].pop();
}
pqi = 1 - pqi;
search_q[pqi].push(next_node);
*/
}
}
}
}
}
void Map::JPS() {
priority_queue <Node> JumpPoints;
Node startPoint(sx, sy, gx, gy, 0, -1, mode, 1);
startPoint.calculateDistanceToGoal();
startPoint.updatePriority();
JumpPoints.push(startPoint);
while (!JumpPoints.empty()) {
int x = JumpPoints.top().getX();
int y = JumpPoints.top().getY();
if (x == gx && y == gy) {
cout << "JPS find!!!" << endl;
double shortestLength = 0;
while (!(x == Map::getSx() && y == Map::getSy())) {
int fix_x = x;
int fix_y = y;
int tmp_dir = getDirectionData(fix_x, fix_y);
int px = Map::getParentMapData(fix_x, fix_y) % 512;
int py = Map::getParentMapData(fix_x, fix_y) / 512;
//while(!(Map::getOutMapData(x, y) == 'J')){
while (!(px == x && py == y)) {
if (Map::getParentMapData(fix_x, fix_y) == (y * Map::getWidth() + x)) break;
//if(!(Map::getOutMapData(x, y) == 'J')){
if (!(Map::getOutMapData(x, y) == 'J')) {
setOutMap(x, y, '#');
}
x -= DIR_X[tmp_dir];
y -= DIR_Y[tmp_dir];
if (tmp_dir % 2 == 1)
shortestLength += sqrt(2.0);
else
shortestLength += 1.0;
}
}
optimal_length = shortestLength;
cout << "Path Length = " << shortestLength << endl;
//cout<<"Path Length = "<< passedLength_c << endl;
return;
}
else
Map::identifySuccessors(JumpPoints);
}
while (!JumpPoints.empty()) {
JumpPoints.pop();
}
cout << "not found" << endl;
}
/*
Node Map::jump(Node const node, int dir, int& off) {
int nx = node.getX() + DIR_X[dir];
int ny = node.getY() + DIR_Y[dir];
if (nx > (w - 1) || nx < 0 || ny >(h - 1) || ny < 0) {
// Map::setOutMap(nx, ny, 'B');
Node NULL_node(-100, -100, 0, 0, 0, 0, "OCTILE", 1);
return NULL_node;
}
char n_MapData = Map::getOutMapData(nx, ny);
if (n_MapData == '#') {
Node NULL_node(-100, -100, 0, 0, 0, 0, "OCTILE", 1);
return NULL_node;
}
Node n_node(nx, ny, gx, gy, node.getPassedLength(), dir);
if (n_MapData == 'I')
Map::setOutMap(nx, ny, 'X');
if (nx == gx && ny == gy) {
off = 1;
return n_node;
}
int forced_neighbours_bits = Map::forced_neighbours(nx, ny, dir);
if (forced_neighbours_bits > 0) {
//Map::setOutMap(nx, ny, 'F');
return n_node;
}
if (dir % 2 == 1) {
// Algorithm 2 function jump 8th line)
if (Map::jump(n_node, (dir + 7) % 8, off).getX() != -100)
return n_node;
if (Map::jump(n_node, (dir + 1) % 8, off).getX() != -100)
return n_node;
}
if (n_MapData != 'S' && n_MapData != 'I' && n_MapData != 'G' && n_MapData != '#' && n_MapData != 'J')
Map::setOutMap(nx, ny, 'I');
//draw_map();
return Map::jump(n_node, dir, off);
}
*/
int Map::jump(int index, int dir, int& off) {
int x = index % w;
int y = index / w;
int nx = x + DIR_X[dir];
int ny = y + DIR_Y[dir];
int n_index = ny * w + nx;
if (nx > (w - 1) || nx < 0 || ny >(h - 1) || ny < 0) {
// Map::setOutMap(nx, ny, 'B');
return -1;
}
char n_MapData = Map::getOutMapData(nx, ny);
if (n_MapData == '#') {
return -1;
}
if (n_MapData == 'I')
Map::setOutMap(nx, ny, 'X');
if (nx == gx && ny == gy) {
off = 1;
return n_index;
}
int forced_neighbours_bits = Map::forced_neighbours(nx, ny, dir);
if (forced_neighbours_bits > 0) {
//Map::setOutMap(nx, ny, 'F');
return n_index;
}
if (dir % 2 == 1) {
// Algorithm 2 function jump 8th line)
if (Map::jump(n_index, (dir + 7) % 8, off) != -1)
return n_index;
if (Map::jump(n_index, (dir + 1) % 8, off) != -1)
return n_index;
}
if (n_MapData != 'S' && n_MapData != 'I' && n_MapData != 'G' && n_MapData != '#' && n_MapData != 'J')
Map::setOutMap(nx, ny, 'I');
//draw_map();
return Map::jump(n_index, dir, off);
}
void Map::identifySuccessors(priority_queue <Node>& successors) {
int x = successors.top().getX();
int y = successors.top().getY();
if (x == gx && y == gy)
return;
int index = y * Map::getWidth() + x;
int dir = successors.top().getDirection();
double passedLength = successors.top().getPassedLength();
Node start(x, y, gx, gy, passedLength, dir, mode, 1);
start.updatePassedLength();
start.calculateDistanceToGoal();
start.updatePriority();
successors.pop();
vector<int> candidate_dir;
if (dir == -1) {
for (int i = 0; i<8; i++) {
int dx = x + DIR_X[i];
int dy = y + DIR_Y[i];
if (!(dx < 0 || dx >(w - 1) || dy < 0 || dy >(h - 1) || Map::getOutMapData(dx, dy) == '#'))
candidate_dir.push_back(i);
}
}
else {
int bits = Map::forced_neighbours(x, y, dir);
for (int i = 0; i<8; i++) {
if (bits & (1 << i))
candidate_dir.push_back(i);
}
if (dir % 2 == 1) {
int dx = x + DIR_X[(dir + 1) % 8];
int dy = y + DIR_Y[(dir + 1) % 8];
if (!(dx < 0 || dx >(w - 1) || dy < 0 || dy >(h - 1) || Map::getOutMapData(dx, dy) == '#'))
candidate_dir.push_back((dir + 1) % 8);
dx = x + DIR_X[(dir + 7) % 8];
dy = y + DIR_Y[(dir + 7) % 8];
if (!(dx < 0 || dx >(w - 1) || dy < 0 || dy >(h - 1) || Map::getOutMapData(dx, dy) == '#'))
candidate_dir.push_back((dir + 7) % 8);
dx = x + DIR_X[dir];
dy = y + DIR_Y[dir];
if (!(dx < 0 || dx >(w - 1) || dy < 0 || dy >(h - 1) || Map::getOutMapData(dx, dy) == '#'))
candidate_dir.push_back(dir);
}
else {
int dx = x + DIR_X[dir];
int dy = y + DIR_Y[dir];
if (!(dx < 0 || dx >(w - 1) || dy < 0 || dy >(h - 1) || Map::getOutMapData(dx, dy) == '#'))
candidate_dir.push_back(dir);
}
}
for (int i = 0; i<candidate_dir.size(); i++) {
int nx, ny, n_index;
int n_dir = candidate_dir[i];
nx = x + DIR_X[n_dir];
ny = y + DIR_Y[n_dir];
int jx, jy;
double j_passedLength, s_dist = 0.0, d_dist = 0.0;
int off = 0;
int j_index = Map::jump(index, n_dir, off);
if (j_index == -1)
continue;
jx = j_index % w;
jy = j_index / w;
j_passedLength = passedLength + sqrt((x - jx)*(x - jx) + (y - jy)*(y - jy));
Node j_node(jx, jy, gx, gy, j_passedLength, n_dir, mode, 1);
j_node.setPassedLength(j_passedLength);
j_node.calculateDistanceToGoal();
j_node.updatePriority();
if (Map::getOpen_NodeData(jx, jy) == 0) {
Map::setOutMap(jx, jy, 'J');
Map::setParentMap(jx, jy, y * Map::getWidth() + x);
Map::setDirectionMap(jx, jy, n_dir);
Map::setOpen_NodeMap(jx, jy, j_node.getPriority());
successors.push(j_node);
}
else if (Map::getOpen_NodeData(jx, jy) > j_node.getPriority()) {
Map::setOpen_NodeMap(jx, jy, j_node.getPriority());
Map::setDirectionMap(jx, jy, n_dir);
Map::setParentMap(jx, jy, y * Map::getWidth() + x);
}
}
candidate_dir.clear();
return;
cout << "not found" << endl;
}
/*
*/
bool Map::is_obstacle(int x, int y, int dir) {
int nx = x + DIR_X[dir];
int ny = y + DIR_Y[dir];
if (nx < 0 || nx >(w - 1) || ny < 0 || ny >(h - 1))
return false;
if (Map::getInMapData(nx, ny) == '#')
return true;
else
return false;
}
int Map::forced_neighbours(int x, int y, int dir) {
int bits = 0;
if (dir == -1)
return -1;
if (dir % 2 == 0) {
// straight
int ndir1 = (dir + 2) % 8;
int ndir2 = (dir + 6) % 8;
if (Map::is_obstacle(x, y, ndir1)) {
if (!Map::is_obstacle(x, y, (dir + 1) % 8))
bits = bits | 1 << ((dir + 1) % 8);
}
if (Map::is_obstacle(x, y, ndir2))
if (!Map::is_obstacle(x, y, (dir + 7) % 8))
bits = bits | 1 << ((dir + 7) % 8);
}
else {
int ndir1 = (dir + 3) % 8;
int ndir2 = (dir + 5) % 8;
if (Map::is_obstacle(x, y, ndir1))
if (!Map::is_obstacle(x, y, (dir + 1) % 8))
bits = bits | 1 << ((dir + 1) % 8);
if (Map::is_obstacle(x, y, ndir2))
if (!Map::is_obstacle(x, y, (dir + 7) % 8))
bits = bits | 1 << ((dir + 7) % 8);
}
return bits;
}
Node::Node(int const x, int const y, int const gx, int const gy, double const passedLength, int const direction, string const mode, int const k) {
this->x = x; this->y = y;
this->direction = direction;
this->passedLength = passedLength;
this->gx = gx; this->gy = gy;
this->mode = mode;
if (k == 1) {
updatePassedLength();
calculateDistanceToGoal();
updatePriority();
}
}
Node::~Node() {}
int Node::getX() const { return x; }
int Node::getY() const { return y; }
int Node::getDirection() const { return direction; }
double Node::getPriority() const { return priority; }
double Node::getPassedLength() const { return passedLength; }
double Node::getDistanceToGoal() const { return distanceToGoal; }
void Node::calculateDistanceToGoal() {
double xd = abs(x - gx);
double yd = abs(y - gy);
if (mode.compare("MANHATTAN") == 0)
distanceToGoal = abs(xd) + abs(yd);
else if (mode.compare("EUCLIDIAN") == 0)
distanceToGoal = sqrt((xd*xd) + (yd*yd));
else if (mode.compare("CHEBYSHEV") == 0)
distanceToGoal = max(abs(xd), abs(yd));
else if (mode.compare("OCTILE") == 0)
distanceToGoal = max(xd, yd) + (sqrt(2.0) - 1) + min(xd, yd);
else
cout << "plz input mode" << endl;
}
void Node::updatePassedLength() {
if (direction == -1);
else if (direction % 2 == 1)
passedLength += (sqrt(2.0));
else
passedLength += (1);
}
void Node::updatePriority() {
priority = passedLength + distanceToGoal;
}
void Node::setPassedLength(double data) {
passedLength = data;
}
util.h
#include <iostream>
#include <string>
#include <queue>
#include <math.h>
#include <fstream>
#include <istream>
#include <cstdio>
#include <vector>
using namespace std;
class Node {
private:
int x;
int y;
int gx;
int gy;
int direction; // direction from past node
double passedLength;
double distanceToGoal;
double priority;
string mode; // mode -> "MANHATTAN", "EUCLIDIAN", CHEBYSHEV", "OCTILE"
public:
Node(int const x, int const y, int const gx, int const gy, double const passedLength, int const direction = 0, string const mode = "MANHATTAN" , int const k = 0 );
~Node();
int getX() const;
int getY() const;
int getDirection() const;
double getPriority() const;
double getPassedLength() const;
double getDistanceToGoal() const;
void setPassedLength(double data);
void calculateDistanceToGoal();
void updatePassedLength();
void updatePriority();
};
class Map {
public:
int gx, gy, sx, sy, w, h;
char* in_path, *out_path;
string mode;
char** in_map;
int** direction_map;
bool** visit_map;
char** out_map;
int** parent_map;
double** open_node_map;
double optimal_length;
public:
Map(int* START_GOAL, char* IN_PATH, char* OUT_PATH, string MODE);
~Map();
int getGx() const;
int getGy() const;
int getSx() const;
int getSy() const;
char getInMapData(int x, int y);
int getDirectionData(int x, int y);
bool getVisitMapData(int x, int y);
int getParentMapData(int x, int y);
double getOpen_NodeData(int x, int y);
int getHeight() const;
int getWidth() const;
double getOptimalLength() const;
char getOutMapData(int x, int y) const;
void setVisitMap(int x, int y, bool data);
void setDirectionMap(int x, int y, int data);
void setOutMap(int x, int y, char data);
void setParentMap(int x, int y, int data);
void setOpen_NodeMap(int x, int y, double data);
void initialize();
void draw_map();
void A_star();
void JPS();
//int identifySuccessors(int x, int y);
void identifySuccessors(priority_queue <Node>& successors);
//int jump(int node_index, int dir, double& s_distance, priority_queue <Node>& successors, double& d_distance, int& trig, int& fx_fy);
//Node Map::jump(Node node, int dir, int& off);
int Map::jump(int index, int dir, int& off);
bool is_obstacle(int x, int n, int dir);
int Map::forced_neighbours(int x, int y, int dir);
};
void read_scenario(char* path, char(*scenarios)[256], int& total);
Without an individual review of each line of code, it appears that the speed difference is due to your programming style.
To take just the first two examples: vector_clear is a poor reimplementation of std::vector::clear and operator<(Node a, Node b) makes unnecessary copies of both nodes. And glancing at the rest of the code, these do not appear to be exceptions.
Measuring the speed of a debug executable is pointless. The compiler settings used for debugging do not take speed of the resulting executable into account. And you've further complicated it by using a debug version of new. Only the speed in release mode is reasonable, and then only if you have good code to start with.