/// <summary>
/// Does dykstra calculation(s) with several options.
/// </summary>
/// <param name="graph"></param>
/// <param name="interpreter"></param>
/// <param name="vehicle"></param>
/// <param name="sourceList"></param>
/// <param name="targetList"></param>
/// <param name="weight"></param>
/// <param name="stopAtFirst"></param>
/// <param name="returnAtWeight"></param>
/// <param name="forward"></param>
/// <returns></returns>
private PathSegment <long>[] DoCalculation(IBasicRouterDataSource <LiveEdge> graph, IRoutingInterpreter interpreter, Vehicle vehicle,
PathSegmentVisitList sourceList, PathSegmentVisitList[] targetList, double weight,
bool stopAtFirst, bool returnAtWeight, bool forward)
{
// make copies of the target and source visitlist.
PathSegmentVisitList source = sourceList.Clone() as PathSegmentVisitList;
PathSegmentVisitList[] targets = new PathSegmentVisitList[targetList.Length];
for (int targetIdx = 0; targetIdx < targetList.Length; targetIdx++)
{
targets[targetIdx] = targetList[targetIdx].Clone() as PathSegmentVisitList;
}
// initialize the result data structures.
var segmentsAtWeight = new List <PathSegment <long> >();
var segmentsToTarget = new PathSegment <long> [targets.Length]; // the resulting target segments.
long foundTargets = 0;
// intialize dykstra data structures.
IPriorityQueue <PathSegment <long> > heap = new BinairyHeap <PathSegment <long> >();
var chosenVertices = new HashSet <long>();
var labels = new Dictionary <long, IList <RoutingLabel> >();
foreach (long vertex in source.GetVertices())
{
labels[vertex] = new List <RoutingLabel>();
PathSegment <long> path = source.GetPathTo(vertex);
heap.Push(path, (float)path.Weight);
}
// set the from node as the current node and put it in the correct data structures.
// intialize the source's neighbours.
PathSegment <long> current = heap.Pop();
while (current != null &&
chosenVertices.Contains(current.VertexId))
{ // keep dequeuing.
current = heap.Pop();
}
// test each target for the source.
// test each source for any of the targets.
var pathsFromSource = new Dictionary <long, PathSegment <long> >();
foreach (long sourceVertex in source.GetVertices())
{ // get the path to the vertex.
PathSegment <long> sourcePath = source.GetPathTo(sourceVertex); // get the source path.
sourcePath = sourcePath.From;
while (sourcePath != null)
{ // add the path to the paths from source.
pathsFromSource[sourcePath.VertexId] = sourcePath;
sourcePath = sourcePath.From;
}
}
// loop over all targets
for (int idx = 0; idx < targets.Length; idx++)
{ // check for each target if there are paths to the source.
foreach (long targetVertex in targets[idx].GetVertices())
{
PathSegment <long> targetPath = targets[idx].GetPathTo(targetVertex); // get the target path.
targetPath = targetPath.From;
while (targetPath != null)
{ // add the path to the paths from source.
PathSegment <long> pathFromSource;
if (pathsFromSource.TryGetValue(targetPath.VertexId, out pathFromSource))
{ // a path is found.
// get the existing path if any.
PathSegment <long> existing = segmentsToTarget[idx];
if (existing == null)
{ // a path did not exist yet!
segmentsToTarget[idx] = targetPath.Reverse().ConcatenateAfter(pathFromSource);
foundTargets++;
}
else if (existing.Weight > targetPath.Weight + pathFromSource.Weight)
{ // a new path is found with a lower weight.
segmentsToTarget[idx] = targetPath.Reverse().ConcatenateAfter(pathFromSource);
}
}
targetPath = targetPath.From;
}
}
}
if (foundTargets == targets.Length && targets.Length > 0)
{ // routing is finished!
return(segmentsToTarget.ToArray());
}
if (stopAtFirst)
{ // only one entry is needed.
if (foundTargets > 0)
{ // targets found, return the shortest!
PathSegment <long> shortest = null;
foreach (PathSegment <long> foundTarget in segmentsToTarget)
{
if (shortest == null)
{
shortest = foundTarget;
}
else if (foundTarget != null &&
shortest.Weight > foundTarget.Weight)
{
shortest = foundTarget;
}
}
segmentsToTarget = new PathSegment <long> [1];
segmentsToTarget[0] = shortest;
return(segmentsToTarget);
}
else
{ // not targets found yet!
segmentsToTarget = new PathSegment <long> [1];
}
}
// test for identical start/end point.
for (int idx = 0; idx < targets.Length; idx++)
{
PathSegmentVisitList target = targets[idx];
if (returnAtWeight)
{ // add all the reached vertices larger than weight to the results.
if (current.Weight > weight)
{
PathSegment <long> toPath = target.GetPathTo(current.VertexId);
toPath.Reverse();
toPath = toPath.ConcatenateAfter(current);
segmentsAtWeight.Add(toPath);
}
}
else if (target.Contains(current.VertexId))
{ // the current is a target!
PathSegment <long> toPath = target.GetPathTo(current.VertexId);
toPath = toPath.Reverse();
toPath = toPath.ConcatenateAfter(current);
if (stopAtFirst)
{ // stop at the first occurance.
segmentsToTarget[0] = toPath;
return(segmentsToTarget);
}
else
{ // normal one-to-many; add to the result.
// check if routing is finished.
if (segmentsToTarget[idx] == null)
{ // make sure only the first route is set.
foundTargets++;
segmentsToTarget[idx] = toPath;
if (foundTargets == targets.Length)
{ // routing is finished!
return(segmentsToTarget.ToArray());
}
}
else if (segmentsToTarget[idx].Weight > toPath.Weight)
{ // check if the second, third or later is shorter.
segmentsToTarget[idx] = toPath;
}
}
}
}
// start OsmSharp.Routing.
KeyValuePair <uint, LiveEdge>[] arcs = graph.GetArcs(
Convert.ToUInt32(current.VertexId));
chosenVertices.Add(current.VertexId);
// loop until target is found and the route is the shortest!
while (true)
{
// get the current labels list (if needed).
IList <RoutingLabel> currentLabels = null;
if (interpreter.Constraints != null)
{ // there are constraints, get the labels.
currentLabels = labels[current.VertexId];
labels.Remove(current.VertexId);
}
float latitude, longitude;
graph.GetVertex(Convert.ToUInt32(current.VertexId), out latitude, out longitude);
var currentCoordinates = new GeoCoordinate(latitude, longitude);
// update the visited nodes.
foreach (KeyValuePair <uint, LiveEdge> neighbour in arcs)
{
// check the tags against the interpreter.
TagsCollection tags = graph.TagsIndex.Get(neighbour.Value.Tags);
if (vehicle.CanTraverse(tags))
{ // it's ok; the edge can be traversed by the given vehicle.
bool?oneWay = vehicle.IsOneWay(tags);
bool canBeTraversedOneWay = (!oneWay.HasValue || oneWay.Value == neighbour.Value.Forward);
if ((current.From == null ||
interpreter.CanBeTraversed(current.From.VertexId, current.VertexId, neighbour.Key)) && // test for turning restrictions.
canBeTraversedOneWay &&
!chosenVertices.Contains(neighbour.Key))
{ // the neigbour is forward and is not settled yet!
// check the labels (if needed).
bool constraintsOk = true;
if (interpreter.Constraints != null)
{ // check if the label is ok.
RoutingLabel neighbourLabel = interpreter.Constraints.GetLabelFor(
graph.TagsIndex.Get(neighbour.Value.Tags));
// only test labels if there is a change.
if (currentLabels.Count == 0 || !neighbourLabel.Equals(currentLabels[currentLabels.Count - 1]))
{ // labels are different, test them!
constraintsOk = interpreter.Constraints.ForwardSequenceAllowed(currentLabels,
neighbourLabel);
if (constraintsOk)
{ // update the labels.
var neighbourLabels = new List <RoutingLabel>(currentLabels);
neighbourLabels.Add(neighbourLabel);
labels[neighbour.Key] = neighbourLabels;
}
}
else
{ // set the same label(s).
labels[neighbour.Key] = currentLabels;
}
}
if (constraintsOk)
{ // all constraints are validated or there are none.
graph.GetVertex(Convert.ToUInt32(neighbour.Key), out latitude, out longitude);
var neighbourCoordinates = new GeoCoordinate(latitude, longitude);
// calculate the weight.
double weightToNeighbour = vehicle.Weight(tags, currentCoordinates, neighbourCoordinates);
// calculate neighbours weight.
double totalWeight = current.Weight + weightToNeighbour;
// update the visit list;
var neighbourRoute = new PathSegment <long>(neighbour.Key, totalWeight, current);
heap.Push(neighbourRoute, (float)neighbourRoute.Weight);
}
}
}
}
// while the visit list is not empty.
current = null;
if (heap.Count > 0)
{
// choose the next vertex.
current = heap.Pop();
while (current != null &&
chosenVertices.Contains(current.VertexId))
{ // keep dequeuing.
current = heap.Pop();
}
if (current != null)
{
chosenVertices.Add(current.VertexId);
}
}
while (current != null && current.Weight > weight)
{
if (returnAtWeight)
{ // add all the reached vertices larger than weight to the results.
segmentsAtWeight.Add(current);
}
// choose the next vertex.
current = heap.Pop();
while (current != null &&
chosenVertices.Contains(current.VertexId))
{ // keep dequeuing.
current = heap.Pop();
}
}
if (current == null)
{ // route is not found, there are no vertices left
// or the search whent outside of the max bounds.
break;
}
// check target.
for (int idx = 0; idx < targets.Length; idx++)
{
PathSegmentVisitList target = targets[idx];
if (target.Contains(current.VertexId))
{ // the current is a target!
PathSegment <long> toPath = target.GetPathTo(current.VertexId);
toPath = toPath.Reverse();
toPath = toPath.ConcatenateAfter(current);
if (stopAtFirst)
{ // stop at the first occurance.
segmentsToTarget[0] = toPath;
return(segmentsToTarget);
}
else
{ // normal one-to-many; add to the result.
// check if routing is finished.
if (segmentsToTarget[idx] == null)
{ // make sure only the first route is set.
segmentsToTarget[idx] = toPath;
}
else if (segmentsToTarget[idx].Weight > toPath.Weight)
{ // check if the second, third or later is shorter.
segmentsToTarget[idx] = toPath;
}
// remove this vertex from this target's paths.
target.Remove(current.VertexId);
// if this target is empty it's optimal route has been found.
if (target.Count == 0)
{ // now the shortest route has been found for sure!
foundTargets++;
if (foundTargets == targets.Length)
{ // routing is finished!
return(segmentsToTarget.ToArray());
}
}
}
}
}
// get the neigbours of the current node.
arcs = graph.GetArcs(Convert.ToUInt32(current.VertexId));
}
// return the result.
if (!returnAtWeight)
{
return(segmentsToTarget.ToArray());
}
return(segmentsAtWeight.ToArray());
}
/// <summary>
/// Implements a very simple dykstra version.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="via"></param>
/// <param name="max_weight"></param>
/// <param name="max_settles"></param>
/// <returns></returns>
private float CalculateWeight(uint from, uint to, uint via, float max_weight, int max_settles)
{
int max_hops = 5;
float weight = float.MaxValue;
// creates the settled list.
HashSet<uint> settled = new HashSet<uint>();
settled.Add(via);
// creates the priorty queue.
BinairyHeap<SettledVertex> heap = new BinairyHeap<SettledVertex>();
heap.Push(new SettledVertex(from, 0, 0), 0);
// keep looping until the queue is empty or the target is found!
while (heap.Count > 0)
{
// pop the first customer.
SettledVertex current = heap.Pop();
if (!settled.Contains(current.VertexId))
{ // the current vertex has net been settled.
settled.Add(current.VertexId); // settled the vertex.
// test stop conditions.
if (current.VertexId == to)
{ // target is found!
return current.Weight;
}
// test the hop count.
if (current.Hops < max_hops)
{ // the neighbours will only increase hops!
if (settled.Count >= max_settles)
{ // do not continue searching.
return float.MaxValue;
}
// get the neighbours.
KeyValuePair<uint, CHEdgeData>[] neighbours = _data.GetArcs(current.VertexId);
for (int idx = 0; idx < neighbours.Length; idx++)
{
if (neighbours[idx].Value.Forward && (neighbours[idx].Key == to || !settled.Contains(neighbours[idx].Key)))
{
SettledVertex neighbour = new SettledVertex(neighbours[idx].Key,
neighbours[idx].Value.Weight + current.Weight, current.Hops + 1);
if (neighbour.Weight < max_weight)
{
if (neighbours[idx].Key == to)
{
return neighbour.Weight;
}
heap.Push(neighbour, neighbour.Weight);
}
}
}
}
}
}
return weight;
}
public void TestBinairyHeapQueueDeQueueRandom()
{
// the elements.
List <KeyValuePair <string, float> > elements =
new List <KeyValuePair <string, float> >();
elements.Add(new KeyValuePair <string, float>("one", 1));
elements.Add(new KeyValuePair <string, float>("two", 2));
elements.Add(new KeyValuePair <string, float>("three", 3));
elements.Add(new KeyValuePair <string, float>("four", 4));
elements.Add(new KeyValuePair <string, float>("five", 5));
elements.Add(new KeyValuePair <string, float>("six", 6));
elements.Add(new KeyValuePair <string, float>("seven", 7));
elements.Add(new KeyValuePair <string, float>("eight", 8));
elements.Add(new KeyValuePair <string, float>("nine", 9));
elements.Add(new KeyValuePair <string, float>("ten", 10));
// creates a new binairy heap.
BinairyHeap <string> heap = new BinairyHeap <string>();
// enqueue one item.
while (elements.Count > 0)
{ // keep selecting existing elements.
int selected = OsmSharp.Math.Random.StaticRandomGenerator.Get().Generate(elements.Count);
KeyValuePair <string, float> selected_pair =
elements[selected];
elements.RemoveAt(selected);
// add to the heap.
heap.Push(selected_pair.Key, selected_pair.Value);
}
// test the result.
Assert.AreEqual(10, heap.Count);
Assert.AreEqual(1, heap.PeekWeight());
Assert.AreEqual("one", heap.Peek());
// remove the items one by one and test the results again.
while (heap.Count > 0)
{ // keep removing.
Assert.AreEqual(10 - elements.Count, heap.Count);
Assert.AreEqual(elements.Count + 1, heap.PeekWeight());
// dequeue.
elements.Add(new KeyValuePair <string, float>(heap.Pop(), elements.Count + 1));
}
// try to dequeue again.
Assert.AreEqual(null, heap.Pop());
// clear the elements list and try again!
elements.Clear();
elements.Add(new KeyValuePair <string, float>("one", 1));
elements.Add(new KeyValuePair <string, float>("two", 2));
elements.Add(new KeyValuePair <string, float>("three", 3));
elements.Add(new KeyValuePair <string, float>("four", 4));
elements.Add(new KeyValuePair <string, float>("five", 5));
elements.Add(new KeyValuePair <string, float>("six", 6));
elements.Add(new KeyValuePair <string, float>("seven", 7));
elements.Add(new KeyValuePair <string, float>("eight", 8));
elements.Add(new KeyValuePair <string, float>("nine", 9));
elements.Add(new KeyValuePair <string, float>("ten", 10));
// enqueue one item.
while (elements.Count > 0)
{ // keep selecting existing elements.
int selected = OsmSharp.Math.Random.StaticRandomGenerator.Get().Generate(elements.Count);
KeyValuePair <string, float> selected_pair =
elements[selected];
elements.RemoveAt(selected);
// add to the heap.
heap.Push(selected_pair.Key, selected_pair.Value);
}
// test the result.
Assert.AreEqual(10, heap.Count);
Assert.AreEqual(1, heap.PeekWeight());
Assert.AreEqual("one", heap.Peek());
// remove the items one by one and test the results again.
while (heap.Count > 0)
{ // keep removing.
Assert.AreEqual(10 - elements.Count, heap.Count);
Assert.AreEqual(elements.Count + 1, heap.PeekWeight());
// dequeue.
elements.Add(new KeyValuePair <string, float>(heap.Pop(), elements.Count + 1));
}
// try to dequeue again.
Assert.AreEqual(null, heap.Pop());
}
/// <summary>
/// Implements a very simple dykstra version.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="via"></param>
/// <param name="max_weight"></param>
/// <param name="max_settles"></param>
/// <returns></returns>
private float CalculateWeight(uint from, uint to, uint via, float max_weight, int max_settles)
{
int max_hops = 5;
float weight = float.MaxValue;
// creates the settled list.
HashSet <uint> settled = new HashSet <uint>();
settled.Add(via);
// creates the priorty queue.
BinairyHeap <SettledVertex> heap = new BinairyHeap <SettledVertex>();
heap.Push(new SettledVertex(from, 0, 0), 0);
// keep looping until the queue is empty or the target is found!
while (heap.Count > 0)
{
// pop the first customer.
SettledVertex current = heap.Pop();
if (!settled.Contains(current.VertexId))
{ // the current vertex has net been settled.
settled.Add(current.VertexId); // settled the vertex.
// test stop conditions.
if (current.VertexId == to)
{ // target is found!
return(current.Weight);
}
// test the hop count.
if (current.Hops < max_hops)
{ // the neighbours will only increase hops!
if (settled.Count >= max_settles)
{ // do not continue searching.
return(float.MaxValue);
}
// get the neighbours.
KeyValuePair <uint, CHEdgeData>[] neighbours = _data.GetArcs(current.VertexId);
for (int idx = 0; idx < neighbours.Length; idx++)
{
if (neighbours[idx].Value.Forward && (neighbours[idx].Key == to || !settled.Contains(neighbours[idx].Key)))
{
SettledVertex neighbour = new SettledVertex(neighbours[idx].Key,
neighbours[idx].Value.Weight + current.Weight, current.Hops + 1);
if (neighbour.Weight < max_weight)
{
if (neighbours[idx].Key == to)
{
return(neighbour.Weight);
}
heap.Push(neighbour, neighbour.Weight);
}
}
}
}
}
}
return(weight);
}
public void TestBinairyHeapQueueDeQueueRandom()
{
// the elements.
List<KeyValuePair<string, float>> elements =
new List<KeyValuePair<string, float>>();
elements.Add(new KeyValuePair<string, float>("one", 1));
elements.Add(new KeyValuePair<string, float>("two", 2));
elements.Add(new KeyValuePair<string, float>("three", 3));
elements.Add(new KeyValuePair<string, float>("four", 4));
elements.Add(new KeyValuePair<string, float>("five", 5));
elements.Add(new KeyValuePair<string, float>("six", 6));
elements.Add(new KeyValuePair<string, float>("seven", 7));
elements.Add(new KeyValuePair<string, float>("eight", 8));
elements.Add(new KeyValuePair<string, float>("nine", 9));
elements.Add(new KeyValuePair<string, float>("ten", 10));
// creates a new binairy heap.
BinairyHeap<string> heap = new BinairyHeap<string>();
// enqueue one item.
while (elements.Count > 0)
{ // keep selecting existing elements.
int selected = OsmSharp.Tools.Math.Random.StaticRandomGenerator.Get().Generate(elements.Count);
KeyValuePair<string, float> selected_pair =
elements[selected];
elements.RemoveAt(selected);
// add to the heap.
heap.Push(selected_pair.Key, selected_pair.Value);
}
// test the result.
Assert.AreEqual(10, heap.Count);
Assert.AreEqual(1, heap.PeekWeight());
Assert.AreEqual("one", heap.Peek());
// remove the items one by one and test the results again.
while (heap.Count > 0)
{ // keep removing.
Assert.AreEqual(10 - elements.Count, heap.Count);
Assert.AreEqual(elements.Count + 1, heap.PeekWeight());
// dequeue.
elements.Add(new KeyValuePair<string, float>(heap.Pop(), elements.Count + 1));
}
// try to dequeue again.
Assert.AreEqual(null, heap.Pop());
// clear the elements list and try again!
elements.Clear();
elements.Add(new KeyValuePair<string, float>("one", 1));
elements.Add(new KeyValuePair<string, float>("two", 2));
elements.Add(new KeyValuePair<string, float>("three", 3));
elements.Add(new KeyValuePair<string, float>("four", 4));
elements.Add(new KeyValuePair<string, float>("five", 5));
elements.Add(new KeyValuePair<string, float>("six", 6));
elements.Add(new KeyValuePair<string, float>("seven", 7));
elements.Add(new KeyValuePair<string, float>("eight", 8));
elements.Add(new KeyValuePair<string, float>("nine", 9));
elements.Add(new KeyValuePair<string, float>("ten", 10));
// enqueue one item.
while (elements.Count > 0)
{ // keep selecting existing elements.
int selected = OsmSharp.Tools.Math.Random.StaticRandomGenerator.Get().Generate(elements.Count);
KeyValuePair<string, float> selected_pair =
elements[selected];
elements.RemoveAt(selected);
// add to the heap.
heap.Push(selected_pair.Key, selected_pair.Value);
}
// test the result.
Assert.AreEqual(10, heap.Count);
Assert.AreEqual(1, heap.PeekWeight());
Assert.AreEqual("one", heap.Peek());
// remove the items one by one and test the results again.
while (heap.Count > 0)
{ // keep removing.
Assert.AreEqual(10 - elements.Count, heap.Count);
Assert.AreEqual(elements.Count + 1, heap.PeekWeight());
// dequeue.
elements.Add(new KeyValuePair<string, float>(heap.Pop(), elements.Count + 1));
}
// try to dequeue again.
Assert.AreEqual(null, heap.Pop());
}