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)); } }