glax/OSMServer
Archived
1
0
Fork 0
Code Pull Requests Releases Wiki Activity

Datastructure rewrite

Browse Source
This commit is contained in:
C9Glax
2023-02-06 17:32:55 +01:00
parent 2c18162398
commit e265c56bce
19 changed files with 567 additions and 658 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
Pathfinding/PathNode.cs
View File

@ -1,22 +0,0 @@
using OSMDatastructure;
namespace Pathfinding;
public class PathNode : Node
{
public PathNode? previousPathNode = null;
public double currentPathWeight = double.MaxValue;
public double DirectDistanceToGoal = double.MaxValue;
public PathNode(float lat, float lon) : base(lat, lon)
{
}
public static PathNode FromNode(Node node)
{
return new PathNode(node.lat, node.lon);
}
}

74
Pathfinding/Pathfinder.cs
View File

@ -6,48 +6,50 @@ namespace Pathfinding;
public class Pathfinder
{
public static List<PathNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
public static List<OsmNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
{
RegionManager regionManager = new RegionManager(workingDir);
Region startRegion, goalRegion;
PathNode startNode, goalNode;
OsmNode? startNode, goalNode;
try
{
startRegion = regionManager.GetRegion(start);
startNode = PathNode.FromNode(ClosestNodeToCoordinates(start, startRegion)!); //TODO null handling
}
catch (FileNotFoundException e)
{
throw new Exception(string.Format("No region at coordinates {0}", start), e);
}
try
{
goalRegion = regionManager.GetRegion(goal);
goalNode = PathNode.FromNode(ClosestNodeToCoordinates(goal, goalRegion)!); //TODO null handling
}
catch (FileNotFoundException e)
{
throw new Exception(string.Format("No region at coordinates {0}", start), e);
throw new Exception(string.Format("No region at coordinates {0}", e.FileName), e);
}
startNode = ClosestNodeToCoordinates(start, startRegion);
goalNode = ClosestNodeToCoordinates(goal, goalRegion);
if (startNode == null || goalNode == null)
return new List<OsmNode>();
List<PathNode> toVisit = new() { startNode };
startNode.DirectDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
List<OsmNode> toVisit = new() { startNode };
OsmNode closestNodeToGoal;
bool stop = false;
while (toVisit.Count > 0 && !stop)
{
PathNode closestNodeToGoal = toVisit.First();
foreach (PathNode node in toVisit)
Console.WriteLine("toVisit length: {0}", toVisit.Count.ToString());
closestNodeToGoal = toVisit.First();
foreach (OsmNode node in toVisit)
{
if (node.DirectDistanceToGoal.Equals(double.MaxValue))
{
node.DirectDistanceToGoal = Utils.DistanceBetween(node, goalNode);
}
if (node.DirectDistanceToGoal < closestNodeToGoal.DirectDistanceToGoal)
{
closestNodeToGoal = node;
}
}
foreach (Connection connection in closestNodeToGoal.GetConnections())
foreach (OsmEdge connection in closestNodeToGoal.edges)
{
PathNode? neighbor = (PathNode)regionManager.GetNode(connection.endNodeCoordinates);
OsmNode? neighbor = regionManager.GetNode(connection.neighborCoordinates);
Console.WriteLine(neighbor);
if (neighbor != null && neighbor.currentPathWeight < closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor))
{
neighbor.previousPathNode = closestNodeToGoal;
@ -60,40 +62,36 @@ public class Pathfinder
toVisit.Add(neighbor);
}
}
toVisit.Remove(closestNodeToGoal);
}
List<PathNode> path = new();
PathNode currentNode = goalNode;
while (!currentNode.Equals(startNode))
List<OsmNode> path = new();
OsmNode? currentNode = goalNode;
while (currentNode != null && !currentNode.Equals(startNode))
{
path.Add(currentNode);
currentNode = currentNode.previousPathNode!;
currentNode = currentNode.previousPathNode;
}
path.Add(startNode);
return path;
}
private static Node? ClosestNodeToCoordinates(Coordinates coordinates, Region region)
private static OsmNode? ClosestNodeToCoordinates(Coordinates coordinates, Region region)
{
ulong? closestId = ClosestNodeIdToCoordinates(coordinates, region);
return closestId != null ? region.GetNode((ulong)closestId) : null;
}
private static ulong? ClosestNodeIdToCoordinates(Coordinates coordinates, Region region)
{
ulong? closestId = null;
double closestDistance = double.MaxValue, distance;
foreach (KeyValuePair<ulong, Node> kv in region.GetNodes())
OsmNode? closest = null;
double distance = double.MaxValue;
foreach (OsmNode node in region.nodes)
{
distance = Utils.DistanceBetween(kv.Value, coordinates);
if (distance < closestDistance)
double nodeDistance = Utils.DistanceBetween(node, coordinates);
if (nodeDistance < distance)
{
closestDistance = distance;
closestId = kv.Key;
closest = node;
distance = nodeDistance;
}
}
return closestId;
return closest;
}
}

19
Pathfinding/RegionManager.cs
View File

@ -1,4 +1,5 @@
using OSMDatastructure;
using Pathfinding;
namespace OSMImporter
{
@ -59,23 +60,9 @@ namespace OSMImporter
return ByteConverter.ToRegion(regionBytes);
}
public Node? GetNode(ulong id)
public OsmNode? GetNode(Coordinates coordinates)
{
foreach (Region region in this._regions.Values)
{
Node? node = region.GetNode(id);
if (node != null)
return node;
}
return null;
}
public Node? GetNode(Coordinates coordinates)
{
Region? regionWithNode = GetRegion(coordinates);
if (regionWithNode == null)
return null;//TODO null handling
Region regionWithNode = GetRegion(coordinates);
return regionWithNode.GetNode(coordinates);
}
}

19
Pathfinding/Utils.cs
View File

@ -6,12 +6,12 @@ namespace Pathfinding
{
public static double DistanceBetween(Coordinates c1, Coordinates n2)
{
return DistanceBetween(c1.lat, c1.lon, n2.lat, n2.lon);
return DistanceBetween(c1.latitude, c1.longitude, n2.latitude, n2.longitude);
}
public static double DistanceBetween(Coordinates c1, float lat2, float lon2)
{
return DistanceBetween(c1.lat, c1.lon, lat2, lon2);
return DistanceBetween(c1.latitude, c1.longitude, lat2, lon2);
}
public static double DistanceBetween(float lat1, float lon1, Coordinates c2)
@ -19,6 +19,21 @@ namespace Pathfinding
return DistanceBetween(c2, lat1, lon1);
}
public static double DistanceBetween(OsmNode node1, OsmNode node2)
{
return DistanceBetween(node1.coordinates, node2.coordinates);
}
public static double DistanceBetween(OsmNode node1, Coordinates c2)
{
return DistanceBetween(node1.coordinates, c2);
}
public static double DistanceBetween(Coordinates c1, OsmNode n2)
{
return DistanceBetween(c1, n2.coordinates);
}
public static double DistanceBetween(float lat1, float lon1, float lat2, float lon2) //Law of Cosines
{
/*