这篇本来是自己备忘,后来想想如果去认真研究Jps+源码的也应该差不多花个1天左右能研究出来。所以直接就公开吧
JPS+是JPS的升级版,对地图做了预处理,可以更快的去找到合适的跳点忽略大量中间寻找的过程。
主要如下:
第一步计算主要的跳点
观察每个跳点至少都有2个方向
代码就是遍历地图上所有的障碍。
所有的障碍遍历斜向点。斜向点然后再根据方向反向1格看看是否可行走。然后标记方向
north_east_index = getNorthEastIndex( row, column );
if ( north_east_index != -1 )
{
Node node = gridNodes[ north_east_index ];
if ( ! node.isObstacle )
{
// If nodes to the south and west are empty, then this node will be a jump point for those directions
if ( isEmpty( getIndexOfNodeTowardsDirection( north_east_index, eDirections.SOUTH ) )
&& isEmpty( getIndexOfNodeTowardsDirection( north_east_index, eDirections.WEST ) ) )
{
node.isJumpPoint = true;
node.jumpPointDirection[ (int) eDirections.SOUTH ] = true;
node.jumpPointDirection[ (int) eDirections.WEST ] = true;
}
}
}
图示:箭头是障碍的东北方向,然后那个节点看其西和南方向 发现都是可行走,则他的方向包含东北方向。
第二步:计算直线跳点的距离
一旦确定了所有的主跳点,就可以找到直跳点了。直跳点是沿基本方向移动最终会遇到该方向的主要跳点的节点(在撞到墙上之前)。
以绿色点向西方向查找如下,观测红框格子的左边数(西)
这块为什么没数字,因为东南西北方向上的没有跳点或者跳点都被障碍挡住。
为什么没垂直方向的数字,是因为其垂直方向上没有跳点。有该方向上的跳点才会存在距离数字。
public void buildStraightJumpPoints()
{
// Calcin' Jump Distance, left and right
// For all the rows in the grid
for ( int row = 0 ; row < maxRows ; ++row )
{
// Calc moving left to right
int jumpDistanceSoFar = -1;
bool jumpPointSeen = false;
// Checking for jump disances where nodes are moving WEST
for ( int column = 0 ; column < rowSize ; ++column )
{
Node node = gridNodes[ rowColumnToIndex( row, column ) ];
// If we've reach a wall, then reset everything :(
if ( node.isObstacle )
{
jumpDistanceSoFar = -1;
jumpPointSeen = false;
node.jpDistances[ (int) eDirections.WEST ] = 0;
continue;
}
++jumpDistanceSoFar;
if ( jumpPointSeen )
{
// If we've seen a jump point heading left, then we can tell this node he's got a jump point coming up to his LEFT ( WEST )
node.jpDistances[ (int) eDirections.WEST ] = jumpDistanceSoFar;
}
else
{
node.jpDistances[ (int) eDirections.WEST ] = -jumpDistanceSoFar; // Set wall distance
}
// If we just found a new jump point, then set everything up for this new jump point
if ( node.isJumpPointComingFrom( eDirections.EAST ) )
{
jumpDistanceSoFar = 0;
jumpPointSeen = true;
}
}
jumpDistanceSoFar = -1;
jumpPointSeen = false;
// Checking for jump disances where nodes are moving WEST
for ( int column = rowSize - 1 ; column >= 0 ; --column )
{
Node node = gridNodes[ rowColumnToIndex( row, column ) ];
// If we've reach a wall, then reset everything :(
if ( node.isObstacle )
{
jumpDistanceSoFar = -1;
jumpPointSeen = false;
node.jpDistances[ (int) eDirections.EAST ] = 0;
continue;
}
++jumpDistanceSoFar;
if ( jumpPointSeen )
{
// If we've seen a jump point heading left, then we can tell this node he's got a jump point coming up to his RIGTH ( EAST )
node.jpDistances[ (int) eDirections.EAST ] = jumpDistanceSoFar;
}
else
{
node.jpDistances[ (int) eDirections.EAST ] = -jumpDistanceSoFar; // Set wall distance
}
// If we just found a new jump point, then set everything up for this new jump point
if ( node.isJumpPointComingFrom( eDirections.WEST ) )
{
jumpDistanceSoFar = 0;
jumpPointSeen = true;
}
}
}
// Calcin' Jump Distance, up and down
// For all the columns in the grid
for ( int column = 0 ; column < rowSize ; ++column )
{
// Calc moving left to right
int jumpDistanceSoFar = -1;
bool jumpPointSeen = false;
// Checking for jump disances where nodes are moving NORTH
for ( int row = 0 ; row < maxRows ; ++row )
{
Node node = gridNodes[ rowColumnToIndex( row, column ) ];
// If we've reach a wall, then reset everything :(
if ( node.isObstacle )
{
jumpDistanceSoFar = -1;
jumpPointSeen = false;
node.jpDistances[ (int) eDirections.NORTH ] = 0;
continue;
}
++jumpDistanceSoFar;
if ( jumpPointSeen )
{
// If we've seen a jump point heading UP, then we can tell this node he's got a jump point coming up ABOVE ( NORTH )
node.jpDistances[ (int) eDirections.NORTH ] = jumpDistanceSoFar;
}
else
{
node.jpDistances[ (int) eDirections.NORTH ] = -jumpDistanceSoFar; // Set wall distance
}
// If we just found a new jump point, then set everything up for this new jump point
if ( node.isJumpPointComingFrom( eDirections.SOUTH ) )
{
jumpDistanceSoFar = 0;
jumpPointSeen = true;
}
}
jumpDistanceSoFar = -1;
jumpPointSeen = false;
// Checking for jump disances where nodes are moving SOUTH
for ( int row = maxRows - 1 ; row >= 0 ; --row )
{
Node node = gridNodes[ rowColumnToIndex( row, column ) ];
// If we've reach a wall, then reset everything :(
if ( node.isObstacle )
{
jumpDistanceSoFar = -1;
jumpPointSeen = false;
node.jpDistances[ (int) eDirections.SOUTH ] = 0;
continue;
}
++jumpDistanceSoFar;
if ( jumpPointSeen )
{
// If we've seen a jump point heading down, then we can tell this node he's got a jump point coming up BELOW( SOUTH )
node.jpDistances[ (int) eDirections.SOUTH ] = jumpDistanceSoFar;
}
else
{
node.jpDistances[ (int) eDirections.SOUTH ] = -jumpDistanceSoFar; // Set wall distance
}
// If we just found a new jump point, then set everything up for this new jump point
if ( node.isJumpPointComingFrom( eDirections.NORTH ) )
{
jumpDistanceSoFar = 0;
jumpPointSeen = true;
}
}
}
}
3.计算斜线跳点距离
以下列距离:
当遍历到1节点时,发现其右上方是可行走节点,并且其 向右或向上均距离大于 0.
所以此节点的东北方向记为1.
遍历到2节点时,其东北方向的节点没有向右或者向上距离大于0,但是其东北方向为1,
所以此节点的东北方向记为 1+1 = 2
public void buildDiagonalJumpPoints()
{
// Calcin' Jump Distance, Diagonally Upleft and upright
// For all the rows in the grid
for ( int row = 0 ; row < maxRows ; ++row )
{
// foreach column
for ( int column = 0 ; column < rowSize ; ++column )
{
// if this node is an obstacle, then skip
if ( isObstacleOrWall( row, column ) ) continue;
Node node = gridNodes[ rowColumnToIndex( row, column ) ]; // Grab the node ( will not be NULL! )
// Calculate NORTH WEST DISTNACES
if ( row == 0 || column == 0 || ( // If we in the north west corner
isObstacleOrWall( row - 1, column ) || // If the node to the north is an obstacle
isObstacleOrWall( row, column - 1) || // If the node to the left is an obstacle
isObstacleOrWall( row - 1, column - 1 ) ) ) // if the node to the North west is an obstacle
{
// Wall one away
node.jpDistances[ (int) eDirections.NORTH_WEST ] = 0;
}
else if ( isEmpty(row - 1, column) && // if the node to the north is empty
isEmpty(row, column - 1) && // if the node to the west is empty
(getNode( row - 1, column - 1 ).jpDistances[ (int) eDirections.NORTH ] > 0 || // If the node to the north west has is a straight jump point ( or primary jump point) going north
getNode( row - 1, column - 1 ).jpDistances[ (int) eDirections.WEST ] > 0)) // If the node to the north west has is a straight jump point ( or primary jump point) going West
{
// Diagonal one away
node.jpDistances[ (int) eDirections.NORTH_WEST ] = 1;
}
else
{
// Increment from last
int jumpDistance = getNode( row - 1, column - 1 ).jpDistances[ (int) eDirections.NORTH_WEST ];
if (jumpDistance > 0)
{
node.jpDistances[ (int) eDirections.NORTH_WEST ] = 1 + jumpDistance;
}
else //if( jumpDistance <= 0 )
{
node.jpDistances[ (int) eDirections.NORTH_WEST ] = -1 + jumpDistance;
}
}
// Calculate NORTH EAST DISTNACES
if ( row == 0 || column == rowSize -1 || ( // If we in the top right corner
isObstacleOrWall( row - 1, column ) || // If the node to the north is an obstacle
isObstacleOrWall( row, column + 1) || // If the node to the east is an obstacle
isObstacleOrWall( row - 1, column + 1 ) ) ) // if the node to the North East is an obstacle
{
// Wall one away
node.jpDistances[ (int) eDirections.NORTH_EAST ] = 0;
}
else if ( isEmpty(row - 1, column) && // if the node to the north is empty
isEmpty(row, column + 1) && // if the node to the east is empty
(getNode( row - 1, column + 1 ).jpDistances[ (int) eDirections.NORTH ] > 0 || // If the node to the north east has is a straight jump point ( or primary jump point) going north
getNode( row - 1, column + 1 ).jpDistances[ (int) eDirections.EAST ] > 0)) // If the node to the north east has is a straight jump point ( or primary jump point) going east
{
// Diagonal one away
node.jpDistances[ (int) eDirections.NORTH_EAST ] = 1;
}
else
{
// Increment from last
int jumpDistance = getNode( row - 1, column + 1 ).jpDistances[ (int) eDirections.NORTH_EAST ];
if (jumpDistance > 0)
{
node.jpDistances[ (int) eDirections.NORTH_EAST ] = 1 + jumpDistance;
}
else //if( jumpDistance <= 0 )
{
node.jpDistances[ (int) eDirections.NORTH_EAST ] = -1 + jumpDistance;
}
}
}
}
// Calcin' Jump Distance, Diagonally DownLeft and Downright
// For all the rows in the grid
for ( int row = maxRows - 1 ; row >= 0 ; --row )
{
// foreach column
for ( int column = 0 ; column < rowSize ; ++column )
{
// if this node is an obstacle, then skip
if ( isObstacleOrWall( row, column ) ) continue;
Node node = gridNodes[ rowColumnToIndex( row, column ) ]; // Grab the node ( will not be NULL! )
// Calculate SOUTH WEST DISTNACES
if ( row == maxRows - 1 || column == 0 || ( // If we in the south west most node
isObstacleOrWall( row + 1, column ) || // If the node to the south is an obstacle
isObstacleOrWall( row, column - 1) || // If the node to the west is an obstacle
isObstacleOrWall( row + 1, column - 1 ) ) ) // if the node to the south West is an obstacle
{
// Wall one away
node.jpDistances[ (int) eDirections.SOUTH_WEST ] = 0;
}
else if ( isEmpty(row + 1, column) && // if the node to the south is empty
isEmpty(row, column - 1) && // if the node to the west is empty
(getNode( row + 1, column - 1 ).jpDistances[ (int) eDirections.SOUTH ] > 0 || // If the node to the south west has is a straight jump point ( or primary jump point) going south
getNode( row + 1, column - 1 ).jpDistances[ (int) eDirections.WEST ] > 0)) // If the node to the south west has is a straight jump point ( or primary jump point) going West
{
// Diagonal one away
node.jpDistances[ (int) eDirections.SOUTH_WEST ] = 1;
}
else
{
// Increment from last
int jumpDistance = getNode( row + 1, column - 1 ).jpDistances[ (int) eDirections.SOUTH_WEST ];
if (jumpDistance > 0)
{
node.jpDistances[ (int) eDirections.SOUTH_WEST ] = 1 + jumpDistance;
}
else //if( jumpDistance <= 0 )
{
node.jpDistances[ (int) eDirections.SOUTH_WEST ] = -1 + jumpDistance;
}
}
// Calculate SOUTH EAST DISTNACES
if ( row == maxRows - 1 || column == rowSize -1 || ( // If we in the south east corner
isObstacleOrWall( row + 1, column ) || // If the node to the south is an obstacle
isObstacleOrWall( row, column + 1) || // If the node to the east is an obstacle
isObstacleOrWall( row + 1, column + 1 ) ) ) // if the node to the south east is an obstacle
{
// Wall one away
node.jpDistances[ (int) eDirections.SOUTH_EAST ] = 0;
}
else if ( isEmpty(row + 1, column) && // if the node to the south is empty
isEmpty(row, column + 1) && // if the node to the east is empty
(getNode( row + 1, column + 1 ).jpDistances[ (int) eDirections.SOUTH ] > 0 || // If the node to the south east has is a straight jump point ( or primary jump point) going south
getNode( row + 1, column + 1 ).jpDistances[ (int) eDirections.EAST ] > 0)) // If the node to the south east has is a straight jump point ( or primary jump point) going east
{
// Diagonal one away
node.jpDistances[ (int) eDirections.SOUTH_EAST ] = 1;
}
else
{
// Increment from last
int jumpDistance = getNode( row + 1, column + 1 ).jpDistances[ (int) eDirections.SOUTH_EAST ];
if (jumpDistance > 0)
{
node.jpDistances[ (int) eDirections.SOUTH_EAST ] = 1 + jumpDistance;
}
else //if( jumpDistance <= 0 )
{
node.jpDistances[ (int) eDirections.SOUTH_EAST ] = -1 + jumpDistance;
}
}
}
}
}
4.计算到墙的距离:
看起来之前的代码已经算好了到墙体之间的距离。
结果是每个节点的每个没有跳点方向的到墙体或者到边界的距离算出来。
越远负数越大
5.寻路
初始设置
左边为起点。右边为终点
流程图:
因为不用在运行时查找跳点了,直接根据方向一步找到合适的跳点了。所以比JPS要快
代码:
public List< Point > getPath( Point start, Point goal )
{
List< Point > path = new List< Point >();
PriorityQueue< PathfindingNode, float > open_set = new PriorityQueue< PathfindingNode, float >();
ResetPathfindingNodeData();
PathfindingNode starting_node = this.pathfindingNodes[ pointToIndex( start ) ];
starting_node.pos = start;
starting_node.parent = null;
starting_node.givenCost = 0;
starting_node.finalCost = 0;
starting_node.listStatus = ListStatus.ON_OPEN;
open_set.push( starting_node, 0 );
while ( ! open_set.isEmpty() )
{
PathfindingNode curr_node = open_set.pop();
PathfindingNode parent = curr_node.parent;
Node jp_node = gridNodes[ pointToIndex( curr_node.pos ) ]; // get jump point info
// Check if we've reached the goal
if ( curr_node.pos.Equals( goal ) )
{
// end and return path
return reconstructPath( curr_node, start );
}
// foreach direction from parent
foreach ( eDirections dir in getAllValidDirections( curr_node ) )
{
PathfindingNode new_successor = null;
int given_cost = 0;
// goal is closer than wall distance or closer than or equal to jump point distnace
if ( isCardinal( dir ) &&
goalIsInExactDirection( curr_node.pos, dir, goal ) &&
Point.diff( curr_node.pos, goal ) <= Mathf.Abs( jp_node.jpDistances[ (int) dir ] ) )
{
new_successor = this.pathfindingNodes[ pointToIndex( goal ) ];
given_cost = curr_node.givenCost + Point.diff( curr_node.pos, goal );
}
// Goal is closer or equal in either row or column than wall or jump point distance
else if ( isDiagonal( dir ) &&
goalIsInGeneralDirection( curr_node.pos, dir, goal ) &&
( Mathf.Abs( goal.column - curr_node.pos.column ) <= Mathf.Abs( jp_node.jpDistances[ (int) dir ] ) ||
Mathf.Abs( goal.row - curr_node.pos.row ) <= Mathf.Abs( jp_node.jpDistances[ (int) dir ] ) ) )
{
// Create a target jump point
// int minDiff = min(RowDiff(curNode, goalNode),
// ColDiff(curNode, goalNode));
int min_diff = Mathf.Min( Mathf.Abs( goal.column - curr_node.pos.column ),
Mathf.Abs( goal.row - curr_node.pos.row ) );
// newSuccessor = GetNode (curNode, minDiff, direction);
new_successor = getNodeDist(
curr_node.pos.row,
curr_node.pos.column,
dir,
min_diff );
// givenCost = curNode->givenCost + (SQRT2 * DiffNodes(curNode, newSuccessor));
given_cost = curr_node.givenCost + (int)( SQRT_2 * Point.diff( curr_node.pos, new_successor.pos ) );
}
else if ( jp_node.jpDistances[ (int) dir ] > 0 )
{
// Jump Point in this direction
// newSuccessor = GetNode(curNode, direction);
new_successor = getNodeDist(
curr_node.pos.row,
curr_node.pos.column,
dir,
jp_node.jpDistances[ (int) dir ] );
// givenCost = DiffNodes(curNode, newSuccessor);
given_cost = Point.diff( curr_node.pos, new_successor.pos );
// if (diagonal direction) { givenCost *= SQRT2; }
if ( isDiagonal( dir ) )
{
given_cost = (int)( given_cost * SQRT_2 );
}
// givenCost += curNode->givenCost;
given_cost += curr_node.givenCost;
}
// Traditional A* from this point
if ( new_successor != null )
{
// if (newSuccessor not on OpenList)
if ( new_successor.listStatus != ListStatus.ON_OPEN )
{
// newSuccessor->parent = curNode;
new_successor.parent = curr_node;
// newSuccessor->givenCost = givenCost;
new_successor.givenCost = given_cost;
new_successor.directionFromParent = dir;
// newSuccessor->finalCost = givenCost +
// CalculateHeuristic(curNode, goalNode);
new_successor.finalCost = given_cost + octileHeuristic( new_successor.pos.column, new_successor.pos.row, goal.column, goal.row );
new_successor.listStatus = ListStatus.ON_OPEN;
// OpenList.Push(newSuccessor);
open_set.push( new_successor, new_successor.finalCost );
}
// else if(givenCost < newSuccessor->givenCost)
else if ( given_cost < new_successor.givenCost )
{
// newSuccessor->parent = curNode;
new_successor.parent = curr_node;
// newSuccessor->givenCost = givenCost;
new_successor.givenCost = given_cost;
new_successor.directionFromParent = dir;
// newSuccessor->finalCost = givenCost +
// CalculateHeuristic(curNode, goalNode);
new_successor.finalCost = given_cost + octileHeuristic( new_successor.pos.column, new_successor.pos.row, goal.column, goal.row );
new_successor.listStatus = ListStatus.ON_OPEN;
// OpenList.Update(newSuccessor);
open_set.push( new_successor, new_successor.finalCost );
}
}
}
}
return path;
}
public List< Point > reconstructPath( PathfindingNode goal, Point start )
{
List< Point > path = new List< Point >();
PathfindingNode curr_node = goal;
while ( curr_node.parent != null )
{
path.Add( curr_node.pos );
curr_node = curr_node.parent;
}
// Push starting node on there too
path.Add( start );
path.Reverse(); // really wish I could have just push_front but NO!
return path;
}