/// <summary>
/// Matches the given GPX track to the map
/// </summary>
/// <param name="gpx">The GPS track log</param>
/// <returns>List of the CandidatePoints that match GPS log the best</returns>
public IList <CandidatePoint> Match(GPXTrackSegment gpx)
{
_candidatesGraph = new CandidatesGraph();
CreateTrackCutout(gpx);
//Find candidate points + ObservationProbability
foreach (var gpxPoint in gpx.Nodes)
{
var candidates = FindCandidatePoints(gpxPoint);
_candidatesGraph.CreateLayer(gpxPoint, candidates.OrderByDescending(c => c.ObservationProbability).Take(Math.Min(candidates.Count(), TMM.MaxCandidatesCount)));
}
// Calculate transmission probability
_candidatesGraph.ConnectLayers();
AssignTransmissionProbability();
//Evaluates paths in the graph
EvaluateGraph();
//Extract result
List <CandidatePoint> result = new List <CandidatePoint>();
CandidatePoint current = _candidatesGraph.Layers[_candidatesGraph.Layers.Count - 1].Candidates.OrderByDescending(c => c.HighestProbability).FirstOrDefault();
while (current != null)
{
result.Add(current);
current = current.HighesScoreParent;
}
result.Reverse();
return(result);
}
/// <summary>
/// Creates connection between two points
/// </summary>
/// <param name="from">The point, where the connections starts</param>
/// <param name="to">The point, where the connection ends</param>
void ConnectPoints(CandidatePoint from, CandidatePoint to)
{
CandidatesConnection c = new CandidatesConnection()
{
From = from, To = to
};
from.OutgoingConnections.Add(c);
to.IncomingConnections.Add(c);
}
/// <summary>
/// Removes temporary objects after search
/// </summary>
/// <param name="to">The destination point</param>
void Finalize(CandidatePoint to)
{
foreach (var connection in _temporaryConnections)
{
connection.From.Connections.Remove(connection);
}
// Remove temporary connections
//foreach (var targetConnections in to.Road.Connections) {
// var connection = targetConnections.From.Connections.Where(c => c.To.MapPoint == to.MapPoint).Single();
// targetConnections.From.Connections.Remove(connection);
//}
}
/// <summary>
/// Finds shortest path between two points along routes
/// </summary>
/// <param name="from">Start point</param>
/// <param name="to">Destination point</param>
/// <returns>length of the path in meters</returns>
double FindShortestPath(CandidatePoint from, CandidatePoint to)
{
if (from.Road == to.Road)
{
return(Calculations.GetPathLength(from.MapPoint, to.MapPoint, from.Road));
}
else
{
double length = double.PositiveInfinity;
_pathfinder.FindPath(from, to, ref length);
return(length);
}
}
/// <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),
Id = connection.Id
};
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>
/// 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, Id = lastPathPart.Id
});
lastPathPart = _close[lastPathPart.PreviousNode];
}
result.Add(new PathSegment()
{
From = new Node(from.MapPoint), To = lastPathPart.End, Road = lastPathPart.PathFromPrevious, Id = lastPathPart.Id
});
result.Reverse();
return(result);
}
/// <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);
p.Id = link.Id;
}
}
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;
_close[link.To].Id = link.Id;
}
}
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, Id = link.Id
};
_open.Add(expanded);
}
}
}
Finalize(to);
return(null);
}
