/// <summary>
/// Called when a forward vertex was found.
/// </summary>
/// <returns></returns>
private bool ForwardVertexFound(Dykstra <T> dykstra, int i, uint pointer, uint vertex, T weight)
{
Dictionary <int, EdgePath <T> > bucket;
if (!_buckets.TryGetValue(vertex, out bucket))
{
bucket = new Dictionary <int, EdgePath <T> >();
_buckets.Add(vertex, bucket);
bucket[i] = dykstra.GetPath(pointer);
}
else
{
EdgePath <T> existing;
if (bucket.TryGetValue(i, out existing))
{
if (_weightHandler.IsSmallerThan(weight, existing.Weight))
{
bucket[i] = dykstra.GetPath(pointer);
}
}
else
{
bucket[i] = dykstra.GetPath(pointer);
}
}
return(false);
}
/// <summary>
/// Called when a forward vertex was found.
/// </summary>
/// <returns></returns>
private bool ForwardVertexFound(int i, uint vertex, T weight)
{
Dictionary <int, T> bucket;
if (!_buckets.TryGetValue(vertex, out bucket))
{
bucket = new Dictionary <int, T>();
_buckets.Add(vertex, bucket);
bucket[i] = weight;
}
else
{
T existing;
if (bucket.TryGetValue(i, out existing))
{
if (_weightHandler.IsSmallerThan(weight, existing))
{
bucket[i] = weight;
}
}
else
{
bucket[i] = weight;
}
}
return(false);
}
/// <summary>
/// Called when a forward vertex was found.
/// </summary>
/// <returns></returns>
private bool ForwardVertexFound(int i, EdgePath <T> path)
{
Dictionary <int, EdgePath <T> > bucket;
if (!_buckets.TryGetValue(path.Vertex, out bucket))
{
bucket = new Dictionary <int, EdgePath <T> >();
_buckets.Add(path.Vertex, bucket);
bucket[i] = path;
}
else
{
EdgePath <T> existing;
if (bucket.TryGetValue(i, out existing))
{
if (_weightHandler.IsSmallerThan(path.Weight, existing.Weight))
{
bucket[i] = path;
}
}
else
{
bucket[i] = path;
}
}
return(false);
}
/// <summary>
/// Removes witnessed shortcuts.
/// </summary>
/// <param name="shortcuts"></param>
/// <param name="witnesses"></param>
/// <returns></returns>
public static bool RemoveWitnessed <T>(this Shortcuts <T> shortcuts, WeightHandler <T> weightHandler, Dictionary <OriginalEdge, T> witnesses)
where T : struct
{
bool witnessed = false;
foreach (var witness in witnesses)
{
var edge = witness.Key;
if (shortcuts.TryGetValue(edge, out Shortcut <T> shortcut))
{
if (weightHandler.IsSmallerThan(witness.Value, shortcut.Forward))
{
shortcut.Forward = witness.Value;
shortcuts.AddOrUpdate(edge, shortcut, weightHandler);
witnessed = true;
}
}
// TODO: this should be 'else', in theory this extra check isn't needed.
edge = edge.Reverse();
if (shortcuts.TryGetValue(edge, out shortcut))
{
if (weightHandler.IsSmallerThan(witness.Value, shortcut.Backward))
{
shortcut.Backward = witness.Value;
shortcuts.AddOrUpdate(edge, shortcut, weightHandler);
witnessed = true;
}
}
}
return(witnessed);
}
/// <summary>
/// Finds the best edge between the two given vertices.
/// </summary>
/// <typeparam name="T"></typeparam>
/// <param name="graph"></param>
/// <param name="weightHandler"></param>
/// <param name="vertex1"></param>
/// <param name="vertex2"></param>
/// <returns></returns>
public static long FindBestEdge <T>(this Graph.EdgeEnumerator edgeEnumerator, WeightHandler <T> weightHandler, uint vertex1, uint vertex2, out T bestWeight)
where T : struct
{
edgeEnumerator.MoveTo(vertex1);
bestWeight = weightHandler.Infinite;
long bestEdge = Constants.NO_EDGE;
Factor factor;
while (edgeEnumerator.MoveNext())
{
if (edgeEnumerator.To == vertex2)
{
float distance;
ushort edgeProfile;
EdgeDataSerializer.Deserialize(edgeEnumerator.Data0, out distance, out edgeProfile);
var weight = weightHandler.Calculate(edgeProfile, distance, out factor);
if (factor.Value > 0 && (factor.Direction == 0 ||
((factor.Direction == 1) && !edgeEnumerator.DataInverted) ||
((factor.Direction == 2) && edgeEnumerator.DataInverted)))
{ // it's ok; the edge can be traversed by the given vehicle.
if (weightHandler.IsSmallerThan(weight, bestWeight))
{
bestWeight = weight;
bestEdge = edgeEnumerator.IdDirected();
}
}
}
}
if (bestEdge == Constants.NO_EDGE)
{
edgeEnumerator.Reset();
while (edgeEnumerator.MoveNext())
{
if (edgeEnumerator.To == vertex2)
{
float distance;
ushort edgeProfile;
EdgeDataSerializer.Deserialize(edgeEnumerator.Data0, out distance, out edgeProfile);
var weight = weightHandler.Calculate(edgeProfile, distance, out factor);
//if (factor.Value > 0 && (factor.Direction == 0 ||
// ((factor.Direction == 1) && !edgeEnumerator.DataInverted) ||
// ((factor.Direction == 2) && edgeEnumerator.DataInverted)))
//{ // it's ok; the edge can be traversed by the given vehicle.
if (weightHandler.IsSmallerThan(weight, bestWeight))
{
bestWeight = weight;
bestEdge = edgeEnumerator.IdDirected();
}
//}
}
}
}
return(bestEdge);
}
/// <summary>
/// Called when a backward vertex was found.
/// </summary>
/// <returns></returns>
protected virtual bool BackwardVertexFound(int i, uint vertex, LinkedEdgePath <T> backwardVisit)
{
Dictionary <int, LinkedEdgePath <T> > bucket;
if (_buckets.TryGetValue(vertex, out bucket))
{
var edgeEnumerator = _graph.GetEdgeEnumerator();
var originalBackwardVisit = backwardVisit;
foreach (var pair in bucket)
{
var best = _weights[pair.Key][i];
var forwardVisit = pair.Value;
while (forwardVisit != null)
{
var forwardCurrent = forwardVisit.Path;
if (_weightHandler.IsLargerThan(forwardCurrent.Weight, best))
{
forwardVisit = forwardVisit.Next;
continue;
}
backwardVisit = originalBackwardVisit;
while (backwardVisit != null)
{
var backwardCurrent = backwardVisit.Path;
var totalCurrentWeight = _weightHandler.Add(forwardCurrent.Weight, backwardCurrent.Weight);
if (_weightHandler.IsSmallerThan(totalCurrentWeight, best))
{ // potentially a weight improvement.
var allowed = true;
// check u-turn.
var sequence2Forward = backwardCurrent.GetSequence2(edgeEnumerator);
var sequence2Current = forwardCurrent.GetSequence2(edgeEnumerator);
if (sequence2Current != null && sequence2Current.Length > 0 &&
sequence2Forward != null && sequence2Forward.Length > 0)
{
if (sequence2Current[sequence2Current.Length - 1] ==
sequence2Forward[sequence2Forward.Length - 1])
{
allowed = false;
}
}
if (allowed)
{
best = totalCurrentWeight;
}
}
backwardVisit = backwardVisit.Next;
}
forwardVisit = forwardVisit.Next;
}
_weights[pair.Key][i] = best;
}
}
return(false);
}
/// <summary>
/// Executes the algorithm.
/// </summary>
protected override void DoRun()
{
_bestVertex = uint.MaxValue;
_bestWeight = _weightHandler.Infinite;
_maxForward = _weightHandler.Zero;
_maxBackward = _weightHandler.Zero;
_sourceSearch.WasFound = (vertex, weight) =>
{
_maxForward = weight;
return(this.ReachedVertexForward(vertex, weight));
};
_targetSearch.WasFound = (vertex, weight) =>
{
_maxBackward = weight;
return(this.ReachedVertexBackward(vertex, weight));
};
_sourceSearch.Initialize();
_targetSearch.Initialize();
var source = true;
var target = true;
while (source || target)
{
source = false;
if (_weightHandler.IsSmallerThan(_maxForward, _bestWeight))
{ // still a need to search, not best found or max < best.
source = _sourceSearch.Step();
}
target = false;
if (_weightHandler.IsSmallerThan(_maxBackward, _bestWeight))
{ // still a need to search, not best found or max < best.
target = _targetSearch.Step();
}
if (!source && !target)
{ // both source and target search failed or useless.
break;
}
}
}
/// <summary>
/// Executes the algorithm.
/// </summary>
protected override void DoRun(CancellationToken cancellationToken)
{
_best = new Tuple <EdgePath <T>, EdgePath <T>, T>(null, null, _weightHandler.Infinite);
_maxForward = _weightHandler.Zero;
_maxBackward = _weightHandler.Zero;
_sourceSearch.Visit = (path) =>
{
_maxForward = path.Weight;
return(this.ReachedForward(path));
};
_targetSearch.Visit = (path) =>
{
_maxBackward = path.Weight;
return(this.ReachedBackward(path));
};
_sourceSearch.Initialize();
_targetSearch.Initialize();
var source = true;
var target = true;
while (source || target)
{
source = false;
if (_weightHandler.IsSmallerThan(_maxForward, _best.Item3))
{ // still a need to search, not best found or max < best.
source = _sourceSearch.Step();
}
target = false;
if (_weightHandler.IsSmallerThan(_maxBackward, _best.Item3))
{ // still a need to search, not best found or max < best.
target = _targetSearch.Step();
}
if (!source && !target)
{ // both source and target search failed or useless.
break;
}
}
}
/// <summary>
/// Gets the best path in this linked list.
/// </summary>
public EdgePath <T> Best(WeightHandler <T> weightHandler)
{
var best = this.Path;
var current = this.Next;
while (current != null)
{
if (weightHandler.IsSmallerThan(current.Path.Weight, best.Weight))
{
best = current.Path;
}
current = current.Next;
}
return(best);
}
/// <summary>
/// Executes one step in the search.
/// </summary>
public bool Step()
{
if (_pointerHeap.Count == 0)
{
return(false);
}
var cPointer = _pointerHeap.Pop();
uint cVertex, cPrevious;
T cWeight;
_weightHandler.GetPathTree(_pathTree, cPointer, out cVertex, out cWeight, out cPrevious);
if (_visited.Contains(cVertex))
{
return(true);
}
_visited.Add(cVertex);
if (this.WasFound != null)
{
if (this.WasFound(cPointer, cVertex, cWeight))
{ // when true is returned, the listener signals it knows what it wants to know.
return(false);
}
}
_edgeEnumerator.MoveTo(cVertex);
while (_edgeEnumerator.MoveNext())
{
var nWeight = _weightHandler.GetEdgeWeight(_edgeEnumerator);
if ((!_backward && nWeight.Direction.F) ||
(_backward && nWeight.Direction.B))
{ // the edge is forward, and is to higher or was not contracted at all.
var nVertex = _edgeEnumerator.Neighbour;
var totalWeight = _weightHandler.Add(nWeight.Weight, cWeight);
if (!_weightHandler.IsSmallerThan(totalWeight, _max))
{
continue;
}
var nPointer = _weightHandler.AddPathTree(_pathTree, nVertex, totalWeight, cPointer);
_pointerHeap.Push(nPointer, _weightHandler.GetMetric(totalWeight));
}
}
return(true);
}
/// <summary>
/// Executes one step in the search.
/// </summary>
public bool Step()
{
// while the visit list is not empty.
_current = null;
if (_heap.Count > 0)
{ // choose the next vertex.
_current = _heap.Pop();
while (_current != null && _visits.ContainsKey(_current.Vertex))
{ // keep dequeuing.
if (_heap.Count == 0)
{ // nothing more to pop.
break;
}
if (this.Visit != null &&
this.Visit(_current))
{ // edge was found and true was returned, this search should stop.
return(false);
}
_current = _heap.Pop();
}
}
if (_current != null &&
!_visits.ContainsKey(_current.Vertex))
{ // we visit this one, set visit.
_visits[_current.Vertex] = _current;
}
else
{ // route is not found, there are no vertices left
// or the search went outside of the max bounds.
return(false);
}
if (this.WasFound != null &&
this.WasFound(_current.Vertex, _current.Weight))
{ // vertex was found and true was returned, this search should stop.
return(false);
}
// check for restrictions.
var restriction = Constants.NO_VERTEX;
if (_getRestriction != null)
{
restriction = _getRestriction(_current.Vertex);
}
if (restriction != Constants.NO_VERTEX)
{ // this vertex is restricted, step is a success but just move
// to the next one because this vertex's neighbours are not allowed.
return(true);
}
if (this.Visit != null &&
this.Visit(_current))
{ // edge was found and true was returned, this search should stop.
return(false);
}
// get neighbours and queue them.
_edgeEnumerator.MoveTo(_current.Vertex);
while (_edgeEnumerator.MoveNext())
{
var edge = _edgeEnumerator;
var neighbour = edge.To;
if (_current.From != null &&
_current.From.Vertex == neighbour)
{ // don't go back
continue;
}
if (this.Visit == null)
{
if (_visits.ContainsKey(neighbour))
{ // has already been choosen
continue;
}
}
// get the speed from cache or calculate.
float distance;
ushort edgeProfile;
EdgeDataSerializer.Deserialize(edge.Data0, out distance, out edgeProfile);
var factor = Factor.NoFactor;
var totalWeight = _weightHandler.Add(_current.Weight, edgeProfile, distance, out factor);
// check the tags against the interpreter.
if (factor.Value > 0 && (factor.Direction == 0 ||
(!_backward && (factor.Direction == 1) != edge.DataInverted) ||
(_backward && (factor.Direction == 1) == edge.DataInverted)))
{ // it's ok; the edge can be traversed by the given vehicle.
// calculate neighbors weight.
var edgeWeight = (distance * factor.Value);
if (_weightHandler.IsSmallerThan(totalWeight, _sourceMax))
{ // update the visit list.
_heap.Push(new EdgePath <T>(neighbour, totalWeight, edge.IdDirected(), _current),
_weightHandler.GetMetric(totalWeight));
}
else
{ // the maxium was reached.
this.MaxReached = true;
}
}
}
return(true);
}
/// <summary>
/// Calculates a route between the two locations.
/// </summary>
/// <returns></returns>
public sealed override Result <EdgePath <T> > TryCalculateRaw <T>(IProfileInstance profileInstance, WeightHandler <T> weightHandler, RouterPoint source, RouterPoint target,
RoutingSettings <T> settings)
{
try
{
if (!_db.Supports(profileInstance.Profile))
{
return(new Result <EdgePath <T> >("Routing profile is not supported.", (message) =>
{
return new Exception(message);
}));
}
var maxSearch = weightHandler.Infinite;
if (settings != null)
{
if (!settings.TryGetMaxSearch(profileInstance.Profile.FullName, out maxSearch))
{
maxSearch = weightHandler.Infinite;
}
}
ContractedDb contracted;
bool useContracted = false;
if (_db.TryGetContracted(profileInstance.Profile, out contracted))
{ // contracted calculation.
useContracted = true;
if (_db.HasComplexRestrictions(profileInstance.Profile) &&
(!contracted.HasEdgeBasedGraph && !contracted.NodeBasedIsEdgedBased))
{ // there is no edge-based graph for this profile but the db has complex restrictions, don't use the contracted graph.
Logging.Logger.Log("Router", Logging.TraceEventType.Warning,
"There is a vertex-based contracted graph but also complex restrictions. Not using the contracted graph, add an edge-based contracted graph.");
useContracted = false;
}
}
EdgePath <T> path = null;
if (source.EdgeId == target.EdgeId)
{ // check for a path on the same edge.
var edgePath = source.EdgePathTo(_db, weightHandler, target);
if (edgePath != null)
{
path = edgePath;
}
}
if (useContracted)
{ // use the contracted graph.
List <uint> vertexPath = null;
if (contracted.HasEdgeBasedGraph)
{ // use edge-based routing.
var bidirectionalSearch = new Itinero.Algorithms.Contracted.EdgeBased.BidirectionalDykstra <T>(contracted.EdgeBasedGraph, weightHandler,
source.ToEdgePaths(_db, weightHandler, true), target.ToEdgePaths(_db, weightHandler, false), _db.GetGetRestrictions(profileInstance.Profile, null));
bidirectionalSearch.Run();
if (!bidirectionalSearch.HasSucceeded)
{
if (path == null)
{
return(new Result <EdgePath <T> >(bidirectionalSearch.ErrorMessage, (message) =>
{
return new RouteNotFoundException(message);
}));
}
}
else
{
vertexPath = bidirectionalSearch.GetPath();
}
}
else if (contracted.NodeBasedIsEdgedBased)
{// use vertex-based graph for edge-based routing.
var sourceDirectedId1 = new DirectedEdgeId(source.EdgeId, true);
var sourceDirectedId2 = new DirectedEdgeId(source.EdgeId, false);
var targetDirectedId1 = new DirectedEdgeId(target.EdgeId, true);
var targetDirectedId2 = new DirectedEdgeId(target.EdgeId, false);
var bidirectionalSearch = new Itinero.Algorithms.Contracted.BidirectionalDykstra <T>(contracted.NodeBasedGraph, null, weightHandler,
new EdgePath <T>[] { new EdgePath <T>(sourceDirectedId1.Raw), new EdgePath <T>(sourceDirectedId2.Raw) },
new EdgePath <T>[] { new EdgePath <T>(targetDirectedId1.Raw), new EdgePath <T>(targetDirectedId2.Raw) });
bidirectionalSearch.Run();
if (!bidirectionalSearch.HasSucceeded)
{
return(new Result <EdgePath <T> >(bidirectionalSearch.ErrorMessage, (message) =>
{
return new RouteNotFoundException(message);
}));
}
var directedEdgePath = Algorithms.Dual.BidirectionalDykstraExtensions.GetDualPath(bidirectionalSearch);
// convert directed edge-path to an original vertex path.
var enumerator = _db.Network.GetEdgeEnumerator();
vertexPath = new List <uint>();
var edge = new List <OriginalEdge>();
for (var i = 0; i < directedEdgePath.Count; i++)
{
var e = new DirectedEdgeId()
{
Raw = directedEdgePath[i]
};
enumerator.MoveToEdge(e.EdgeId);
var original = new OriginalEdge(enumerator.From, enumerator.To);
if (!e.Forward)
{
original = original.Reverse();
}
edge.Add(original);
if (vertexPath.Count == 0)
{
vertexPath.Add(original.Vertex1);
}
vertexPath.Add(original.Vertex2);
}
vertexPath[0] = Constants.NO_VERTEX;
vertexPath[vertexPath.Count - 1] = Constants.NO_VERTEX;
}
else
{ // use node-based routing.
var bidirectionalSearch = new Itinero.Algorithms.Contracted.BidirectionalDykstra <T>(contracted.NodeBasedGraph, _db.GetRestrictions(profileInstance.Profile), weightHandler,
source.ToEdgePaths(_db, weightHandler, true), target.ToEdgePaths(_db, weightHandler, false));
bidirectionalSearch.Run();
if (!bidirectionalSearch.HasSucceeded)
{
if (path == null)
{
return(new Result <EdgePath <T> >(bidirectionalSearch.ErrorMessage, (message) =>
{
return new RouteNotFoundException(message);
}));
}
}
else
{
vertexPath = Algorithms.Contracted.BidirectionalDykstraExtensions.GetPath(bidirectionalSearch);
}
}
// expand vertex path using the regular graph.
if (vertexPath != null)
{
var localPath = _db.BuildEdgePath(weightHandler, source, target, vertexPath);
if (path == null ||
weightHandler.IsSmallerThan(localPath.Weight, path.Weight))
{
path = localPath;
}
}
}
else
{ // use the regular graph.
EdgePath <T> localPath = null;
if (_db.HasComplexRestrictions(profileInstance.Profile))
{
var sourceSearch = new Algorithms.Default.EdgeBased.Dykstra <T>(_db.Network.GeometricGraph.Graph, weightHandler,
_db.GetGetRestrictions(profileInstance.Profile, true), source.ToEdgePaths(_db, weightHandler, true), maxSearch, false);
var targetSearch = new Algorithms.Default.EdgeBased.Dykstra <T>(_db.Network.GeometricGraph.Graph, weightHandler,
_db.GetGetRestrictions(profileInstance.Profile, false), target.ToEdgePaths(_db, weightHandler, false), maxSearch, true);
var bidirectionalSearch = new Algorithms.Default.EdgeBased.BidirectionalDykstra <T>(sourceSearch, targetSearch, weightHandler);
bidirectionalSearch.Run();
if (!bidirectionalSearch.HasSucceeded)
{
if (path == null)
{
return(new Result <EdgePath <T> >(bidirectionalSearch.ErrorMessage, (message) =>
{
return new RouteNotFoundException(message);
}));
}
}
else
{
localPath = bidirectionalSearch.GetPath();
}
}
else
{
var sourceSearch = new Dykstra <T>(_db.Network.GeometricGraph.Graph, _db.GetGetSimpleRestrictions(profileInstance.Profile), weightHandler,
source.ToEdgePaths(_db, weightHandler, true), maxSearch, false);
var targetSearch = new Dykstra <T>(_db.Network.GeometricGraph.Graph, _db.GetGetSimpleRestrictions(profileInstance.Profile), weightHandler,
target.ToEdgePaths(_db, weightHandler, false), maxSearch, true);
var bidirectionalSearch = new BidirectionalDykstra <T>(sourceSearch, targetSearch, weightHandler);
bidirectionalSearch.Run();
if (!bidirectionalSearch.HasSucceeded)
{
if (path == null)
{
return(new Result <EdgePath <T> >(bidirectionalSearch.ErrorMessage, (message) =>
{
return new RouteNotFoundException(message);
}));
}
}
else
{
localPath = bidirectionalSearch.GetPath();
}
}
// choose best path.
if (localPath != null)
{
if (path == null ||
weightHandler.IsSmallerThan(localPath.Weight, path.Weight))
{
path = localPath;
}
}
}
return(new Result <EdgePath <T> >(path));
}
catch (Exception ex)
{
return(new Result <EdgePath <T> >(ex.Message, (m) => ex));
}
}
/// <summary>
/// Executes the actual run.
/// </summary>
protected override void DoRun()
{
// keep settled vertices.
_forwardVisits = new Dictionary <uint, EdgePath <T> >();
_backwardVisits = new Dictionary <uint, EdgePath <T> >();
// initialize the queues.
var forwardQueue = new BinaryHeap <EdgePath <T> >();
var backwardQueue = new BinaryHeap <EdgePath <T> >();
// queue sources.
foreach (var source in _sources)
{
forwardQueue.Push(source, _weightHandler.GetMetric(source.Weight));
}
// queue targets.
foreach (var target in _targets)
{
backwardQueue.Push(target, _weightHandler.GetMetric(target.Weight));
}
// update best with current visits.
_best = new Tuple <uint, T>(Constants.NO_VERTEX, _weightHandler.Infinite);
// calculate stopping conditions.
var queueBackwardWeight = backwardQueue.PeekWeight();
var queueForwardWeight = forwardQueue.PeekWeight();
while (true)
{ // keep looping until stopping conditions.
if (backwardQueue.Count == 0 && forwardQueue.Count == 0)
{ // stop the search; both queues are empty.
break;
}
if (_weightHandler.GetMetric(_best.Item2) < queueForwardWeight &&
_weightHandler.GetMetric(_best.Item2) < queueBackwardWeight)
{ // stop the search: it now became impossible to find a shorter route by further searching.
break;
}
// do a forward search.
if (forwardQueue.Count > 0)
{ // first check for better path.
// get the current queued with the smallest weight that hasn't been visited yet.
var current = forwardQueue.Pop();
while (current != null && _forwardVisits.ContainsKey(current.Vertex))
{ // keep trying.
current = forwardQueue.Pop();
}
if (current != null)
{
EdgePath <T> toBest;
if (_backwardVisits.TryGetValue(current.Vertex, out toBest))
{ // check for a new best.
var total = _weightHandler.Add(current.Weight, toBest.Weight);
if (_weightHandler.IsSmallerThan(total, _best.Item2))
{ // a better path was found.
_best = new Tuple <uint, T>(current.Vertex, total);
this.HasSucceeded = true;
}
}
this.SearchForward(forwardQueue, current);
}
}
// do a backward search.
if (backwardQueue.Count > 0)
{// first check for better path.
// get the current vertex with the smallest weight.
var current = backwardQueue.Pop();
while (current != null && _backwardVisits.ContainsKey(current.Vertex))
{ // keep trying.
current = backwardQueue.Pop();
}
if (current != null)
{
EdgePath <T> toBest;
if (_forwardVisits.TryGetValue(current.Vertex, out toBest))
{ // check for a new best.
var total = _weightHandler.Add(current.Weight, toBest.Weight);
if (_weightHandler.IsSmallerThan(total, _best.Item2))
{ // a better path was found.
_best = new Tuple <uint, T>(current.Vertex, total);
this.HasSucceeded = true;
}
}
this.SearchBackward(backwardQueue, current);
}
}
// calculate stopping conditions.
if (forwardQueue.Count > 0)
{
queueForwardWeight = forwardQueue.PeekWeight();
}
if (backwardQueue.Count > 0)
{
queueBackwardWeight = backwardQueue.PeekWeight();
}
}
}
private void DoDualBased()
{
var uniqueSet = new HashSet <DirectedEdgeId>();
var sources = new List <DirectedEdgeId>(_correctedResolvedPoints.Count * 2);
for (var i = 0; i < _correctedResolvedPoints.Count; i++)
{
var f = new DirectedEdgeId(_correctedResolvedPoints[i].EdgeId, false);
if (!uniqueSet.Contains(f))
{
sources.Add(f);
sources.Add(new DirectedEdgeId(_correctedResolvedPoints[i].EdgeId, true));
uniqueSet.Add(f);
}
}
var dykstraSources = Itinero.Algorithms.Contracted.Dual.DykstraSourceExtensions.ToDykstraSources <T>(sources);
var dykstraTargets = Itinero.Algorithms.Contracted.Dual.DykstraSourceExtensions.ToDykstraSources <T>(sources);
var algorithm = new Itinero.Algorithms.Contracted.Dual.ManyToMany.VertexToVertexWeightAlgorithm <T>(_dualGraph, _weightHandler,
dykstraSources, dykstraTargets, _max);
algorithm.Run();
var map = new Dictionary <uint, int>();
for (var i = 0; i < sources.Count; i += 2)
{
map[sources[i].EdgeId] = i / 2;
}
for (var s = 0; s < _correctedResolvedPoints.Count; s++)
{
T?sourceBackward = _sourcePaths[s * 2 + 0] == null ? (T?)null : _sourcePaths[s * 2 + 0].Weight;
T?sourceForward = _sourcePaths[s * 2 + 1] == null ? (T?)null : _sourcePaths[s * 2 + 1].Weight;
int sourceIdx;
if (sourceForward == null && sourceBackward == null)
{
continue;
}
map.TryGetValue(_correctedResolvedPoints[s].EdgeId, out sourceIdx);
for (var t = 0; t < _correctedResolvedPoints.Count; t++)
{
T?targetBackward = _targetPaths[t * 2 + 0] == null ? (T?)null : _targetPaths[t * 2 + 0].Weight;
T?targetForward = _targetPaths[t * 2 + 1] == null ? (T?)null : _targetPaths[t * 2 + 1].Weight;
int targetIdx;
map.TryGetValue(_correctedResolvedPoints[t].EdgeId, out targetIdx);
if (targetForward != null)
{
if (sourceForward != null)
{
var w = _weightHandler.Add(_weightHandler.Add(sourceForward.Value, targetForward.Value),
algorithm.Weights[sourceIdx * 2 + 1][targetIdx * 2 + 1]);
if (_weightHandler.IsSmallerThan(w, _weights[s * 2 + 1][t * 2 + 1]))
{
_weights[s * 2 + 1][t * 2 + 1] = w;
}
}
if (sourceBackward != null)
{
var w = _weightHandler.Add(_weightHandler.Add(sourceBackward.Value, targetForward.Value),
algorithm.Weights[sourceIdx * 2 + 0][targetIdx * 2 + 1]);
if (_weightHandler.IsSmallerThan(w, _weights[s * 2 + 0][t * 2 + 1]))
{
_weights[s * 2 + 0][t * 2 + 1] = w;
}
}
}
if (targetBackward != null)
{
if (sourceForward != null)
{
var w = _weightHandler.Add(_weightHandler.Add(sourceForward.Value, targetBackward.Value),
algorithm.Weights[sourceIdx * 2 + 1][targetIdx * 2 + 0]);
if (_weightHandler.IsSmallerThan(w, _weights[s * 2 + 1][t * 2 + 0]))
{
_weights[s * 2 + 1][t * 2 + 0] = w;
}
}
if (sourceBackward != null)
{
var w = _weightHandler.Add(_weightHandler.Add(sourceBackward.Value, targetBackward.Value),
algorithm.Weights[sourceIdx * 2 + 0][targetIdx * 2 + 0]);
if (_weightHandler.IsSmallerThan(w, _weights[s * 2 + 0][t * 2 + 0]))
{
_weights[s * 2 + 0][t * 2 + 0] = w;
}
}
}
}
}
}
/// <summary>
/// Executes the actual run.
/// </summary>
protected override void DoRun(CancellationToken cancellationToken)
{
// keep settled vertices.
_pathTree = new PathTree();
// initialize the queues.
var forwardQueue = new BinaryHeap <uint>();
var backwardQueue = new BinaryHeap <uint>();
// queue sources.
if (_source.Vertex1 != Constants.NO_VERTEX)
{
forwardQueue.Push(_weightHandler.AddPathTree(_pathTree, _source.Vertex1, _source.Weight1, uint.MaxValue), 0);
}
if (_source.Vertex2 != Constants.NO_VERTEX)
{
forwardQueue.Push(_weightHandler.AddPathTree(_pathTree, _source.Vertex2, _source.Weight2, uint.MaxValue), 0);
}
// queue targets.
if (_target.Vertex1 != Constants.NO_VERTEX)
{
backwardQueue.Push(_weightHandler.AddPathTree(_pathTree, _target.Vertex1, _target.Weight1, uint.MaxValue), 0);
}
if (_target.Vertex2 != Constants.NO_VERTEX)
{
backwardQueue.Push(_weightHandler.AddPathTree(_pathTree, _target.Vertex2, _target.Weight2, uint.MaxValue), 0);
}
// update best with current visits.
_best = new Tuple <uint, uint, T>(uint.MaxValue, uint.MaxValue, _weightHandler.Infinite);
// calculate stopping conditions.
var queueBackwardWeight = backwardQueue.PeekWeight();
var queueForwardWeight = forwardQueue.PeekWeight();
while (true)
{ // keep looping until stopping conditions.
if (backwardQueue.Count == 0 && forwardQueue.Count == 0)
{ // stop the search; both queues are empty.
break;
}
var bestItem2 = _weightHandler.GetMetric(_best.Item3);
if (bestItem2 < queueForwardWeight && bestItem2 < queueBackwardWeight)
{ // stop the search: it now became impossible to find a shorter route by further searching.
break;
}
// do a forward search.
if (forwardQueue.Count > 0)
{ // first check for better path.
// get the current queued with the smallest weight that hasn't been visited yet.
var cPointer = forwardQueue.Pop();
uint cVertex, cPreviousPointer;
T cWeight;
_weightHandler.GetPathTree(_pathTree, cPointer, out cVertex, out cWeight, out cPreviousPointer);
while (_forwardVisits.ContainsKey(cVertex))
{ // keep trying.
if (forwardQueue.Count == 0)
{
cPointer = uint.MaxValue;
}
else
{
cPointer = forwardQueue.Pop();
_weightHandler.GetPathTree(_pathTree, cPointer, out cVertex, out cWeight, out cPreviousPointer);
}
}
if (cPointer != uint.MaxValue)
{
uint bPointer;
if (_backwardVisits.TryGetValue(cVertex, out bPointer))
{ // check for a new best.
uint bVertex, bPreviousPointer;
T bWeight;
_weightHandler.GetPathTree(_pathTree, bPointer, out bVertex, out bWeight, out bPreviousPointer);
var total = _weightHandler.Add(cWeight, bWeight);
if (_weightHandler.IsSmallerThan(total, _best.Item2))
{ // a better path was found.
_best = new Tuple <uint, uint, T>(cPointer, bPointer, total);
this.HasSucceeded = true;
}
}
this.SearchForward(forwardQueue, cPointer, cVertex, cWeight);
}
}
// do a backward search.
if (backwardQueue.Count > 0)
{// first check for better path.
// get the current queued with the smallest weight that hasn't been visited yet.
var cPointer = backwardQueue.Pop();
uint cVertex, cPreviousPointer;
T cWeight;
_weightHandler.GetPathTree(_pathTree, cPointer, out cVertex, out cWeight, out cPreviousPointer);
while (_backwardVisits.ContainsKey(cVertex))
{ // keep trying.
if (backwardQueue.Count == 0)
{
cPointer = uint.MaxValue;
}
else
{
cPointer = backwardQueue.Pop();
_weightHandler.GetPathTree(_pathTree, cPointer, out cVertex, out cWeight, out cPreviousPointer);
}
}
if (cPointer != uint.MaxValue)
{
uint bPointer; // best pointer.
if (_forwardVisits.TryGetValue(cVertex, out bPointer))
{ // check for a new best.
uint bVertex, bPreviousPointer;
T bWeight;
_weightHandler.GetPathTree(_pathTree, bPointer, out bVertex, out bWeight, out bPreviousPointer);
var total = _weightHandler.Add(cWeight, bWeight);
if (_weightHandler.IsSmallerThan(total, _best.Item2))
{ // a better path was found.
_best = new Tuple <uint, uint, T>(bPointer, cPointer, total);
this.HasSucceeded = true;
}
}
this.SearchBackward(backwardQueue, cPointer, cVertex, cWeight);
}
}
// calculate stopping conditions.
if (forwardQueue.Count > 0)
{
queueForwardWeight = forwardQueue.PeekWeight();
}
if (backwardQueue.Count > 0)
{
queueBackwardWeight = backwardQueue.PeekWeight();
}
}
}
/// <summary>
/// Executes one step in the search.
/// </summary>
public bool Step()
{
// while the visit list is not empty.
var cPointer = uint.MaxValue;
while (cPointer == uint.MaxValue)
{ // choose the next edge.
if (_pointerHeap.Count == 0)
{
return(false);
}
cPointer = _pointerHeap.Pop();
}
// get details.
uint cEdge, cPreviousPointer;
T cWeight;
_weightHandler.GetPathTree(_pathTree, cPointer, out cEdge, out cWeight, out cPreviousPointer);
if (_visits.Contains(cEdge))
{
return(true);
}
_visits.Add(cEdge);
var cDirectedEdge = new DirectedEdgeId()
{
Raw = cEdge
};
// signal was found.
if (this.WasFound != null)
{
this.WasFound(cPointer, cDirectedEdge, cWeight);
}
// move to the current edge's target vertex.
_edgeEnumerator.MoveToEdge(cDirectedEdge.EdgeId);
var cOriginalEdge = new OriginalEdge(_edgeEnumerator.From, _edgeEnumerator.To);
if (!cDirectedEdge.Forward)
{
cOriginalEdge = cOriginalEdge.Reverse();
}
var cEdgeWeightAndDirection = _weightHandler.GetEdgeWeight(_edgeEnumerator);
// calculate total weight.
var totalWeight = _weightHandler.Add(cEdgeWeightAndDirection.Weight, cWeight);
if (!_weightHandler.IsSmallerThan(totalWeight, _sourceMax))
{ // weight is too big.
this.MaxReached = true;
return(true);
}
// loop over all neighbours.
_edgeEnumerator.MoveTo(cOriginalEdge.Vertex2);
var turn = new Turn(cOriginalEdge, Constants.NO_VERTEX);
_restrictions.Update(turn.Vertex2);
while (_edgeEnumerator.MoveNext())
{
turn.Vertex3 = _edgeEnumerator.To;
if (turn.IsUTurn ||
turn.IsRestrictedBy(_restrictions))
{ // turn is restricted.
continue;
}
var nDirectedEdgeId = _edgeEnumerator.DirectedEdgeId();
if (_visits.Contains(nDirectedEdgeId.Raw))
{ // has already been choosen.
continue;
}
// get the speed from cache or calculate.
var nWeightAndDirection = _weightHandler.GetEdgeWeight(_edgeEnumerator);
var nWeightMetric = _weightHandler.GetMetric(nWeightAndDirection.Weight);
if (nWeightMetric <= 0)
{ // edge is not valid for profile.
continue;
}
if (!_backward && !nWeightAndDirection.Direction.F)
{ // cannot do forward search on edge.
continue;
}
if (_backward && !nWeightAndDirection.Direction.B)
{ // cannot do backward on edge.
continue;
}
// update the visit list.
_pointerHeap.Push(_weightHandler.AddPathTree(_pathTree, nDirectedEdgeId.Raw, totalWeight, cPointer), _weightHandler.GetMetric(totalWeight));
}
return(true);
}
/// <summary>
/// Calculates a route between the two locations.
/// </summary>
/// <returns></returns>
public sealed override Result <EdgePath <T> > TryCalculateRaw <T>(IProfileInstance profileInstance, WeightHandler <T> weightHandler, RouterPoint source, RouterPoint target,
RoutingSettings <T> settings)
{
try
{
if (!_db.Supports(profileInstance.Profile))
{
return(new Result <EdgePath <T> >("Routing profile is not supported.", (message) =>
{
return new Exception(message);
}));
}
var maxSearch = weightHandler.Infinite;
if (settings != null)
{
if (!settings.TryGetMaxSearch(profileInstance.Profile.FullName, out maxSearch))
{
maxSearch = weightHandler.Infinite;
}
}
EdgePath <T> path;
ContractedDb contracted;
bool useContracted = false;
if (_db.TryGetContracted(profileInstance.Profile, out contracted))
{ // contracted calculation.
useContracted = true;
if (_db.HasComplexRestrictions(profileInstance.Profile) && !contracted.HasEdgeBasedGraph)
{ // there is no edge-based graph for this profile but the db has complex restrictions, don't use the contracted graph.
Logging.Logger.Log("Router", Logging.TraceEventType.Warning,
"There is a vertex-based contracted graph but also complex restrictions. Not using the contracted graph, add an edge-based contracted graph.");
useContracted = false;
}
}
if (useContracted)
{ // use the contracted graph.
path = null;
List <uint> vertexPath = null;
if (!contracted.HasEdgeBasedGraph)
{ // use node-based routing.
var bidirectionalSearch = new Itinero.Algorithms.Contracted.BidirectionalDykstra <T>(contracted.NodeBasedGraph, weightHandler,
source.ToEdgePaths(_db, weightHandler, true), target.ToEdgePaths(_db, weightHandler, false));
bidirectionalSearch.Run();
if (!bidirectionalSearch.HasSucceeded)
{
return(new Result <EdgePath <T> >(bidirectionalSearch.ErrorMessage, (message) =>
{
return new RouteNotFoundException(message);
}));
}
vertexPath = bidirectionalSearch.GetPath();
}
else
{ // use edge-based routing.
var bidirectionalSearch = new Itinero.Algorithms.Contracted.EdgeBased.BidirectionalDykstra <T>(contracted.EdgeBasedGraph, weightHandler,
source.ToEdgePaths(_db, weightHandler, true), target.ToEdgePaths(_db, weightHandler, false), _db.GetGetRestrictions(profileInstance.Profile, null));
bidirectionalSearch.Run();
if (!bidirectionalSearch.HasSucceeded)
{
return(new Result <EdgePath <T> >(bidirectionalSearch.ErrorMessage, (message) =>
{
return new RouteNotFoundException(message);
}));
}
vertexPath = bidirectionalSearch.GetPath();
}
// expand vertex path using the regular graph.
path = _db.BuildEdgePath(weightHandler, source, target, vertexPath);
}
else
{ // use the regular graph.
if (_db.HasComplexRestrictions(profileInstance.Profile))
{
var sourceSearch = new Algorithms.Default.EdgeBased.Dykstra <T>(_db.Network.GeometricGraph.Graph, weightHandler,
_db.GetGetRestrictions(profileInstance.Profile, true), source.ToEdgePaths(_db, weightHandler, true), maxSearch, false);
var targetSearch = new Algorithms.Default.EdgeBased.Dykstra <T>(_db.Network.GeometricGraph.Graph, weightHandler,
_db.GetGetRestrictions(profileInstance.Profile, false), target.ToEdgePaths(_db, weightHandler, false), maxSearch, true);
var bidirectionalSearch = new Algorithms.Default.EdgeBased.BidirectionalDykstra <T>(sourceSearch, targetSearch, weightHandler);
bidirectionalSearch.Run();
if (!bidirectionalSearch.HasSucceeded)
{
return(new Result <EdgePath <T> >(bidirectionalSearch.ErrorMessage, (message) =>
{
return new RouteNotFoundException(message);
}));
}
path = bidirectionalSearch.GetPath();
}
else
{
var sourceSearch = new Dykstra <T>(_db.Network.GeometricGraph.Graph, null, weightHandler,
source.ToEdgePaths(_db, weightHandler, true), maxSearch, false);
var targetSearch = new Dykstra <T>(_db.Network.GeometricGraph.Graph, null, weightHandler,
target.ToEdgePaths(_db, weightHandler, false), maxSearch, true);
var bidirectionalSearch = new BidirectionalDykstra <T>(sourceSearch, targetSearch, weightHandler);
bidirectionalSearch.Run();
if (!bidirectionalSearch.HasSucceeded)
{
return(new Result <EdgePath <T> >(bidirectionalSearch.ErrorMessage, (message) =>
{
return new RouteNotFoundException(message);
}));
}
path = bidirectionalSearch.GetPath();
}
}
if (source.EdgeId == target.EdgeId)
{ // check for a shorter path on the same edge.
var edgePath = source.EdgePathTo(_db, weightHandler, target);
if (edgePath != null &&
weightHandler.IsSmallerThan(edgePath.Weight, path.Weight))
{
path = edgePath;
}
}
return(new Result <EdgePath <T> >(path));
}
catch (Exception ex)
{
return(new Result <EdgePath <T> >(ex.Message, (m) => ex));
}
}
