/// <summary>
/// 生成最终的路径点
/// </summary>
/// <param name="startPos">起始点</param>
/// <param name="endPos">终点</param>
/// <param name="triPathList">三角形路径列表</param>
/// <param name="wayPoints">路径点</param>
/// <param name="offSet">移动物体宽度</param>
/// <returns></returns>
private PathResCode CreateWayPoints(Vector2 startPos, Vector2 endPos
, List <NavTriangle> triPathList, out List <Vector2> wayPoints, int offSet)
{
wayPoints = new List <Vector2>();
if (triPathList.Count == 0 || startPos == null || endPos == null)
{
return(PathResCode.Failed);
}
// 保证从起点到终点的顺序
triPathList.Reverse();
// 保存出边编号
for (int i = 0; i < triPathList.Count; i++)
{
NavTriangle tri = triPathList[i];
if (i != triPathList.Count - 1)
{
NavTriangle nextTri = triPathList[i + 1];
tri.SetOutWallIndex(tri.GetWallIndex(nextTri.GetID()));
}
}
wayPoints.Add(startPos);
//起点与终点在同一三角形中
if (triPathList.Count == 1)
{
wayPoints.Add(endPos);
return(PathResCode.Success);
}
WayPoint way = new WayPoint(startPos, triPathList[0]);
while (!NMath.IsEqualZero(way.GetPoint() - endPos))
{
way = GetFurthestWayPoint(way, triPathList, endPos, offSet);
if (way == null)
{
return(PathResCode.CanNotGetNextWayPoint);
}
wayPoints.Add(way.GetPoint());
}
return(PathResCode.Success);
}
/// <summary>
/// 寻路路径三角形
/// </summary>
/// <param name="strPos">起始点</param>
/// <param name="endPos">终点</param>
/// <param name="pathTriangle">输出三角形</param>
/// <param name="offset">移动物品大小</param>
/// <returns>结果</returns>
private PathResCode SeekTrianglePath(ref Vector2 startPos, ref Vector2 endPos, out List <NavTriangle> pathList, int offset)
{
pathList = new List <NavTriangle>();
NavTriangle startTri = null, endTri = null;
//获得起始与终点三角形
foreach (NavTriangle navTri in this.m_lstTriangle)
{
if (startTri == null)
{
if (navTri.IsPointIn(startPos))
{
startTri = navTri;
}
}
if (endTri == null)
{
if (navTri.IsPointIn(endPos))
{
endTri = navTri;
}
}
if (startTri != null && endTri != null)
{
break;
}
}
//检查和修复位置
PathResCode posErr = CheckAndFixPos(ref startTri, ref startPos, ref endTri, ref endPos);
if (posErr != PathResCode.Success)
{
return(posErr);
}
//////////////////////////////////// A*算法 ///////////////////////////////////////
int pathSessionId = 1;
bool foundPath = false;
List <NavTriangle> openList = new List <NavTriangle>(); //开放列表
List <NavTriangle> closeList = new List <NavTriangle>();
startTri.SetSessionID(pathSessionId);
openList.Add(startTri);
while (openList.Count > 0)
{
// 1. 把当前节点从开放列表删除, 加入到封闭列表
NavTriangle currNode;
currNode = openList[openList.Count - 1];
openList.Remove(currNode);
closeList.Add(currNode);
//已经找到目的地
if (currNode.GetID() == endTri.GetID())
{
foundPath = true;
break;
}
// 2. 对当前节点相邻的每一个节点依次执行以下步骤:
// 遍历所有邻接三角型
for (int i = 0; i < 3; i++)
{
int neighborID = currNode.GetNeighbor(i);
NavTriangle neighborTri;
// 3. 如果该相邻节点不可通行,则什么操作也不执行,继续检验下一个节点;
if (neighborID < 0)
{
//没有该邻居节点
continue;
}
else
{
neighborTri = this.m_lstTriangle[neighborID];
if (neighborTri == null || neighborTri.GetID() != neighborID)
{
return(PathResCode.NavIDNotMatch);
}
}
if (neighborTri.GetGroupID() == startTri.GetGroupID())
{
if (neighborTri.GetSessionID() != pathSessionId)
{
// 4. 如果该相邻节点不在开放列表中,则将该节点添加到开放列表中,
// 并将该相邻节点的父节点设为当前节点,同时保存该相邻节点的G和F值;
neighborTri.SetSessionID(pathSessionId);
neighborTri.SetParentID(currNode.GetID());
neighborTri.SetOpen(true);
// 计算启发值h
neighborTri.CalcHeuristic(endPos);
// 计算三角形花费g
neighborTri.SetGValue(currNode.GetGValue() + currNode.GetCost(neighborTri.GetID()));
//放入开放列表并排序
openList.Add(neighborTri);
openList.Sort(CompareTriWithGValue);
//保存穿入边
neighborTri.SetArrivalWall(currNode.GetID());
}
else
{
// 5. 如果该相邻节点在开放列表中,
// 则判断若经由当前节点到达该相邻节点的G值是否小于原来保存的G值,
// 若小于,则将该相邻节点的父节点设为当前节点,并重新设置该相邻节点的G和F值
if (neighborTri.GetOpen())
{
if (neighborTri.GetGValue() + neighborTri.GetCost(currNode.GetID()) < currNode.GetGValue())
{
currNode.SetGValue(neighborTri.GetGValue() + neighborTri.GetCost(currNode.GetID()));
currNode.SetParentID(neighborTri.GetID());
currNode.SetArrivalWall(neighborTri.GetID());
}
}
else
{
neighborTri = null;
continue;
}
}
}
}
}
if (closeList.Count != 0)
{
NavTriangle path = closeList[closeList.Count - 1];
pathList.Add(path);
while (path.GetParentID() != -1)
{
pathList.Add(this.m_lstTriangle[path.GetParentID()]);
path = this.m_lstTriangle[path.GetParentID()];
}
}
if (!foundPath)
{
return(PathResCode.NotFoundPath);
}
else
{
return(PathResCode.Success);
}
}