public List <MapLink> BuildRouteToNearestType(MapPoint start, EntityTypes entityType) { List <MapLink> result = new List <MapLink>(); mapNode beginning = null; mapNode ending = null; PlottedMapPoint pStart = (PlottedMapPoint)_mappedPoints[start.Id]; if ((pStart.EntityTypes & entityType) == entityType) { return(result); } Dictionary <int, mapNode> nodes = new Dictionary <int, mapNode>(); beginning = new mapNode(pStart, 0); nodes.Add(pStart.MapPoint.Id, beginning); foreach (DictionaryEntry entry in _mappedPoints) { if (nodes.ContainsKey(((PlottedMapPoint)entry.Value).MapPoint.Id)) { continue; } mapNode node = new mapNode((PlottedMapPoint)entry.Value, double.MaxValue); nodes.Add(node.MapPoint.MapPoint.Id, node); } SortableList openList = new SortableList(new MapNodeComparer()); openList.KeepSorted = true; openList.AddDuplicates = true; openList.Add(beginning); Dictionary <int, mapNode> closedList = new Dictionary <int, mapNode>(); bool found = false; while ((!found) && (openList.Count > 0)) { mapNode current = (mapNode)openList[0]; if ((current.MapPoint.EntityTypes & entityType) == entityType) { found = true; ending = current; } else { closedList.Add(current.MapPoint.MapPoint.Id, current); openList.Remove(current); foreach (MapLink link in current.MapPoint.Links) { PlottedMapPoint end = (PlottedMapPoint)_mappedPoints[link.End.Id]; mapNode nEnd = nodes[link.End.Id]; double distance = 0; if (link.LinkType == LinkType.Teleport) { distance = 2000 + current.distance; } else { distance = current.Location.Distance(end.Location) + current.distance; } if (distance < nEnd.distance) { nEnd.distance = distance; nEnd.Parent = current; nEnd.link = link; if ((!closedList.ContainsKey(end.MapPoint.Id)) && (!_sortedListContains(openList, nEnd))) { openList.Add(nEnd); } } } } } if (found) { mapNode node = ending; while (node != beginning) { result.Insert(0, node.link); node = node.Parent; } } return(result); }