/// <summary> /// 寻找附近可到坐标 /// 附近的8个点 /// </summary> /// <param name="cp">当前坐标</param> /// <returns>附近可达坐标数组</returns> private List <RoutePoint> FindNearCell(RoutePoint p0) { List <RoutePoint> NearCellPoints = new List <RoutePoint>(); for (int xt = p0.x - 1; xt <= p0.x + 1; xt++) { for (int yt = p0.y - 1; yt <= p0.y + 1; yt++) { //排除超过边界的点 if (xt < 0 || yt < 0 || xt >= w || yt >= h) { continue; } //排除自己 if (xt == p0.x && yt == p0.y) { continue; } //排除不通的点 if (R[xt, yt] != 1) { continue; } NearCellPoints.Add(new RoutePoint(xt, yt)); } } return(NearCellPoints); }
//从开启列表删除 private void OpenRemove(RoutePoint p) { //Open_List.Remove(p); string key = p.x + "," + p.y; Open_dic.Remove(key); }
public RoutePoint(int x0, int y0, int G0, int H0, RoutePoint F) { x = x0; y = y0; G = G0; H = H0; father = F; }
//加入开启列表 private void OpenAdd(RoutePoint p) { //Open_List.Add(p); string key = p.x + "," + p.y; if (!Open_dic.ContainsKey(key)) { Open_dic.Add(key, p); } }
//从开启列表查找F值最小的节点(就是G+H最小的点) private RoutePoint GetMinFFromOpenList() { RoutePoint Pmin = null; //foreach (RoutePoint p in Open_List) if (Pmin == null || Pmin.G + Pmin.H > p.G + p.H) Pmin = p; foreach (RoutePoint p in Open_dic.Values) { if (Pmin == null || Pmin.G + Pmin.H > p.G + p.H) { Pmin = p; } } return(Pmin); }
//计算某个点的G值 private int GetG(RoutePoint p) { if (p.father == null) { return(0); } if (p.x == p.father.x || p.y == p.father.y) { return(p.father.G + 10); } else { return(p.father.G + 14); } }
//入口,开始节点pa,目标节点pb public List <Point> FindeWay(Point _pa, Point _pb) { RoutePoint pa = new RoutePoint(_pa); RoutePoint pb = new RoutePoint(_pb); List <Point> myp = new List <Point>(); OpenAdd(pa); bool isEnd = false;//是否结束 while (Open_dic.Count > 0 && !isEnd) { RoutePoint p0 = GetMinFFromOpenList(); if (p0 == null) { return(myp); } OpenRemove(p0); R[p0.x, p0.y] = 3;//关闭掉 foreach (RoutePoint childCell in FindNearCell(p0)) { //如果已经到了终点,把终点的父亲指向当前点,并直接结束掉 if (childCell.x == pb.x && childCell.y == pb.y) { pb.father = p0; isEnd = true; break; } //在开启列表,则取出来,重新计算G值,如果从当前节点过去的G值更低,则更新那个点的G值和父亲 RoutePoint pt = GetPointFromOpenList(childCell.x, childCell.y); if (pt != null) { int G_new = 0; if (p0.x == pt.x || p0.y == pt.y) { G_new = p0.G + 10; } else { G_new = p0.G + 14; } if (G_new < pt.G) { pt.father = p0; pt.G = G_new; } } else { //不在开启列表中,全新的点,则加入开启列表 childCell.father = p0; childCell.G = GetG(childCell); childCell.H = GetH(childCell, pb); OpenAdd(childCell); } } } //终点在列表里,起点不在 while (pb.father != null) { myp.Add(pb.getPoint()); pb = pb.father; } return(myp); }
//计算某个点的H值 private int GetH(RoutePoint p, RoutePoint pb) { return(Math.Abs(p.x - pb.x) * 10 + Math.Abs(p.y - pb.y) * 10); }