| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using Itinero.MapMatching.Model; |
| | | 4 | | |
| | | 5 | | namespace Itinero.MapMatching.Solver; |
| | | 6 | | |
| | | 7 | | public class ModelSolver |
| | | 8 | | { |
| | | 9 | | public IEnumerable<int> Solve(GraphModel model) |
| | 29 | 10 | | { |
| | 29 | 11 | | var binaryHeap = new BinaryHeap<uint>(); |
| | 29 | 12 | | if (model.FirstNode == null) throw new Exception("Model is empty"); |
| | 29 | 13 | | if (model.LastNode == null) throw new Exception("Model is not empty, should have a last node"); |
| | | 14 | | |
| | 29 | 15 | | var path = new List<int>(); |
| | 29 | 16 | | var pathTree = new PathTree(); |
| | 29 | 17 | | var pointer = pathTree.Add((uint)model.FirstNode.Value, uint.MaxValue); |
| | 29 | 18 | | binaryHeap.Push(pointer, 0); |
| | | 19 | | |
| | 29 | 20 | | var settled = new HashSet<int>(); |
| | 10169 | 21 | | while (true) |
| | 10169 | 22 | | { |
| | 10169 | 23 | | pointer = binaryHeap.Pop(out var priority); |
| | 10169 | 24 | | pathTree.Get(pointer, out var node, out var previous); |
| | 10169 | 25 | | var popSuccess = !settled.Contains((int)node); |
| | 95508 | 26 | | while (binaryHeap.Count > 0 && |
| | 95508 | 27 | | !popSuccess) |
| | 85339 | 28 | | { |
| | 85339 | 29 | | pointer = binaryHeap.Pop(out priority); |
| | 85339 | 30 | | pathTree.Get(pointer, out node, out previous); |
| | 85339 | 31 | | popSuccess = !settled.Contains((int)node); |
| | 85339 | 32 | | } |
| | 10169 | 33 | | if (popSuccess == false) break; |
| | 10169 | 34 | | settled.Add((int)node); |
| | | 35 | | |
| | 10169 | 36 | | if (node == model.LastNode) |
| | 29 | 37 | | { |
| | | 38 | | // TODO: compose final path. |
| | 2120 | 39 | | while (true) |
| | 2120 | 40 | | { |
| | 2120 | 41 | | path.Add((int)node); |
| | 2120 | 42 | | if (node == model.FirstNode) |
| | 29 | 43 | | { |
| | 29 | 44 | | path.Reverse(); |
| | 29 | 45 | | break; |
| | | 46 | | } |
| | | 47 | | |
| | 2091 | 48 | | pointer = previous; |
| | 2091 | 49 | | pathTree.Get(pointer, out node, out previous); |
| | 2091 | 50 | | } |
| | 29 | 51 | | break; |
| | | 52 | | } |
| | | 53 | | |
| | | 54 | | // add neighbours. |
| | 10140 | 55 | | var graphNode = model.GetNode((int)node); |
| | 10140 | 56 | | var neighbours = model.GetNeighbours((int)node); |
| | 259344 | 57 | | foreach (var neighbour in neighbours) |
| | 114462 | 58 | | { |
| | 114462 | 59 | | var neighbourPointer = pathTree.Add((uint)neighbour.Node2, pointer); |
| | 114462 | 60 | | binaryHeap.Push(neighbourPointer, priority + graphNode.Cost + neighbour.Cost); |
| | 114462 | 61 | | } |
| | 10140 | 62 | | } |
| | | 63 | | |
| | 29 | 64 | | return path; |
| | 29 | 65 | | } |
| | | 66 | | } |