astar/Astar.cs

@@ -8,9 +8,9 @@ namespace astar
 {
     public class Astar(ValueTuple<float, float, float, float>? priorityWeights = null, int? explorationMultiplier = null, float? nonPriorityRoadSpeedPenalty = null)
     {
-        private readonly ValueTuple<float, float, float, float> DefaultPriorityWeights = priorityWeights ?? new(.6f, 1, 0, 0);
+        private readonly ValueTuple<float, float, float, float> DefaultPriorityWeights = priorityWeights ?? new(0.7f, 1.08f, 0, 0);
-        private readonly int _explorationMultiplier = explorationMultiplier ?? 1500;
+        private readonly int _explorationMultiplier = explorationMultiplier ?? 150;
-        private readonly float _nonPriorityRoadSpeedPenalty = nonPriorityRoadSpeedPenalty ?? 0.75f;
+        private readonly float _nonPriorityRoadSpeedPenalty = nonPriorityRoadSpeedPenalty ?? 0.9f;
         
         public Route FindPath(float startLat, float startLon, float endLat, float endLon, float regionSize, bool car = true, PathMeasure pathing = PathMeasure.Distance, string? importFolderPath = null, ILogger? logger = null)
         {
@@ -43,18 +43,22 @@ namespace astar
             ValueTuple<Node, Node>? meetingEnds = null;
             while (toVisitStart.Count > 0 && toVisitEnd.Count > 0)
             {
-                for (int i = 0; i < Math.Min(toVisitStart.Count * 0.5, 50) && meetingEnds is null; i++)
+                ulong closestEndNodeId = toVisitEnd.UnorderedItems
+                    .MinBy(n => graph.Nodes[n.Element].DistanceTo(startNode.Value)).Element;
+                Node closestEndNode = graph.Nodes[closestEndNodeId];
+                if (toVisitStart.Count >= toVisitEnd.Count && meetingEnds is null)
                 {
-                    ulong closestEndNodeId = toVisitEnd.UnorderedItems.MinBy(node => graph.Nodes[node.Element].DistanceTo(graph.Nodes[toVisitStart.Peek()])).Element;
+                    for(int i = 0; i < Math.Min(toVisitStart.Count * 0.5, 50) && meetingEnds is null; i++)
-                    Node closestEndNode = graph.Nodes[closestEndNodeId];
+                        meetingEnds = ExploreSide(true, graph, toVisitStart, rl, priorityHelper, closestEndNode, car, DefaultPriorityWeights, pathing, logger);
-                    meetingEnds = ExploreSide(true, graph, toVisitStart, rl, priorityHelper, closestEndNode, car, DefaultPriorityWeights, pathing, logger);
                 }
                 
-                for (int i = 0; i < Math.Min(toVisitEnd.Count * 0.5, 50) && meetingEnds is null; i++)
+                ulong closestStartNodeId = toVisitStart.UnorderedItems
+                    .MinBy(n => graph.Nodes[n.Element].DistanceTo(endNode.Value)).Element;
+                Node closestStartNode = graph.Nodes[closestStartNodeId];
+                if (toVisitEnd.Count >= toVisitStart.Count && meetingEnds is null)
                 {
-                    ulong closestStartNodeId = toVisitStart.UnorderedItems.MinBy(node => graph.Nodes[node.Element].DistanceTo(graph.Nodes[toVisitEnd.Peek()])).Element;
+                    for(int i = 0; i < Math.Min(toVisitEnd.Count * 0.5, 50) && meetingEnds is null; i++)
-                    Node closestStartNode = graph.Nodes[closestStartNodeId];
+                        meetingEnds = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, closestStartNode, car, DefaultPriorityWeights, pathing, logger);
-                    meetingEnds = ExploreSide(false, graph, toVisitEnd, rl, priorityHelper, closestStartNode, car, DefaultPriorityWeights, pathing, logger);
                 }
 
                 if (meetingEnds is not null)
@@ -94,7 +98,8 @@ namespace astar
                 
                 Node neighborNode = graph.Nodes[neighborId];
 
-                if (neighborNode.PreviousIsFromStart == !fromStart) //Check if we found the opposite End
+                if (neighborNode.PreviousIsFromStart is not null &&
+                    neighborNode.PreviousIsFromStart != fromStart) //Check if we found the opposite End
                     return fromStart ? new(currentNode, neighborNode) : new(neighborNode, currentNode);
 
                 float metric = (currentNode.Metric ?? float.MaxValue) + (pathing is PathMeasure.Distance
@@ -108,6 +113,7 @@ namespace astar
                     toVisit.Enqueue(neighborId,
                         priorityHelper.CalculatePriority(currentNode, neighborNode, goalNode, speed, ratingWeights));
                 }
+                logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
             }
 
             return null;