/// <summary>
/// 寻路接口
/// </summary>
/// <param name="grid_map">地图格子列表</param>
/// <param name="startPos">起点</param>
/// <param name="endPos">终点</param>
/// <param name="isFindNearstPath">失败时，是否返回最近可移动路径</param>
/// <returns></returns>
public eFinderResult search(PathGridMap grid_map, Vector2 startPos, Vector2 endPos, bool isFindNearstPath = false)
{
if (grid_map == null)
{
return(eFinderResult.FAILED);
}
m_array_open.Clear();
m_array_closed.Clear();
m_array_search_path.Clear();
m_array_search_grid.Clear();
m_start_node = grid_map.getNodeByPostion(startPos.x, startPos.y);
m_end_node = grid_map.getNodeByPostion(endPos.x, endPos.y);
//Log.Debug(string.Format("起点{0},{1};终点{2},{3}", m_start_node.row, m_start_node.col, m_end_node.row, m_end_node.col));
if (m_start_node == null)
{
//Log.Error("AStarPathfinder::findPath - 角色起点在障碍里面");
return(eFinderResult.FAILED);
}
if (m_end_node == null || (!m_end_node.walkable && !isFindNearstPath))
{
//Log.Error("AStarPathfinder::findPath - 角色终点在障碍里面");
return(eFinderResult.FAILED);
}
m_start_node.g = 0;
m_start_node.h = onDiagonal(m_start_node);
m_start_node.f = m_start_node.g + m_start_node.h;
//执行寻路
//float old_time = Time.realtimeSinceStartup;
eFinderResult result = travel(grid_map, isFindNearstPath);
//Log.Debug("[AI]AStarPathfinder::search - 寻路总用时:", (Time.realtimeSinceStartup - old_time) + "s");
return(result);
}
/// <summary>
/// 寻路接口
/// </summary>
/// <param name="start_x"></param>
/// <param name="start_y"></param>
/// <param name="target_x"></param>
/// <param name="target_y"></param>
/// <param name="width"></param>
/// <param name="height"></param>
/// <returns></returns>
public eFinderResult search(PathGridMap grid_map, float start_x, float start_y, float target_x, float target_y, float width, float height)
{
if (grid_map == null)
{
return(eFinderResult.FAILED);
}
eFace8Type walk_dir = eFace8Type.NONE;
m_cur_grid = null;
m_collide_rect.width = width;
m_collide_rect.height = height;
//上一步的位置
m_last_pos_x = start_x;
m_last_pos_y = start_y;
//步长增长方向
Vector2 pt_step = Math2DUtils.normalPoint(start_x, start_y, target_x, target_y, STEP_LENGTH);
float x_step_inc = pt_step.x;
float y_step_inc = pt_step.y;
//步长个数
int stepCount = (int)Mathf.Ceil(Math2DUtils.distance(start_x, start_y, target_x, target_y) / STEP_LENGTH);
//确定移动方向
float angle = MathUtils.ToDegree(Math2DUtils.LineRadians(start_x, start_y, target_x, target_y));
walk_dir = (eFace8Type)Math2DUtils.getFace(angle, 8);
m_cur_grid = grid_map.getNodeByPostion(start_x, start_y);
if (m_cur_grid == null)
{
return(eFinderResult.FAILED);
}
//重新调整方向：判断下一步是否可走
walk_dir = adjustDir(grid_map, walk_dir, start_x + x_step_inc, start_y + y_step_inc);
switch (walk_dir)
{
case eFace8Type.LEFT:
case eFace8Type.RIGHT:
y_step_inc = 0;
break;
case eFace8Type.UP:
case eFace8Type.DOWN:
x_step_inc = 0;
break;
case eFace8Type.NONE:
return(eFinderResult.FAILED);
}
//开始寻路，每次移动一个步长
eFinderResult result = eFinderResult.FAILED;
int i = 1;
for (i = 1; i <= stepCount; i++) //从1开始，当前位置不判断
{
if (isWalkableRect(grid_map, walk_dir, start_x + x_step_inc * i, start_y + y_step_inc * i))
{
m_last_pos_x = start_x + x_step_inc * i;
m_last_pos_y = start_y + y_step_inc * i;
result = eFinderResult.SUCCEEDED;
}
else
{
//如果第一步都不能走，设置查询失败
if (i == 1)
{
result = eFinderResult.FAILED;
}
else
{
result = eFinderResult.SUCCEEDED_NEAREST;
}
break;
}
}
m_target_pos_x = m_last_pos_x;
m_target_pos_y = m_last_pos_y;
return(result);
}
/// <summary>
/// 斜角走的情况下，如果一个方向挡住，自动调整到另外一个方向
/// </summary>
/// <param name="dir"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <returns></returns>
private eFace8Type adjustDir(PathGridMap grid_map, eFace8Type dir, float x, float y)
{
PathGrid cur_tile = grid_map.getNodeByPostion(x, y);
if (cur_tile == null || !cur_tile.walkable)
{
return(eFace8Type.NONE);
}
m_collide_rect.x = x - m_collide_rect.width * 0.5f;
m_collide_rect.y = y - m_collide_rect.height * 0.5f;
int grid_row = cur_tile.row;
int grid_col = cur_tile.col;
switch (dir)
{ //地图{0,0}在左下角
case eFace8Type.RIGHT_DOWN:
{
PathGrid right_tile = grid_map.getNode(grid_row, grid_col + 1);
PathGrid down_tile = grid_map.getNode(grid_row - 1, grid_col);
bool can_right = checkCanMove(right_tile, m_collide_rect);
bool can_down = checkCanMove(down_tile, m_collide_rect);
if (!can_right && !can_down)
{
return(eFace8Type.NONE);
}
else if (!can_right)
{
return(eFace8Type.DOWN);
}
else if (!can_down)
{
return(eFace8Type.RIGHT);
}
}
break;
case eFace8Type.RIGHT_UP:
{
PathGrid right_tile = grid_map.getNode(grid_row, grid_col + 1);
PathGrid up_tile = grid_map.getNode(grid_row + 1, grid_col);
bool can_right = checkCanMove(right_tile, m_collide_rect);
bool can_up = checkCanMove(up_tile, m_collide_rect);
if (!can_right && !can_up)
{
return(eFace8Type.NONE);
}
else if (!can_right)
{
return(eFace8Type.UP);
}
else if (!can_up)
{
return(eFace8Type.RIGHT);
}
}
break;
case eFace8Type.LEFT_DOWN:
{
PathGrid left_tile = grid_map.getNode(grid_row, grid_col - 1);
PathGrid down_tile = grid_map.getNode(grid_row - 1, grid_col);
bool can_left = checkCanMove(left_tile, m_collide_rect);
bool can_down = checkCanMove(down_tile, m_collide_rect);
if (!can_left && !can_down)
{
return(eFace8Type.NONE);
}
else if (!can_left)
{
return(eFace8Type.DOWN);
}
else if (!can_down)
{
return(eFace8Type.LEFT);
}
}
break;
case eFace8Type.LEFT_UP:
{
PathGrid left_tile = grid_map.getNode(grid_row, grid_col - 1);
PathGrid up_tile = grid_map.getNode(grid_row + 1, grid_col);
bool can_left = checkCanMove(left_tile, m_collide_rect);
bool can_up = checkCanMove(up_tile, m_collide_rect);
if (!can_left && !can_up)
{
return(eFace8Type.NONE);
}
else if (!can_left)
{
return(eFace8Type.UP);
}
else if (!can_up)
{
return(eFace8Type.LEFT);
}
}
break;
}
return(dir);
}
/// <summary>
/// 是否可行走
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <returns></returns>
private bool isWalkableRect(PathGridMap grid_map, eFace8Type dir, float x, float y)
{
m_collide_rect.x = x - m_collide_rect.width * 0.5f;
m_collide_rect.y = y - m_collide_rect.height * 0.5f;
PathGrid grid = grid_map.getNodeByPostion(x, y);
//在障碍里面
if (grid == null || !grid.walkable)
{
return(false);
}
int grid_row = grid.row;
int grid_col = grid.col;
PathGrid tempTile = null;
switch (dir)
{
case eFace8Type.RIGHT: //校验相邻格子
tempTile = grid_map.getNode(grid_row, grid_col + 1);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
return(true);
case eFace8Type.DOWN:
tempTile = grid_map.getNode(grid_row - 1, grid_col);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
return(true);
case eFace8Type.LEFT:
tempTile = grid_map.getNode(grid_row, grid_col - 1);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
return(true);
case eFace8Type.UP:
tempTile = grid_map.getNode(grid_row + 1, grid_col);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
return(true);
case eFace8Type.RIGHT_DOWN:
tempTile = grid_map.getNode(grid_row, grid_col + 1);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
tempTile = grid_map.getNode(grid_row - 1, grid_col);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
tempTile = grid_map.getNode(grid_row - 1, grid_col + 1);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
return(true);
case eFace8Type.RIGHT_UP:
tempTile = grid_map.getNode(grid_row, grid_col + 1);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
tempTile = grid_map.getNode(grid_row + 1, grid_col);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
tempTile = grid_map.getNode(grid_row + 1, grid_col + 1);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
return(true);
case eFace8Type.LEFT_DOWN:
tempTile = grid_map.getNode(grid_row, grid_col - 1);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
tempTile = grid_map.getNode(grid_row - 1, grid_col);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
tempTile = grid_map.getNode(grid_row - 1, grid_col - 1);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
return(true);
case eFace8Type.LEFT_UP:
tempTile = grid_map.getNode(grid_row, grid_col - 1);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
tempTile = grid_map.getNode(grid_row + 1, grid_col);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
tempTile = grid_map.getNode(grid_row + 1, grid_col - 1);
if (!checkCanMove(tempTile, m_collide_rect))
{
return(false);
}
return(true);
}
return(false);
}
private eFinderResult travel(PathGridMap grid_map, bool isFindNearstPath)
{
PathGrid node = m_start_node;
int search_count = 0;
while (!node.equal(m_end_node) && search_count < m_max_search_count)
{
++search_count;
int start_col = Mathf.Max(0, node.col - 1);
int end_col = Mathf.Min(grid_map.numCols - 1, node.col + 1);
int start_row = Mathf.Max(0, node.row - 1);
int end_row = Mathf.Min(grid_map.numRows - 1, node.row + 1);
for (int col = start_col; col <= end_col; col++)
{
for (int row = start_row; row <= end_row; row++)
{
PathGrid test = grid_map.getNode(row, col);
if (test == null || test.equal(node) ||
!test.walkable ||
!grid_map.getNode(node.row, test.col).walkable || //拐角不能通过
!grid_map.getNode(test.row, node.col).walkable
)
{
continue;
}
float cost = m_straight_cost;
if (!((node.col == test.col) || (node.row == test.row)))
{
cost = m_diag_cost;
}
/*
* 经过前面的这些，留下的就是需要计算的节点。首先计算从开始节点到测试节点的代价（g），
* 方法是当前节点的 g 值加上当前节点到测试节点的代价。简化以后就是水平、竖直方向直接加上
* _straightCost，对角加上_diagCost.h 通过估价函数计算，然后g和h 求和，得到 f（总代价）
*/
float g = node.g + cost * test.cost;
float h = onDiagonal(test);
float f = g + h;
/*
* 下面这个部分有一点小技巧，之前我们并没有谈到。开始的时候，我说过如果一个节点在待考
* 察表/已考察表里，因为它已经被考察过了，所以我们不需要再考察。不过这次计算出的结果有可
* 能小于你之前计算的结果（比如说，上次计算时是对角，而这次确是上下或左右关系，代价就小一
* 些）。所以，就算一个节点在待考察表/已考察表里面，最好还是比较一下当前值和之前值之间的大
* 小。具体做法是比较测试节点的总代价与以前计算出来的总代价。如果以前的大，我们就找到了更
* 好的节点，我们就需要重新给测试点的 f，g，h 赋值，同时，我们还要把测试点的父节点设为当前
* 点。这就要我们向后追溯
*/
if (isOpen(test) || isClosed(test))
{
if (test.f > f)
{
test.f = f;
test.g = g;
test.h = h;
test.parent = node;
}
}
else
{
/*
* 如果测试节点不再待考察表/已考察表里面，我们只需要赋值给 f，g，h 和父节点。然后把测
* 试点加到待考察表，然后是下一个测试点，找出最佳点
*/
test.f = f;
test.g = g;
test.h = h;
test.parent = node;
m_array_open.Add(test);
}
}
}
m_array_closed.Add(node);
if (m_array_open.Count == 0)
{
return(eFinderResult.FAILED);
}
m_array_open.Sort();
node = m_array_open[0];
m_array_open.RemoveAt(0);
}
if (search_count >= m_max_search_count)
{
return(eFinderResult.FAILED);
}
buildPath(node);
return(eFinderResult.SUCCEEDED);
}
