void ExtendVertexWaypoint(VertexWaypoint w)
{
//判断该顶点属于哪些多边形
List <Polygon> l = new List <Polygon>(8);
foreach (Polygon p in w.grid.polygons)
{
if (p.enterable)
{
foreach (Border b in p.borders)
{
if (w.fp.Equals(b.from))
{
l.Add(p);
break;
}
}
}
}
//逐一将这些多边形的顶点和边中点添入
foreach (Polygon p in l)
{
foreach (Border b in p.borders)
{
FPoint fp = b.GetMid();
PushEdgeWaypoint(fp, b, w);
PushVertexWaypoint(b.from, b.from_idx, w);
}
}
}
void PushVertexWaypoint(FPoint fp, int fp_idx, Waypoint parent) //凭借fp_idx可以判断done标志
{
//在Close表中，退出
if (close.IsContainsPos(fp))
{
return;
}
//在Open表中否？
Waypoint w = open.GetWaypointFromXY(fp);
if (w != null)
{
//在Open表中，可优化否？
float d = fp.Distance(parent.fp);
if (w.G > parent.G + d) //可优化
{
open.Delete(w);
w.parent = parent;
w.G = parent.G + d;
w.F = w.G + w.H;
open.Add(w);
}
}
else
{
//不在Open表中
int y_idx = (int)(fp.y / 64);
int x_idx = (int)(fp.x / 64);
Grid grid = topo.grids[y_idx, x_idx];
bool done = false;
foreach (int idx in dst_fpidx) //会不会慢?...
{
if (fp_idx == idx)
{
done = true;
break;
}
}
w = new VertexWaypoint(fp, parent, grid, done);
open.Add(w);
}
}