dbc522a224
Modified weapon scripts to eliminate specialized projectile behavior - allows charged weapons to have projectiles and modify weapon stats when charged Added machine gun weapon & VFX Moved third party code to Plugins folder to improve compile times
743 lines
39 KiB
C#
743 lines
39 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.Linq;
|
|
using Microsoft.Msagl.Core.DataStructures;
|
|
using Microsoft.Msagl.Core.Geometry;
|
|
using Microsoft.Msagl.Core.Geometry.Curves;
|
|
using Microsoft.Msagl.Core.Layout;
|
|
using Microsoft.Msagl.DebugHelpers;
|
|
using Microsoft.Msagl.Routing.Visibility;
|
|
|
|
//following "Orthogonal Connector Routing" of Michael Wybrow and others
|
|
|
|
namespace Microsoft.Msagl.Routing.Rectilinear {
|
|
/// <summary>
|
|
/// single source single target rectilinear path
|
|
/// </summary>
|
|
internal class SsstRectilinearPath {
|
|
internal double LengthImportance { get; set; }
|
|
internal double BendsImportance { get; set; }
|
|
|
|
// Only bends importance needs to be public.
|
|
internal const double DefaultBendPenaltyAsAPercentageOfDistance = 4.0;
|
|
|
|
private VisibilityVertexRectilinear Target { get; set; }
|
|
private VisibilityVertexRectilinear Source { get; set; }
|
|
private Directions EntryDirectionsToTarget { get; set; }
|
|
private double upperBoundOnCost;
|
|
private double sourceCostAdjustment;
|
|
private double targetCostAdjustment;
|
|
|
|
/// <summary>
|
|
/// The cost of the path calculation
|
|
/// </summary>
|
|
private double CombinedCost(double length, double numberOfBends) {
|
|
return LengthImportance * length + BendsImportance * numberOfBends;
|
|
}
|
|
|
|
private double TotalCostFromSourceToVertex(double length, double numberOfBends) {
|
|
return this.CombinedCost(length, numberOfBends) + this.sourceCostAdjustment;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The priority queue for path extensions.
|
|
/// </summary>
|
|
private GenericBinaryHeapPriorityQueueWithTimestamp<VertexEntry> queue;
|
|
|
|
/// <summary>
|
|
/// The list of vertices we've visited for all paths.
|
|
/// </summary>
|
|
private List<VisibilityVertexRectilinear> visitedVertices;
|
|
|
|
// For consistency and speed, path extensions impose an ordering as in the paper: straight, right, left. We
|
|
// enqueue entries in the reverse order of preference so the latest timestamp will be the preferred direction.
|
|
// Thus straight-ahead neighbors are in slot 2, right in slot 1, left in slot 0. (If the target happens
|
|
// to be to the Left, then the heuristic lookahead score will override the Right preference).
|
|
private class NextNeighbor {
|
|
internal VisibilityVertexRectilinear Vertex;
|
|
internal double Weight;
|
|
|
|
internal NextNeighbor() {
|
|
Clear();
|
|
}
|
|
|
|
internal void Set(VisibilityVertexRectilinear v, double w) {
|
|
this.Vertex = v;
|
|
this.Weight = w;
|
|
}
|
|
|
|
internal void Clear() {
|
|
this.Vertex = null;
|
|
this.Weight = double.NaN;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// The next neighbors to extend the path to from the current vertex.
|
|
/// </summary>
|
|
private readonly NextNeighbor[] nextNeighbors = new[] {new NextNeighbor(), new NextNeighbor(), new NextNeighbor() };
|
|
|
|
public SsstRectilinearPath() {
|
|
LengthImportance = 1.0;
|
|
BendsImportance = 1.0;
|
|
}
|
|
|
|
private bool InitPath(VertexEntry[] sourceVertexEntries, VisibilityVertexRectilinear source, VisibilityVertexRectilinear target) {
|
|
if ((source == target) || !InitEntryDirectionsAtTarget(target)) {
|
|
return false;
|
|
}
|
|
this.Target = target;
|
|
this.Source = source;
|
|
double cost = this.TotalCostFromSourceToVertex(0, 0) + HeuristicDistanceFromVertexToTarget(source.Point, Directions. None);
|
|
if (cost >= this.upperBoundOnCost) {
|
|
return false;
|
|
}
|
|
|
|
// This path starts lower than upperBoundOnCost, so create our structures and process it.
|
|
this.queue = new GenericBinaryHeapPriorityQueueWithTimestamp<VertexEntry>();
|
|
this.visitedVertices = new List<VisibilityVertexRectilinear> { source };
|
|
|
|
if (sourceVertexEntries == null) {
|
|
EnqueueInitialVerticesFromSource(cost);
|
|
} else {
|
|
EnqueueInitialVerticesFromSourceEntries(sourceVertexEntries);
|
|
}
|
|
return this.queue.Count > 0;
|
|
}
|
|
|
|
private bool InitEntryDirectionsAtTarget(VisibilityVertex vert) {
|
|
EntryDirectionsToTarget = Directions. None;
|
|
|
|
// This routine is only called once so don't worry about optimizing foreach.
|
|
foreach (var edge in vert.OutEdges) {
|
|
#if SHARPKIT //http://code.google.com/p/sharpkit/issues/detail?id=368 property assignment not working with |= operator
|
|
EntryDirectionsToTarget = EntryDirectionsToTarget | CompassVector.DirectionsFromPointToPoint(edge.TargetPoint, vert.Point);
|
|
#else
|
|
EntryDirectionsToTarget |= CompassVector.DirectionsFromPointToPoint(edge.TargetPoint, vert.Point);
|
|
#endif
|
|
}
|
|
foreach (var edge in vert.InEdges) {
|
|
#if SHARPKIT //http://code.google.com/p/sharpkit/issues/detail?id=368 property assignment not working with |= operator
|
|
EntryDirectionsToTarget = EntryDirectionsToTarget | CompassVector.DirectionsFromPointToPoint(edge.SourcePoint, vert.Point);
|
|
#else
|
|
EntryDirectionsToTarget |= CompassVector.DirectionsFromPointToPoint(edge.SourcePoint, vert.Point);
|
|
#endif
|
|
}
|
|
// If this returns false then the target is isolated.
|
|
return EntryDirectionsToTarget != Directions. None;
|
|
}
|
|
|
|
private static bool IsInDirs(Directions direction, Directions dirs) {
|
|
return direction == (direction & dirs);
|
|
}
|
|
|
|
internal double MultistageAdjustedCostBound(double bestCost) {
|
|
// Allow an additional bend's cost for intermediate stages so we don't jump out early.
|
|
return !double.IsPositiveInfinity(bestCost) ? bestCost + this.BendsImportance : bestCost;
|
|
}
|
|
|
|
/// <summary>
|
|
/// estimation from below for the distance
|
|
/// </summary>
|
|
/// <param name="point"></param>
|
|
/// <param name="entryDirToVertex"></param>
|
|
/// <returns></returns>
|
|
private double HeuristicDistanceFromVertexToTarget(Point point, Directions entryDirToVertex) {
|
|
Point vectorToTarget = Target.Point - point;
|
|
if (ApproximateComparer.Close(vectorToTarget.X, 0) && ApproximateComparer.Close(vectorToTarget.Y, 0)) {
|
|
// We are at the target.
|
|
return this.targetCostAdjustment;
|
|
}
|
|
Directions dirToTarget = CompassVector.VectorDirection(vectorToTarget);
|
|
|
|
int numberOfBends;
|
|
if (entryDirToVertex == Directions. None) {
|
|
entryDirToVertex = Directions.East | Directions.North | Directions.West | Directions.South;
|
|
numberOfBends = GetNumberOfBends(entryDirToVertex, dirToTarget);
|
|
} else {
|
|
numberOfBends = GetNumberOfBends(entryDirToVertex, dirToTarget);
|
|
}
|
|
return CombinedCost(ManhattanDistance(point, Target.Point), numberOfBends) + this.targetCostAdjustment;
|
|
}
|
|
|
|
private int GetNumberOfBends(Directions entryDirToVertex, Directions dirToTarget) {
|
|
return CompassVector.IsPureDirection(dirToTarget)
|
|
? GetNumberOfBendsForPureDirection(entryDirToVertex, dirToTarget)
|
|
: GetBendsForNotPureDirection(dirToTarget, entryDirToVertex, EntryDirectionsToTarget);
|
|
}
|
|
|
|
private int GetNumberOfBendsForPureDirection(Directions entryDirToVertex, Directions dirToTarget) {
|
|
if ( (dirToTarget & entryDirToVertex) == dirToTarget) {
|
|
if (IsInDirs(dirToTarget, EntryDirectionsToTarget)) {
|
|
return 0;
|
|
}
|
|
if (IsInDirs(Left(dirToTarget), EntryDirectionsToTarget) || IsInDirs(Right(dirToTarget), EntryDirectionsToTarget)) {
|
|
return 2;
|
|
}
|
|
return 4;
|
|
}
|
|
return GetNumberOfBendsForPureDirection(AddOneTurn[(int)entryDirToVertex], dirToTarget) + 1;
|
|
}
|
|
|
|
private static int GetBendsForNotPureDirection(Directions dirToTarget, Directions entryDirToVertex, Directions entryDirectionsToTarget) {
|
|
Directions a = dirToTarget & entryDirToVertex;
|
|
if (a == Directions. None) {
|
|
return GetBendsForNotPureDirection(dirToTarget, AddOneTurn[(int)entryDirToVertex], entryDirectionsToTarget) + 1;
|
|
}
|
|
Directions b = dirToTarget & entryDirectionsToTarget;
|
|
if (b == Directions. None) {
|
|
return GetBendsForNotPureDirection(dirToTarget, entryDirToVertex, AddOneTurn[(int)entryDirectionsToTarget]) + 1;
|
|
}
|
|
return (a | b) == dirToTarget ? 1 : 2;
|
|
}
|
|
|
|
private static readonly Directions[] AddOneTurn=new[] {
|
|
Directions.None, //Directions. None-> None
|
|
Directions.North|Directions.East|Directions.West, // 1=N -> N,E,W
|
|
Directions.North|Directions.East|Directions.South, // 2 =E->E|N|S
|
|
(Directions)15, // 3 =E|N->E|N|S
|
|
Directions.East|Directions.South|Directions.West, // 4 =S->E|S|W
|
|
(Directions)15, // 5 =E|N->E|N|S|W
|
|
(Directions)15, //6 - S|E
|
|
(Directions)15, //7
|
|
(Directions)13, //8=W
|
|
(Directions)15, //9
|
|
(Directions)15, //10
|
|
(Directions)15, //11
|
|
(Directions)15, //12
|
|
(Directions)15, //13
|
|
(Directions)15, //14
|
|
(Directions)15, //15
|
|
};
|
|
|
|
private static Directions Left(Directions direction) {
|
|
switch (direction) {
|
|
case Directions. None:
|
|
return Directions. None;
|
|
case Directions.North:
|
|
return Directions.West;
|
|
case Directions.East:
|
|
return Directions.North;
|
|
case Directions.South:
|
|
return Directions.East;
|
|
case Directions.West:
|
|
return Directions.South;
|
|
default:
|
|
throw new ArgumentOutOfRangeException("direction");
|
|
}
|
|
}
|
|
|
|
private static Directions Right(Directions direction) {
|
|
switch (direction) {
|
|
case Directions. None:
|
|
return Directions. None;
|
|
case Directions.North:
|
|
return Directions.East;
|
|
case Directions.East:
|
|
return Directions.South;
|
|
case Directions.South:
|
|
return Directions.West;
|
|
case Directions.West:
|
|
return Directions.North;
|
|
default:
|
|
throw new ArgumentOutOfRangeException("direction");
|
|
}
|
|
}
|
|
|
|
internal static IEnumerable<Point> RestorePath(VertexEntry entry) {
|
|
return RestorePath(ref entry, null);
|
|
}
|
|
|
|
internal static IEnumerable<Point> RestorePath(ref VertexEntry entry, VisibilityVertex firstVertexInStage) {
|
|
if (entry == null) {
|
|
return null;
|
|
}
|
|
var list = new List<Point>();
|
|
bool skippedCollinearEntry = false;
|
|
Directions lastEntryDir = Directions. None;
|
|
while (true) {
|
|
// Reduce unnecessary AxisEdge creations in Nudger by including only bend points, not points in the middle of a segment.
|
|
if (lastEntryDir == entry.Direction) {
|
|
skippedCollinearEntry = true;
|
|
} else {
|
|
skippedCollinearEntry = false;
|
|
list.Add(entry.Vertex.Point);
|
|
lastEntryDir = entry.Direction;
|
|
}
|
|
|
|
var previousEntry = entry.PreviousEntry;
|
|
if ((previousEntry == null) || (entry.Vertex == firstVertexInStage)) {
|
|
break;
|
|
}
|
|
entry = previousEntry;
|
|
}
|
|
if (skippedCollinearEntry) {
|
|
list.Add(entry.Vertex.Point);
|
|
}
|
|
list.Reverse();
|
|
return list;
|
|
}
|
|
|
|
#region TEST_MSAGL
|
|
// #if TEST_MSAGL
|
|
// // ReSharper disable UnusedMember.Local
|
|
// [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Naming", "CA2204:Literals should be spelled correctly", MessageId = "len"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Naming", "CA2204:Literals should be spelled correctly", MessageId = "newEntry"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Naming", "CA2204:Literals should be spelled correctly", MessageId = "nbend"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Naming", "CA2204:Literals should be spelled correctly", MessageId = "iters"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Naming", "CA2204:Literals should be spelled correctly", MessageId = "hcost"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Naming", "CA2204:Literals should be spelled correctly", MessageId = "ccost"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1804:RemoveUnusedLocals", MessageId = "so"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1804:RemoveUnusedLocals", MessageId = "q"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1804:RemoveUnusedLocals", MessageId = "edges"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Globalization", "CA1303:Do not pass literals as localized parameters", MessageId = "System.Console.WriteLine(System.String,System.Object[])"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1811:AvoidUncalledPrivateCode")]
|
|
// private void TestShowAllPaths(VisibilityVertex source, VertexEntry mostRecentlyExtendedPath) {
|
|
// // ReSharper restore UnusedMember.Local
|
|
// var edges = GetAllEdgesTest(source).Select(e => (ICurve) (new LineSegment(e.SourcePoint, e.TargetPoint))).
|
|
// Select(c => new DebugCurve(c));
|
|
// var q = queue.Select(ent => CurveFactory.CreateDiamond(2, 2, ent.Vertex.Point)).Select(c => new DebugCurve(c));
|
|
// var so = new[] {
|
|
// new DebugCurve(1, "brown", new Ellipse(3, 3, source.Point)),
|
|
// new DebugCurve(1, "purple", CurveFactory.CreateDiamond(4, 4, Target.Point)),
|
|
// new DebugCurve(1, "red", CurveFactory.CreateDiamond(6, 6, mostRecentlyExtendedPath.Vertex.Point))
|
|
// };
|
|
//
|
|
// var pathEdges = new List<DebugCurve>();
|
|
// var newEntries = new List<VertexEntry>();
|
|
// var count = this.visitedVertices.Count;
|
|
// for (int ii = 0; ii < count; ++ii) {
|
|
// var vertex = this.visitedVertices[ii];
|
|
// if (vertex.VertexEntries == null) {
|
|
// continue; // this is the source vertex
|
|
// }
|
|
// foreach (var entry in vertex.VertexEntries) {
|
|
// if (entry == null) {
|
|
// continue;
|
|
// }
|
|
// var color = "green";
|
|
// if (entry.PreviousEntry == mostRecentlyExtendedPath) {
|
|
// newEntries.Add(entry);
|
|
// color = "red";
|
|
// } else if (!entry.IsClosed) {
|
|
// color = "yellow";
|
|
// }
|
|
// pathEdges.Add(new DebugCurve(2, color, new LineSegment(entry.PreviousVertex.Point, vertex.Point)));
|
|
// }
|
|
// }
|
|
// Console.WriteLine("entry {0} seq = {1} len = {2} nbend = {3} ccost = {4} hcost = {5} iters = {6}/{7}", mostRecentlyExtendedPath,
|
|
// this.lastDequeueTimestamp,
|
|
// mostRecentlyExtendedPath.Length, mostRecentlyExtendedPath.NumberOfBends,
|
|
// this.CombinedCost(mostRecentlyExtendedPath.Length, mostRecentlyExtendedPath.NumberOfBends),
|
|
// this.HeuristicDistanceFromVertexToTarget(mostRecentlyExtendedPath.Vertex.Point, mostRecentlyExtendedPath.Direction),
|
|
// this.currentIterations, totalIterations);
|
|
// foreach (var newEntry in newEntries) {
|
|
// Console.WriteLine(" newEntry {0} len = {1} nbend = {2} ccost = {3} hcost = {4}", newEntry,
|
|
// newEntry.Length, newEntry.NumberOfBends,
|
|
// this.CombinedCost(newEntry.Length, newEntry.NumberOfBends),
|
|
// this.HeuristicDistanceFromVertexToTarget(newEntry.Vertex.Point, newEntry.Direction));
|
|
// }
|
|
// DevTraceDisplay(edges.Concat(q).Concat(so).Concat(pathEdges));
|
|
// }
|
|
//
|
|
// // ReSharper disable UnusedMember.Local
|
|
// [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Naming", "CA2204:Literals should be spelled correctly", MessageId = "len"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Naming", "CA2204:Literals should be spelled correctly", MessageId = "nbend"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Naming", "CA2204:Literals should be spelled correctly", MessageId = "iters"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1804:RemoveUnusedLocals", MessageId = "so"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1804:RemoveUnusedLocals", MessageId = "pathEdges"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1804:RemoveUnusedLocals", MessageId = "edges"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Globalization", "CA1303:Do not pass literals as localized parameters", MessageId = "System.Console.WriteLine(System.String,System.Object[])"), System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1811:AvoidUncalledPrivateCode")]
|
|
// private void TestShowPath(VisibilityVertex source, IEnumerable<Point> pathPoints, double cost, double length, int numberOfBends) {
|
|
// // ReSharper restore UnusedMember.Local
|
|
// var edges = GetAllEdgesTest(source).Select(e => (ICurve) (new LineSegment(e.SourcePoint, e.TargetPoint))).
|
|
// Select(c => new DebugCurve(c));
|
|
// var so = new[] {
|
|
// new DebugCurve(1, "brown", new Ellipse(3, 3, source.Point)),
|
|
// new DebugCurve(1, "purple", CurveFactory.CreateDiamond(4, 4, Target.Point)),
|
|
// };
|
|
//
|
|
// List<DebugCurve> pathEdges = GetPathEdgeDebugCurves(pathPoints, "green");
|
|
// Console.WriteLine("path {0} -> {1} cost = {2} len = {3} nbend = {4} iters = {5}/{6}",
|
|
// source.Point, this.Target.Point, cost, length, numberOfBends, this.currentIterations, totalIterations);
|
|
// DevTraceDisplay(edges.Concat(so).Concat(pathEdges));
|
|
// }
|
|
//
|
|
// internal static List<DebugCurve> GetPathEdgeDebugCurves(IEnumerable<Point> pathPoints, string color) {
|
|
// var pathEdges = new List<DebugCurve>();
|
|
// if (pathPoints != null) {
|
|
// bool first = true;
|
|
// var prevPoint = new Point();
|
|
// foreach (var point in pathPoints) {
|
|
// if (first) {
|
|
// prevPoint = point;
|
|
// first = false;
|
|
// continue;
|
|
// }
|
|
// pathEdges.Add(new DebugCurve(2, color, new LineSegment(prevPoint, point)));
|
|
// prevPoint = point;
|
|
// }
|
|
// }
|
|
// return pathEdges;
|
|
// }
|
|
//
|
|
// [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1811:AvoidUncalledPrivateCode")]
|
|
// private IEnumerable<VisibilityEdge> GetAllEdgesTest(VisibilityVertex vertex) {
|
|
// var processedVertices = new Set<VisibilityVertex>();
|
|
// var q = new Queue<VisibilityVertex>();
|
|
// q.Enqueue(vertex);
|
|
//
|
|
// // Target may not be connected to source.
|
|
// bool targetFound = (vertex == this.Target);
|
|
// while (!targetFound) {
|
|
// while (q.Count > 0) {
|
|
// VisibilityVertex v = q.Dequeue();
|
|
// if (processedVertices.Contains(v)) {
|
|
// continue;
|
|
// }
|
|
// targetFound |= (v == this.Target);
|
|
// processedVertices.Insert(v);
|
|
// foreach (var u in VertsToGo(v).Where(u => !processedVertices.Contains(u))) {
|
|
// q.Enqueue(u);
|
|
// }
|
|
// foreach (VisibilityEdge edge in v.OutEdges) {
|
|
// yield return edge;
|
|
// }
|
|
// }
|
|
// q.Enqueue(this.Target);
|
|
// targetFound = true;
|
|
// }
|
|
// }
|
|
//
|
|
// private static IEnumerable<VisibilityVertex> VertsToGo(VisibilityVertex visibilityVertex) {
|
|
// foreach (var edge in visibilityVertex.OutEdges) {
|
|
// yield return edge.Target;
|
|
// }
|
|
// foreach (var edge in visibilityVertex.InEdges) {
|
|
// yield return edge.Source;}
|
|
// }
|
|
//
|
|
// #endif // TEST_MSAGL
|
|
#endregion // TEST_MSAGL
|
|
|
|
private void QueueReversedEntryToNeighborVertexIfNeeded(VertexEntry bestEntry, VertexEntry entryFromNeighbor, double weight) {
|
|
// If we have a lower-cost path from bestEntry to entryFromNeighbor.PreviousVertex than the cost of entryFromNeighbor,
|
|
// or bestEntry has degree 1 (it is a dead-end), enqueue a path in the opposite direction (entryFromNeighbor will probably
|
|
// never be extended from this point).
|
|
int numberOfBends;
|
|
double length;
|
|
var neigVer = entryFromNeighbor.PreviousVertex;
|
|
var dirToNeighbor = GetLengthAndNumberOfBendsToNeighborVertex(bestEntry, neigVer, weight, out numberOfBends, out length);
|
|
if ((CombinedCost(length, numberOfBends) < CombinedCost(entryFromNeighbor.Length, entryFromNeighbor.NumberOfBends))
|
|
|| (bestEntry.Vertex.Degree == 1)) {
|
|
var cost = this.TotalCostFromSourceToVertex(length, numberOfBends) + HeuristicDistanceFromVertexToTarget(neigVer.Point, dirToNeighbor);
|
|
EnqueueEntry(bestEntry, neigVer, length, numberOfBends, cost);
|
|
}
|
|
}
|
|
|
|
private void UpdateEntryToNeighborVertexIfNeeded(VertexEntry bestEntry, VertexEntry neigEntry, double weight) {
|
|
int numberOfBends;
|
|
double length;
|
|
var dirToNeighbor = GetLengthAndNumberOfBendsToNeighborVertex(bestEntry, neigEntry.Vertex, weight, out numberOfBends, out length);
|
|
if (CombinedCost(length, numberOfBends) < CombinedCost(neigEntry.Length, neigEntry.NumberOfBends)) {
|
|
var newCost = this.TotalCostFromSourceToVertex(length, numberOfBends) + HeuristicDistanceFromVertexToTarget(neigEntry.Vertex.Point, dirToNeighbor);
|
|
neigEntry.ResetEntry(bestEntry, length, numberOfBends, newCost);
|
|
queue.DecreasePriority(neigEntry, newCost);
|
|
}
|
|
}
|
|
|
|
private void CreateAndEnqueueEntryToNeighborVertex(VertexEntry bestEntry, VisibilityVertexRectilinear neigVer, double weight) {
|
|
int numberOfBends;
|
|
double length;
|
|
var dirToNeighbor = GetLengthAndNumberOfBendsToNeighborVertex(bestEntry, neigVer, weight, out numberOfBends, out length);
|
|
var cost = this.TotalCostFromSourceToVertex(length, numberOfBends) + HeuristicDistanceFromVertexToTarget(neigVer.Point, dirToNeighbor);
|
|
if (cost < this.upperBoundOnCost) {
|
|
if (neigVer.VertexEntries == null) {
|
|
this.visitedVertices.Add(neigVer);
|
|
}
|
|
EnqueueEntry(bestEntry, neigVer, length, numberOfBends, cost);
|
|
}
|
|
}
|
|
|
|
private void EnqueueEntry(VertexEntry bestEntry, VisibilityVertexRectilinear neigVer, double length, int numberOfBends, double cost) {
|
|
var entry = new VertexEntry(neigVer, bestEntry, length, numberOfBends, cost);
|
|
neigVer.SetVertexEntry(entry);
|
|
this.queue.Enqueue(entry, entry.Cost);
|
|
}
|
|
|
|
private static Directions GetLengthAndNumberOfBendsToNeighborVertex(VertexEntry prevEntry,
|
|
VisibilityVertex vertex, double weight, out int numberOfBends, out double length) {
|
|
length = prevEntry.Length + ManhattanDistance(prevEntry.Vertex.Point, vertex.Point)*weight;
|
|
Directions directionToVertex = CompassVector.PureDirectionFromPointToPoint(prevEntry.Vertex.Point, vertex.Point);
|
|
numberOfBends = prevEntry.NumberOfBends;
|
|
if (prevEntry.Direction != Directions. None && directionToVertex != prevEntry.Direction) {
|
|
numberOfBends++;
|
|
}
|
|
return directionToVertex;
|
|
}
|
|
|
|
internal static double ManhattanDistance(Point a, Point b) {
|
|
return Math.Abs(b.X - a.X) + Math.Abs(b.Y - a.Y);
|
|
}
|
|
|
|
internal VertexEntry GetPathWithCost(VertexEntry[] sourceVertexEntries, VisibilityVertexRectilinear source, double adjustmentToSourceCost,
|
|
VertexEntry[] targetVertexEntries, VisibilityVertexRectilinear target, double adjustmentToTargetCost,
|
|
double priorBestCost) {
|
|
this.upperBoundOnCost = priorBestCost;
|
|
this.sourceCostAdjustment = adjustmentToSourceCost;
|
|
this.targetCostAdjustment = adjustmentToTargetCost;
|
|
|
|
DevTracePrintSourceAndTarget(source, target);
|
|
if (!InitPath(sourceVertexEntries, source, target)) {
|
|
this.DevTraceShowPath(source, null);
|
|
return null;
|
|
}
|
|
#if TEST_MSAGL
|
|
this.DevTraceShowAllPartialPaths(source, queue.Peek());
|
|
#endif // TEST_MSAGL
|
|
|
|
while (queue.Count > 0) {
|
|
this.TestPreDequeue();
|
|
var bestEntry = queue.Dequeue();
|
|
var bestVertex = bestEntry.Vertex;
|
|
if (bestVertex == Target) {
|
|
this.DevTraceShowPath(source, bestEntry);
|
|
if (targetVertexEntries == null) {
|
|
Cleanup();
|
|
return bestEntry;
|
|
}
|
|
|
|
// We'll never get a duplicate entry direction here; we either relaxed the cost via UpdateEntryToNeighborIfNeeded
|
|
// before we dequeued it, or it was closed. So, we simply remove the direction from the valid target entry directions
|
|
// and if we get to none, we're done. We return a null path until the final stage.
|
|
#if SHARPKIT //http://code.google.com/p/sharpkit/issues/detail?id=368 property assignment not working with &= operator
|
|
this.EntryDirectionsToTarget = this.EntryDirectionsToTarget & ~bestEntry.Direction;
|
|
#else
|
|
this.EntryDirectionsToTarget &= ~bestEntry.Direction;
|
|
#endif
|
|
if (this.EntryDirectionsToTarget == Directions. None) {
|
|
this.Target.VertexEntries.CopyTo(targetVertexEntries, 0);
|
|
Cleanup();
|
|
return null;
|
|
}
|
|
this.upperBoundOnCost = Math.Min(this.MultistageAdjustedCostBound(bestEntry.Cost), this.upperBoundOnCost);
|
|
continue;
|
|
}
|
|
|
|
// It's safe to close this after removing it from the queue. Any updateEntryIfNeeded that changes it must come
|
|
// while it is still on the queue; it is removed from the queue only if it has the lowest cost path, and we have
|
|
// no negative path weights, so any other path that might try to extend to it after this cannot have a lower cost.
|
|
bestEntry.IsClosed = true;
|
|
|
|
// PerfNote: Array.ForEach is optimized, but don't use .Where.
|
|
foreach (var bendNeighbor in this.nextNeighbors) {
|
|
bendNeighbor.Clear();
|
|
}
|
|
var preferredBendDir = Right(bestEntry.Direction);
|
|
this.ExtendPathAlongInEdges(bestEntry, bestVertex.InEdges, preferredBendDir);
|
|
this.ExtendPathAlongOutEdges(bestEntry, bestVertex.OutEdges, preferredBendDir);
|
|
foreach (var bendNeighbor in this.nextNeighbors) {
|
|
if (bendNeighbor.Vertex != null) {
|
|
this.ExtendPathToNeighborVertex(bestEntry, bendNeighbor.Vertex, bendNeighbor.Weight);
|
|
}
|
|
}
|
|
this.DevTraceShowAllPartialPaths(source, bestEntry);
|
|
}
|
|
|
|
// Either there is no path to the target, or we have abandoned the path due to exceeding priorBestCost.
|
|
if ((targetVertexEntries != null) && (this.Target.VertexEntries != null)) {
|
|
this.Target.VertexEntries.CopyTo(targetVertexEntries, 0);
|
|
}
|
|
this.DevTraceShowPath(source, null);
|
|
Cleanup();
|
|
return null;
|
|
}
|
|
|
|
private void ExtendPathAlongInEdges(VertexEntry bestEntry, List<VisibilityEdge> edges, Directions preferredBendDir) {
|
|
// Iteration is faster than foreach and much faster than .Where.
|
|
int count = edges.Count;
|
|
for (int ii = 0; ii < count; ++ii) {
|
|
var edge = edges[ii];
|
|
ExtendPathAlongEdge(bestEntry, edge, true, preferredBendDir);
|
|
}
|
|
}
|
|
|
|
private void ExtendPathAlongOutEdges(VertexEntry bestEntry, RbTree<VisibilityEdge> edges, Directions preferredBendDir) {
|
|
// Avoid GetEnumerator overhead.
|
|
var outEdgeNode = edges.IsEmpty() ? null : edges.TreeMinimum();
|
|
for (; outEdgeNode != null; outEdgeNode = edges.Next(outEdgeNode)) {
|
|
ExtendPathAlongEdge(bestEntry, outEdgeNode.Item, false, preferredBendDir);
|
|
}
|
|
}
|
|
|
|
private void ExtendPathAlongEdge(VertexEntry bestEntry, VisibilityEdge edge, bool isInEdges, Directions preferredBendDir) {
|
|
if (!IsPassable(edge)) {
|
|
return;
|
|
}
|
|
|
|
// This is after the initial source vertex so PreviousEntry won't be null.
|
|
var neigVer = (VisibilityVertexRectilinear)(isInEdges ? edge.Source : edge.Target);
|
|
if (neigVer == bestEntry.PreviousVertex) {
|
|
// For multistage paths, the source may be a waypoint outside the graph boundaries that is collinear
|
|
// with both the previous and next points in the path; in that case it may have only one degree.
|
|
// For other cases, we just ignore it and the path will be abandoned.
|
|
if ((bestEntry.Vertex.Degree > 1) || (bestEntry.Vertex != this.Source)) {
|
|
return;
|
|
}
|
|
this.ExtendPathToNeighborVertex(bestEntry, neigVer, edge.Weight);
|
|
return;
|
|
}
|
|
|
|
// Enqueue in reverse order of preference per comments on NextNeighbor class.
|
|
var neigDir = CompassVector.PureDirectionFromPointToPoint(bestEntry.Vertex.Point, neigVer.Point);
|
|
var nextNeighbor = this.nextNeighbors[2];
|
|
if (neigDir != bestEntry.Direction) {
|
|
nextNeighbor = this.nextNeighbors[(neigDir == preferredBendDir) ? 1 : 0];
|
|
}
|
|
Debug.Assert(nextNeighbor.Vertex == null, "bend neighbor already exists");
|
|
nextNeighbor.Set(neigVer, edge.Weight);
|
|
}
|
|
|
|
private void EnqueueInitialVerticesFromSource(double cost) {
|
|
var bestEntry = new VertexEntry(this.Source, null, 0, 0, cost) {
|
|
IsClosed = true
|
|
};
|
|
|
|
// This routine is only called once so don't worry about optimizing foreach.where
|
|
foreach (var edge in this.Source.OutEdges.Where(IsPassable)) {
|
|
this.ExtendPathToNeighborVertex(bestEntry, (VisibilityVertexRectilinear)edge.Target, edge.Weight);
|
|
}
|
|
foreach (var edge in this.Source.InEdges.Where(IsPassable)) {
|
|
this.ExtendPathToNeighborVertex(bestEntry, (VisibilityVertexRectilinear)edge.Source, edge.Weight);
|
|
}
|
|
}
|
|
|
|
private void EnqueueInitialVerticesFromSourceEntries(VertexEntry[] sourceEntries) {
|
|
foreach (var entry in sourceEntries) {
|
|
if (entry != null) {
|
|
this.queue.Enqueue(entry, entry.Cost);
|
|
}
|
|
}
|
|
}
|
|
|
|
private void ExtendPathToNeighborVertex(VertexEntry bestEntry, VisibilityVertexRectilinear neigVer, double weight) {
|
|
var dirToNeighbor = CompassVector.PureDirectionFromPointToPoint(bestEntry.Vertex.Point, neigVer.Point);
|
|
|
|
var neigEntry = (neigVer.VertexEntries != null) ? neigVer.VertexEntries[CompassVector.ToIndex(dirToNeighbor)] : null;
|
|
if (neigEntry == null) {
|
|
if (!this.CreateAndEnqueueReversedEntryToNeighborVertex(bestEntry, neigVer, weight)) {
|
|
this.CreateAndEnqueueEntryToNeighborVertex(bestEntry, neigVer, weight);
|
|
}
|
|
} else if (!neigEntry.IsClosed) {
|
|
this.UpdateEntryToNeighborVertexIfNeeded(bestEntry, neigEntry, weight);
|
|
}
|
|
}
|
|
|
|
private bool CreateAndEnqueueReversedEntryToNeighborVertex(VertexEntry bestEntry, VisibilityVertexRectilinear neigVer, double weight) {
|
|
// VertexEntries is null for the initial source. Otherwise, if there is already a path into bestEntry's vertex
|
|
// from neigVer, we're turning back on the path; therefore we have already enqueued the neighbors of neigVer.
|
|
// However, the path cost includes both path length to the current point and the lookahead; this means that we
|
|
// may now be coming into the neigVer from the opposite side with an equal score to the previous entry, but
|
|
// the new path may be going toward the target while the old one (from neigVer to bestEntry) went away from
|
|
// the target. So, if we score better going in the opposite direction, enqueue bestEntry->neigVer; ignore
|
|
// neigVer->bestEntry as it probably won't be extended again.
|
|
if (bestEntry.Vertex.VertexEntries != null) {
|
|
var dirFromNeighbor = CompassVector.PureDirectionFromPointToPoint(neigVer.Point, bestEntry.Vertex.Point);
|
|
var entryFromNeighbor = bestEntry.Vertex.VertexEntries[CompassVector.ToIndex(dirFromNeighbor)];
|
|
if (entryFromNeighbor != null) {
|
|
Debug.Assert(entryFromNeighbor.PreviousVertex == neigVer, "mismatch in turnback PreviousEntry");
|
|
Debug.Assert(entryFromNeighbor.PreviousEntry.IsClosed, "turnback PreviousEntry should be closed");
|
|
this.QueueReversedEntryToNeighborVertexIfNeeded(bestEntry, entryFromNeighbor, weight);
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
private static bool IsPassable(VisibilityEdge edge) {
|
|
return edge.IsPassable == null || edge.IsPassable();
|
|
}
|
|
|
|
private void Cleanup()
|
|
{
|
|
foreach (var v in this.visitedVertices) {
|
|
v.RemoveVertexEntries();
|
|
}
|
|
this.visitedVertices.Clear();
|
|
this.queue = null;
|
|
this.TestClearIterations();
|
|
}
|
|
|
|
#region DevTrace
|
|
#if DEVTRACE
|
|
private static readonly DevTrace ssstTrace = new DevTrace("Rectilinear_SsstTrace", "Ssst");
|
|
private static readonly DevTrace ssstDisplay = new DevTrace("Rectilinear_SsstDisplay");
|
|
#endif // DEVTRACE
|
|
|
|
[Conditional("DEVTRACE")]
|
|
[System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1822:MarkMembersAsStatic")]
|
|
void DevTraceShowAllPartialPaths(VisibilityVertex source, VertexEntry mostRecentlyExtendedPath) {
|
|
#if DEVTRACE
|
|
if (ssstTrace.IsLevel(3))
|
|
{
|
|
this.TestShowAllPaths(source, mostRecentlyExtendedPath);
|
|
}
|
|
#endif // DEVTRACE
|
|
}
|
|
|
|
[Conditional("DEVTRACE")]
|
|
[System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1822:MarkMembersAsStatic")]
|
|
private void DevTraceShowPath(VisibilityVertex source, VertexEntry lastEntry) {
|
|
#if DEVTRACE
|
|
if (ssstTrace.IsLevel(1)) {
|
|
var pathPoints = RestorePath(lastEntry);
|
|
if (pathPoints != null) {
|
|
this.TestShowPath(source, pathPoints, lastEntry.Cost, lastEntry.Length, lastEntry.NumberOfBends);
|
|
return;
|
|
}
|
|
Console.WriteLine("path abandoned; iters = {0}/{1}", this.currentIterations, totalIterations);
|
|
}
|
|
#endif // DEVTRACE
|
|
}
|
|
|
|
[Conditional("DEVTRACE")]
|
|
[System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1822:MarkMembersAsStatic")]
|
|
private void DevTracePrintSourceAndTarget(VisibilityVertex source, VisibilityVertex target) {
|
|
#if DEVTRACE
|
|
if (ssstTrace.IsLevel(2))
|
|
{
|
|
// Don't use this.Target as it hasn't been set yet.
|
|
Console.WriteLine("find path: {0} -> {1} distance {2} sourceAdjust {3} targetAdjust {4} bendPenalty {5}",
|
|
source.Point, target.Point, ManhattanDistance(source.Point, target.Point),
|
|
this.sourceCostAdjustment, this.targetCostAdjustment, this.BendsImportance);
|
|
}
|
|
#endif // DEVTRACE
|
|
}
|
|
|
|
// #if TEST_MSAGL
|
|
// [Conditional("DEVTRACE")]
|
|
// [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1822:MarkMembersAsStatic")]
|
|
// private static void DevTraceDisplay(IEnumerable<DebugCurve> debugCurves)
|
|
// {
|
|
// #if DEVTRACE
|
|
// if (!ssstDisplay.IsLevel(1))
|
|
// {
|
|
// return;
|
|
// }
|
|
// #endif // DEVTRACE
|
|
// LayoutAlgorithmSettings.ShowDebugCurvesEnumeration(debugCurves);
|
|
// }
|
|
// #endif // TEST_MSAGL
|
|
|
|
// #if TEST_MSAGL
|
|
// private int currentIterations;
|
|
// private static int totalIterations;
|
|
// private UInt64 lastDequeueTimestamp;
|
|
// #endif // TEST_MSAGL
|
|
|
|
[Conditional("TEST_MSAGL")]
|
|
[System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1822:MarkMembersAsStatic")]
|
|
private void TestPreDequeue() {
|
|
// #if TEST_MSAGL
|
|
// ++currentIterations;
|
|
// ++totalIterations;
|
|
// this.lastDequeueTimestamp = this.queue.PeekTimestamp();
|
|
// #endif // TEST_MSAGL
|
|
}
|
|
|
|
[Conditional("TEST_MSAGL")]
|
|
[System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1822:MarkMembersAsStatic")]
|
|
private void TestClearIterations() {
|
|
// #if TEST_MSAGL
|
|
// currentIterations = 0;
|
|
// #endif // TEST_MSAGL
|
|
}
|
|
|
|
#endregion // DevTrace
|
|
}
|
|
}
|