Updated!
I get this code from this site
It's A* Search Algorithm(finding shortest path with heuristics)
I modify most of variable names and some if conditions from the original version to satisfy my syntactic taste.
It works in C++ (as I can't see any trouble with it) but fails in Java version.
Java
Code:
String findPath(int startX, int startY, int finishX, int finishY) {
@SuppressWarnings("unchecked")
LinkedList<Node>[] nodeList = (LinkedList<Node>[]) new LinkedList<?>[2];
nodeList[0] = new LinkedList<Node>();
nodeList[1] = new LinkedList<Node>();
Node n0;
Node m0;
int nlIndex = 0; // queueList index
// reset the node maps
for(int y = 0;y < ROW_COUNT; ++y) {
for(int x = 0;x < COL_COUNT; ++x) {
close_nodes_map[y][x] = 0;
open_nodes_map[y][x] = 0;
}
}
// create the start node and push into list of open nodes
n0 = new Node( startX, startY, 0, 0 );
n0.updatePriority( finishX, finishY );
nodeList[nlIndex].push( n0 );
open_nodes_map[startY][startX] = n0.getPriority(); // mark it on the open nodes map
// A* search
while( !nodeList[nlIndex].isEmpty() )
{
LinkedList<Node> pq = nodeList[nlIndex];
// get the current node w/ the highest priority
// from the list of open nodes
n0 = new Node( pq.peek().getX(), pq.peek().getY(),
pq.peek().getIterCount(), pq.peek().getPriority());
int x = n0.getX();
int y = n0.getY();
nodeList[nlIndex].pop(); // remove the node from the open list
open_nodes_map[y][x] = 0;
// mark it on the closed nodes map
close_nodes_map[y][x] = 1;
// quit searching when the goal state is reached
//if((*n0).estimate(finishX, finishY) == 0)
if( x == finishX && y == finishY )
{
// generate the path from finish to start
// by following the directions
String path = "";
while( !( x == startX && y == startY) )
{
int j = dir_map[y][x];
int c = '0' + ( j + Node.DIRECTION_COUNT / 2 ) % Node.DIRECTION_COUNT;
path = (char)c + path;
x += DIR_X[j];
y += DIR_Y[j];
}
return path;
}
// generate moves (child nodes) in all possible directions
for(int i = 0; i < Node.DIRECTION_COUNT; ++i)
{
int xdx = x + DIR_X[i];
int ydy = y + DIR_Y[i];
// boundary check
if (!(xdx >= 0 && xdx < COL_COUNT
&& ydy >= 0 && ydy < ROW_COUNT)) continue;
if ( ( gridMap.getData( ydy, xdx ) == GridMap.WALKABLE
|| gridMap.getData( ydy, xdx ) == GridMap.FINISH)
&& close_nodes_map[ydy][xdx] != 1 )
{
// generate a child node
m0 = new Node( xdx, ydy, n0.getIterCount(), n0.getPriority() );
m0.nextLevel( i );
m0.updatePriority( finishX, finishY );
// if it is not in the open list then add into that
if( open_nodes_map[ydy][xdx] == 0 )
{
open_nodes_map[ydy][xdx] = m0.getPriority();
nodeList[nlIndex].push( m0 );
// mark its parent node direction
dir_map[ydy][xdx] = ( i + Node.DIRECTION_COUNT / 2 ) % Node.DIRECTION_COUNT;
}
else if( open_nodes_map[ydy][xdx] > m0.getPriority() )
{
// update the priority info
open_nodes_map[ydy][xdx] = m0.getPriority();
// update the parent direction info
dir_map[ydy][xdx] = ( i + Node.DIRECTION_COUNT / 2 ) % Node.DIRECTION_COUNT;
// replace the node
// by emptying one queueList to the other one
// except the node to be replaced will be ignored
// and the new node will be pushed in instead
while( !(nodeList[nlIndex].peek().getX() == xdx &&
nodeList[nlIndex].peek().getY() == ydy ) )
{
nodeList[1 - nlIndex].push( nodeList[nlIndex].pop() );
}
nodeList[nlIndex].pop(); // remove the wanted node
// empty the larger size queueList to the smaller one
if( nodeList[nlIndex].size() > nodeList[ 1 - nlIndex ].size() )
nlIndex = 1 - nlIndex;
while( !nodeList[nlIndex].isEmpty() )
{
nodeList[1 - nlIndex].push( nodeList[nlIndex].pop() );
}
nlIndex = 1 - nlIndex;
nodeList[nlIndex].push( m0 ); // add the better node instead
}
}
}
}
return ""; // no route found
}
Output1:
Legends
. = PATH
? = START
X = FINISH
3,2,1 = OBSTACLES
(Misleading path)
Output2:
Changing these lines:
n0 = new Node( a, b, c, d );
m0 = new Node( e, f, g, h );
to
n0.set( a, b, c, d );
m0.set( e, f, g, h );
I get
(I'm really confused)
C++
Code:
std::string A_Star::findPath(int startX, int startY, int finishX, int finishY)
{
typedef std::queue<Node> List_Container;
List_Container nodeList[2]; // list of open (not-yet-tried) nodes
Node n0;
Node m0;
int pqIndex = 0; // nodeList index
// reset the node maps
for(int y = 0;y < ROW_COUNT; ++y)
{
for(int x = 0;x < COL_COUNT; ++x)
{
close_nodes_map[y][x] = 0;
open_nodes_map[y][x] = 0;
}
}
// create the start node and push into list of open nodes
n0 = Node( startX, startY, 0, 0 );
n0.updatePriority( finishX, finishY );
nodeList[pqIndex].push( n0 );
open_nodes_map[startY][startX] = n0.getPriority(); // mark it on the open nodes map
// A* search
while( !nodeList[pqIndex].empty() )
{
List_Container &pq = nodeList[pqIndex];
// get the current node w/ the highest priority
// from the list of open nodes
n0 = Node( pq.front().getX(), pq.front().getY(),
pq.front().getIterCount(), pq.front().getPriority());
int x = n0.getX();
int y = n0.getY();
nodeList[pqIndex].pop(); // remove the node from the open list
open_nodes_map[y][x] = 0;
// mark it on the closed nodes map
close_nodes_map[y][x] = 1;
// quit searching when the goal state is reached
//if((*n0).estimate(finishX, finishY) == 0)
if( x == finishX && y == finishY )
{
// generate the path from finish to start
// by following the directions
std::string path = "";
while( !( x == startX && y == startY) )
{
int j = dir_map[y][x];
char c = '0' + ( j + DIRECTION_COUNT / 2 ) % DIRECTION_COUNT;
path = c + path;
x += DIR_X[j];
y += DIR_Y[j];
}
return path;
}
// generate moves (child nodes) in all possible directions
for(int i = 0; i < DIRECTION_COUNT; ++i)
{
int xdx = x + DIR_X[i];
int ydy = y + DIR_Y[i];
// boundary check
if (!( xdx >= 0 && xdx < COL_COUNT
&& ydy >= 0 && ydy < ROW_COUNT)) continue;
if ( ( pGrid->getData(ydy,xdx) == WALKABLE
|| pGrid->getData(ydy, xdx) == FINISH)
&& close_nodes_map[ydy][xdx] != 1 )
{
// generate a child node
m0 = Node( xdx, ydy, n0.getIterCount(), n0.getPriority() );
m0.nextLevel( i );
m0.updatePriority( finishX, finishY );
// if it is not in the open list then add into that
if( open_nodes_map[ydy][xdx] == 0 )
{
open_nodes_map[ydy][xdx] = m0.getPriority();
nodeList[pqIndex].push( m0 );
// mark its parent node direction
dir_map[ydy][xdx] = ( i + DIRECTION_COUNT / 2 ) % DIRECTION_COUNT;
}
else if( open_nodes_map[ydy][xdx] > m0.getPriority() )
{
// update the priority info
open_nodes_map[ydy][xdx] = m0.getPriority();
// update the parent direction info
dir_map[ydy][xdx] = ( i + DIRECTION_COUNT / 2 ) % DIRECTION_COUNT;
// replace the node
// by emptying one nodeList to the other one
// except the node to be replaced will be ignored
// and the new node will be pushed in instead
while ( !( nodeList[pqIndex].front().getX() == xdx &&
nodeList[pqIndex].front().getY() == ydy ) )
{
nodeList[1 - pqIndex].push( nodeList[pqIndex].front() );
nodeList[pqIndex].pop();
}
nodeList[pqIndex].pop(); // remove the wanted node
// empty the larger size nodeList to the smaller one
if( nodeList[pqIndex].size() > nodeList[ 1 - pqIndex ].size() )
pqIndex = 1 - pqIndex;
while( !nodeList[pqIndex].empty() )
{
nodeList[1-pqIndex].push(nodeList[pqIndex].front());
nodeList[pqIndex].pop();
}
pqIndex = 1 - pqIndex;
nodeList[pqIndex].push( m0 ); // add the better node instead
}
}
}
}
return ""; // no route found
}
Output:
Legends
. = PATH
? = START
X = FINISH
3,2,1 = OBSTACLES
(Just right)
From what I read about Java's documentation, I came up with the conclusion:
C++'s std::queue<T>::front() == Java's LinkedList<T>.peek()
Java's LinkedList<T>.pop() == C++'s std::queue<T>::front() + std::queue<T>::pop()
What might I be missing in my Java version? In what way does it became different algorithmically from the C++ version?
