/// <summary>
/// Builds result path
/// </summary>
/// <param name="lastPathPart"></param>
/// <param name="from"></param>
/// <returns></returns>
IList <PathSegment> BuildPath(PartialPath lastPathPart, CandidatePoint from)
{
List <PathSegment> result = new List <PathSegment>();
while (lastPathPart.PreviousNode != null)
{
result.Add(new PathSegment()
{
From = lastPathPart.PreviousNode, To = lastPathPart.End, Road = lastPathPart.PathFromPrevious
});
lastPathPart = _close[lastPathPart.PreviousNode];
}
result.Add(new PathSegment()
{
From = new Node(from.MapPoint), To = lastPathPart.End, Road = lastPathPart.PathFromPrevious
});
result.Reverse();
return(result);
}
/// <summary>
/// Initializes internal properties before search
/// </summary>
/// <param name="from">The start point</param>
/// <param name="to">The destination point</param>
void Initialize(CandidatePoint from, CandidatePoint to)
{
_open.Clear();
_close.Clear();
// Add nodes reachable from the From point to the open list
foreach (var connection in from.Road.Connections)
{
PartialPath path = new PartialPath()
{
End = connection.To, PathFromPrevious = connection.Geometry,
Length = Calculations.GetPathLength(from.MapPoint, connection.To.MapPoint, connection.Geometry),
EstimationToEnd = Calculations.GetDistance2D(connection.To.MapPoint, to.MapPoint)
};
if (_open.Contains(path))
{
if (_open[path.End].Length > path.Length)
{
_open.Remove(_open[path.End]);
_open.Add(path);
}
}
else
{
_open.Add(path);
}
}
_temporaryConnections.Clear();
// Add temporary connections to the TO point
foreach (var targetConnections in to.Road.Connections)
{
Connection destinationConnection = new Connection(targetConnections.From, new Node(to.MapPoint))
{
Geometry = to.Road
};
_temporaryConnections.Add(destinationConnection);
}
}
/// <summary>
/// Finds path between From and To points
/// </summary>
/// <param name="from">The start point</param>
/// <param name="to">The destination point</param>
/// <param name="length">Length of the path in meters</param>
/// <returns>Path as list of PathSegments beteewn two point. If path wasn't found returns null.</returns>
public IList <PathSegment> FindPath(CandidatePoint from, CandidatePoint to, ref double length)
{
Initialize(from, to);
while (_open.Count > 0)
{
PartialPath current = _open.RemoveTop();
_close.Add(current.End, current);
// Path found
if (current.End.MapPoint == to.MapPoint)
{
length = current.Length;
var result = BuildPath(current, from);
Finalize(to);
return(result);
}
foreach (var link in current.End.Connections)
{
if (link.From != current.End)
{
continue;
}
double distance;
if (link.To.MapPoint == to.MapPoint)
{
distance = current.Length + Calculations.GetPathLength(current.End.MapPoint, to.MapPoint, link.Geometry);
}
else
{
distance = current.Length + link.Geometry.Length;
}
if (_open.Contains(link.To))
{
// Update previously found path in the open list (if this one is shorter)
PartialPath p = _open[link.To];
if (p.Length > distance)
{
p.PreviousNode = current.End;
p.PathFromPrevious = link.Geometry;
_open.Update(p, distance);
}
}
else if (_close.ContainsKey(link.To))
{
// Update previously found path in the close list (if this one is shorter)
if (_close[link.To].Length > distance)
{
_close[link.To].Length = distance;
_close[link.To].End = current.End;
_close[link.To].PathFromPrevious = link.Geometry;
}
}
else
{
// Expand path to new node
PartialPath expanded = new PartialPath()
{
Length = distance,
EstimationToEnd = Calculations.GetDistance2D(link.To.MapPoint, to.MapPoint),
End = link.To, PreviousNode = current.End, PathFromPrevious = link.Geometry
};
_open.Add(expanded);
}
}
}
Finalize(to);
return(null);
}