Compare commits
No commits in common. "6eab23ff1653d17432e4e27b8a0aaa49b8ec7fde" and "fe0ccd0fca955fa04052ea9765290ca012c00ab3" have entirely different histories.
6eab23ff16
...
fe0ccd0fca
@ -1,6 +1,6 @@
|
|||||||
using System.Text.Json.Serialization;
|
using System.Text.Json.Serialization;
|
||||||
using OSMDatastructure;
|
|
||||||
using OSMDatastructure.Graph;
|
using OSMDatastructure.Graph;
|
||||||
|
using OSMImporter;
|
||||||
using Pathfinding;
|
using Pathfinding;
|
||||||
|
|
||||||
var builder = WebApplication.CreateBuilder(args);
|
var builder = WebApplication.CreateBuilder(args);
|
||||||
@ -16,20 +16,11 @@ var app = builder.Build();
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
app.MapGet("/getRouteDistance", (float latStart, float lonStart, float latEnd, float lonEnd) =>
|
app.MapGet("/getRoute", (float latStart, float lonStart, float latEnd, float lonEnd) =>
|
||||||
{
|
{
|
||||||
DateTime startCalc = DateTime.Now;
|
ValueTuple<TimeSpan, List<PathNode>> result = Pathfinder.CustomAStar("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), new Coordinates(latEnd, lonEnd),
|
||||||
List<PathNode> result = Pathfinder.AStarDistance("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), new Coordinates(latEnd, lonEnd));
|
Tag.SpeedType.car);
|
||||||
PathResult pathResult = new PathResult(DateTime.Now - startCalc, result);
|
PathResult pathResult = new PathResult(result.Item1, result.Item2);
|
||||||
return pathResult;
|
|
||||||
}
|
|
||||||
);
|
|
||||||
|
|
||||||
app.MapGet("/getRouteTime", (float latStart, float lonStart, float latEnd, float lonEnd, Tag.SpeedType vehicle) =>
|
|
||||||
{
|
|
||||||
DateTime startCalc = DateTime.Now;
|
|
||||||
List<PathNode> result = Pathfinder.AStarTime("D:/stuttgart-regbez-latest", new Coordinates(latStart, lonStart), new Coordinates(latEnd, lonEnd), vehicle);
|
|
||||||
PathResult pathResult = new PathResult(DateTime.Now - startCalc, result);
|
|
||||||
return pathResult;
|
return pathResult;
|
||||||
}
|
}
|
||||||
);
|
);
|
||||||
@ -37,7 +28,7 @@ app.MapGet("/getRouteTime", (float latStart, float lonStart, float latEnd, float
|
|||||||
app.MapGet("/getClosestNode", (float lat, float lon) =>
|
app.MapGet("/getClosestNode", (float lat, float lon) =>
|
||||||
{
|
{
|
||||||
RegionManager regionManager = new RegionManager("D:/stuttgart-regbez-latest");
|
RegionManager regionManager = new RegionManager("D:/stuttgart-regbez-latest");
|
||||||
return regionManager.ClosestNodeToCoordinates(new Coordinates(lat, lon), Tag.SpeedType.road);
|
return Pathfinder.ClosestNodeToCoordinates(new Coordinates(lat, lon), Tag.SpeedType.car, ref regionManager);
|
||||||
});
|
});
|
||||||
|
|
||||||
// Configure the HTTP request pipeline.
|
// Configure the HTTP request pipeline.
|
||||||
@ -58,18 +49,15 @@ app.Run();
|
|||||||
internal class PathResult
|
internal class PathResult
|
||||||
{
|
{
|
||||||
[JsonInclude]public TimeSpan calcTime;
|
[JsonInclude]public TimeSpan calcTime;
|
||||||
[JsonInclude] public double pathWeight = double.MaxValue;
|
[JsonInclude] public double pathWeight;
|
||||||
[JsonInclude] public double pathTravelDistance = double.MaxValue;
|
[JsonInclude] public double pathTravelDistance;
|
||||||
[JsonInclude]public List<PathNode> pathNodes;
|
[JsonInclude]public List<PathNode> pathNodes;
|
||||||
|
|
||||||
public PathResult(TimeSpan calcTime, List<PathNode> pathNodes)
|
public PathResult(TimeSpan calcTime, List<PathNode> pathNodes)
|
||||||
{
|
{
|
||||||
this.calcTime = calcTime;
|
this.calcTime = calcTime;
|
||||||
this.pathNodes = pathNodes;
|
this.pathNodes = pathNodes;
|
||||||
if (pathNodes.Count > 0)
|
this.pathWeight = pathNodes.Last().currentPathWeight;
|
||||||
{
|
this.pathTravelDistance = pathNodes.Last().currentPathLength;
|
||||||
this.pathWeight = pathNodes.Last().currentPathWeight;
|
|
||||||
this.pathTravelDistance = pathNodes.Last().currentPathLength;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
@ -1,3 +1,4 @@
|
|||||||
|
using System.ComponentModel;
|
||||||
using System.Runtime.Serialization;
|
using System.Runtime.Serialization;
|
||||||
using System.Text.Json.Serialization;
|
using System.Text.Json.Serialization;
|
||||||
|
|
||||||
|
@ -59,4 +59,16 @@ public class Region
|
|||||||
else return null;
|
else return null;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public static Region? FromFile(string filePath)
|
||||||
|
{
|
||||||
|
if (File.Exists(filePath))
|
||||||
|
return JsonSerializer.Deserialize<Region>(new FileStream(filePath, FileMode.Open), serializerOptions)!;
|
||||||
|
else return null;
|
||||||
|
}
|
||||||
|
|
||||||
|
public static Region? FromId(string path, ulong regionId)
|
||||||
|
{
|
||||||
|
string filePath = Path.Join(path, $"{regionId}.region");
|
||||||
|
return FromFile(filePath);
|
||||||
|
}
|
||||||
}
|
}
|
@ -1,7 +1,7 @@
|
|||||||
using System.Text.Json;
|
using System.Text.Json;
|
||||||
using System.Text.Json.Serialization;
|
using System.Text.Json.Serialization;
|
||||||
|
|
||||||
namespace OSMDatastructure;
|
namespace OSMDatastructure.Graph;
|
||||||
|
|
||||||
[Serializable]
|
[Serializable]
|
||||||
public class Tag
|
public class Tag
|
||||||
@ -40,6 +40,25 @@ public class Tag
|
|||||||
this.value = value;
|
this.value = value;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public static Tag FromBytes(byte[] bytes)
|
||||||
|
{
|
||||||
|
TagType type = (TagType)bytes[0];
|
||||||
|
dynamic value = false;
|
||||||
|
switch (type)
|
||||||
|
{
|
||||||
|
case TagType.highway:
|
||||||
|
case TagType.oneway:
|
||||||
|
case TagType.forward:
|
||||||
|
value = BitConverter.ToBoolean(bytes, 1);
|
||||||
|
break;
|
||||||
|
case TagType.maxspeed:
|
||||||
|
value = bytes[1];
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
return new Tag(type, value);
|
||||||
|
}
|
||||||
|
|
||||||
public static Tag? ConvertToTag(string key, string value)
|
public static Tag? ConvertToTag(string key, string value)
|
||||||
{
|
{
|
||||||
@ -97,7 +116,7 @@ public class Tag
|
|||||||
}
|
}
|
||||||
|
|
||||||
public static readonly Dictionary<WayType, byte> defaultSpeedCar = new() {
|
public static readonly Dictionary<WayType, byte> defaultSpeedCar = new() {
|
||||||
{ WayType.NONE, 0 },
|
{ WayType.NONE, 1 },
|
||||||
{ WayType.motorway, 110 },
|
{ WayType.motorway, 110 },
|
||||||
{ WayType.trunk, 100 },
|
{ WayType.trunk, 100 },
|
||||||
{ WayType.primary, 80 },
|
{ WayType.primary, 80 },
|
||||||
@ -110,7 +129,7 @@ public class Tag
|
|||||||
{ WayType.primary_link, 30 },
|
{ WayType.primary_link, 30 },
|
||||||
{ WayType.secondary_link, 25 },
|
{ WayType.secondary_link, 25 },
|
||||||
{ WayType.tertiary_link, 25 },
|
{ WayType.tertiary_link, 25 },
|
||||||
{ WayType.living_street, 5 },
|
{ WayType.living_street, 10 },
|
||||||
{ WayType.service, 1 },
|
{ WayType.service, 1 },
|
||||||
{ WayType.pedestrian, 0 },
|
{ WayType.pedestrian, 0 },
|
||||||
{ WayType.track, 15 },
|
{ WayType.track, 15 },
|
||||||
@ -149,19 +168,17 @@ public class Tag
|
|||||||
{ WayType.bus_guideway, 0 },
|
{ WayType.bus_guideway, 0 },
|
||||||
{ WayType.escape, 1 },
|
{ WayType.escape, 1 },
|
||||||
{ WayType.raceway, 0 },
|
{ WayType.raceway, 0 },
|
||||||
{ WayType.road, 2 },
|
{ WayType.road, 3 },
|
||||||
{ WayType.busway, 0 },
|
{ WayType.busway, 0 },
|
||||||
{ WayType.footway, 4 },
|
{ WayType.footway, 4 },
|
||||||
{ WayType.bridleway, 1 },
|
{ WayType.bridleway, 1 },
|
||||||
{ WayType.steps, 2 },
|
{ WayType.steps, 2 },
|
||||||
{ WayType.corridor, 3 },
|
{ WayType.corridor, 3 },
|
||||||
{ WayType.path, 4 },
|
{ WayType.path, 4 },
|
||||||
{ WayType.cycleway, 1 },
|
{ WayType.cycleway, 2 },
|
||||||
{ WayType.construction, 0 }
|
{ WayType.construction, 0 }
|
||||||
};
|
};
|
||||||
// ReSharper disable InconsistentNaming
|
|
||||||
public enum WayType : byte { NONE, motorway, trunk, primary, secondary, tertiary, unclassified, residential, motorway_link, trunk_link, primary_link, secondary_link, tertiary_link, living_street, service, pedestrian, track, bus_guideway, escape, raceway, road, busway, footway, bridleway, steps, corridor, path, cycleway, construction }
|
public enum WayType : byte { NONE, motorway, trunk, primary, secondary, tertiary, unclassified, residential, motorway_link, trunk_link, primary_link, secondary_link, tertiary_link, living_street, service, pedestrian, track, bus_guideway, escape, raceway, road, busway, footway, bridleway, steps, corridor, path, cycleway, construction }
|
||||||
// ReSharper restore InconsistentNaming
|
|
||||||
|
|
||||||
public enum SpeedType { pedestrian, car, road }
|
public enum SpeedType { pedestrian, car, road }
|
||||||
}
|
}
|
@ -1,7 +1,6 @@
|
|||||||
using System.Text.Json.Serialization;
|
using System.Text.Json.Serialization;
|
||||||
using OSMDatastructure.Graph;
|
|
||||||
|
|
||||||
namespace OSMDatastructure;
|
namespace OSMDatastructure.Graph;
|
||||||
|
|
||||||
[Serializable]
|
[Serializable]
|
||||||
public class TagManager
|
public class TagManager
|
||||||
|
@ -1,18 +1,14 @@
|
|||||||
using System.Text.Json.Serialization;
|
using System.Text.Json.Serialization;
|
||||||
using OSMDatastructure;
|
|
||||||
using OSMDatastructure.Graph;
|
using OSMDatastructure.Graph;
|
||||||
|
|
||||||
namespace Pathfinding;
|
namespace Pathfinding;
|
||||||
|
|
||||||
public class PathNode : OsmNode
|
public class PathNode : OsmNode
|
||||||
{
|
{
|
||||||
[JsonInclude]public new double directDistanceToGoal = double.MaxValue;
|
|
||||||
[JsonInclude]public new double currentPathLength = double.MaxValue;
|
|
||||||
[JsonInclude]public double distanceBetweenNodes = double.MaxValue;
|
|
||||||
[JsonInclude]public new double currentPathWeight = double.MaxValue;
|
[JsonInclude]public new double currentPathWeight = double.MaxValue;
|
||||||
[JsonInclude]public double weightBetweenNodes = double.MaxValue;
|
[JsonInclude]public new double currentPathLength = double.MaxValue;
|
||||||
[JsonInclude]public Dictionary<string, string> tags = new();
|
[JsonInclude]public new double directDistanceToGoal = double.MaxValue;
|
||||||
|
|
||||||
public PathNode(ulong nodeId, float lat, float lon) : base(nodeId, lat, lon)
|
public PathNode(ulong nodeId, float lat, float lon) : base(nodeId, lat, lon)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
@ -21,24 +17,16 @@ public class PathNode : OsmNode
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
public static PathNode? FromOsmNode(OsmNode? node, HashSet<Tag>? tags, double distance, double weight)
|
public static PathNode? FromOsmNode(OsmNode? node)
|
||||||
{
|
{
|
||||||
if (node is null)
|
if (node is null)
|
||||||
return null;
|
return null;
|
||||||
PathNode retNode = new(node.nodeId, node.coordinates)
|
PathNode retNode = new PathNode(node.nodeId, node.coordinates)
|
||||||
{
|
{
|
||||||
currentPathLength = node.currentPathLength,
|
currentPathLength = node.currentPathLength,
|
||||||
currentPathWeight = double.IsPositiveInfinity(node.currentPathWeight) ? double.MaxValue : node.currentPathWeight,
|
currentPathWeight = node.currentPathWeight,
|
||||||
directDistanceToGoal = node.directDistanceToGoal,
|
directDistanceToGoal = node.directDistanceToGoal
|
||||||
distanceBetweenNodes = distance,
|
|
||||||
weightBetweenNodes = weight
|
|
||||||
};
|
};
|
||||||
if (tags != null)
|
|
||||||
foreach (Tag tag in tags)
|
|
||||||
{
|
|
||||||
retNode.tags.Add(tag.key.ToString(), tag.value.ToString());
|
|
||||||
}
|
|
||||||
|
|
||||||
return retNode;
|
return retNode;
|
||||||
}
|
}
|
||||||
}
|
}
|
@ -1,60 +1,135 @@
|
|||||||
using OSMDatastructure;
|
using OSMDatastructure;
|
||||||
using OSMDatastructure.Graph;
|
using OSMDatastructure.Graph;
|
||||||
|
using OSMImporter;
|
||||||
|
|
||||||
namespace Pathfinding;
|
namespace Pathfinding;
|
||||||
|
|
||||||
public static partial class Pathfinder
|
public static class Pathfinder
|
||||||
{
|
{
|
||||||
|
public static ValueTuple<TimeSpan, List<PathNode>> CustomAStar(string workingDir, Coordinates start, Coordinates goal, Tag.SpeedType vehicle)
|
||||||
private static ValueTuple<OsmNode?, OsmNode?> SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager )
|
|
||||||
{
|
{
|
||||||
ValueTuple<OsmNode?, OsmNode?> retTuple = new();
|
DateTime startTime = DateTime.Now;
|
||||||
retTuple.Item1 = regionManager.ClosestNodeToCoordinates(startCoordinates, Tag.SpeedType.road);
|
TimeSpan calcTime;
|
||||||
retTuple.Item2 = regionManager.ClosestNodeToCoordinates(goalCoordinates, Tag.SpeedType.road);
|
RegionManager regionManager = new (workingDir);
|
||||||
if (retTuple.Item1 is null || retTuple.Item2 is null)
|
OsmNode? startNode = ClosestNodeToCoordinates(start, vehicle, ref regionManager);
|
||||||
return retTuple;
|
OsmNode? goalNode = ClosestNodeToCoordinates(goal, vehicle, ref regionManager);
|
||||||
retTuple.Item1.currentPathWeight = 0;
|
if (startNode == null || goalNode == null)
|
||||||
retTuple.Item1.currentPathLength = 0;
|
{
|
||||||
retTuple.Item1.directDistanceToGoal = Utils.DistanceBetween(retTuple.Item1, retTuple.Item2);
|
calcTime = DateTime.Now - startTime;
|
||||||
return retTuple;
|
return new ValueTuple<TimeSpan, List<PathNode>>(calcTime, new List<PathNode>());
|
||||||
}
|
}
|
||||||
|
|
||||||
private static double EdgeWeight(OsmNode node1, OsmEdge edge, Tag.SpeedType vehicle, RegionManager regionManager)
|
PriorityQueue<OsmNode, double> toVisit = new();
|
||||||
{
|
toVisit.Enqueue(startNode, 0);
|
||||||
OsmNode? node2 = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
startNode.currentPathWeight = 0;
|
||||||
if (node2 is null)
|
startNode.currentPathLength = 0;
|
||||||
return double.MaxValue;
|
startNode.directDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
|
||||||
double distance = Utils.DistanceBetween(node1, node2);
|
bool stop = false;
|
||||||
double speed = regionManager.GetSpeedForEdge(node1, edge.wayId, vehicle);
|
|
||||||
if (speed is 0)
|
while (toVisit.Count > 0)
|
||||||
return double.MaxValue;
|
{
|
||||||
return distance / speed;
|
OsmNode closestNodeToGoal = toVisit.Dequeue();
|
||||||
}
|
//Console.WriteLine($"{toVisit.Count:000} {closestNodeToGoal.directDistanceToGoal:#.00} current:{closestNodeToGoal}");
|
||||||
|
|
||||||
|
foreach (OsmEdge edge in closestNodeToGoal.edges)
|
||||||
|
{
|
||||||
|
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
||||||
|
if (neighbor is not null)
|
||||||
|
{
|
||||||
|
double newPotentialWeight =
|
||||||
|
closestNodeToGoal.currentPathWeight + EdgeWeight(closestNodeToGoal, neighbor, edge.wayId, vehicle, ref regionManager);
|
||||||
|
if (newPotentialWeight < neighbor.currentPathWeight)
|
||||||
|
{
|
||||||
|
neighbor.previousPathNode = closestNodeToGoal;
|
||||||
|
neighbor.currentPathWeight = newPotentialWeight;
|
||||||
|
neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
|
||||||
|
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
|
||||||
|
|
||||||
|
if (neighbor.Equals(goalNode) || closestNodeToGoal.directDistanceToGoal < 10)
|
||||||
|
{
|
||||||
|
stop = true;
|
||||||
|
goalNode = neighbor;
|
||||||
|
}
|
||||||
|
else if(!stop)
|
||||||
|
{
|
||||||
|
toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
private static List<PathNode> GetRouteFromCalc(OsmNode goalNode, RegionManager regionManager)
|
|
||||||
{
|
|
||||||
List<PathNode> path = new();
|
List<PathNode> path = new();
|
||||||
OsmNode? currentNode = goalNode;
|
OsmNode? currentNode = goalNode;
|
||||||
while (currentNode is not null)
|
while (currentNode is not null)
|
||||||
{
|
{
|
||||||
HashSet<Tag>? tags = null;
|
path.Add(PathNode.FromOsmNode(currentNode)!);
|
||||||
double distance = 0;
|
|
||||||
double weight = 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);
|
|
||||||
distance = currentNode.currentPathLength - currentNode.previousPathNode.currentPathLength;
|
|
||||||
weight = currentNode.currentPathWeight - currentNode.previousPathNode.currentPathWeight;
|
|
||||||
}
|
|
||||||
|
|
||||||
PathNode? pn = PathNode.FromOsmNode(currentNode, tags, distance, weight);
|
|
||||||
if(pn is not null)
|
|
||||||
path.Add(pn!);
|
|
||||||
currentNode = currentNode.previousPathNode;
|
currentNode = currentNode.previousPathNode;
|
||||||
}
|
}
|
||||||
|
|
||||||
path.Reverse();
|
path.Reverse();
|
||||||
return path;
|
|
||||||
|
calcTime = DateTime.Now - startTime;
|
||||||
|
return new ValueTuple<TimeSpan, List<PathNode>>(calcTime, path);
|
||||||
|
}
|
||||||
|
|
||||||
|
public static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Tag.SpeedType vehicle, ref RegionManager regionManager)
|
||||||
|
{
|
||||||
|
OsmNode? closest = null;
|
||||||
|
double distance = double.MaxValue;
|
||||||
|
Region? region = regionManager.GetRegion(coordinates);
|
||||||
|
if (region is null)
|
||||||
|
return null;
|
||||||
|
foreach (OsmNode node in region.nodes)
|
||||||
|
{
|
||||||
|
bool hasConnectionUsingVehicle = false;
|
||||||
|
foreach (OsmEdge edge in node.edges)
|
||||||
|
{
|
||||||
|
double speed = GetSpeed(node, edge.wayId, vehicle, ref regionManager);
|
||||||
|
if (speed != 0)
|
||||||
|
hasConnectionUsingVehicle = true;
|
||||||
|
}
|
||||||
|
double nodeDistance = Utils.DistanceBetween(node, coordinates);
|
||||||
|
if (nodeDistance < distance && hasConnectionUsingVehicle)
|
||||||
|
{
|
||||||
|
closest = node;
|
||||||
|
distance = nodeDistance;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return closest;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static double GetSpeed(OsmNode node1, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
|
||||||
|
{
|
||||||
|
TagManager tags = regionManager.GetRegion(node1.coordinates)!.tagManager;
|
||||||
|
Tag.WayType wayType = (Tag.WayType)tags.GetTag(wayId, Tag.TagType.highway)!;
|
||||||
|
switch (vehicle)
|
||||||
|
{
|
||||||
|
case Tag.SpeedType.pedestrian:
|
||||||
|
byte speed = Tag.defaultSpeedPedestrian[wayType];
|
||||||
|
if (speed is not 0)
|
||||||
|
return speed;
|
||||||
|
return 0;
|
||||||
|
case Tag.SpeedType.car:
|
||||||
|
case Tag.SpeedType.road:
|
||||||
|
byte? maxSpeed = (byte?)tags.GetTag(wayId, Tag.TagType.maxspeed);
|
||||||
|
if (maxSpeed is not null)
|
||||||
|
return (double)maxSpeed;
|
||||||
|
maxSpeed = Tag.defaultSpeedCar[wayType];
|
||||||
|
if(maxSpeed is not 0)
|
||||||
|
return (byte)maxSpeed;
|
||||||
|
return 0;
|
||||||
|
default:
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private static double EdgeWeight(OsmNode node1, OsmNode node2, ulong wayId, Tag.SpeedType vehicle, ref RegionManager regionManager)
|
||||||
|
{
|
||||||
|
double distance = Utils.DistanceBetween(node1, node2);
|
||||||
|
double speed = GetSpeed(node1, wayId, vehicle, ref regionManager);
|
||||||
|
if (speed is not 0)
|
||||||
|
return distance / speed;
|
||||||
|
return double.PositiveInfinity;
|
||||||
}
|
}
|
||||||
}
|
}
|
@ -1,51 +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);
|
|
||||||
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 closestNodeToGoal = toVisit.Dequeue();
|
|
||||||
|
|
||||||
foreach (OsmEdge edge in closestNodeToGoal.edges)
|
|
||||||
{
|
|
||||||
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
|
||||||
if (neighbor is not null)
|
|
||||||
{
|
|
||||||
double newPotentialLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
|
|
||||||
if (newPotentialLength < neighbor.currentPathLength)
|
|
||||||
{
|
|
||||||
neighbor.previousPathNode = closestNodeToGoal;
|
|
||||||
neighbor.currentPathLength = newPotentialLength;
|
|
||||||
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
|
|
||||||
|
|
||||||
if (neighbor.Equals(goalNode))
|
|
||||||
{
|
|
||||||
stop = true;
|
|
||||||
}
|
|
||||||
else if(!stop)
|
|
||||||
{
|
|
||||||
toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return GetRouteFromCalc(goalNode, regionManager);
|
|
||||||
}
|
|
||||||
}
|
|
@ -1,55 +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);
|
|
||||||
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 closestNodeToGoal = toVisit.Dequeue();
|
|
||||||
|
|
||||||
foreach (OsmEdge edge in closestNodeToGoal.edges.Where(
|
|
||||||
edge => regionManager.TestValidConnectionForType(closestNodeToGoal, edge, vehicle)))
|
|
||||||
{
|
|
||||||
OsmNode? neighbor = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
|
||||||
if (neighbor is not null)
|
|
||||||
{
|
|
||||||
double newPotentialWeight = closestNodeToGoal.currentPathWeight +
|
|
||||||
EdgeWeight(closestNodeToGoal, edge, vehicle, regionManager);
|
|
||||||
if (newPotentialWeight < neighbor.currentPathWeight)
|
|
||||||
{
|
|
||||||
neighbor.previousPathNode = closestNodeToGoal;
|
|
||||||
neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor);
|
|
||||||
neighbor.currentPathWeight = newPotentialWeight;
|
|
||||||
neighbor.directDistanceToGoal = Utils.DistanceBetween(neighbor, goalNode);
|
|
||||||
|
|
||||||
if (neighbor.Equals(goalNode))
|
|
||||||
{
|
|
||||||
stop = true;
|
|
||||||
}
|
|
||||||
else if(!stop)
|
|
||||||
{
|
|
||||||
toVisit.Enqueue(neighbor, neighbor.directDistanceToGoal);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return GetRouteFromCalc(goalNode, regionManager);
|
|
||||||
}
|
|
||||||
}
|
|
@ -1,8 +1,7 @@
|
|||||||
using System.Text.Json;
|
|
||||||
using OSMDatastructure;
|
using OSMDatastructure;
|
||||||
using OSMDatastructure.Graph;
|
using OSMDatastructure.Graph;
|
||||||
|
|
||||||
namespace Pathfinding
|
namespace OSMImporter
|
||||||
{
|
{
|
||||||
public class RegionManager
|
public class RegionManager
|
||||||
{
|
{
|
||||||
@ -25,7 +24,7 @@ namespace Pathfinding
|
|||||||
return value;
|
return value;
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
Region? loadedRegion = RegionFromId(id);
|
Region? loadedRegion = LoadRegion(id);
|
||||||
if(loadedRegion is not null)
|
if(loadedRegion is not null)
|
||||||
_regions.Add(loadedRegion!.regionHash, value: loadedRegion);
|
_regions.Add(loadedRegion!.regionHash, value: loadedRegion);
|
||||||
return loadedRegion;
|
return loadedRegion;
|
||||||
@ -37,22 +36,23 @@ namespace Pathfinding
|
|||||||
return this._regions.Values.ToArray();
|
return this._regions.Values.ToArray();
|
||||||
}
|
}
|
||||||
|
|
||||||
private Region? RegionFromFile(string filePath)
|
private Region? LoadRegion(Coordinates coordinates)
|
||||||
{
|
{
|
||||||
Region? retRegion = null;
|
return LoadRegion(Coordinates.GetRegionHashCode(coordinates));
|
||||||
if (File.Exists(filePath))
|
|
||||||
{
|
|
||||||
FileStream regionFile = new FileStream(filePath, FileMode.Open);
|
|
||||||
retRegion = JsonSerializer.Deserialize<Region>(regionFile, Region.serializerOptions)!;
|
|
||||||
regionFile.Dispose();
|
|
||||||
}
|
|
||||||
return retRegion;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private Region? RegionFromId(ulong regionId)
|
private Region? LoadRegion(ulong id)
|
||||||
{
|
{
|
||||||
string filePath = Path.Join(workingDirectory, $"{regionId}.region");
|
Console.WriteLine($"Load Region {id}");
|
||||||
return RegionFromFile(filePath);
|
return Region.FromId(workingDirectory, id);
|
||||||
|
}
|
||||||
|
|
||||||
|
public OsmNode? GetNode(Coordinates coordinates)
|
||||||
|
{
|
||||||
|
Region? regionWithNode = GetRegion(coordinates);
|
||||||
|
if (regionWithNode is not null)
|
||||||
|
return regionWithNode.GetNode(coordinates);
|
||||||
|
else return null;
|
||||||
}
|
}
|
||||||
|
|
||||||
public OsmNode? GetNode(ulong nodeId, ulong regionId)
|
public OsmNode? GetNode(ulong nodeId, ulong regionId)
|
||||||
@ -60,76 +60,5 @@ namespace Pathfinding
|
|||||||
Region? r = GetRegion(regionId);
|
Region? r = GetRegion(regionId);
|
||||||
return r?.GetNode(nodeId);
|
return r?.GetNode(nodeId);
|
||||||
}
|
}
|
||||||
|
|
||||||
public bool TestValidConnectionForType(OsmNode node1, OsmNode node2, Tag.SpeedType type)
|
|
||||||
{
|
|
||||||
foreach (OsmEdge edge in node1.edges)
|
|
||||||
{
|
|
||||||
if (edge.neighborId.Equals(node2.nodeId))
|
|
||||||
return TestValidConnectionForType(node1, edge, type);
|
|
||||||
}
|
|
||||||
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
public bool TestValidConnectionForType(OsmNode node1, OsmEdge edge, Tag.SpeedType type)
|
|
||||||
{
|
|
||||||
double speed = GetSpeedForEdge(node1, edge.wayId, type);
|
|
||||||
if (speed != 0)
|
|
||||||
return true;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
public OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Tag.SpeedType vehicle)
|
|
||||||
{
|
|
||||||
OsmNode? closest = null;
|
|
||||||
double distance = double.MaxValue;
|
|
||||||
Region? region = GetRegion(coordinates);
|
|
||||||
if (region is null)
|
|
||||||
return null;
|
|
||||||
foreach (OsmNode node in region.nodes)
|
|
||||||
{
|
|
||||||
bool hasConnectionUsingVehicle = false;
|
|
||||||
foreach (OsmEdge edge in node.edges)
|
|
||||||
{
|
|
||||||
double speed = GetSpeedForEdge(node, edge.wayId, vehicle);
|
|
||||||
if (speed != 0)
|
|
||||||
hasConnectionUsingVehicle = true;
|
|
||||||
}
|
|
||||||
double nodeDistance = Utils.DistanceBetween(node, coordinates);
|
|
||||||
if (nodeDistance < distance && hasConnectionUsingVehicle)
|
|
||||||
{
|
|
||||||
closest = node;
|
|
||||||
distance = nodeDistance;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return closest;
|
|
||||||
}
|
|
||||||
|
|
||||||
public double GetSpeedForEdge(OsmNode node1, ulong wayId, Tag.SpeedType vehicle)
|
|
||||||
{
|
|
||||||
TagManager tags = GetRegion(node1.coordinates)!.tagManager;
|
|
||||||
Tag.WayType wayType = (Tag.WayType)tags.GetTag(wayId, Tag.TagType.highway)!;
|
|
||||||
switch (vehicle)
|
|
||||||
{
|
|
||||||
case Tag.SpeedType.pedestrian:
|
|
||||||
byte speed = Tag.defaultSpeedPedestrian[wayType];
|
|
||||||
if (speed is not 0)
|
|
||||||
return speed;
|
|
||||||
return 0;
|
|
||||||
case Tag.SpeedType.car:
|
|
||||||
case Tag.SpeedType.road:
|
|
||||||
byte? maxSpeed = (byte?)tags.GetTag(wayId, Tag.TagType.maxspeed);
|
|
||||||
if (maxSpeed is not null)
|
|
||||||
return (double)maxSpeed;
|
|
||||||
maxSpeed = Tag.defaultSpeedCar[wayType];
|
|
||||||
if(maxSpeed is not 0)
|
|
||||||
return (byte)maxSpeed;
|
|
||||||
return 0;
|
|
||||||
default:
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
Loading…
Reference in New Issue
Block a user