public LinkedList<Point> Route(Point from, Point to)
{
var openSet = new List<VMWalkableRect>();
var startRect = new VMWalkableRect(from.X, from.Y, from.X, from.Y);
ConstructFirstFree(startRect);
startRect.Start = true;
startRect.ParentSource = from;
startRect.OriginalG = 0;
openSet.Add(startRect);
while (openSet.Count > 0)
{
var current = openSet[0];
openSet.RemoveAt(0);
if (current.Contains(to))
{
var result = new LinkedList<Point>();
result.AddFirst(to);
if (!to.Equals(current.ParentSource)) result.AddFirst(current.ParentSource);
Point last = current.ParentSource;
while (current != startRect)
{
current = current.Parent;
if (!last.Equals(current.ParentSource)) result.AddFirst(current.ParentSource);
last = current.ParentSource;
}
return result;
}
current.State = 2; //this rectangle is now closed
//generate all adj
ExtendFrom(current, 0);
ExtendFrom(current, 1);
ExtendFrom(current, 2);
ExtendFrom(current, 3);
foreach (VMWalkableRect r in current.Adj)
{
if (r.State == 2) continue; //closed
bool newcomer = (r.State == 0);
var parentPt = RectIntersect(r, current, current.ParentSource);
var originalG = current.OriginalG + PointDist(current.ParentSource, parentPt);
var closest = r.Closest(to.X, to.Y);
var newGScore = originalG + PointDist(parentPt, closest);
if (newcomer || newGScore < r.GScore)
{
r.State = 1;
r.ParentSource = parentPt;
r.Parent = current;
r.OriginalG = originalG;
r.GScore = newGScore;
r.FScore = newGScore + PointDist(closest, to);
if (newcomer)
{
OpenSetSortedInsert(openSet, r);
}
else
{
openSet.Remove(r);
OpenSetSortedInsert(openSet, r);
}
}
}
}
return null; //failed
}