/// <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>
/// 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();
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>
/// Search backward from one vertex.
/// </summary>
/// <returns></returns>
private void SearchBackward(DirectedDynamicGraph.EdgeEnumerator edgeEnumerator, EdgePath <T> current, IEnumerable <uint[]> restrictions)
{
if (current != null)
{ // there is a next vertex found.
// 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)
{ // 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.
neighbourSequence.Reverse();
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 edgePath = null;
if (!_backwardVisits.TryGetValue(current.Vertex, out edgePath) ||
!edgePath.HasPath(routeToNeighbour))
{ // this vertex has not been visited in this way before.
_backwardQueue.Push(routeToNeighbour, _weightHandler.GetMetric(routeToNeighbour.Weight));
}
}
}
}
}