List <SdBoneEdge> RouteOnKnownSourceTargetVertices(Point pathDirection, bool lookingForMonotonePath)
{
LowestCostToTarget = Double.PositiveInfinity;
ClosestTargetVertex = null;
while (Queue.Count > 0)
{
double hu;
SdVertex bestNode = Queue.Dequeue(out hu);
if (hu >= LowestCostToTarget)
{
continue;
}
//update the rest
for (int i = 0; i < bestNode.OutBoneEdges.Count; i++)
{
var outBoneEdge = bestNode.OutBoneEdges[i];
if (outBoneEdge.IsPassable)
{
ProcessOutcomingBoneEdge(bestNode, outBoneEdge, pathDirection, lookingForMonotonePath);
}
}
for (int i = 0; i < bestNode.InBoneEdges.Count; i++)
{
var inBoneEdge = bestNode.InBoneEdges[i];
if (inBoneEdge.IsPassable)
{
ProcessIncomingBoneEdge(bestNode, inBoneEdge, pathDirection, lookingForMonotonePath);
}
}
}
return(GetPathAndUpdateRelatedCosts());
}
void ProcessBoneEdge(SdVertex v, SdVertex queueCandidate, SdBoneEdge boneEdge)
{
double newCost = GetEdgeAdditionalCost(boneEdge, v.Cost);
if (queueCandidate.Cost <= newCost)
{
return;
}
queueCandidate.Cost = newCost;
queueCandidate.PrevEdge = boneEdge;
if (Queue.ContainsElement(queueCandidate))
{
Queue.DecreasePriority(queueCandidate, newCost);
}
else
{
if (queueCandidate.IsTargetOfRouting)
{
double costToTarget = 0;
if (CurrentEdgeGeometry.TargetPort is ClusterBoundaryPort)
{
costToTarget = LengthCoefficient * (queueCandidate.Point - CurrentEdgeGeometry.TargetPort.Location).Length;
}
if (newCost + costToTarget < LowestCostToTarget)
{
LowestCostToTarget = newCost + costToTarget;
ClosestTargetVertex = queueCandidate;
}
return; //do not enqueue the target vertices
}
Enqueue(queueCandidate);
}
}
void SetEdgeGeometryCurve(EdgeGeometry edgeGeometry)
{
Polyline poly = new Polyline();
SdVertex curV = EdgesToRouteSources[edgeGeometry];
poly.AddPoint(curV.Point);
foreach (var edge in EdgesToRoutes[edgeGeometry])
{
if (edge.SourcePoint == curV.Point)
{
poly.AddPoint(edge.TargetPoint);
curV = edge.Target;
}
else
{
poly.AddPoint(edge.SourcePoint);
curV = edge.Source;
}
}
edgeGeometry.Curve = poly;
var clusterSourcePort = edgeGeometry.SourcePort as ClusterBoundaryPort;
if (clusterSourcePort != null)
{
ExtendPolylineStartToClusterBoundary(poly, clusterSourcePort.Curve);
}
var clusterTargetPort = edgeGeometry.TargetPort as ClusterBoundaryPort;
if (clusterTargetPort != null)
{
ExtendPolylineEndToClusterBoundary(poly, clusterTargetPort.Curve);
}
}
void ProcessIncomingBoneEdge(SdVertex v, SdBoneEdge inBoneEdge, Point pathDirection, bool lookingForMonotonePath)
{
Debug.Assert(v == inBoneEdge.Target);
if (lookingForMonotonePath && pathDirection * (inBoneEdge.SourcePoint - inBoneEdge.TargetPoint) < 0)
{
return;
}
ProcessBoneEdge(v, inBoneEdge.Source, inBoneEdge);
}
void ProcessOutcomingBoneEdge(SdVertex v, SdBoneEdge outBoneEdge, Point pathDirection, bool lookingForMonotonePath)
{
Debug.Assert(v == outBoneEdge.Source);
if (lookingForMonotonePath && pathDirection * (outBoneEdge.TargetPoint - outBoneEdge.SourcePoint) < 0)
{
return;
}
ProcessBoneEdge(v, outBoneEdge.Target, outBoneEdge);
}
void CreateRoutingGraph() {
vertexArray = new SdVertex[VisibilityGraph.VertexCount];
int i = 0;
VisibilityVerticesToSdVerts = new Dictionary<VisibilityVertex, SdVertex>();
foreach (var v in VisibilityGraph.Vertices()) {
var sdVert = new SdVertex(v);
vertexArray[i++] = sdVert;
VisibilityVerticesToSdVerts[v] = sdVert;
}
CreateGraphElements();
}
void TryToAssigenTriangleToVertex(CdtTriangle triangle, SdVertex vertex)
{
if (vertex.Triangle != null)
{
return;
}
if (Cdt.PointIsInsideOfTriangle(vertex.Point, triangle))
{
vertex.Triangle = triangle;
}
}
void CreateRoutingGraph()
{
vertexArray = new SdVertex[VisibilityGraph.VertexCount];
int i = 0;
VisibilityVerticesToSdVerts = new Dictionary <VisibilityVertex, SdVertex>();
foreach (var v in VisibilityGraph.Vertices())
{
var sdVert = new SdVertex(v);
vertexArray[i++] = sdVert;
VisibilityVerticesToSdVerts[v] = sdVert;
}
CreateGraphElements();
}
void ProcessOutcomingBoneEdge(SdVertex v, SdBoneEdge outBoneEdge, Point pathDirection, bool lookingForMonotonePath) {
Debug.Assert(v == outBoneEdge.Source);
if (lookingForMonotonePath && pathDirection * (outBoneEdge.TargetPoint - outBoneEdge.SourcePoint) < 0) return;
ProcessBoneEdge(v, outBoneEdge.Target, outBoneEdge);
}
void TryToAssigenTriangleToVertex(CdtTriangle triangle, SdVertex vertex) {
if (vertex.Triangle != null)
return;
if (Cdt.PointIsInsideOfTriangle(vertex.Point, triangle))
vertex.Triangle = triangle;
}
void Enqueue(SdVertex simpleSdVertex) {
Queue.Enqueue(simpleSdVertex, H(simpleSdVertex));
}
double H(SdVertex v) {
return v.Cost + LengthCoefficient * (v.Point - CurrentEdgeGeometry.TargetPort.Location).Length;
}
void ProcessBoneEdge(SdVertex v, SdVertex queueCandidate, SdBoneEdge boneEdge) {
double newCost = GetEdgeAdditionalCost(boneEdge, v.Cost);
if (queueCandidate.Cost <= newCost) return;
queueCandidate.Cost = newCost;
queueCandidate.PrevEdge = boneEdge;
if (Queue.ContainsElement(queueCandidate))
Queue.DecreasePriority(queueCandidate, newCost);
else {
if (queueCandidate.IsTargetOfRouting) {
double costToTarget = 0;
if (CurrentEdgeGeometry.TargetPort is ClusterBoundaryPort)
costToTarget = LengthCoefficient * (queueCandidate.Point - CurrentEdgeGeometry.TargetPort.Location).Length;
if (newCost + costToTarget < LowestCostToTarget) {
LowestCostToTarget = newCost + costToTarget;
ClosestTargetVertex = queueCandidate;
}
return; //do not enqueue the target vertices
}
Enqueue(queueCandidate);
}
}
void ProcessIncomingBoneEdge(SdVertex v, SdBoneEdge inBoneEdge, Point pathDirection, bool lookingForMonotonePath) {
Debug.Assert(v == inBoneEdge.Target);
if (lookingForMonotonePath && pathDirection * (inBoneEdge.SourcePoint - inBoneEdge.TargetPoint) < 0) return;
ProcessBoneEdge(v, inBoneEdge.Source, inBoneEdge);
}
double H(SdVertex v)
{
return(v.Cost + LengthCoefficient * (v.Point - CurrentEdgeGeometry.TargetPort.Location).Length);
}
void Enqueue(SdVertex simpleSdVertex)
{
Queue.Enqueue(simpleSdVertex, H(simpleSdVertex));
}
internal SdBoneEdge(VisibilityEdge visibilityEdge, SdVertex source, SdVertex target) {
VisibilityEdge = visibilityEdge;
Source = source;
Target = target;
}
internal SdBoneEdge(VisibilityEdge visibilityEdge, SdVertex source, SdVertex target)
{
VisibilityEdge = visibilityEdge;
Source = source;
Target = target;
}
List<SdBoneEdge> RouteOnKnownSourceTargetVertices(Point pathDirection, bool lookingForMonotonePath) {
LowestCostToTarget = Double.PositiveInfinity;
ClosestTargetVertex = null;
while (Queue.Count > 0) {
double hu;
SdVertex bestNode = Queue.Dequeue(out hu);
if (hu >= LowestCostToTarget)
continue;
//update the rest
for (int i = 0; i < bestNode.OutBoneEdges.Count; i++) {
var outBoneEdge = bestNode.OutBoneEdges[i];
if (outBoneEdge.IsPassable)
ProcessOutcomingBoneEdge(bestNode, outBoneEdge, pathDirection, lookingForMonotonePath);
}
for (int i = 0; i < bestNode.InBoneEdges.Count; i++) {
var inBoneEdge = bestNode.InBoneEdges[i];
if (inBoneEdge.IsPassable)
ProcessIncomingBoneEdge(bestNode, inBoneEdge, pathDirection, lookingForMonotonePath);
}
}
return GetPathAndUpdateRelatedCosts();
}