Compare commits
3 Commits
3a1ca0cc3d
...
eabceed59d
Author | SHA1 | Date | |
---|---|---|---|
eabceed59d | |||
0bbda3dfd0 | |||
f8b3d72292 |
@ -25,19 +25,20 @@ namespace astar
|
|||||||
endNode.Value.PreviousNodeId = endNode.Key;
|
endNode.Value.PreviousNodeId = endNode.Key;
|
||||||
endNode.Value.Distance = 0f;
|
endNode.Value.Distance = 0f;
|
||||||
|
|
||||||
|
double totalDistance = NodeUtils.DistanceBetween(startNode.Value, endNode.Value);
|
||||||
|
PriorityHelper priorityHelper = new(totalDistance, SpeedHelper.GetMaxSpeed(car));
|
||||||
|
|
||||||
logger?.Log(LogLevel.Information,
|
logger?.Log(LogLevel.Information,
|
||||||
"From {0:00.00000}#{1:000.00000} to {2:00.00000}#{3:000.00000} Great-Circle {4:00000.00}km",
|
"From {0:00.00000}#{1:000.00000} to {2:00.00000}#{3:000.00000} Great-Circle {4:00000.00}km",
|
||||||
startNode.Value.Lat, startNode.Value.Lon, endNode.Value.Lat, endNode.Value.Lon,
|
startNode.Value.Lat, startNode.Value.Lon, endNode.Value.Lat, endNode.Value.Lon, totalDistance / 1000);
|
||||||
NodeUtils.DistanceBetween(startNode.Value, endNode.Value) / 1000);
|
|
||||||
|
|
||||||
PriorityQueue<ulong, double> toVisitStart = new();
|
PriorityQueue<ulong, int> toVisitStart = new();
|
||||||
toVisitStart.Enqueue(startNode.Key, NodeUtils.DistanceBetween(startNode.Value, endNode.Value));
|
toVisitStart.Enqueue(startNode.Key, 0);
|
||||||
PriorityQueue<ulong, double> toVisitEnd = new();
|
PriorityQueue<ulong, int> toVisitEnd = new();
|
||||||
toVisitEnd.Enqueue(endNode.Key, NodeUtils.DistanceBetween(endNode.Value, startNode.Value));
|
toVisitEnd.Enqueue(endNode.Key, 0);
|
||||||
|
|
||||||
while (toVisitStart.Count > 0 && toVisitEnd.Count > 0)
|
while (toVisitStart.Count > 0 && toVisitEnd.Count > 0)
|
||||||
{
|
{
|
||||||
logger?.LogDebug($"Length toVisit-Start: {toVisitStart.Count} -End: {toVisitEnd.Count}");
|
|
||||||
ulong currentNodeStartId = toVisitStart.Dequeue();
|
ulong currentNodeStartId = toVisitStart.Dequeue();
|
||||||
Node currentNodeStart = graph.Nodes[currentNodeStartId];
|
Node currentNodeStart = graph.Nodes[currentNodeStartId];
|
||||||
foreach ((ulong neighborId, ulong wayId) in currentNodeStart.Neighbors)
|
foreach ((ulong neighborId, ulong wayId) in currentNodeStart.Neighbors)
|
||||||
@ -60,12 +61,12 @@ namespace astar
|
|||||||
return PathFound(graph, currentNodeStart, neighborNode, logger);
|
return PathFound(graph, currentNodeStart, neighborNode, logger);
|
||||||
|
|
||||||
float distance = (currentNodeStart.Distance??float.MaxValue) + (float)currentNodeStart.DistanceTo(neighborNode);
|
float distance = (currentNodeStart.Distance??float.MaxValue) + (float)currentNodeStart.DistanceTo(neighborNode);
|
||||||
if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance && currentNodeStart.PreviousNodeId != neighborId)
|
if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance)
|
||||||
{
|
{
|
||||||
neighborNode.PreviousNodeId = currentNodeStartId;
|
neighborNode.PreviousNodeId = currentNodeStartId;
|
||||||
neighborNode.Distance = distance;
|
neighborNode.Distance = distance;
|
||||||
neighborNode.PreviousIsFromStart = true;
|
neighborNode.PreviousIsFromStart = true;
|
||||||
toVisitStart.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, endNode.Value) / speed);
|
toVisitStart.Enqueue(neighborId, priorityHelper.CalculatePriority(currentNodeStart, neighborNode, endNode.Value, speed));
|
||||||
}
|
}
|
||||||
logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
|
logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
|
||||||
}
|
}
|
||||||
@ -91,16 +92,17 @@ namespace astar
|
|||||||
if (neighborNode.PreviousIsFromStart is true)//Check if we found the opposite End
|
if (neighborNode.PreviousIsFromStart is true)//Check if we found the opposite End
|
||||||
return PathFound(graph, neighborNode, currentNodeEnd, logger);
|
return PathFound(graph, neighborNode, currentNodeEnd, logger);
|
||||||
|
|
||||||
float distance = (currentNodeStart.Distance??float.MaxValue) + (float)currentNodeStart.DistanceTo(neighborNode);
|
float distance = (currentNodeEnd.Distance??float.MaxValue) + (float)currentNodeEnd.DistanceTo(neighborNode);
|
||||||
if (neighborNode.PreviousIsFromStart is null || neighborNode.Distance > distance && currentNodeEnd.PreviousNodeId != neighborId)
|
if (neighborNode.PreviousNodeId is null || neighborNode.Distance > distance)
|
||||||
{
|
{
|
||||||
neighborNode.PreviousNodeId = currentNodeEndId;
|
neighborNode.PreviousNodeId = currentNodeEndId;
|
||||||
neighborNode.Distance = distance;
|
neighborNode.Distance = distance;
|
||||||
neighborNode.PreviousIsFromStart = false;
|
neighborNode.PreviousIsFromStart = false;
|
||||||
toVisitEnd.Enqueue(neighborId, NodeUtils.DistanceBetween(neighborNode, startNode.Value) / speed);
|
toVisitEnd.Enqueue(neighborId, priorityHelper.CalculatePriority(currentNodeEnd, neighborNode, startNode.Value, speed));
|
||||||
}
|
}
|
||||||
logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
|
logger?.LogTrace($"Neighbor {neighborId} {neighborNode}");
|
||||||
}
|
}
|
||||||
|
logger?.LogDebug($"Distance {currentNodeStart.DistanceTo(currentNodeEnd):000000.00}m toVisit-Queues: {toVisitStart.Count} {toVisitStart.UnorderedItems.MinBy(i => i.Priority).Priority} {toVisitEnd.Count} {toVisitEnd.UnorderedItems.MinBy(i => i.Priority).Priority}");
|
||||||
|
|
||||||
}
|
}
|
||||||
return new Route(graph, Array.Empty<Step>().ToList(), false);
|
return new Route(graph, Array.Empty<Step>().ToList(), false);
|
||||||
|
@ -10,6 +10,6 @@ public class Node(float lat, float lon, Dictionary<ulong, ulong>? neighbors = nu
|
|||||||
|
|
||||||
public override string ToString()
|
public override string ToString()
|
||||||
{
|
{
|
||||||
return $"{Lat:00.000000} {Lon:000.000000} Previous {PreviousNodeId} {(PreviousIsFromStart is not null ? PreviousIsFromStart.Value ?"Start":"End" : null)}";
|
return $"{Lat:00.000000} {Lon:000.000000} Previous {PreviousNodeId} {Distance} {(PreviousIsFromStart is not null ? PreviousIsFromStart.Value ?"Start":"End" : null)}";
|
||||||
}
|
}
|
||||||
}
|
}
|
25
astar/PathingHelper/PriorityHelper.cs
Normal file
25
astar/PathingHelper/PriorityHelper.cs
Normal file
@ -0,0 +1,25 @@
|
|||||||
|
using Graph;
|
||||||
|
|
||||||
|
namespace astar.PathingHelper;
|
||||||
|
|
||||||
|
public class PriorityHelper(double totalDistance, byte maxSpeed)
|
||||||
|
{
|
||||||
|
private readonly double _totalDistance = totalDistance;
|
||||||
|
private readonly byte _maxSpeed = maxSpeed;
|
||||||
|
public int CalculatePriority(Node current, Node neighbor, Node goal, byte speed)
|
||||||
|
{
|
||||||
|
double neighborDistanceToGoal = neighbor.DistanceTo(goal); //we want this to be small
|
||||||
|
/*double currentDistanceToGoal = current.DistanceTo(goal);
|
||||||
|
double currentDistanceToNeighbor = current.DistanceTo(neighbor);
|
||||||
|
double angle = //we want this to be small
|
||||||
|
Math.Acos((currentDistanceToGoal * currentDistanceToGoal + currentDistanceToNeighbor +
|
||||||
|
currentDistanceToNeighbor - neighborDistanceToGoal * neighborDistanceToGoal) / (2 *
|
||||||
|
currentDistanceToGoal * currentDistanceToNeighbor));*/
|
||||||
|
|
||||||
|
double distanceRating = 100 - neighborDistanceToGoal / _totalDistance * 100;
|
||||||
|
//double angleRating = Math.Abs((180 - angle) / 180) * 100;
|
||||||
|
double speedRating = speed * 1.0 / _maxSpeed * 100;
|
||||||
|
|
||||||
|
return 350 - (int)(distanceRating * 2 + speedRating * 1.5);
|
||||||
|
}
|
||||||
|
}
|
@ -10,7 +10,19 @@ internal static class SpeedHelper
|
|||||||
return 0;
|
return 0;
|
||||||
if (!Enum.TryParse(highwayTypeStr, out HighwayType highwayType))
|
if (!Enum.TryParse(highwayTypeStr, out HighwayType highwayType))
|
||||||
return 0;
|
return 0;
|
||||||
return car ? SpeedCar[highwayType] : SpeedPedestrian[highwayType];
|
byte speed = car ? SpeedCar[highwayType] : SpeedPedestrian[highwayType];
|
||||||
|
if (speed < 1)
|
||||||
|
return speed;
|
||||||
|
if(!way.Tags.TryGetValue("maxspeed", out string? maxSpeedStr))
|
||||||
|
return speed;
|
||||||
|
if (!byte.TryParse(maxSpeedStr, out speed))
|
||||||
|
return speed;
|
||||||
|
return speed;
|
||||||
|
}
|
||||||
|
|
||||||
|
public static byte GetMaxSpeed(bool car = true)
|
||||||
|
{
|
||||||
|
return car ? SpeedCar.MaxBy(s => s.Value).Value : SpeedPedestrian.MaxBy(s => s.Value).Value;
|
||||||
}
|
}
|
||||||
|
|
||||||
private static Dictionary<HighwayType, byte> SpeedPedestrian = new() {
|
private static Dictionary<HighwayType, byte> SpeedPedestrian = new() {
|
||||||
@ -48,25 +60,25 @@ internal static class SpeedHelper
|
|||||||
private static Dictionary<HighwayType, byte> SpeedCar = new() {
|
private static Dictionary<HighwayType, byte> SpeedCar = new() {
|
||||||
{ HighwayType.NONE, 0 },
|
{ HighwayType.NONE, 0 },
|
||||||
{ HighwayType.motorway, 110 },
|
{ HighwayType.motorway, 110 },
|
||||||
{ HighwayType.trunk, 100 },
|
{ HighwayType.trunk, 80 },
|
||||||
{ HighwayType.primary, 80 },
|
{ HighwayType.primary, 80 },
|
||||||
{ HighwayType.secondary, 80 },
|
{ HighwayType.secondary, 80 },
|
||||||
{ HighwayType.tertiary, 70 },
|
{ HighwayType.tertiary, 70 },
|
||||||
{ HighwayType.unclassified, 20 },
|
{ HighwayType.unclassified, 30 },
|
||||||
{ HighwayType.residential, 10 },
|
{ HighwayType.residential, 10 },
|
||||||
{ HighwayType.motorway_link, 50 },
|
{ HighwayType.motorway_link, 50 },
|
||||||
{ HighwayType.trunk_link, 50 },
|
{ HighwayType.trunk_link, 50 },
|
||||||
{ HighwayType.primary_link, 30 },
|
{ HighwayType.primary_link, 50 },
|
||||||
{ HighwayType.secondary_link, 25 },
|
{ HighwayType.secondary_link, 30 },
|
||||||
{ HighwayType.tertiary_link, 25 },
|
{ HighwayType.tertiary_link, 25 },
|
||||||
{ HighwayType.living_street, 10 },
|
{ HighwayType.living_street, 5 },
|
||||||
{ HighwayType.service, 0 },
|
{ HighwayType.service, 0 },
|
||||||
{ HighwayType.pedestrian, 0 },
|
{ HighwayType.pedestrian, 0 },
|
||||||
{ HighwayType.track, 0 },
|
{ HighwayType.track, 0 },
|
||||||
{ HighwayType.bus_guideway, 0 },
|
{ HighwayType.bus_guideway, 0 },
|
||||||
{ HighwayType.escape, 0 },
|
{ HighwayType.escape, 0 },
|
||||||
{ HighwayType.raceway, 0 },
|
{ HighwayType.raceway, 0 },
|
||||||
{ HighwayType.road, 25 },
|
{ HighwayType.road, 30 },
|
||||||
{ HighwayType.busway, 0 },
|
{ HighwayType.busway, 0 },
|
||||||
{ HighwayType.footway, 0 },
|
{ HighwayType.footway, 0 },
|
||||||
{ HighwayType.bridleway, 0 },
|
{ HighwayType.bridleway, 0 },
|
||||||
|
Loading…
Reference in New Issue
Block a user