public Point ClosestOnSharedEdge(VMWalkableRect other, Point pt)
{
if (x2 == other.x1) //right side
{
return(new Point(x2, Math.Min(Math.Min(y2, other.y2), Math.Max(Math.Max(y1, other.y1), pt.Y))));
}
else if (x1 == other.x2) //left side
{
return(new Point(x1, Math.Min(Math.Min(y2, other.y2), Math.Max(Math.Max(y1, other.y1), pt.Y))));
}
else if (y1 == other.y2) //top side
{
return(new Point(Math.Min(Math.Min(other.x2, x2), Math.Max(Math.Max(x1, other.x1), pt.X)), y1));
}
else if (y2 == other.y1) //bottom side
{
return(new Point(Math.Min(Math.Min(other.x2, x2), Math.Max(Math.Max(x1, other.x1), pt.X)), y2));
}
return(pt); //not in contact...
}
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
}
private void OpenSetSortedInsert(List<VMWalkableRect> set, VMWalkableRect item)
{
for (var i = 0; i < set.Count; i++)
{
if (set[i].FScore > item.FScore)
{
set.Insert(i, item);
return;
}
}
set.Add(item);
}
private void ExtendFrom(VMWalkableRect source, int dir)
{
var free = source.Free[dir].List;
foreach (VMFreeListRegion line in free)
{
VMExtendRectResult extension = new VMExtendRectResult();
VMWalkableRect newRect = null;
switch (dir)
{
case 0:
extension = ExtendRect(dir, line.a, line.b, source.y1);
newRect = (extension.BestN == source.y1) ? null : new VMWalkableRect(line.a, extension.BestN, line.b, source.y1);
break;
case 1:
extension = ExtendRect(dir, line.a, line.b, source.x2);
newRect = (extension.BestN == source.x2) ? null : new VMWalkableRect(source.x2, line.a, extension.BestN, line.b);
break;
case 2:
extension = ExtendRect(dir, line.a, line.b, source.y2);
newRect = (extension.BestN == source.y2) ? null : new VMWalkableRect(line.a, source.y2, line.b, extension.BestN);
break;
case 3:
extension = ExtendRect(dir, line.a, line.b, source.x1);
newRect = (extension.BestN == source.x1) ? null : new VMWalkableRect(extension.BestN, line.a, source.x1, line.b);
break;
}
if (newRect == null || extension.BestN == int.MaxValue || extension.BestN == int.MinValue) continue;
source.Adj.Add(newRect);
newRect.Adj.Add(source);
var bounds = ((dir % 2) == 0) ? new VMFreeListRegion(newRect.x1, newRect.x2) : new VMFreeListRegion(newRect.y1, newRect.y2);
var free2 = new VMFreeList(bounds);
foreach (var r in extension.Best)
{
free2.Subtract(new VMFreeListRegion(r.a, r.b));
if (r.rect is VMWalkableRect)
{
var w = (VMWalkableRect)r.rect;
w.Free[(dir + 2) % 4].Subtract(bounds);
w.Adj.Add(newRect);
newRect.Adj.Add(w);
}
}
newRect.Free[dir] = free2;
newRect.Free[(dir + 2) % 4] = new VMFreeList(0, 0);
ConstructFree(newRect, ((dir % 2) == 1), ((dir % 2) == 0), ((dir % 2) == 1), ((dir % 2) == 0));
if (!source.Start) Map.Add(newRect);
}
}
private void ConstructFree(VMWalkableRect rect, bool d1, bool d2, bool d3, bool d4)
{
if (d1) rect.Free[0] = new VMFreeList(rect.x1, rect.x2);
if (d2) rect.Free[1] = new VMFreeList(rect.y1, rect.y2);
if (d3) rect.Free[2] = new VMFreeList(rect.x1, rect.x2);
if (d4) rect.Free[3] = new VMFreeList(rect.y1, rect.y2);
foreach (VMObstacle r in Map)
{
if (r == rect) continue;
if (d1 && r.y2 == rect.y1 && !(r.x2 <= rect.x1 || r.x1 >= rect.x2))
{
rect.Free[0].Subtract(new VMFreeListRegion(r.x1, r.x2));
if (r is VMWalkableRect)
{
var w = (VMWalkableRect)r;
w.Free[2].Subtract(new VMFreeListRegion(rect.x1, rect.x2));
rect.Adj.Add(w);
w.Adj.Add(rect);
}
}
if (d2 && r.x1 == rect.x2 && !(r.y2 <= rect.y1 || r.y1 >= rect.y2)) {
rect.Free[1].Subtract(new VMFreeListRegion(r.y1, r.y2));
if (r is VMWalkableRect)
{
var w = (VMWalkableRect)r;
w.Free[3].Subtract(new VMFreeListRegion(rect.y1, rect.y2));
rect.Adj.Add(w);
w.Adj.Add(rect);
}
}
if (d3 && r.y1 == rect.y2 && !(r.x2 <= rect.x1 || r.x1 >= rect.x2)) {
rect.Free[2].Subtract(new VMFreeListRegion(r.x1, r.x2));
if (r is VMWalkableRect)
{
var w = (VMWalkableRect)r;
w.Free[0].Subtract(new VMFreeListRegion(rect.x1, rect.x2));
rect.Adj.Add(w);
w.Adj.Add(rect);
}
}
if (d4 && r.x2 == rect.x1 && !(r.y2 <= rect.y1 || r.y1 >= rect.y2)) {
rect.Free[3].Subtract(new VMFreeListRegion(r.y1, r.y2));
if (r is VMWalkableRect)
{
var w = (VMWalkableRect)r;
w.Free[1].Subtract(new VMFreeListRegion(rect.y1, rect.y2));
rect.Adj.Add(w);
w.Adj.Add(rect);
}
}
}
}
private void ConstructFirstFree(VMWalkableRect rect)
{
rect.Free[0] = new VMFreeList(rect.x1);
rect.Free[1] = new VMFreeList(rect.y1);
rect.Free[2] = new VMFreeList(rect.x1);
rect.Free[3] = new VMFreeList(rect.y1);
foreach (VMObstacle r in Map)
{
if (r == rect) continue;
if (r.y2 == rect.y1 && !(r.x2 <= rect.x1 || r.x1 >= rect.x2))
rect.Free[0].Subtract(new VMFreeListRegion(r.x1, r.x2));
if (r.x1 == rect.x2 && !(r.y2 <= rect.y1 || r.y1 >= rect.y2))
rect.Free[1].Subtract(new VMFreeListRegion(r.y1, r.y2));
if (r.y1 == rect.y2 && !(r.x2 <= rect.x1 || r.x1 >= rect.x2))
rect.Free[2].Subtract(new VMFreeListRegion(r.x1, r.x2));
if (r.x2 == rect.x1 && !(r.y2 <= rect.y1 || r.y1 >= rect.y2))
rect.Free[3].Subtract(new VMFreeListRegion(r.y1, r.y2));
}
}
