/// <summary> /// 检查和修复所有错误路径点 /// </summary> /// <param name="sTri"></param> /// <param name="startPos"></param> /// <param name="eTri"></param> /// <param name="endPos"></param> /// <returns></returns> private PathResCode CheckAndFixPos(ref NavTriangle startTri, ref Vector2 startPos, ref NavTriangle endTri, ref Vector2 endPos) { if (startTri != null && endTri != null && startTri.GetGroupID() != endTri.GetGroupID()) { return(PathResCode.GroupNotMatch); } if (endTri == null) { if (FixPos(ref endTri, ref endPos, startTri, startPos, END_POS_EXTEND_LENGTH) != PathResCode.Success) { return(PathResCode.Failed); } } if (startTri == null) { PathResCode fixRet = FixPos(ref startTri, ref startPos, endTri, endPos, START_POS_EXTEND_LENGTH); } if (startTri == null || endTri == null) { return(PathResCode.Failed); } if (startTri.GetGroupID() != endTri.GetGroupID()) { return(PathResCode.GroupNotMatch); } return(PathResCode.Success); }
/// <summary> /// 根据原始点计算出正确的落在导航区内的位置,修复无效点和三角形 /// </summary> /// <param name="orgTarTri">起源三角形</param> /// <param name="orgTarPos">起源点</param> /// <param name="otherTri">参考方向三角形</param> /// <param name="otherPos">参考方向点</param> /// <returns>结果</returns> private PathResCode FixPos(ref NavTriangle orgTarTri, ref Vector2 orgTarPos, NavTriangle otherTri, Vector2 otherPos, int extendLength) { Vector2 tarPos = orgTarPos; ////////////////////////////////////////////////////////////////////////// //为了精确判断,需要逆向延长线段一定长度 if (extendLength > 0) { Vector2 newTarPos = NMath.ExtendPos(otherPos, tarPos, extendLength); tarPos = newTarPos; } ////////////////////////////////////////////////////////////////////////// Line2D linePath = new Line2D(tarPos, otherPos); //参考线段 Rect lineRect = NMath.LineRect(linePath); //获取线段矩形包围盒 //1)找到所有与参考线段矩形相交的三角形,并判断是否groupID相等 List <NavTriangle> crossNavTris = new List <NavTriangle>(); foreach (NavTriangle tri in this.m_lstTriangle) { if (NMath.CheckCross(lineRect, tri.GetBoxCollider())) { if (otherTri != null && otherTri.GetGroupID() != tri.GetGroupID()) { continue; } crossNavTris.Add(tri); } } //2)找出所有与参考线段相交的三角形,并记录相交点 List <Vector2> crossPoints = new List <Vector2>(); //相交点列表 List <int> triIndex = new List <int>(); //相交三角形索引列表 for (int index = 0; index < crossNavTris.Count; index++) { NavTriangle crossTri = crossNavTris[index]; Line2D triLine; for (int i = 0; i < 3; i++) { Vector2 insPoint; triLine = new Line2D(crossTri.GetPoint(i), crossTri.GetPoint((i + 1) % 3)); if (linePath.Intersection(triLine, out insPoint) == LineCrossState.CROSS) { crossPoints.Add(insPoint); triIndex.Add(index); } } } if (crossPoints.Count == 0) { return(PathResCode.NoCrossPoint); } //3)找到最接近起源点的点 Vector2 lastPos = crossPoints[0]; int lastTriIndex = triIndex[0]; double lastLength = Math.Pow(lastPos.x - orgTarPos.x, 2.0) + Math.Pow(lastPos.y - orgTarPos.y, 2.0); for (int i = 1; i < crossPoints.Count; i++) { double newLength = Math.Pow(crossPoints[i].x - orgTarPos.x, 2.0) + Math.Pow(crossPoints[i].y - orgTarPos.y, 2.0); if (newLength < lastLength) { lastPos = crossPoints[i]; lastTriIndex = triIndex[i]; lastLength = newLength; } } //4)保存目标 orgTarPos = lastPos; orgTarTri = crossNavTris[lastTriIndex]; return(PathResCode.Success); }
/// <summary> /// 根据原始点计算出正确的落在导航区内的位置,修复无效点和三角形 /// </summary> /// <param name="orgTarTri">起源三角形</param> /// <param name="orgTarPos">起源点</param> /// <param name="otherTri">参考方向三角形</param> /// <param name="otherPos">参考方向点</param> /// <returns>结果</returns> private PathResCode FixPos(ref NavTriangle orgTarTri, ref Vector2 orgTarPos, NavTriangle otherTri, Vector2 otherPos, int extendLength) { Vector2 tarPos = orgTarPos; ////////////////////////////////////////////////////////////////////////// //为了精确判断,需要逆向延长线段一定长度 if (extendLength > 0) { Vector2 newTarPos = NMath.ExtendPos(otherPos, tarPos, extendLength); tarPos = newTarPos; } ////////////////////////////////////////////////////////////////////////// Line2D linePath = new Line2D(tarPos, otherPos); //参考线段 Rect lineRect = NMath.LineRect(linePath); //获取线段矩形包围盒 //1)找到所有与参考线段矩形相交的三角形,并判断是否groupID相等 List<NavTriangle> crossNavTris = new List<NavTriangle>(); foreach (NavTriangle tri in this.m_lstTriangle) { if (NMath.CheckCross(lineRect, tri.GetBoxCollider())) { if (otherTri != null && otherTri.GetGroupID() != tri.GetGroupID()) continue; crossNavTris.Add(tri); } } //2)找出所有与参考线段相交的三角形,并记录相交点 List<Vector2> crossPoints = new List<Vector2>(); //相交点列表 List<int> triIndex = new List<int>(); //相交三角形索引列表 for (int index = 0; index < crossNavTris.Count; index++) { NavTriangle crossTri = crossNavTris[index]; Line2D triLine; for (int i = 0; i < 3; i++) { Vector2 insPoint; triLine = new Line2D(crossTri.GetPoint(i), crossTri.GetPoint((i + 1)%3)); if (linePath.Intersection(triLine, out insPoint) == LineCrossState.CROSS) { crossPoints.Add(insPoint); triIndex.Add(index); } } } if (crossPoints.Count == 0) return PathResCode.NoCrossPoint; //3)找到最接近起源点的点 Vector2 lastPos = crossPoints[0]; int lastTriIndex = triIndex[0]; double lastLength = Math.Pow(lastPos.x - orgTarPos.x, 2.0) + Math.Pow(lastPos.y - orgTarPos.y, 2.0); for (int i = 1; i < crossPoints.Count; i++) { double newLength = Math.Pow(crossPoints[i].x - orgTarPos.x, 2.0) + Math.Pow(crossPoints[i].y - orgTarPos.y, 2.0); if (newLength < lastLength) { lastPos = crossPoints[i]; lastTriIndex = triIndex[i]; lastLength = newLength; } } //4)保存目标 orgTarPos = lastPos; orgTarTri = crossNavTris[lastTriIndex]; 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); } }
/// <summary> /// 检查和修复所有错误路径点 /// </summary> /// <param name="sTri"></param> /// <param name="startPos"></param> /// <param name="eTri"></param> /// <param name="endPos"></param> /// <returns></returns> private PathResCode CheckAndFixPos(ref NavTriangle startTri, ref Vector2 startPos, ref NavTriangle endTri, ref Vector2 endPos) { if ( startTri != null && endTri != null && startTri.GetGroupID() != endTri.GetGroupID()) return PathResCode.GroupNotMatch; if (endTri == null) { if (FixPos(ref endTri, ref endPos, startTri, startPos, END_POS_EXTEND_LENGTH) != PathResCode.Success) { return PathResCode.Failed; } } if (startTri == null) { PathResCode fixRet = FixPos(ref startTri, ref startPos, endTri, endPos, START_POS_EXTEND_LENGTH); } if (startTri == null || endTri == null) return PathResCode.Failed; if (startTri.GetGroupID() != endTri.GetGroupID()) return PathResCode.GroupNotMatch; return PathResCode.Success; }