/// <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>
/// 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>
/// 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>
/// 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>
/// 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>
/// 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 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()
{
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>
/// 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));
}
/// <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.
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>
/// Remove all witnessed edges.
/// </summary>
private void RemoveWitnessedEdges()
{
_logger.Log(TraceEventType.Information, "Removing witnessed edges...");
var witnessEdgeEnumerator = _witnessGraph.GetEdgeEnumerator();
var edgeEnumerator = _graph.GetEdgeEnumerator();
var edges = new List <MetaEdge>();
for (uint vertex = 0; vertex < _graph.VertexCount; vertex++)
{
// collect all relevant edges.
edges.Clear();
if (witnessEdgeEnumerator.MoveTo(vertex) &&
edgeEnumerator.MoveTo(vertex))
{
while (edgeEnumerator.MoveNext())
{
if (vertex < edgeEnumerator.Neighbour)
{ // only check in on directions, all edges are added twice initially.
edges.Add(edgeEnumerator.Current);
}
}
}
// check witness paths.
for (var i = 0; i < edges.Count; i++)
{
var edge = edges[0];
while (witnessEdgeEnumerator.MoveNext())
{
if (witnessEdgeEnumerator.Neighbour == edge.Neighbour)
{ // this edge is witnessed, figure out how.
var forwardWitnessWeight = DirectedGraphExtensions.FromData(witnessEdgeEnumerator.Data0);
var backwardWitnessWeight = DirectedGraphExtensions.FromData(witnessEdgeEnumerator.Data1);
var weightAndDir = _weightHandler.GetEdgeWeight(edge);
var weight = _weightHandler.GetMetric(weightAndDir.Weight);
var witnessed = false;
if (weightAndDir.Direction.F &&
weight > forwardWitnessWeight)
{ // witnessed in forward direction.
weightAndDir.Direction = new Dir(false, weightAndDir.Direction.B);
witnessed = true;
}
if (weightAndDir.Direction.B &&
weight > backwardWitnessWeight)
{ // witnessed in backward direction.
weightAndDir.Direction = new Dir(weightAndDir.Direction.F, false);
witnessed = true;
}
if (witnessed)
{ // edge was witnessed, do something.
// remove the edge (in both directions)
_graph.RemoveEdge(vertex, edge.Neighbour);
_graph.RemoveEdge(edge.Neighbour, vertex);
if (weightAndDir.Direction.B || weightAndDir.Direction.F)
{ // add it again if there is something relevant still left.
_weightHandler.AddEdge(_graph, vertex, edge.Neighbour, Constants.NO_VERTEX,
weightAndDir.Direction.AsNullableBool(), weightAndDir.Weight);
weightAndDir.Direction.Reverse();
_weightHandler.AddEdge(_graph, edge.Neighbour, vertex, Constants.NO_VERTEX,
weightAndDir.Direction.AsNullableBool(), weightAndDir.Weight);
}
}
}
}
}
}
}
/// <summary>
/// Remove all witnessed edges.
/// </summary>
private void RemoveWitnessedEdges()
{
_logger.Log(TraceEventType.Information, "Removing witnessed edges...");
var edges = new List <MetaEdge>();
var weights = new List <T>();
var metrics = new List <float>();
var targets = new List <uint>();
for (uint vertex = 0; vertex < _graph.VertexCount; vertex++)
{
edges.Clear();
weights.Clear();
metrics.Clear();
targets.Clear();
edges.AddRange(_graph.GetEdgeEnumerator(vertex));
var forwardWitnesses = new bool[edges.Count];
var backwardWitnesses = new bool[edges.Count];
for (var i = 0; i < edges.Count; i++)
{
var edge = edges[i];
bool?edgeDirection;
var edgeWeight = _weightHandler.GetEdgeWeight(edge, out edgeDirection);
var edgeCanMoveForward = edgeDirection == null || edgeDirection.Value;
var edgeCanMoveBackward = edgeDirection == null || !edgeDirection.Value;
forwardWitnesses[i] = !edgeCanMoveForward;
backwardWitnesses[i] = !edgeCanMoveBackward;
weights.Add(edgeWeight);
metrics.Add(_weightHandler.GetMetric(edgeWeight));
targets.Add(edge.Neighbour);
}
// calculate all witness paths.
_witnessCalculator.Calculate(_graph.Graph, vertex, targets, metrics,
ref forwardWitnesses, ref backwardWitnesses, uint.MaxValue);
// check witness paths.
for (var i = 0; i < edges.Count; i++)
{
if (forwardWitnesses[i] && backwardWitnesses[i])
{ // in both directions the edge does not represent the shortest path.
_graph.RemoveEdge(vertex, targets[i]);
}
else if (forwardWitnesses[i])
{ // only in forward direction is this edge useless.
_graph.RemoveEdge(vertex, targets[i]);
_weightHandler.AddEdge(_graph, vertex, targets[i], Constants.NO_VERTEX, false, weights[i]);
//_graph.AddEdge(vertex, targets[i], weights[i], false, Constants.NO_VERTEX);
}
else if (backwardWitnesses[i])
{ // only in backward direction is this edge useless.
_graph.RemoveEdge(vertex, targets[i]);
_weightHandler.AddEdge(_graph, vertex, targets[i], Constants.NO_VERTEX, true, weights[i]);
}
}
}
}
