diff --git a/OSMDatastructure/OsmNode.cs b/OSMDatastructure/OsmNode.cs index f77d7ee..5aaa550 100644 --- a/OSMDatastructure/OsmNode.cs +++ b/OSMDatastructure/OsmNode.cs @@ -7,6 +7,7 @@ public class OsmNode public OsmNode? previousPathNode = null; public double currentPathWeight = double.MaxValue; + public double currentPathLength = double.MaxValue; public double directDistanceToGoal = double.MaxValue; public OsmNode(float lat, float lon) @@ -43,13 +44,13 @@ public class OsmNode { if(this.previousPathNode != null) return string.Format( - "NODE {0} Edges-Count: {1} previousPathNode: {2} currentPathWeight: {3} directDistanceToGoal: {4}", + "NODE {0} Edges-Count: {1} previousPathNode: {2} currentPathWeight: {3} currentPathLength: {4} directDistanceToGoal: {5}", this.coordinates.ToString(), this.edges.Count, this.previousPathNode.coordinates.ToString(), - this.currentPathWeight, this.directDistanceToGoal); + this.currentPathWeight, this.currentPathLength, this.directDistanceToGoal); else return string.Format( - "NODE {0} Edges-Count: {1} previousPathNode: NO PREVIOUS NODE currentPathWeight: {2} directDistanceToGoal: {3}", + "NODE {0} Edges-Count: {1} previousPathNode: NO PREVIOUS NODE currentPathWeight: {2} currentPathLength: {3} directDistanceToGoal: {4}", this.coordinates.ToString(), this.edges.Count, - this.currentPathWeight, this.directDistanceToGoal); + this.currentPathWeight, this.currentPathLength, this.directDistanceToGoal); } } \ No newline at end of file diff --git a/Pathfinding/Pathfinder.cs b/Pathfinding/Pathfinder.cs index ede82ff..81b012d 100644 --- a/Pathfinding/Pathfinder.cs +++ b/Pathfinding/Pathfinder.cs @@ -28,6 +28,7 @@ public class Pathfinder List toVisit = new() { startNode }; OsmNode closestNodeToGoal = toVisit.First(); closestNodeToGoal.currentPathWeight = 0; + closestNodeToGoal.currentPathLength = 0; bool stop = false; while (toVisit.Count > 0 && !stop) @@ -57,6 +58,7 @@ public class Pathfinder { neighbor.previousPathNode = closestNodeToGoal; neighbor.currentPathWeight = newPotentialWeight; + neighbor.currentPathLength = closestNodeToGoal.currentPathLength + Utils.DistanceBetween(closestNodeToGoal, neighbor); if (neighbor.Equals(goalNode)) stop = true;