/// <summary>
/// Called when a backward vertex was found.
/// </summary>
/// <returns></returns>
private bool BackwardVertexFound(Dykstra <T> dykstra, int i, uint pointer, uint vertex, T weight)
{
Dictionary <int, EdgePath <T> > bucket;
if (_buckets.TryGetValue(vertex, out bucket))
{
foreach (var pair in bucket)
{
var existing = _solutions[pair.Key][i];
var total = _weightHandler.Add(weight, pair.Value.Weight);
var existingWeight = _weightHandler.Infinite;
if (existing.Path != null)
{
existingWeight = existing.Path.Weight;
}
else if (existing.Path1 != null &&
existing.Path2 != null)
{
existingWeight = _weightHandler.Add(existing.Path1.Weight,
existing.Path2.Weight);
}
if (_weightHandler.IsSmallerThan(total, existingWeight))
{ // append the backward to the forward path.
_solutions[pair.Key][i] = new Solution()
{
Path1 = pair.Value,
Path2 = dykstra.GetPath(pointer)
};
}
}
}
return(false);
}
/// <summary>
/// Called when a vertex was reached during a forward search.
/// </summary>
/// <returns></returns>
private bool ReachedForward(EdgePath <T> forwardVisit)
{
// check backward search for the same vertex.
EdgePath <T> backwardVisit;
if (_targetSearch.TryGetVisit(-forwardVisit.Edge, out backwardVisit))
{ // there is a status for this edge in the target search.
var localWeight = _weightHandler.Zero;
if (forwardVisit.From != null)
{
localWeight = _weightHandler.Subtract(forwardVisit.Weight, forwardVisit.From.Weight);
}
var totalWeight = _weightHandler.Subtract(_weightHandler.Add(forwardVisit.Weight, backwardVisit.Weight), localWeight);
if (_weightHandler.IsSmallerThan(totalWeight, _best.Item3))
{ // this is a better match.
if (forwardVisit.Vertex == backwardVisit.From.Vertex &&
(forwardVisit.From.From != null || backwardVisit.From.From != null))
{ // paths match and are bigger than one edge.
_best = new Tuple <EdgePath <T>, EdgePath <T>, T>(forwardVisit, backwardVisit, totalWeight);
this.HasSucceeded = true;
}
}
}
return(false);
}
/// <summary>
/// Builds the potential shortcuts.
/// </summary>
public static void BuildShortcuts <T>(this VertexInfo <T> vertexinfo, WeightHandler <T> weightHandler)
where T : struct
{
var vertex = vertexinfo.Vertex;
var shortcuts = vertexinfo.Shortcuts;
// loop over all edge-pairs once.
var shortcut = new Shortcut <T>()
{
Backward = weightHandler.Infinite,
Forward = weightHandler.Infinite
};
var shortcutEdge = new OriginalEdge();
for (var j = 1; j < vertexinfo.Count; j++)
{
var edge1 = vertexinfo[j];
var edge1Weight = weightHandler.GetEdgeWeight(edge1);
shortcutEdge.Vertex1 = edge1.Neighbour;
// figure out what witness paths to calculate.
for (var k = 0; k < j; k++)
{
var edge2 = vertexinfo[k];
var edge2Weight = weightHandler.GetEdgeWeight(edge2);
shortcutEdge.Vertex2 = edge2.Neighbour;
if (!(edge1Weight.Direction.B && edge2Weight.Direction.F) &&
!(edge1Weight.Direction.F && edge2Weight.Direction.B))
{ // impossible route, do nothing.
continue;
}
shortcut.Backward = weightHandler.Infinite;
shortcut.Forward = weightHandler.Infinite;
if (edge1Weight.Direction.B && edge2Weight.Direction.F)
{
shortcut.Forward = weightHandler.Add(edge1Weight.Weight, edge2Weight.Weight);
}
if (edge1Weight.Direction.F && edge2Weight.Direction.B)
{
shortcut.Backward = weightHandler.Add(edge1Weight.Weight, edge2Weight.Weight);
}
shortcuts.AddOrUpdate(shortcutEdge, shortcut, weightHandler);
}
}
}
/// <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>
/// Called when a vertex was reached during a forward search.
/// </summary>
/// <returns></returns>
private bool ReachedVertexForward(uint vertex, T weight)
{
// check backward search for the same vertex.
EdgePath <T> backwardVisit;
if (_targetSearch.TryGetVisit(vertex, out backwardVisit))
{ // there is a status for this vertex in the source search.
weight = _weightHandler.Add(weight, backwardVisit.Weight);
if (_weightHandler.IsSmallerThan(weight, _bestWeight))
{ // this vertex is a better match.
_bestWeight = weight;
_bestVertex = vertex;
this.HasSucceeded = true;
}
}
return(false);
}
/// <summary>
/// Expands the last edge in the given edge path.
/// </summary>
private static EdgePath <T> ExpandLast <T>(this EdgePath <T> edgePath, DirectedDynamicGraph.EdgeEnumerator enumerator, WeightHandler <T> weightHandler)
where T : struct
{
bool?direction;
if (edgePath.Edge != Constants.NO_EDGE &&
edgePath.From != null &&
edgePath.From.Vertex != Constants.NO_VERTEX)
{
enumerator.MoveToEdge(edgePath.Edge);
var contractedId = enumerator.GetContracted();
if (contractedId.HasValue)
{ // there is a contracted vertex here!
// get source/target sequences.
var sequence1 = enumerator.GetSequence1();
sequence1.Reverse();
var sequence2 = enumerator.GetSequence2();
// move to the first edge (contracted -> from vertex) and keep details.
enumerator.MoveToEdge(contractedId.Value, edgePath.From.Vertex, sequence1);
var edge1 = enumerator.IdDirected();
var weight1 = weightHandler.GetEdgeWeight(enumerator.Current, out direction);
// move to the second edge (contracted -> to vertex) and keep details.
enumerator.MoveToEdge(contractedId.Value, edgePath.Vertex, sequence2);
var weight2 = weightHandler.GetEdgeWeight(enumerator.Current, out direction);
var edge2 = enumerator.IdDirected();
if (edgePath.Edge > 0)
{
var contractedPath = new EdgePath <T>(contractedId.Value, weightHandler.Add(edgePath.From.Weight, weight1), -edge1, edgePath.From);
contractedPath = contractedPath.ExpandLast(enumerator, weightHandler);
return((new EdgePath <T>(edgePath.Vertex, edgePath.Weight, edge2, contractedPath)).ExpandLast(enumerator, weightHandler));
}
else
{
var contractedPath = new EdgePath <T>(contractedId.Value, weightHandler.Add(edgePath.From.Weight, weight1), edge1, edgePath.From);
contractedPath = contractedPath.ExpandLast(enumerator, weightHandler);
return((new EdgePath <T>(edgePath.Vertex, edgePath.Weight, -edge2, contractedPath)).ExpandLast(enumerator, weightHandler));
}
}
}
return(edgePath);
}
/// <summary>
/// Builds an edge path from a path consisiting of only vertices.
/// </summary>
public static EdgePath <T> BuildEdgePath <T>(this RouterDb routerDb, WeightHandler <T> weightHandler, RouterPoint source, RouterPoint target, List <uint> vertexPath)
where T : struct
{
if (vertexPath == null || vertexPath.Count == 0)
{
return(null);
}
var path = new EdgePath <T>(vertexPath[0]);
var i = 1;
if (path.Vertex == Constants.NO_VERTEX)
{ // add first router point segment from source.
path = source.EdgePathTo(routerDb, weightHandler, vertexPath[1]);
i = 2;
}
var edgeEnumerator = routerDb.Network.GeometricGraph.Graph.GetEdgeEnumerator();
for (; i < vertexPath.Count; i++)
{
var vertex = vertexPath[i];
if (vertex == Constants.NO_VERTEX)
{
if (i != vertexPath.Count - 1)
{
throw new Exception("Invalid data found in vertex path: a non-vertex id was found at an invalid location.");
}
var toTarget = target.EdgePathTo(routerDb, weightHandler, path.Vertex, true);
path = new EdgePath <T>(toTarget.Vertex, weightHandler.Add(toTarget.Weight, path.Weight), toTarget.Edge, path);
break;
}
T weight;
var best = edgeEnumerator.FindBestEdge(weightHandler, path.Vertex, vertexPath[i], out weight);
if (best == Constants.NO_EDGE)
{
throw new Exception(string.Format("Cannot build vertex path, edge {0} -> {1} not found.", path.Vertex, vertexPath[i]));
}
path = new EdgePath <T>(vertexPath[i], weightHandler.Add(weight, path.Weight), best, path);
}
return(path);
}
/// <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>
/// Expands the last edge in the given edge path.
/// </summary>
private static EdgePath <T> ExpandLast <T>(this EdgePath <T> edgePath, DirectedDynamicGraph.EdgeEnumerator enumerator, WeightHandler <T> weightHandler, bool direction)
where T : struct
{
if (edgePath.Edge != Constants.NO_EDGE &&
edgePath.From != null &&
edgePath.From.Vertex != Constants.NO_VERTEX)
{
enumerator.MoveToEdge(edgePath.Edge);
var contractedId = enumerator.GetContracted();
if (contractedId.HasValue)
{ // there is a contracted vertex here!
// get source/target sequences.
var sequence1 = enumerator.GetSequence1();
sequence1.Reverse();
var sequence2 = enumerator.GetSequence2();
if (enumerator.Neighbour != edgePath.Vertex)
{
sequence1.Reverse();
sequence2.Reverse();
var t = sequence2;
sequence2 = sequence1;
sequence1 = t;
}
// move to the first edge (contracted -> from vertex) and keep details.
bool?localDirection;
if (!enumerator.MoveToEdge(contractedId.Value, edgePath.From.Vertex, sequence1, weightHandler, !direction))
{
throw new Exception(string.Format("Edge between {0} -> {1} with sequence {2} could not be found.",
contractedId.Value, edgePath.From.Vertex, sequence1.ToStringSafe()));
}
var edge1 = enumerator.IdDirected();
var weight1 = weightHandler.GetEdgeWeight(enumerator.Current, out localDirection);
// move to the second edge (contracted -> to vertex) and keep details.
if (!enumerator.MoveToEdge(contractedId.Value, edgePath.Vertex, sequence2, weightHandler, direction))
{
throw new Exception(string.Format("Edge between {0} -> {1} with sequence {2} could not be found.",
contractedId.Value, edgePath.Vertex, sequence2.ToStringSafe()));
}
var weight2 = weightHandler.GetEdgeWeight(enumerator.Current, out localDirection);
var edge2 = enumerator.IdDirected();
var contractedPath = new EdgePath <T>(contractedId.Value, weightHandler.Add(edgePath.From.Weight, weight1), edge1, edgePath.From);
contractedPath = contractedPath.ExpandLast(enumerator, weightHandler, direction);
return((new EdgePath <T>(edgePath.Vertex, edgePath.Weight, edge2, contractedPath)).ExpandLast(enumerator, weightHandler, direction));
}
}
return(edgePath);
}
/// <summary>
/// Called when a backward vertex was found.
/// </summary>
/// <returns></returns>
private bool BackwardVertexFound(int i, EdgePath <T> path)
{
Dictionary <int, EdgePath <T> > bucket;
if (_buckets.TryGetValue(path.Vertex, out bucket))
{
foreach (var pair in bucket)
{
var existing = _paths[pair.Key][i];
var better = true;
if (existing.Path != null)
{
var existingWeight = existing.Path.Weight;
var total = _weightHandler.Add(path.Weight, pair.Value.Weight);
better = _weightHandler.IsSmallerThan(total, existingWeight);
}
else if (existing.Path1 != null &&
existing.Path2 != null)
{
var existingWeight = _weightHandler.Add(existing.Path1.Weight,
existing.Path2.Weight);
var total = _weightHandler.Add(path.Weight, pair.Value.Weight);
better = _weightHandler.IsSmallerThan(total, existingWeight);
}
if (better)
{ // append the backward to the forward path.
_paths[pair.Key][i] = new Solution()
{
Path1 = pair.Value,
Path2 = path
};
}
}
}
return(false);
}
/// <summary>
/// Appends the given path in reverse to the edge path.
/// </summary>
public static EdgePath <T> Append <T>(this EdgePath <T> path, EdgePath <T> reversePath, WeightHandler <T> weightHandler)
where T : struct
{
if (path.Vertex != reversePath.Vertex)
{
throw new System.Exception("Cannot append path that ends with a different vertex.");
}
while (reversePath.From != null)
{
var localWeight = weightHandler.Subtract(reversePath.Weight, reversePath.From.Weight);
path = new EdgePath <T>(reversePath.From.Vertex, weightHandler.Add(path.Weight, localWeight), -reversePath.Edge, path);
reversePath = reversePath.From;
}
return(path);
}
/// <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>
/// Called when a vertex was reached during a backward search.
/// </summary>
/// <returns></returns>
private bool ReachedBackward(uint vertex1, T weight1, float length, EdgePath <T> edge)
{
// check forward search for the same vertex.
EdgePath <T> forwardVisit;
if (_sourceSearch.TryGetVisit(-edge.Edge, out forwardVisit))
{ // there is a status for this vertex in the source search.
var localWeight = _weightHandler.Subtract(edge.Weight, weight1);
var totalWeight = _weightHandler.Subtract(_weightHandler.Add(edge.Weight, forwardVisit.Weight), localWeight);
if (_weightHandler.IsSmallerThan(totalWeight, _best.Item3))
{ // this vertex is a better match.
_best = new Tuple <EdgePath <T>, EdgePath <T>, T>(forwardVisit, edge, totalWeight);
this.HasSucceeded = true;
}
}
return(false);
}
/// <summary>
/// Called when a backward vertex was found.
/// </summary>
/// <returns></returns>
private bool BackwardVertexFound(int i, uint vertex, T weight)
{
Dictionary <int, T> bucket;
if (_buckets.TryGetValue(vertex, out bucket))
{
foreach (var pair in bucket)
{
var existing = _weights[pair.Key][i];
var totalWeight = _weightHandler.Add(weight, pair.Value);
if (_weightHandler.IsSmallerThan(totalWeight, existing))
{
_weights[pair.Key][i] = totalWeight;
}
}
}
return(false);
}
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 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>
/// Contracts the given vertex.
/// </summary>
private void Contract(uint vertex)
{
// get and keep edges.
var enumerator = _graph.GetEdgeEnumerator(vertex);
var edges = new List <DynamicEdge>(enumerator);
// check if this vertex has a potential restrictions.
var hasRestrictions = _restrictionFlags[vertex];
// loop over all edge-pairs once.
for (var j = 1; j < edges.Count; j++)
{
var edge1 = edges[j];
var edge1Sequence2 = edges[j].GetSequence2();
if (edge1Sequence2.Length == 0)
{
edge1Sequence2 = new uint[] { vertex };
}
bool?edge1Direction;
var edge1Weight = _weightHandler.GetEdgeWeight(edge1, out edge1Direction);
var edge1CanMoveForward = edge1Direction == null || edge1Direction.Value;
var edge1CanMoveBackward = edge1Direction == null || !edge1Direction.Value;
// figure out what witness paths to calculate.
var forwardWitnesses = new EdgePath <T> [j];
var backwardWitnesses = new EdgePath <T> [j];
var targets = new List <uint>(j);
var targetWeights = new List <T>(j);
for (var k = 0; k < j; k++)
{
var edge2 = edges[k];
bool?edge2Direction;
var edge2Weight = _weightHandler.GetEdgeWeight(edge2, out edge2Direction);
var edge2CanMoveForward = edge2Direction == null || edge2Direction.Value;
var edge2CanMoveBackward = edge2Direction == null || !edge2Direction.Value;
// use witness flags to represent impossible routes.
if (!(edge1CanMoveBackward && edge2CanMoveForward))
{
forwardWitnesses[k] = new EdgePath <T>();
}
if (!(edge1CanMoveForward && edge2CanMoveBackward))
{
backwardWitnesses[k] = new EdgePath <T>();
}
targets.Add(edge2.Neighbour);
if (hasRestrictions)
{
targetWeights.Add(_weightHandler.Infinite);
}
else
{
targetWeights.Add(_weightHandler.Add(edge1Weight, edge2Weight));
}
}
// calculate all witness paths.
_witnessCalculator.Calculate(_graph, _getRestrictions, edge1.Neighbour, targets, targetWeights, ref forwardWitnesses,
ref backwardWitnesses, Constants.NO_VERTEX);
// get all sequences where needed.
var s1forward = new uint[forwardWitnesses.Length][];
var s2forward = new uint[forwardWitnesses.Length][];
var s1backward = new uint[backwardWitnesses.Length][];
var s2backward = new uint[backwardWitnesses.Length][];
for (var k = 0; k < j; k++)
{
var edge2Sequence2 = edges[k].GetSequence2();
if (edge2Sequence2.Length == 0)
{
edge2Sequence2 = new uint[] { vertex };
}
if (forwardWitnesses[k].HasVertex(vertex))
{ // get forward sequences.
s1forward[k] = forwardWitnesses[k].GetSequence1(enumerator, 1);
s2forward[k] = forwardWitnesses[k].GetSequence2(enumerator, 1);
if (!s1forward[k].IsSequenceIdentical(edge1Sequence2) ||
!s2forward[k].IsSequenceIdentical(edge2Sequence2))
{ // start and end sequences of shortest paths need to match.
s1forward[k] = null;
s2forward[k] = null;
}
}
if (backwardWitnesses[k].HasVertex(vertex))
{ // get backward sequences.
s1backward[k] = backwardWitnesses[k].GetSequence1(enumerator, 1);
s2backward[k] = backwardWitnesses[k].GetSequence2(enumerator, 1);
if (!s1backward[k].IsSequenceIdentical(edge1Sequence2) ||
!s2backward[k].IsSequenceIdentical(edge2Sequence2))
{ // start and end sequences of shortest paths need to match.
s1backward[k] = null;
s2backward[k] = null;
}
}
}
// add contracted edges if needed.
for (var k = 0; k < j; k++)
{
var edge2 = edges[k];
if (edge1.Neighbour == edge2.Neighbour)
{ // do not try to add a shortcut between identical vertices.
continue;
}
//if (s1forward[k] != null && s1backward[k] != null &&
// System.Math.Abs(_weightHandler.GetMetric(forwardWitnesses[k].Weight) - _weightHandler.GetMetric(backwardWitnesses[k].Weight)) < E)
//{ // paths in both direction are possible and with the same weight, add just one edge in each direction.
// s1backward[k].Reverse();
// s2backward[k].Reverse();
// _weightHandler.AddOrUpdateEdge(_graph, edge1.Neighbour, edge2.Neighbour, vertex, null,
// forwardWitnesses[k].Weight, s1forward[k], s2forward[k]);
// //_graph.AddOrUpdateEdge(edge1.Neighbour, edge2.Neighbour,
// // forwardWitnesses[k].Weight, null, vertex, s1forward[k], s2forward[k]);
// _weightHandler.AddOrUpdateEdge(_graph, edge2.Neighbour, edge1.Neighbour, vertex, null,
// backwardWitnesses[k].Weight, s2backward[k], s1backward[k]);
// //_graph.AddOrUpdateEdge(edge2.Neighbour, edge1.Neighbour,
// // backwardWitnesses[k].Weight, null, vertex, s2backward[k], s1backward[k]);
//}
//else
//{ // add two edge per direction.
if (s1forward[k] != null)
{ // add forward edge.
_weightHandler.AddOrUpdateEdge(_graph, edge1.Neighbour, edge2.Neighbour, vertex, true,
forwardWitnesses[k].Weight, s1forward[k], s2forward[k]);
//_graph.AddOrUpdateEdge(edge1.Neighbour, edge2.Neighbour,
// forwardWitnesses[k].Weight, true, vertex, s1forward[k], s2forward[k]);
s1forward[k].Reverse();
s2forward[k].Reverse();
_weightHandler.AddOrUpdateEdge(_graph, edge2.Neighbour, edge1.Neighbour, vertex, false,
forwardWitnesses[k].Weight, s2forward[k], s1forward[k]);
//_graph.AddOrUpdateEdge(edge2.Neighbour, edge1.Neighbour,
// forwardWitnesses[k].Weight, false, vertex, s2forward[k], s1forward[k]);
}
if (s1backward[k] != null)
{ // add forward edge.
_weightHandler.AddOrUpdateEdge(_graph, edge1.Neighbour, edge2.Neighbour, vertex, false,
backwardWitnesses[k].Weight, s1backward[k], s2backward[k]);
//_graph.AddOrUpdateEdge(edge1.Neighbour, edge2.Neighbour,
// backwardWitnesses[k].Weight, false, vertex, s2backward[k], s1backward[k]);
s1backward[k].Reverse();
s2backward[k].Reverse();
_weightHandler.AddOrUpdateEdge(_graph, edge2.Neighbour, edge1.Neighbour, vertex, true,
backwardWitnesses[k].Weight, s2backward[k], s1backward[k]);
//_graph.AddOrUpdateEdge(edge2.Neighbour, edge1.Neighbour,
// backwardWitnesses[k].Weight, true, vertex, s1backward[k], s2backward[k]);
}
//}
}
}
// remove 'downward' edge to vertex.
var i = 0;
while (i < edges.Count)
{
_graph.RemoveEdge(edges[i].Neighbour, vertex);
if (_contractedFlags[edges[i].Neighbour])
{ // neighbour was already contracted, remove 'downward' edge and exclude it.
_graph.RemoveEdge(vertex, edges[i].Neighbour);
edges.RemoveAt(i);
}
else
{ // move to next edge.
i++;
}
}
_contractedFlags[vertex] = true;
_priorityCalculator.NotifyContracted(vertex);
}
/// <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>
/// 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.
_heap.Push(routeToNeighbour, _weightHandler.GetMetric(routeToNeighbour.Weight));
}
}
}
return(true);
}
/// <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();
}
}
}
/// <summary>
/// Contracts the given vertex.
/// </summary>
private void Contract(uint vertex)
{
// get and keep edges.
var edges = new List <MetaEdge>(_graph.GetEdgeEnumerator(vertex));
// remove 'downward' edge to vertex.
var i = 0;
while (i < edges.Count)
{
_graph.RemoveEdge(edges[i].Neighbour, vertex);
if (_contractedFlags[edges[i].Neighbour])
{ // neighbour was already contracted, remove 'downward' edge and exclude it.
_graph.RemoveEdge(vertex, edges[i].Neighbour);
edges.RemoveAt(i);
}
else
{ // move to next edge.
i++;
}
}
// check for a restriction, if vertex is restricted don't add shortcuts.
if (_restrictions != null &&
_restrictions.Update(vertex))
{
if (_restrictions.Restricts(vertex))
{
return;
}
}
// loop over all edge-pairs once.
for (var j = 1; j < edges.Count; j++)
{
var edge1 = edges[j];
bool?edge1Direction;
var edge1Weight = _weightHandler.GetEdgeWeight(edge1, out edge1Direction);
var edge1CanMoveForward = edge1Direction == null || edge1Direction.Value;
var edge1CanMoveBackward = edge1Direction == null || !edge1Direction.Value;
// figure out what witness paths to calculate.
var forwardWitnesses = new bool[j];
var backwardWitnesses = new bool[j];
var targets = new List <uint>(j);
var targetWeights = new List <T>(j);
var targetMetrics = new List <float>(j);
for (var k = 0; k < j; k++)
{
var edge2 = edges[k];
bool?edge2Direction;
var edge2Weight = _weightHandler.GetEdgeWeight(edge2, out edge2Direction);
var edge2CanMoveForward = edge2Direction == null || edge2Direction.Value;
var edge2CanMoveBackward = edge2Direction == null || !edge2Direction.Value;
// use witness flags to represent impossible routes.
forwardWitnesses[k] = !(edge1CanMoveBackward && edge2CanMoveForward);
backwardWitnesses[k] = !(edge1CanMoveForward && edge2CanMoveBackward);
targets.Add(edge2.Neighbour);
var totalWeight = _weightHandler.Add(edge1Weight, edge2Weight);
targetWeights.Add(totalWeight);
targetMetrics.Add(_weightHandler.GetMetric(totalWeight));
}
// calculate all witness paths.
_witnessCalculator.Calculate(_graph.Graph, edge1.Neighbour, targets, targetMetrics, ref forwardWitnesses,
ref backwardWitnesses, vertex);
// add contracted edges if needed.
for (var k = 0; k < j; k++)
{
var edge2 = edges[k];
if (edge1.Neighbour == edge2.Neighbour)
{ // do not try to add a shortcut between identical vertices.
continue;
}
if (!forwardWitnesses[k] && !backwardWitnesses[k])
{ // add bidirectional edge.
_weightHandler.AddOrUpdateEdge(_graph, edge1.Neighbour, edge2.Neighbour,
vertex, null, targetWeights[k]);
//_graph.AddOrUpdateEdge(edge1.Neighbour, edge2.Neighbour,
// targetWeights[k], null, vertex);
_weightHandler.AddOrUpdateEdge(_graph, edge2.Neighbour, edge1.Neighbour,
vertex, null, targetWeights[k]);
//_graph.AddOrUpdateEdge(edge2.Neighbour, edge1.Neighbour,
// targetWeights[k], null, vertex);
}
else if (!forwardWitnesses[k])
{ // add forward edge.
_weightHandler.AddOrUpdateEdge(_graph, edge1.Neighbour, edge2.Neighbour,
vertex, true, targetWeights[k]);
//_graph.AddOrUpdateEdge(edge1.Neighbour, edge2.Neighbour,
// targetWeights[k], true, vertex);
_weightHandler.AddOrUpdateEdge(_graph, edge2.Neighbour, edge1.Neighbour,
vertex, false, targetWeights[k]);
//_graph.AddOrUpdateEdge(edge2.Neighbour, edge1.Neighbour,
// targetWeights[k], false, vertex);
}
else if (!backwardWitnesses[k])
{ // add forward edge.
_weightHandler.AddOrUpdateEdge(_graph, edge1.Neighbour, edge2.Neighbour,
vertex, false, targetWeights[k]);
//_graph.AddOrUpdateEdge(edge1.Neighbour, edge2.Neighbour,
// targetWeights[k], false, vertex);
_weightHandler.AddOrUpdateEdge(_graph, edge2.Neighbour, edge1.Neighbour,
vertex, true, targetWeights[k]);
//_graph.AddOrUpdateEdge(edge2.Neighbour, edge1.Neighbour,
// targetWeights[k], true, vertex);
}
}
}
_contractedFlags[vertex] = true;
_priorityCalculator.NotifyContracted(vertex);
}
/// <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>
/// Calculates the priority of the given vertex.
/// </summary>
public float Calculate(BitArray32 contractedFlags, Func <uint, IEnumerable <uint[]> > getRestrictions, uint vertex)
{
var removed = 0;
var added = 0;
// get and keep edges.
var edges = new List <DynamicEdge>(_graph.GetEdgeEnumerator(vertex));
// check if this vertex has a potential restrictions.
var restrictions = getRestrictions(vertex);
var hasRestrictions = restrictions != null && restrictions.Any();
// remove 'downward' edge to vertex.
var i = 0;
while (i < edges.Count)
{
var edgeEnumerator = _graph.GetEdgeEnumerator(edges[i].Neighbour);
edgeEnumerator.Reset();
while (edgeEnumerator.MoveNext())
{
if (edgeEnumerator.Neighbour == vertex)
{
removed++;
}
}
if (contractedFlags[edges[i].Neighbour])
{ // neighbour was already contracted, remove 'downward' edge and exclude it.
edgeEnumerator.MoveTo(vertex);
edgeEnumerator.Reset();
while (edgeEnumerator.MoveNext())
{
if (edgeEnumerator.Neighbour == edges[i].Neighbour)
{
removed++;
}
}
edges.RemoveAt(i);
}
else
{ // move to next edge.
i++;
}
}
// loop over all edge-pairs once.
for (var j = 1; j < edges.Count; j++)
{
var edge1 = edges[j];
bool?edge1Direction;
var edge1Weight = _weightHandler.GetEdgeWeight(edge1, out edge1Direction);
var edge1CanMoveForward = edge1Direction == null || edge1Direction.Value;
var edge1CanMoveBackward = edge1Direction == null || !edge1Direction.Value;
// figure out what witness paths to calculate.
var forwardWitnesses = new EdgePath <T> [j];
var backwardWitnesses = new EdgePath <T> [j];
var targets = new List <uint>(j);
var targetWeights = new List <T>(j);
for (var k = 0; k < j; k++)
{
var edge2 = edges[k];
bool?edge2Direction;
var edge2Weight = _weightHandler.GetEdgeWeight(edge2, out edge2Direction);
var edge2CanMoveForward = edge2Direction == null || edge2Direction.Value;
var edge2CanMoveBackward = edge2Direction == null || !edge2Direction.Value;
if (!(edge1CanMoveBackward && edge2CanMoveForward))
{
forwardWitnesses[k] = new EdgePath <T>();
}
if (!(edge1CanMoveForward && edge2CanMoveBackward))
{
backwardWitnesses[k] = new EdgePath <T>();
}
targets.Add(edge2.Neighbour);
if (hasRestrictions)
{ // weight can potentially be bigger.
targetWeights.Add(_weightHandler.Infinite);
}
else
{ // weight can max be the sum of the two edges.
targetWeights.Add(_weightHandler.Add(edge1Weight, edge2Weight));
}
}
// calculate all witness paths.
_witnessCalculator.Calculate(_graph, getRestrictions, edge1.Neighbour, targets, targetWeights,
ref forwardWitnesses, ref backwardWitnesses, Constants.NO_VERTEX);
// add contracted edges if needed.
for (var k = 0; k < j; k++)
{
var edge2 = edges[k];
var removedLocal = 0;
var addedLocal = 0;
if (forwardWitnesses[k].HasVertex(vertex) && backwardWitnesses[k].HasVertex(vertex))
{ // add bidirectional edge.
_graph.TryAddOrUpdateEdge(edge1.Neighbour, edge2.Neighbour,
_weightHandler.GetMetric(targetWeights[k]), null, vertex, out addedLocal, out removedLocal);
added += addedLocal;
removed += removedLocal;
_graph.TryAddOrUpdateEdge(edge2.Neighbour, edge1.Neighbour,
_weightHandler.GetMetric(targetWeights[k]), null, vertex, out addedLocal, out removedLocal);
added += addedLocal;
removed += removedLocal;
}
else if (forwardWitnesses[k].HasVertex(vertex))
{ // add forward edge.
_graph.TryAddOrUpdateEdge(edge1.Neighbour, edge2.Neighbour,
_weightHandler.GetMetric(targetWeights[k]), true, vertex, out addedLocal, out removedLocal);
added += addedLocal;
removed += removedLocal;
_graph.TryAddOrUpdateEdge(edge2.Neighbour, edge1.Neighbour,
_weightHandler.GetMetric(targetWeights[k]), false, vertex, out addedLocal, out removedLocal);
added += addedLocal;
removed += removedLocal;
}
else if (backwardWitnesses[k].HasVertex(vertex))
{ // add forward edge.
_graph.TryAddOrUpdateEdge(edge1.Neighbour, edge2.Neighbour,
_weightHandler.GetMetric(targetWeights[k]), false, vertex, out addedLocal, out removedLocal);
added += addedLocal;
removed += removedLocal;
_graph.TryAddOrUpdateEdge(edge2.Neighbour, edge1.Neighbour,
_weightHandler.GetMetric(targetWeights[k]), true, vertex, out addedLocal, out removedLocal);
added += addedLocal;
removed += removedLocal;
}
}
}
var contracted = 0;
_contractionCount.TryGetValue(vertex, out contracted);
var depth = 0;
_depth.TryGetValue(vertex, out depth);
return(this.DifferenceFactor * (added - removed) + (this.DepthFactor * depth) +
(this.ContractedFactor * contracted));
}
