public List <RouteVertex> FindShortestPath(RouteVertex start, RouteVertex end)
{
start.IsEdgeEndpoint = false;
end.IsEdgeEndpoint = false;
foreach (var vertex in this.graph.Vertices)
{
vertex.Reset();
}
start.Distance = 0;
bool reached = false;
while (!reached)
{
RouteVertex minVertex = null;
foreach (var minCandidate in graph.Vertices.Where(v => !v.IsPermanent && v.IsAvailable))
{
if (minVertex == null || minCandidate.Distance < minVertex.Distance)
{
minVertex = minCandidate;
}
}
minVertex.IsPermanent = true;
if (minVertex == end)
{
reached = true;
}
foreach (var edge in minVertex.Neighbors)
{
double newDist = minVertex.Distance + edge.Length + edge.EndVertex.Penalization;
if (newDist < edge.EndVertex.Distance)
{
edge.EndVertex.Distance = newDist;
edge.EndVertex.Predecessor = minVertex;
}
}
}
List <RouteVertex> path = new List <RouteVertex>();
RouteVertex pathVertex = end;
while (pathVertex != start)
{
if (pathVertex == null)
{
break;
}
path.Add(pathVertex);
//pathVertex.Penalization += 16; // penalize path vertices so that next path tend to choose different vertices
pathVertex = pathVertex.Predecessor;
}
path.Add(start);
start.IsEdgeEndpoint = true;
end.IsEdgeEndpoint = true;
path.Reverse();
return(path);
}
public List<RouteVertex> FindShortestPath(RouteVertex start, RouteVertex end)
{
start.IsEdgeEndpoint = false;
end.IsEdgeEndpoint = false;
foreach (var vertex in this.graph.Vertices) {
vertex.Reset();
}
start.Distance = 0;
bool reached = false;
while (!reached)
{
RouteVertex minVertex = null;
foreach (var minCandidate in graph.Vertices.Where(v => !v.IsPermanent && v.IsAvailable)) {
if (minVertex == null || minCandidate.Distance < minVertex.Distance) {
minVertex = minCandidate;
}
}
minVertex.IsPermanent = true;
if (minVertex == end) {
reached = true;
}
foreach (var edge in minVertex.Neighbors) {
double newDist = minVertex.Distance + edge.Length + edge.EndVertex.Penalization;
if (newDist < edge.EndVertex.Distance) {
edge.EndVertex.Distance = newDist;
edge.EndVertex.Predecessor = minVertex;
}
}
}
List<RouteVertex> path = new List<RouteVertex>();
RouteVertex pathVertex = end;
while (pathVertex != start) {
if (pathVertex == null)
break;
path.Add(pathVertex);
//pathVertex.Penalization += 16; // penalize path vertices so that next path tend to choose different vertices
pathVertex = pathVertex.Predecessor;
}
path.Add(start);
start.IsEdgeEndpoint = true;
end.IsEdgeEndpoint = true;
path.Reverse();
return path;
}
void AddEdgeEndpointVertices(IEdge edge, Point2D?edgeStart, Point2D?edgeEnd)
{
if (edgeStart == null || edgeEnd == null)
{
// should not happen
throw new System.Exception("The line between box centers does not intersect the boxes!");
}
var startPoint = new RouteVertex(edgeStart.Value.X, edgeStart.Value.Y);
startPoint.IsEdgeEndpoint = true;
var endPoint = new RouteVertex(edgeEnd.Value.X, edgeEnd.Value.Y);
endPoint.IsEdgeEndpoint = true;
this.vertices.Add(startPoint);
this.vertices.Add(endPoint);
// remember what RouteVertices we created for this user edge
this.setStartVertex(edge, startPoint);
this.setEndVertex(edge, endPoint);
}
void AddEdgeEndpointVertices(IEdge edge, Point2D?edgeStart, Point2D?edgeEnd)
{
if (edgeStart == null || edgeEnd == null)
{
// should not happen
return;
}
var startPoint = new RouteVertex(edgeStart.Value.X, edgeStart.Value.Y);
startPoint.IsEdgeEndpoint = true;
var endPoint = new RouteVertex(edgeEnd.Value.X, edgeEnd.Value.Y);
endPoint.IsEdgeEndpoint = true;
this.vertices.Add(startPoint);
this.vertices.Add(endPoint);
// remember what RouteVertices we created for this user edge
this.setStartVertex(edge, startPoint);
this.setEndVertex(edge, endPoint);
}
public RouteGraphEdge(RouteVertex startVertex, RouteVertex endVertex)
{
this.StartVertex = startVertex;
this.EndVertex = endVertex;
this.Length = GeomUtils.LineLenght(startVertex, endVertex);
}
void setEndVertex(IEdge edge, RouteVertex value)
{
edgeEnds[edge] = value;
}
void setStartVertex(IEdge edge, RouteVertex value)
{
edgeStarts[edge] = value;
}
public RouteGraphEdge(RouteVertex startVertex, RouteVertex endVertex)
{
this.StartVertex = startVertex;
this.EndVertex = endVertex;
this.Length = GeomUtils.LineLenght(startVertex, endVertex);
}
public void AddNeighbor(RouteVertex target)
{
this.Neighbors.Add(new RouteGraphEdge(this, target));
}
void setEndVertex(IEdge edge, RouteVertex value)
{
edgeEnds[edge] = value;
}
void setStartVertex(IEdge edge, RouteVertex value)
{
edgeStarts[edge] = value;
}
void AddEdgeEndpointVertices(IEdge edge, Point2D? edgeStart, Point2D? edgeEnd)
{
if (edgeStart == null || edgeEnd == null) {
// should not happen
return;
}
var startPoint = new RouteVertex(edgeStart.Value.X, edgeStart.Value.Y);
startPoint.IsEdgeEndpoint = true;
var endPoint = new RouteVertex(edgeEnd.Value.X, edgeEnd.Value.Y);
endPoint.IsEdgeEndpoint = true;
this.vertices.Add(startPoint);
this.vertices.Add(endPoint);
// remember what RouteVertices we created for this user edge
this.setStartVertex(edge, startPoint);
this.setEndVertex(edge, endPoint);
}
public void AddNeighbor(RouteVertex target)
{
this.Neighbors.Add(new RouteGraphEdge(this, target));
}
void AddEdgeEndpointVertices(IEdge edge, Point2D? edgeStart, Point2D? edgeEnd)
{
if (edgeStart == null || edgeEnd == null) {
// should not happen
throw new System.Exception("The line between box centers does not intersect the boxes!");
}
var startPoint = new RouteVertex(edgeStart.Value.X, edgeStart.Value.Y);
startPoint.IsEdgeEndpoint = true;
var endPoint = new RouteVertex(edgeEnd.Value.X, edgeEnd.Value.Y);
endPoint.IsEdgeEndpoint = true;
this.vertices.Add(startPoint);
this.vertices.Add(endPoint);
// remember what RouteVertices we created for this user edge
this.setStartVertex(edge, startPoint);
this.setEndVertex(edge, endPoint);
}
