// STATIC
public static LinkedList <VMIPathSegment> GeneratePath(LinkedList <VMWalkableRect> rects, Point dest)
{
if (rects == null)
{
return(null);
}
var result = new LinkedList <VMIPathSegment>();
VMWalkableRect last = null;
dest = new Point(dest.X * 0x8000, dest.Y * 0x8000);
foreach (var rect in rects)
{
if (last != null)
{
//line between the two ParentSource points
result.AddLast(new VMPathLineSegment(last.ParentSourceHiP, rect.ParentSourceHiP));
}
last = rect;
}
if (last != null)
{
result.AddLast(new VMPathLineSegment(last.ParentSourceHiP, dest));
}
return(result);
}
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));
}
}
}
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 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);
}
}
}
}
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 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);
}
}
}
public static LinkedList <VMIPathSegment> GeneratePath(LinkedList <VMWalkableRect> rects, Point dest, float?dirIn, float?dirOut)
{
if (rects == null)
{
return(null);
}
var result = new LinkedList <VMIPathSegment>();
VMWalkableRect last = null;
VMWalkableRect last2 = null;
VMIPathSegment lastSeg = null;
dest = new Point(dest.X * 0x8000, dest.Y * 0x8000);
int lineProcess = -1;
VMWalkableRect lineStart = null;
VMWalkableRect preLineStart = null;
foreach (var rect in rects)
{
if (last != null)
{
if (lineProcess != -1 || last.LineID != -1)
{
if (lineProcess == -1)
{
//
lineStart = last;
preLineStart = last2;
lineProcess = last.LineID;
}
if (rect.LineID != lineProcess)
{
var seg2 = new VMPathLineSegment(lineStart.ParentSourceHiP, rect.ParentSourceHiP);
//var seg2 = new VMPathBezierSegment() { A = lineStart.ParentSourceHiP, D = rect.ParentSourceHiP };
//seg2.B = seg2.A;
//seg2.C = seg2.D;
GenerateBezierControl(lastSeg, seg2, preLineStart ?? lineStart, lineStart);
result.AddLast(seg2);
lastSeg = seg2;
lineProcess = -1;
}
last2 = last;
last = rect;
continue;
}
if (rect.ParentSourceHiP == last.ParentSourceHiP)
{
last = rect;
continue; //there's not any value in this line, just skip it
}
//new line from
var seg = new VMPathBezierSegment()
{
A = last.ParentSourceHiP, D = rect.ParentSourceHiP
};
if (lastSeg == null)
{
//starting.
if (dirIn != null)
{
//we've been instructed to set up a basic control point for entry.
//right now rather weak
var dirVec = new Vector2((float)Math.Sin(dirIn.Value), (float)-Math.Cos(dirIn.Value)) * (6 * 0x8000);
var dirPt = seg.A + dirVec.ToPoint();
seg.B = last.ClosestHiP(dirPt.X, dirPt.Y);
}
else
{
//initialize control point as empty
seg.B = seg.A;
}
}
else
{
//initialize our first control point and the last segment's second
//based on the angle between each line, within rectangle limits.
//these should be symmetrical
GenerateBezierControl(lastSeg, seg, last2, last);
}
//line between the two ParentSource points
result.AddLast(seg);
lastSeg = seg;
}
last2 = last;
last = rect;
}
//finally, a line to the destination point.
if (last != null)
{
var seg = new VMPathBezierSegment()
{
A = last.ParentSourceHiP, C = dest, D = dest
};
if (dirOut != null)
{
var dirVec = new Vector2((float)Math.Sin(dirOut.Value), (float)-Math.Cos(dirOut.Value)) * (16 * 0x8000);
var dirPt = seg.D - dirVec.ToPoint();
seg.C = last.ClosestHiP(dirPt.X, dirPt.Y);
}
if (last2 == null)
{
//starting.
if (dirIn != null)
{
//we've been instructed to set up a basic control point for entry.
//right now rather weak
var dirVec = new Vector2((float)Math.Sin(dirIn.Value), (float)-Math.Cos(dirIn.Value)) * (6 * 0x8000);
var dirPt = seg.A + dirVec.ToPoint();
seg.B = last.ClosestHiP(dirPt.X, dirPt.Y);
}
else
{
//initialize control point as empty
seg.B = seg.A;
}
}
else
{
//initialize our first control point and the last segment's second
//based on the angle between each line, within rectangle limits.
//these should be symmetrical
GenerateBezierControl(lastSeg, seg, last2, last);
}
//line between the two ParentSource points
result.AddLast(seg);
}
return(result);
}
// STATIC
private static void GenerateBezierControl(VMIPathSegment fromSegI, VMIPathSegment toSegI,
VMWalkableRect from, VMWalkableRect to)
{
//find average direction line
var fromSlice = new VMObstacle(from.x1 * 0x8000, from.y1 * 0x8000, from.x2 * 0x8000, from.y2 * 0x8000); //new VMObstacle(fromSegI.Source, fromSegI.Destination);
var toSlice = new VMObstacle(to.x1 * 0x8000, to.y1 * 0x8000, to.x2 * 0x8000, to.y2 * 0x8000); //new VMObstacle(toSegI.Source, toSegI.Destination);
if (fromSegI is VMPathBezierSegment)
{
var fromSeg = (VMPathBezierSegment)fromSegI;
if (toSegI is VMPathBezierSegment)
{
var toSeg = (VMPathBezierSegment)toSegI;
var fromDiff = (fromSeg.D - fromSeg.A).ToVector2();
var toDiff = (toSeg.D - toSeg.A).ToVector2();
//essentially normalizing, but keeping the lengths stored
var fromLength = fromDiff.Length();
var toLength = toDiff.Length();
fromDiff /= fromLength;
toDiff /= toLength;
//our control points should create a line with average direction between its bordering lines.
var avg = fromDiff + toDiff;
avg.Normalize();
var controlStrength = Math.Min(fromLength, toLength) / 2;
toSeg.B = toSeg.A + (avg * toLength / 3).ToPoint();
fromSeg.C = fromSeg.D - (avg * fromLength / 3).ToPoint();
int changed = 2;
bool testTo = true;
int emergency = 0;
while (emergency++ < 1000 && changed-- > 0)
{
if (testTo)
{
//make sure to is within rect bounds
var n = toSlice.Closest(toSeg.B.X, toSeg.B.Y);
if (n != toSeg.B)
{
//if we changed, we'll need to check the other side again.
changed = 1;
//multiply the other side by the change factors here
var diff = (toSeg.B - toSeg.A);
var diff2 = (n - toSeg.A);
var otherDiff = (fromSeg.C - fromSeg.D);
if (diff.X != 0)
{
otherDiff.X = (int)(((long)otherDiff.X * diff2.X) / diff.X);
}
if (diff.Y != 0)
{
otherDiff.Y = (int)(((long)otherDiff.Y * diff2.Y) / diff.Y);
}
toSeg.B = n;
fromSeg.C = fromSeg.D + otherDiff;
}
}
else
{
//make sure from is within rect bounds
var n = fromSlice.Closest(fromSeg.C.X, fromSeg.C.Y);
if (n != fromSeg.C)
{
//if we changed, we'll need to check the other side again.
changed = 1;
//multiply the other side by the change factors here
var diff = (fromSeg.C - fromSeg.D);
var diff2 = (n - fromSeg.D);
var otherDiff = (toSeg.B - toSeg.A);
if (diff.X != 0)
{
otherDiff.X = (int)(((long)otherDiff.X * diff2.X) / diff.X);
}
if (diff.Y != 0)
{
otherDiff.Y = (int)(((long)otherDiff.Y * diff2.Y) / diff.Y);
}
fromSeg.C = n;
toSeg.B = toSeg.A + otherDiff;
}
}
testTo = !testTo;
}
}
else
{
var fromDiff = (fromSeg.D - fromSeg.A).ToVector2();
var toDiff = (toSegI.Destination - toSegI.Source).ToVector2();
//essentially normalizing, but keeping the lengths stored
var fromLength = fromDiff.Length();
var toLength = toDiff.Length();
fromDiff /= fromLength;
toDiff /= toLength;
//our control points should create a line with average direction between its bordering lines.
fromSeg.C = fromSeg.D - (toDiff * fromLength / 3).ToPoint();
fromSeg.C = fromSlice.Closest(fromSeg.C.X, fromSeg.C.Y);
}
}
else if (toSegI is VMPathBezierSegment)
{
var toSeg = (VMPathBezierSegment)toSegI;
var fromDiff = (fromSegI.Destination - fromSegI.Source).ToVector2();
var toDiff = (toSeg.D - toSeg.A).ToVector2();
//essentially normalizing, but keeping the lengths stored
var fromLength = fromDiff.Length();
var toLength = toDiff.Length();
fromDiff /= fromLength;
toDiff /= toLength;
//our control points should create a line with average direction between its bordering lines.
toSeg.B = toSeg.A + (fromDiff * toLength / 3).ToPoint();
toSeg.B = toSlice.Closest(toSeg.B.X, toSeg.B.Y);
}
}
