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

Added first Pathfinding

Browse Source
This commit is contained in:
C9Glax 2023-02-05 20:03:47 +01:00
parent e3dabf8cb7
commit b6ca87ea72
3 changed files with 67 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Pathfinding/PathNode.cs
View File

@ -4,9 +4,9 @@ namespace Pathfinding;
public class PathNode : OSMDatastructure.Node
{
PathNode? previousNode = null;
private Dictionary<Connection, double> distanceDictionary = new();
double currentWeight = double.MaxValue;
public PathNode? previousPathNode = null;
public double currentPathWeight = double.MaxValue;
public double DirectDistanceToGoal = double.MaxValue;
public PathNode(float lat, float lon) : base(lat, lon)
@ -14,14 +14,4 @@ public class PathNode : OSMDatastructure.Node
}
public double? DistanceTo(Connection connection)
{
if (!this.GetConnections().Contains(connection))
return null; //TODO exception is not actually connected
if (!this.distanceDictionary.ContainsKey(connection))
this.distanceDictionary.Add(connection, Utils.DistanceBetween(this, connection.endNodeCoordinates));
return this.distanceDictionary[connection];
}
}

69
Pathfinding/Pathfinder.cs
View File

@ -6,15 +6,72 @@ namespace Pathfinding;
public class Pathfinder
{
private static void CustomAStar(string workingDir, Coordinates start, Coordinates goal)
public static List<PathNode> CustomAStar(string workingDir, Coordinates start, Coordinates goal)
{
RegionManager regionManager = new RegionManager(workingDir);
Region startRegion = regionManager.GetRegion(start)!; //TODO null handling
PathNode startNode = (PathNode)ClosestNodeToCoordinates(start, startRegion)!; //TODO null handling
Region goalRegion = regionManager.GetRegion(goal)!; //TODO null handling
PathNode goalNode = (PathNode)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
Region startRegion, goalRegion;
PathNode startNode, goalNode;
try
{
startRegion = regionManager.GetRegion(start);
startNode = (PathNode)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)ClosestNodeToCoordinates(goal, goalRegion)!; //TODO null handling
}
catch (FileNotFoundException e)
{
throw new Exception(string.Format("No region at coordinates {0}", start), e);
}
List<PathNode> toVisit = new() { startNode };
startNode.DirectDistanceToGoal = Utils.DistanceBetween(startNode, goalNode);
bool stop = false;
while (toVisit.Count > 0 && !stop)
{
PathNode closestNodeToGoal = toVisit.First();
foreach (PathNode node in toVisit)
{
if (node.DirectDistanceToGoal < closestNodeToGoal.DirectDistanceToGoal)
{
closestNodeToGoal = node;
}
}
foreach (Connection connection in closestNodeToGoal.GetConnections())
{
PathNode? neighbor = (PathNode?)regionManager.GetNode(connection.endNodeCoordinates);
if (neighbor != null && neighbor.currentPathWeight < closestNodeToGoal.currentPathWeight + Utils.DistanceBetween(closestNodeToGoal, neighbor))
{
neighbor.previousPathNode = closestNodeToGoal;
neighbor.currentPathWeight = closestNodeToGoal.currentPathWeight +
Utils.DistanceBetween(closestNodeToGoal, neighbor);
if (neighbor.Equals(goalNode))
stop = true;
else
toVisit.Add(neighbor);
}
}
}
List<PathNode> path = new();
PathNode currentNode = goalNode;
while (!currentNode.Equals(startNode))
{
path.Add(currentNode);
currentNode = currentNode.previousPathNode!;
}
path.Add(startNode);
return path;
}
private static Node? ClosestNodeToCoordinates(Coordinates coordinates, Region region)

1
Server/Server.csproj
View File

@ -9,6 +9,7 @@
<ItemGroup>
<ProjectReference Include="..\OSMSplitter\OSMSplitter.csproj" />
<ProjectReference Include="..\Pathfinding\Pathfinding.csproj" />
</ItemGroup>
</Project>