/// <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); } }