Working. Weight calculation is still wonky, as well as heuristic needing tuning.
This commit is contained in:
parent
2131ac4afe
commit
308579279b
@ -1,4 +1,3 @@
|
||||
using System.Text;
|
||||
using System.Text.Json.Serialization;
|
||||
using OSMDatastructure;
|
||||
using OSMDatastructure.Graph;
|
||||
@ -15,22 +14,37 @@ builder.Services.AddSwaggerGen();
|
||||
|
||||
var app = builder.Build();
|
||||
|
||||
app.MapGet("/getRouteDistance", (float latStart, float lonStart, float latEnd, float lonEnd) =>
|
||||
app.MapGet("/getRouteBeta", (float latStart, float lonStart, float latEnd, float lonEnd, Tag.SpeedType vehicle, double stayOnSameRoadPriority, double useHigherLevelRoadsPriority, double useRoadsWithLessJunctionsPriority) =>
|
||||
{
|
||||
DateTime startCalc = DateTime.Now;
|
||||
List<PathNode> result = Pathfinder.AStarDistance("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), new Coordinates(latEnd, lonEnd));
|
||||
List<PathNode> result = Pathfinder.AStar("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart),
|
||||
new Coordinates(latEnd, lonEnd), vehicle, useHigherLevelRoadsPriority, stayOnSameRoadPriority,
|
||||
useRoadsWithLessJunctionsPriority);
|
||||
PathResult pathResult = new PathResult(DateTime.Now - startCalc, result);
|
||||
return RenderPath.Renderer.DrawFromPath(result);
|
||||
}
|
||||
);
|
||||
|
||||
app.MapGet("/getRoute", (float latStart, float lonStart, float latEnd, float lonEnd, Tag.SpeedType vehicle, double stayOnSameRoadPriority, double useHigherLevelRoadsPriority, double useRoadsWithLessJunctionsPriority) =>
|
||||
{
|
||||
DateTime startCalc = DateTime.Now;
|
||||
List<PathNode> result = Pathfinder.AStar("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart),
|
||||
new Coordinates(latEnd, lonEnd), vehicle, useHigherLevelRoadsPriority, stayOnSameRoadPriority,
|
||||
useRoadsWithLessJunctionsPriority);
|
||||
PathResult pathResult = new PathResult(DateTime.Now - startCalc, result);
|
||||
return pathResult;
|
||||
}
|
||||
);
|
||||
|
||||
app.MapGet("/getRouteTime", (float latStart, float lonStart, float latEnd, float lonEnd, Tag.SpeedType vehicle) =>
|
||||
app.MapGet("/getShortestRoute", (float latStart, float lonStart, float latEnd, float lonEnd) =>
|
||||
{
|
||||
DateTime startCalc = DateTime.Now;
|
||||
List<PathNode> result = Pathfinder.AStarTime("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), new Coordinates(latEnd, lonEnd), vehicle);
|
||||
List<PathNode> result = Pathfinder.AStar("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart),
|
||||
new Coordinates(latEnd, lonEnd), Tag.SpeedType.any, 0, 0,
|
||||
0);
|
||||
PathResult pathResult = new PathResult(DateTime.Now - startCalc, result);
|
||||
return pathResult;
|
||||
}
|
||||
}
|
||||
);
|
||||
|
||||
app.MapGet("/getClosestNode", (float lat, float lon) =>
|
||||
@ -57,18 +71,11 @@ app.Run();
|
||||
internal class PathResult
|
||||
{
|
||||
[JsonInclude]public TimeSpan calcTime;
|
||||
[JsonInclude] public double pathWeight = double.MaxValue;
|
||||
[JsonInclude] public double pathTravelDistance = double.MaxValue;
|
||||
[JsonInclude]public List<PathNode> pathNodes;
|
||||
|
||||
public PathResult(TimeSpan calcTime, List<PathNode> pathNodes)
|
||||
{
|
||||
this.calcTime = calcTime;
|
||||
this.pathNodes = pathNodes;
|
||||
if (pathNodes.Count > 0)
|
||||
{
|
||||
this.pathWeight = pathNodes.Last().currentPathWeight;
|
||||
this.pathTravelDistance = pathNodes.First().currentPathLength;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,4 +1,3 @@
|
||||
using System.Runtime.Serialization;
|
||||
using System.Text.Json.Serialization;
|
||||
|
||||
namespace OSMDatastructure.Graph;
|
||||
@ -8,21 +7,8 @@ public class OsmNode
|
||||
{
|
||||
public ulong nodeId { get; }
|
||||
public HashSet<OsmEdge> edges { get; set; }
|
||||
public Coordinates coordinates { get; }
|
||||
|
||||
[JsonIgnore][NonSerialized]public OsmNode? previousPathNode = null;
|
||||
[JsonIgnore][NonSerialized]public double currentPathWeight = double.MaxValue;
|
||||
[JsonIgnore][NonSerialized]public double currentPathLength = double.MaxValue;
|
||||
[JsonIgnore][NonSerialized]public double directDistanceToGoal = double.MaxValue;
|
||||
public Coordinates coordinates { get; }
|
||||
|
||||
[OnDeserialized]
|
||||
internal void SetDefaultValues(StreamingContext context)
|
||||
{
|
||||
currentPathWeight = double.MaxValue;
|
||||
currentPathLength = double.MaxValue;
|
||||
directDistanceToGoal = double.MaxValue;
|
||||
}
|
||||
|
||||
public OsmNode(ulong nodeId, float lat, float lon)
|
||||
{
|
||||
this.nodeId = nodeId;
|
||||
@ -53,9 +39,6 @@ public class OsmNode
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
if(previousPathNode is not null)
|
||||
return $"{nodeId} {coordinates} ec:{edges.Count} d:{directDistanceToGoal} w:{currentPathWeight} l:{currentPathLength} p:{previousPathNode.nodeId}";
|
||||
return
|
||||
$"{nodeId} {coordinates} ec:{edges.Count} d:{directDistanceToGoal} w:{currentPathWeight} l:{currentPathLength} null";
|
||||
return $"{nodeId} {coordinates} ec:{edges.Count}";
|
||||
}
|
||||
}
|
||||
@ -6,12 +6,6 @@ namespace Pathfinding;
|
||||
|
||||
public class PathNode : OsmNode
|
||||
{
|
||||
[JsonInclude]public new double directDistanceToGoal = double.MaxValue;
|
||||
[JsonInclude]public double directDistanceDelta = double.MaxValue;
|
||||
[JsonInclude]public new double currentPathLength = double.MaxValue;
|
||||
[JsonInclude]public double pathDistanceDelta = double.MaxValue;
|
||||
[JsonInclude]public new double currentPathWeight = double.MaxValue;
|
||||
[JsonInclude]public double pathWeightDelta = double.MaxValue;
|
||||
[JsonInclude]public Dictionary<string, string> tags = new();
|
||||
|
||||
public PathNode(ulong nodeId, float lat, float lon) : base(nodeId, lat, lon)
|
||||
@ -22,19 +16,11 @@ public class PathNode : OsmNode
|
||||
{
|
||||
}
|
||||
|
||||
public static PathNode? FromOsmNode(OsmNode? node, HashSet<Tag>? tags, double pathDistanceDelta, double pathWeightDelta, double directDistanceDelta)
|
||||
public static PathNode? FromOsmNode(OsmNode? node, HashSet<Tag>? tags)
|
||||
{
|
||||
if (node is null)
|
||||
return null;
|
||||
PathNode retNode = new(node.nodeId, node.coordinates)
|
||||
{
|
||||
currentPathLength = node.currentPathLength,
|
||||
currentPathWeight = double.IsPositiveInfinity(node.currentPathWeight) ? double.MaxValue : node.currentPathWeight,
|
||||
directDistanceToGoal = node.directDistanceToGoal,
|
||||
directDistanceDelta = directDistanceDelta,
|
||||
pathDistanceDelta = pathDistanceDelta,
|
||||
pathWeightDelta = pathWeightDelta
|
||||
};
|
||||
PathNode retNode = new(node.nodeId, node.coordinates);
|
||||
if (tags != null)
|
||||
foreach (Tag tag in tags)
|
||||
{
|
||||
|
||||
@ -5,93 +5,124 @@ using WayType = OSMDatastructure.Tag.WayType;
|
||||
|
||||
namespace Pathfinding;
|
||||
|
||||
public static partial class Pathfinder
|
||||
public static class Pathfinder
|
||||
{
|
||||
|
||||
private static ValueTuple<OsmNode?, OsmNode?> SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager, SpeedType vehicle)
|
||||
|
||||
public static List<PathNode> AStar(string workingDir, Coordinates startCoordinates, Coordinates goalCoordinates,
|
||||
SpeedType vehicle, double heuristicRoadLevelPriority, double heuristicSameRoadPriority,
|
||||
double heuristicFewJunctionsPriority)
|
||||
{
|
||||
ValueTuple<OsmNode?, OsmNode?> retTuple = new();
|
||||
retTuple.Item1 = regionManager.ClosestNodeToCoordinates(startCoordinates, vehicle);
|
||||
retTuple.Item2 = regionManager.ClosestNodeToCoordinates(goalCoordinates, vehicle);
|
||||
if (retTuple.Item1 is null || retTuple.Item2 is null)
|
||||
return retTuple;
|
||||
retTuple.Item1.currentPathWeight = 0;
|
||||
retTuple.Item1.currentPathLength = 0;
|
||||
retTuple.Item1.directDistanceToGoal = Utils.DistanceBetween(retTuple.Item1, retTuple.Item2);
|
||||
return retTuple;
|
||||
}
|
||||
RegionManager regionManager = new RegionManager(workingDir);
|
||||
OsmNode? startNode = regionManager.ClosestNodeToCoordinates(startCoordinates, vehicle);
|
||||
OsmNode? goalNode = regionManager.ClosestNodeToCoordinates(goalCoordinates, vehicle);
|
||||
if (startNode is null || goalNode is null)
|
||||
return new List<PathNode>();
|
||||
|
||||
private static double EdgeWeight(OsmNode node1, OsmEdge edge, SpeedType vehicle, RegionManager regionManager)
|
||||
{
|
||||
OsmNode? node2 = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
||||
if (node2 is null)
|
||||
return double.MaxValue;
|
||||
double distance = Utils.DistanceBetween(node1, node2);
|
||||
byte speed = regionManager.GetSpeedForEdge(node1, edge.wayId, vehicle);
|
||||
return speed is 0 ? double.MaxValue : distance / speed;
|
||||
}
|
||||
PriorityQueue<OsmNode, double> openSetfScore = new();
|
||||
openSetfScore.Enqueue(startNode, 0);
|
||||
Dictionary<OsmNode, OsmNode> cameFromDict = new();
|
||||
Dictionary<OsmNode, double> gScore = new();
|
||||
gScore.Add(startNode, 0);
|
||||
|
||||
private static double GetPriority(OsmNode currentNode, OsmNode? previousNode, OsmEdge currentEdge, SpeedType vehicle, RegionManager regionManager)
|
||||
{
|
||||
if (vehicle == SpeedType.any)
|
||||
return 1;
|
||||
const double roadPriorityFactor = 3.5;
|
||||
const double roadSpeedFactor = 1.4;
|
||||
const double roadNameChangeFactor = 1.4;
|
||||
const double distanceDeltaFactor = 0.17;
|
||||
Region r = regionManager.GetRegion(currentNode.coordinates)!;
|
||||
|
||||
double distanceDelta = 0;
|
||||
if(previousNode is not null)
|
||||
distanceDelta = (previousNode.directDistanceToGoal - currentNode.directDistanceToGoal) * distanceDeltaFactor;
|
||||
double roadPriority = GetPriorityVehicleRoad(currentEdge, vehicle, r) * roadPriorityFactor;
|
||||
double roadSpeed = regionManager.GetSpeedForEdge(currentNode, currentEdge.wayId, vehicle) * 0.1 * roadSpeedFactor;
|
||||
|
||||
if (vehicle == SpeedType.pedestrian)
|
||||
return currentNode.directDistanceToGoal / (roadPriority + roadSpeed + distanceDelta);
|
||||
|
||||
double wayChange = 0;
|
||||
if (previousNode is not null && previousNode.edges.Count > 0)
|
||||
while (openSetfScore.Count > 0)
|
||||
{
|
||||
OsmEdge? pEdge = previousNode.edges.FirstOrDefault(e => e.neighborId.Equals(currentNode.nodeId));
|
||||
if (pEdge is not null)
|
||||
OsmNode currentNode = openSetfScore.Dequeue();
|
||||
if (currentNode.Equals(goalNode))
|
||||
return GetPath(cameFromDict, goalNode, regionManager);
|
||||
|
||||
foreach (OsmEdge edge in currentNode.edges)
|
||||
{
|
||||
TagManager? prevTags = regionManager.GetRegion(previousNode.coordinates)?.tagManager;
|
||||
if (prevTags is not null)
|
||||
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
||||
if (neighbor is not null)
|
||||
{
|
||||
TagManager curTags = r.tagManager;
|
||||
|
||||
bool sameName = false;
|
||||
string? curName = (string?)curTags.GetTag(currentEdge.wayId, TagType.name);
|
||||
if (curName is not null && (string?)prevTags.GetTag(pEdge.wayId, TagType.name) == curName)
|
||||
sameName = true;
|
||||
double tentativeGScore = gScore[currentNode] + Weight(currentNode, neighbor, edge, vehicle, regionManager);
|
||||
gScore.TryAdd(neighbor, double.MaxValue);
|
||||
if (tentativeGScore < gScore[neighbor])
|
||||
{
|
||||
if (cameFromDict.ContainsKey(neighbor))
|
||||
cameFromDict[neighbor] = currentNode;
|
||||
else
|
||||
cameFromDict.Add(neighbor, currentNode);
|
||||
if (gScore.ContainsKey(neighbor))
|
||||
gScore[neighbor] = tentativeGScore;
|
||||
else
|
||||
gScore.Add(neighbor, tentativeGScore);
|
||||
double h = Heuristic(currentNode, neighbor, goalNode, edge, vehicle, regionManager,
|
||||
heuristicRoadLevelPriority, heuristicFewJunctionsPriority, heuristicSameRoadPriority);
|
||||
//Console.WriteLine($"Queue: {openSetfScore.Count:00000} Current Distance: {Utils.DistanceBetween(currentNode, goalNode):000000.00} Visited: {cameFromDict.Count:00000} Current heuristic: {h:00000.00}");
|
||||
openSetfScore.Enqueue(neighbor, tentativeGScore + h);
|
||||
|
||||
bool sameRef = false;
|
||||
string? curRef = (string?)curTags.GetTag(currentEdge.wayId, TagType.tagref);
|
||||
if (curRef is not null && (string?)prevTags.GetTag(pEdge.wayId, TagType.tagref) == curRef)
|
||||
sameRef = true;
|
||||
|
||||
wayChange = (sameRef || sameName ? 1 : 0) * roadNameChangeFactor;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
double div = (roadPriority + wayChange + roadSpeed + (distanceDelta > 0 ? distanceDelta : 0)) + 1;
|
||||
double prio = currentNode.directDistanceToGoal / div;
|
||||
Console.WriteLine($"{currentNode.directDistanceToGoal:000000.00}/{div:+00.00;-00.00;000.00}={prio:+00000.00;-00000.00;000000.00} Type{roadPriority:00.00} name{wayChange:00.00} speed{roadSpeed:00.00} distance{distanceDelta:+00.00;-00.00;0}");
|
||||
if (vehicle == SpeedType.car)
|
||||
return prio;
|
||||
|
||||
return double.MaxValue;
|
||||
return new List<PathNode>();
|
||||
}
|
||||
|
||||
private static List<PathNode> GetPath(Dictionary<OsmNode, OsmNode> cameFromDict, OsmNode goalNode, RegionManager regionManager)
|
||||
{
|
||||
List<PathNode> path = new List<PathNode>();
|
||||
OsmNode currentNode = goalNode;
|
||||
while (cameFromDict.ContainsKey(cameFromDict[currentNode]))
|
||||
{
|
||||
OsmEdge? currentEdge = cameFromDict[currentNode].edges.First(edge => edge.neighborId == currentNode.nodeId);
|
||||
HashSet<Tag>? tags =
|
||||
regionManager.GetRegion(currentNode.coordinates)!.tagManager.GetTagsForWayId(currentEdge.wayId);
|
||||
PathNode? newNode = PathNode.FromOsmNode(currentNode, tags);
|
||||
if(newNode is not null)
|
||||
path.Add(newNode);
|
||||
currentNode = cameFromDict[currentNode];
|
||||
}
|
||||
|
||||
path.Reverse();
|
||||
|
||||
return path;
|
||||
}
|
||||
|
||||
private static double Weight(OsmNode fromNode, OsmNode neighborNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager)
|
||||
{
|
||||
double distance = Utils.DistanceBetween(fromNode, neighborNode);
|
||||
double speed = regionManager.GetSpeedForEdge(fromNode, edge.wayId, vehicle);
|
||||
double prio = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!);
|
||||
return distance / (speed + (prio * 10));
|
||||
}
|
||||
|
||||
private static double Heuristic(OsmNode fromNode, OsmNode neighborNode, OsmNode goalNode, OsmEdge edge, SpeedType vehicle, RegionManager regionManager, double roadPriorityFactor, double junctionFactor, double sameRoadFactor)
|
||||
{
|
||||
double roadPriority = GetPriorityVehicleRoad(edge, vehicle, regionManager.GetRegion(fromNode.coordinates)!) * roadPriorityFactor;
|
||||
|
||||
TagManager curTags = regionManager.GetRegion(fromNode.coordinates)!.tagManager;
|
||||
TagManager nextTags = regionManager.GetRegion(neighborNode.coordinates)!.tagManager;
|
||||
|
||||
bool sameName = false;
|
||||
string? curName = (string?)curTags.GetTag(edge.wayId, TagType.name);
|
||||
bool sameRef = false;
|
||||
string? curRef = (string?)curTags.GetTag(edge.wayId, TagType.tagref);
|
||||
if(curName is not null)
|
||||
foreach (OsmEdge pEdge in neighborNode.edges)
|
||||
{
|
||||
if ((string?)nextTags.GetTag(pEdge.wayId, TagType.name) == curName)
|
||||
sameName = true;
|
||||
if ((string?)nextTags.GetTag(pEdge.wayId, TagType.tagref) == curRef)
|
||||
sameRef = true;
|
||||
}
|
||||
double sameRoadName = (sameRef || sameName ? 1 : 0) * sameRoadFactor;
|
||||
|
||||
double junctionCount = (neighborNode.edges.Count > 2 ? 0 : 1) * junctionFactor;
|
||||
|
||||
//Console.WriteLine($"{roadPriority:000.00} {sameRoadName:000.00} {junctionCount:000.00} {distanceImprovement:+000.00;-000.00;0000.00}");
|
||||
|
||||
return Utils.DistanceBetween(neighborNode, goalNode) - (roadPriority + sameRoadName + junctionCount) * 100;
|
||||
}
|
||||
|
||||
private static double GetPriorityVehicleRoad(OsmEdge edge, SpeedType vehicle, Region region)
|
||||
{
|
||||
if (vehicle == SpeedType.any)
|
||||
return 1;
|
||||
WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, TagType.highway);
|
||||
if(wayType is null)
|
||||
return double.MaxValue;
|
||||
return 0;
|
||||
if (vehicle == SpeedType.car)
|
||||
{
|
||||
switch (wayType)
|
||||
@ -106,6 +137,7 @@ public static partial class Pathfinder
|
||||
return 8;
|
||||
case WayType.secondary:
|
||||
case WayType.secondary_link:
|
||||
return 6;
|
||||
case WayType.tertiary:
|
||||
case WayType.tertiary_link:
|
||||
return 5;
|
||||
@ -113,12 +145,10 @@ public static partial class Pathfinder
|
||||
case WayType.residential:
|
||||
case WayType.road:
|
||||
case WayType.living_street:
|
||||
return 3;
|
||||
return 2;
|
||||
case WayType.service:
|
||||
case WayType.track:
|
||||
return 0.01;
|
||||
default:
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
if (vehicle == SpeedType.pedestrian)
|
||||
@ -144,42 +174,9 @@ public static partial class Pathfinder
|
||||
case WayType.tertiary_link:
|
||||
case WayType.escape:
|
||||
return 2;
|
||||
default:
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
return 0.01;
|
||||
|
||||
}
|
||||
|
||||
private static List<PathNode> GetRouteFromCalc(OsmNode goalNode, RegionManager regionManager)
|
||||
{
|
||||
List<PathNode> path = new();
|
||||
OsmNode? currentNode = goalNode;
|
||||
while (currentNode is not null)
|
||||
{
|
||||
HashSet<Tag>? tags = null;
|
||||
double pathDistanceDelta = 0;
|
||||
double pathWeightDelta = 0;
|
||||
double directDistanceDelta = 0;
|
||||
if (currentNode.previousPathNode is not null)
|
||||
{
|
||||
OsmEdge edge = currentNode.previousPathNode!.edges.First(e => e.neighborId.Equals(currentNode.nodeId));
|
||||
tags = regionManager.GetRegion(currentNode.coordinates)!.tagManager.GetTagsForWayId(edge.wayId);
|
||||
pathDistanceDelta = currentNode.currentPathLength - currentNode.previousPathNode.currentPathLength;
|
||||
pathWeightDelta = currentNode.currentPathWeight - currentNode.previousPathNode.currentPathWeight;
|
||||
directDistanceDelta =
|
||||
currentNode.directDistanceToGoal - currentNode.previousPathNode.directDistanceToGoal;
|
||||
}
|
||||
|
||||
PathNode? pn = PathNode.FromOsmNode(currentNode, tags, pathDistanceDelta, pathWeightDelta, directDistanceDelta);
|
||||
if(pn is not null)
|
||||
path.Add(pn!);
|
||||
currentNode = currentNode.previousPathNode;
|
||||
}
|
||||
|
||||
path.Reverse();
|
||||
return path;
|
||||
}
|
||||
}
|
||||
@ -1,50 +0,0 @@
|
||||
using OSMDatastructure;
|
||||
using OSMDatastructure.Graph;
|
||||
|
||||
namespace Pathfinding;
|
||||
|
||||
public static partial class Pathfinder
|
||||
{
|
||||
public static List<PathNode> AStarDistance(string workingDir, Coordinates start,
|
||||
Coordinates goal)
|
||||
{
|
||||
RegionManager regionManager = new (workingDir);
|
||||
ValueTuple<OsmNode?, OsmNode?> startAndEndNode = SetupNodes(start, goal, regionManager, Tag.SpeedType.any);
|
||||
if (startAndEndNode.Item1 is null || startAndEndNode.Item2 is null)
|
||||
return new List<PathNode>();
|
||||
OsmNode goalNode = startAndEndNode.Item2!;
|
||||
|
||||
PriorityQueue<OsmNode, double> toVisit = new();
|
||||
toVisit.Enqueue(startAndEndNode.Item1, 0);
|
||||
bool stop = false;
|
||||
|
||||
while (toVisit.Count > 0)
|
||||
{
|
||||
OsmNode currentNode = toVisit.Dequeue();
|
||||
|
||||
foreach (OsmEdge edge in currentNode.edges)
|
||||
{
|
||||
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
||||
if (neighbor is not null)
|
||||
{
|
||||
if (Math.Abs(neighbor.directDistanceToGoal - double.MaxValue) < 1)
|
||||
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
|
||||
double newPotentialLength = currentNode.currentPathLength + Utils.DistanceBetween(currentNode, neighbor);
|
||||
if (newPotentialLength < neighbor.currentPathLength)
|
||||
{
|
||||
neighbor.previousPathNode = currentNode;
|
||||
neighbor.currentPathLength = newPotentialLength;
|
||||
|
||||
if(neighbor.Equals(goalNode))
|
||||
return GetRouteFromCalc(goalNode, regionManager);
|
||||
if (!toVisit.UnorderedItems.Any(item => item.Element.Equals(neighbor)) && !stop)
|
||||
{
|
||||
toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return GetRouteFromCalc(goalNode, regionManager);
|
||||
}
|
||||
}
|
||||
@ -1,63 +0,0 @@
|
||||
using OSMDatastructure;
|
||||
using OSMDatastructure.Graph;
|
||||
using Utils = OSMDatastructure.Utils;
|
||||
|
||||
namespace Pathfinding;
|
||||
|
||||
public static partial class Pathfinder
|
||||
{
|
||||
public static List<PathNode> AStarTime(string workingDir, Coordinates start,
|
||||
Coordinates goal, Tag.SpeedType vehicle)
|
||||
{
|
||||
RegionManager regionManager = new (workingDir);
|
||||
ValueTuple<OsmNode?, OsmNode?> startAndEndNode = SetupNodes(start, goal, regionManager, vehicle);
|
||||
if (startAndEndNode.Item1 is null || startAndEndNode.Item2 is null)
|
||||
return new List<PathNode>();
|
||||
OsmNode goalNode = startAndEndNode.Item2!;
|
||||
|
||||
PriorityQueue<OsmNode, double> toVisit = new();
|
||||
toVisit.Enqueue(startAndEndNode.Item1, 0);
|
||||
|
||||
while (toVisit.Count > 0)
|
||||
{
|
||||
OsmNode currentNode = toVisit.Dequeue();
|
||||
|
||||
foreach (OsmEdge edge in currentNode.edges.Where(
|
||||
edge => regionManager.TestValidConnectionForType(currentNode, edge, vehicle)))
|
||||
{
|
||||
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
||||
if (neighbor is not null)
|
||||
{
|
||||
if (Math.Abs(neighbor.directDistanceToGoal - double.MaxValue) < 1)
|
||||
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
|
||||
|
||||
double newPotentialWeight = currentNode.currentPathWeight +
|
||||
EdgeWeight(currentNode, edge, vehicle, regionManager);
|
||||
|
||||
if (newPotentialWeight < neighbor.currentPathWeight)
|
||||
{
|
||||
neighbor.previousPathNode = currentNode;
|
||||
neighbor.currentPathWeight = newPotentialWeight;
|
||||
|
||||
if (neighbor.Equals(goalNode))
|
||||
{
|
||||
currentNode = neighbor;
|
||||
currentNode.currentPathLength = 0;
|
||||
while (!currentNode.Equals(startAndEndNode.Item1) && currentNode.previousPathNode is not null)
|
||||
{
|
||||
currentNode.previousPathNode.currentPathLength = currentNode.currentPathLength +
|
||||
Utils.DistanceBetween(currentNode, currentNode.previousPathNode);
|
||||
currentNode = currentNode.previousPathNode;
|
||||
}
|
||||
|
||||
return GetRouteFromCalc(goalNode, regionManager);
|
||||
}
|
||||
|
||||
toVisit.Enqueue(neighbor, GetPriority(currentNode, currentNode.previousPathNode, edge, vehicle, regionManager));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return GetRouteFromCalc(goalNode, regionManager);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user