/// <summary>
/// Search forward from one vertex.
/// </summary>
/// <returns></returns>
private void SearchForward(BinaryHeap <EdgePath <T> > queue, EdgePath <T> current)
{
if (current != null)
{ // there is a next vertex found.
// check restrictions.
if (_restrictions != null &&
_restrictions.Update(current.Vertex) &&
_restrictions.Restricts(current.Vertex))
{ // vertex is restricted, don't settle.
return;
}
// get the edge enumerator.
var edgeEnumerator = _graph.GetEdgeEnumerator();
// add to the settled vertices.
EdgePath <T> previousLinkedRoute;
if (_forwardVisits.TryGetValue(current.Vertex, out previousLinkedRoute))
{
if (_weightHandler.IsLargerThan(previousLinkedRoute.Weight, current.Weight))
{ // settle the vertex again if it has a better weight.
_forwardVisits[current.Vertex] = current;
}
}
else
{ // settled the vertex.
_forwardVisits.Add(current.Vertex, current);
}
// get neighbours.
edgeEnumerator.MoveTo(current.Vertex);
// add the neighbours to the queue.
while (edgeEnumerator.MoveNext())
{
bool?neighbourDirection;
var neighbourWeight = _weightHandler.GetEdgeWeight(edgeEnumerator.Current, out neighbourDirection);
if (neighbourDirection == null || neighbourDirection.Value)
{ // the edge is forward, and is to higher or was not contracted at all.
var neighbourNeighbour = edgeEnumerator.Neighbour;
if (!_forwardVisits.ContainsKey(neighbourNeighbour))
{ // if not yet settled.
var routeToNeighbour = new EdgePath <T>(
neighbourNeighbour, _weightHandler.Add(current.Weight, neighbourWeight), current);
queue.Push(routeToNeighbour, _weightHandler.GetMetric(routeToNeighbour.Weight));
}
}
}
}
}
/// <summary>
/// Search forward from one vertex.
/// </summary>
/// <returns></returns>
private void SearchForward(BinaryHeap <uint> queue, uint cPointer, uint cVertex, T cWeight)
{
if (cPointer != uint.MaxValue)
{ // there is a next vertex found.
// add to the settled vertices.
uint ePointer; // the existing pointer.
if (_forwardVisits.TryGetValue(cVertex, out ePointer))
{
uint eVertex, ePreviousPointer;
T eWeight;
_weightHandler.GetPathTree(_pathTree, ePointer, out eVertex, out eWeight, out ePreviousPointer);
if (_weightHandler.IsLargerThan(eWeight, cWeight))
{ // settle the vertex again if it has a better weight.
_forwardVisits[cVertex] = cPointer;
}
else
{ // this is a worse settled, don't continue the search.
return;
}
}
else
{ // settled the vertex.
_forwardVisits.Add(cVertex, cPointer);
}
// get neighbours.
var edgeEnumerator = _graph.GetEdgeEnumerator();
edgeEnumerator.MoveTo(cVertex);
// add the neighbours to the queue.
while (edgeEnumerator.MoveNext())
{
var nWeightAndDirection = _weightHandler.GetEdgeWeight(edgeEnumerator.Current);
if (nWeightAndDirection.Direction.F)
{
var nVertex = edgeEnumerator.Neighbour;
if (!_forwardVisits.ContainsKey(nVertex))
{ // if not yet settled.
var nWeight = _weightHandler.Add(cWeight, nWeightAndDirection.Weight);
var nPointer = _weightHandler.AddPathTree(_pathTree, nVertex, nWeight,
cPointer);
queue.Push(nPointer, _weightHandler.GetMetric(nWeight));
}
}
}
}
}
/// <summary>
/// Executes one step in the search.
/// </summary>
public bool Step()
{
if (_heap.Count == 0)
{
return(false);
}
_current = _heap.Pop();
if (_current != null)
{
while (_visits.ContainsKey(_current.Vertex))
{
_current = _heap.Pop();
if (_current == null)
{
return(false);
}
}
}
else
{
return(false);
}
_visits.Add(_current.Vertex, _current);
if (this.WasFound != null)
{
this.WasFound(_current);
}
_edgeEnumerator.MoveTo(_current.Vertex);
while (_edgeEnumerator.MoveNext())
{
bool?neighbourDirection;
var neighbourWeight = _weightHandler.GetEdgeWeight(_edgeEnumerator.Current, out neighbourDirection);
if (neighbourDirection == null || neighbourDirection.Value == !_backward)
{ // the edge is forward, and is to higher or was not contracted at all.
var neighbourNeighbour = _edgeEnumerator.Neighbour;
if (!_visits.ContainsKey(neighbourNeighbour))
{ // if not yet settled.
var routeToNeighbour = new EdgePath <T>(
neighbourNeighbour, _weightHandler.Add(_current.Weight, neighbourWeight), _current);
if (_weightHandler.IsLargerThan(routeToNeighbour.Weight, _max))
{
continue;
}
_heap.Push(routeToNeighbour, _weightHandler.GetMetric(routeToNeighbour.Weight));
}
}
}
return(true);
}
/// <summary>
/// Executes the algorithm.
/// </summary>
protected sealed override void DoRun()
{
// run mass resolver if needed.
if (!_massResolver.HasRun)
{
_massResolver.Run();
}
// create error and resolved point management data structures.
_correctedResolvedPoints = _massResolver.RouterPoints;
_errors = new Dictionary <int, RouterPointError>(_correctedResolvedPoints.Count);
_correctedIndices = new List <int>(_correctedResolvedPoints.Count);
// convert sources into directed paths.
_sourcePaths = new EdgePath <T> [_correctedResolvedPoints.Count * 2];
for (var i = 0; i < _correctedResolvedPoints.Count; i++)
{
_correctedIndices.Add(i);
var paths = _correctedResolvedPoints[i].ToEdgePathsDirected(_router.Db, _weightHandler, true);
if (paths.Length == 0)
{
this.ErrorMessage = string.Format("Source at {0} could not be resolved properly.", i);
return;
}
_sourcePaths[i * 2 + 0] = paths[0];
if (paths.Length == 2)
{
_sourcePaths[i * 2 + 1] = paths[1];
}
}
// convert targets into directed paths.
_targetPaths = new EdgePath <T> [_correctedResolvedPoints.Count * 2];
for (var i = 0; i < _correctedResolvedPoints.Count; i++)
{
var paths = _correctedResolvedPoints[i].ToEdgePathsDirected(_router.Db, _weightHandler, false);
if (paths.Length == 0)
{
this.ErrorMessage = string.Format("Target at {0} could not be resolved properly.", i);
return;
}
// make sure paths are the opposive of the sources.
if (paths[0].Edge == _sourcePaths[i * 2 + 0].Edge)
{ // switchs.
_targetPaths[i * 2 + 1] = paths[0];
if (paths.Length == 2)
{
_targetPaths[i * 2 + 0] = paths[1];
}
}
else
{ // keep.
_targetPaths[i * 2 + 0] = paths[0];
if (paths.Length == 2)
{
_targetPaths[i * 2 + 1] = paths[1];
}
}
}
// put in default weights and weights for one-edge-paths.
_weights = new T[_sourcePaths.Length][];
for (var i = 0; i < _sourcePaths.Length; i++)
{
var source = _sourcePaths[i];
_weights[i] = new T[_targetPaths.Length];
for (var j = 0; j < _targetPaths.Length; j++)
{
var target = _targetPaths[j];
_weights[i][j] = _weightHandler.Infinite;
if (source == null ||
target == null)
{
continue;
}
if (target.Edge == -source.Edge)
{
var s = i / 2;
var t = j / 2;
var sourcePoint = _correctedResolvedPoints[s];
var targetPoint = _correctedResolvedPoints[t];
EdgePath <T> newPath = null;
if (source.Edge > 0 &&
sourcePoint.Offset <= targetPoint.Offset)
{
newPath = sourcePoint.EdgePathTo(_router.Db, _weightHandler, targetPoint);
}
else if (source.Edge < 0 &&
sourcePoint.Offset >= targetPoint.Offset)
{
newPath = sourcePoint.EdgePathTo(_router.Db, _weightHandler, targetPoint);
}
if (newPath != null)
{
if (_weightHandler.IsLargerThan(_weights[i][j], newPath.Weight))
{
_weights[i][j] = newPath.Weight;
}
}
}
}
}
// do forward searches into buckets.
for (var i = 0; i < _sourcePaths.Length; i++)
{
var path = _sourcePaths[i];
if (path != null)
{
var forward = new Itinero.Algorithms.Contracted.EdgeBased.Dykstra <T>(_graph, _weightHandler, new EdgePath <T>[] { path }, (v) => null, false, _max);
forward.WasFound += (foundPath) =>
{
LinkedEdgePath <T> visits;
forward.TryGetVisits(foundPath.Vertex, out visits);
return(this.ForwardVertexFound(i, foundPath.Vertex, visits));
};
forward.Run();
}
}
// do backward searches into buckets.
for (var i = 0; i < _targetPaths.Length; i++)
{
var path = _targetPaths[i];
if (path != null)
{
var backward = new Itinero.Algorithms.Contracted.EdgeBased.Dykstra <T>(_graph, _weightHandler, new EdgePath <T>[] { path }, (v) => null, true, _max);
backward.WasFound += (foundPath) =>
{
LinkedEdgePath <T> visits;
backward.TryGetVisits(foundPath.Vertex, out visits);
return(this.BackwardVertexFound(i, foundPath.Vertex, visits));
};
backward.Run();
}
}
// check for invalids.
var originalInvalids = new HashSet <int>();
var invalidTargetCounts = new int[_weights.Length / 2];
for (var s = 0; s < _weights.Length / 2; s++)
{
var invalids = 0;
for (var t = 0; t < _weights[s * 2].Length / 2; t++)
{
if (t != s)
{
if (_weightHandler.GetMetric(_weights[s * 2 + 0][t * 2 + 0]) == float.MaxValue &&
_weightHandler.GetMetric(_weights[s * 2 + 0][t * 2 + 1]) == float.MaxValue &&
_weightHandler.GetMetric(_weights[s * 2 + 1][t * 2 + 0]) == float.MaxValue &&
_weightHandler.GetMetric(_weights[s * 2 + 1][t * 2 + 1]) == float.MaxValue)
{
invalids++;
invalidTargetCounts[t]++;
if (invalidTargetCounts[t] > ((_weights.Length / 2) - 1) / 2)
{
originalInvalids.Add(t);
}
}
}
}
if (invalids > ((_weights.Length / 2) - 1) / 2)
{
originalInvalids.Add(s);
}
}
// take into account the non-null invalids now.
if (originalInvalids.Count > 0)
{ // shrink lists and add errors.
_correctedResolvedPoints = _correctedResolvedPoints.ShrinkAndCopyList(originalInvalids);
_correctedIndices = _correctedIndices.ShrinkAndCopyList(originalInvalids);
// convert back to the path indexes.
var nonNullInvalids = new HashSet <int>();
foreach (var invalid in originalInvalids)
{
nonNullInvalids.Add(invalid * 2);
nonNullInvalids.Add(invalid * 2 + 1);
_errors[invalid] = new RouterPointError()
{
Code = RouterPointErrorCode.NotRoutable,
Message = "Location could not routed to or from."
};
}
_weights = _weights.SchrinkAndCopyMatrix(nonNullInvalids);
_sourcePaths = _sourcePaths.ShrinkAndCopyArray(nonNullInvalids);
_targetPaths = _targetPaths.ShrinkAndCopyArray(nonNullInvalids);
}
this.HasSucceeded = true;
}
/// <summary>
/// Executes one step in the search.
/// </summary>
public bool Step()
{
if (_heap.Count == 0)
{
return(false);
}
_current = _heap.Pop();
while (_current != null)
{ // keep trying.
LinkedEdgePath <T> edgePath = null;
if (!_visits.TryGetValue(_current.Vertex, out edgePath))
{ // this vertex has not been visited before.
_visits.Add(_current.Vertex, new LinkedEdgePath <T>()
{
Path = _current
});
break;
}
else
{ // vertex has been visited before, check if edge has.
if (!edgePath.HasPath(_current))
{ // current edge has not been used to get to this vertex.
_visits[_current.Vertex] = new LinkedEdgePath <T>()
{
Path = _current,
Next = edgePath
};
break;
}
}
_current = _heap.Pop();
}
if (_current == null)
{
return(false);
}
if (this.WasFound != null)
{
this.WasFound(_current.Vertex, _current.Weight);
}
// get relevant restrictions.
var restrictions = _getRestrictions(_current.Vertex);
// get the edge enumerator.
var currentSequence = _current.GetSequence2(_edgeEnumerator);
currentSequence = currentSequence.Append(_current.Vertex);
// get neighbours.
_edgeEnumerator.MoveTo(_current.Vertex);
// add the neighbours to the queue.
while (_edgeEnumerator.MoveNext())
{
bool?neighbourDirection;
var neighbourWeight = _weightHandler.GetEdgeWeight(_edgeEnumerator.Current, out neighbourDirection);
if (neighbourDirection == null || (neighbourDirection.Value != _backward))
{ // the edge is forward, and is to higher or was not contracted at all.
var neighbourNeighbour = _edgeEnumerator.Neighbour;
var neighbourSequence = Constants.EMPTY_SEQUENCE;
if (_edgeEnumerator.IsOriginal())
{ // original edge.
if (currentSequence.Length > 1 && currentSequence[currentSequence.Length - 2] == neighbourNeighbour)
{ // this is a u-turn.
continue;
}
if (restrictions != null)
{
neighbourSequence = currentSequence.Append(neighbourNeighbour);
}
}
else
{ // not an original edge, use the sequence.
neighbourSequence = _edgeEnumerator.GetSequence1();
if (currentSequence.Length > 1 && currentSequence[currentSequence.Length - 2] == neighbourSequence[0])
{ // this is a u-turn.
continue;
}
if (restrictions != null)
{
neighbourSequence = currentSequence.Append(neighbourSequence);
}
}
if (restrictions != null)
{ // check restrictions.
if (!restrictions.IsSequenceAllowed(neighbourSequence))
{
continue;
}
}
// build route to neighbour and check if it has been visited already.
var routeToNeighbour = new EdgePath <T>(
neighbourNeighbour, _weightHandler.Add(_current.Weight, neighbourWeight), _edgeEnumerator.IdDirected(), _current);
LinkedEdgePath <T> edgePath = null;
if (!_visits.TryGetValue(_current.Vertex, out edgePath) ||
!edgePath.HasPath(routeToNeighbour))
{ // this vertex has not been visited in this way before.
if (_weightHandler.IsLargerThan(routeToNeighbour.Weight, _max))
{
continue;
}
_heap.Push(routeToNeighbour, _weightHandler.GetMetric(routeToNeighbour.Weight));
}
}
}
return(true);
}
/// <summary>
/// Executes the algorithm.
/// </summary>
protected sealed override void DoRun(CancellationToken cancellationToken)
{
// run mass resolver if needed.
if (!_massResolver.HasRun)
{
_massResolver.Run(cancellationToken);
}
// create error and resolved point management data structures.
_correctedResolvedPoints = _massResolver.RouterPoints;
_errors = new Dictionary <int, RouterPointError>(_correctedResolvedPoints.Count);
_correctedIndices = new List <int>(_correctedResolvedPoints.Count);
// convert sources into directed paths.
_sourcePaths = new EdgePath <T> [_correctedResolvedPoints.Count * 2];
for (var i = 0; i < _correctedResolvedPoints.Count; i++)
{
_correctedIndices.Add(i);
var paths = _correctedResolvedPoints[i].ToEdgePathsDirectedFixed(_router.Db, _weightHandler, true);
if (paths.Length == 0)
{
this.ErrorMessage = string.Format("Source at {0} could not be resolved properly.", i);
return;
}
_sourcePaths[i * 2 + 0] = paths[0];
_sourcePaths[i * 2 + 1] = paths[1];
}
// convert targets into directed paths.
_targetPaths = new EdgePath <T> [_correctedResolvedPoints.Count * 2];
for (var i = 0; i < _correctedResolvedPoints.Count; i++)
{
var paths = _correctedResolvedPoints[i].ToEdgePathsDirectedFixed(_router.Db, _weightHandler, false);
if (paths.Length == 0)
{
this.ErrorMessage = string.Format("Target at {0} could not be resolved properly.", i);
return;
}
_targetPaths[i * 2 + 0] = paths[1];
_targetPaths[i * 2 + 1] = paths[0];
}
// put in default weights and weights for one-edge-paths.
_weights = new T[_sourcePaths.Length][];
for (var i = 0; i < _sourcePaths.Length; i++)
{
var source = _sourcePaths[i];
_weights[i] = new T[_targetPaths.Length];
for (var j = 0; j < _targetPaths.Length; j++)
{
var target = _targetPaths[j];
_weights[i][j] = _weightHandler.Infinite;
if (source == null ||
target == null)
{
continue;
}
if (target.Edge == -source.Edge)
{
var s = i / 2;
var t = j / 2;
var sourcePoint = _correctedResolvedPoints[s];
var targetPoint = _correctedResolvedPoints[t];
EdgePath <T> newPath = null;
if (source.Edge > 0 &&
sourcePoint.Offset <= targetPoint.Offset)
{
newPath = sourcePoint.EdgePathTo(_router.Db, _weightHandler, targetPoint);
}
else if (source.Edge < 0 &&
sourcePoint.Offset >= targetPoint.Offset)
{
newPath = sourcePoint.EdgePathTo(_router.Db, _weightHandler, targetPoint);
}
if (newPath != null)
{
if (_weightHandler.IsLargerThan(_weights[i][j], newPath.Weight))
{
_weights[i][j] = newPath.Weight;
}
}
}
}
}
// run the actual calculations.
if (_graph != null)
{
this.DoEdgeBased(cancellationToken);
}
else
{
this.DoDualBased(cancellationToken);
}
// check for invalids.
var originalInvalids = new HashSet <int>();
var invalidTargetCounts = new int[_weights.Length / 2];
for (var s = 0; s < _weights.Length / 2; s++)
{
var invalids = 0;
for (var t = 0; t < _weights[s * 2].Length / 2; t++)
{
if (t != s)
{
if (_weightHandler.GetMetric(_weights[s * 2 + 0][t * 2 + 0]) == float.MaxValue &&
_weightHandler.GetMetric(_weights[s * 2 + 0][t * 2 + 1]) == float.MaxValue &&
_weightHandler.GetMetric(_weights[s * 2 + 1][t * 2 + 0]) == float.MaxValue &&
_weightHandler.GetMetric(_weights[s * 2 + 1][t * 2 + 1]) == float.MaxValue)
{
invalids++;
invalidTargetCounts[t]++;
if (invalidTargetCounts[t] > ((_weights.Length / 2) - 1) / 2)
{
originalInvalids.Add(t);
}
}
}
}
if (invalids > ((_weights.Length / 2) - 1) / 2)
{
originalInvalids.Add(s);
}
}
// take into account the non-null invalids now.
if (originalInvalids.Count > 0)
{ // shrink lists and add errors.
_correctedResolvedPoints = _correctedResolvedPoints.ShrinkAndCopyList(originalInvalids);
_correctedIndices = _correctedIndices.ShrinkAndCopyList(originalInvalids);
// convert back to the path indexes.
var nonNullInvalids = new HashSet <int>();
foreach (var invalid in originalInvalids)
{
nonNullInvalids.Add(invalid * 2);
nonNullInvalids.Add(invalid * 2 + 1);
_errors[invalid] = new RouterPointError()
{
Code = RouterPointErrorCode.NotRoutable,
Message = "Location could not routed to or from."
};
}
_weights = _weights.SchrinkAndCopyMatrix(nonNullInvalids);
_sourcePaths = _sourcePaths.ShrinkAndCopyArray(nonNullInvalids);
_targetPaths = _targetPaths.ShrinkAndCopyArray(nonNullInvalids);
}
this.HasSucceeded = true;
}
