Still requires tuning
This commit is contained in:
parent
428fcb9bf8
commit
2131ac4afe
@ -1,5 +1,6 @@
|
||||
using OSMDatastructure;
|
||||
using OSMDatastructure.Graph;
|
||||
using static OSMDatastructure.Tag;
|
||||
using WayType = OSMDatastructure.Tag.WayType;
|
||||
|
||||
namespace Pathfinding;
|
||||
@ -7,7 +8,7 @@ namespace Pathfinding;
|
||||
public static partial class Pathfinder
|
||||
{
|
||||
|
||||
private static ValueTuple<OsmNode?, OsmNode?> SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager, Tag.SpeedType vehicle)
|
||||
private static ValueTuple<OsmNode?, OsmNode?> SetupNodes(Coordinates startCoordinates, Coordinates goalCoordinates, RegionManager regionManager, SpeedType vehicle)
|
||||
{
|
||||
ValueTuple<OsmNode?, OsmNode?> retTuple = new();
|
||||
retTuple.Item1 = regionManager.ClosestNodeToCoordinates(startCoordinates, vehicle);
|
||||
@ -20,7 +21,7 @@ public static partial class Pathfinder
|
||||
return retTuple;
|
||||
}
|
||||
|
||||
private static double EdgeWeight(OsmNode node1, OsmEdge edge, Tag.SpeedType vehicle, RegionManager regionManager)
|
||||
private static double EdgeWeight(OsmNode node1, OsmEdge edge, SpeedType vehicle, RegionManager regionManager)
|
||||
{
|
||||
OsmNode? node2 = regionManager.GetNode(edge.neighborId, edge.neighborRegion);
|
||||
if (node2 is null)
|
||||
@ -30,36 +31,68 @@ public static partial class Pathfinder
|
||||
return speed is 0 ? double.MaxValue : distance / speed;
|
||||
}
|
||||
|
||||
//TODO when refs are imported use same road
|
||||
private static double GetPriority(OsmNode current, OsmEdge edge, Tag.SpeedType vehicle, RegionManager regionManager)
|
||||
private static double GetPriority(OsmNode currentNode, OsmNode? previousNode, OsmEdge currentEdge, SpeedType vehicle, RegionManager regionManager)
|
||||
{
|
||||
if (vehicle == Tag.SpeedType.any)
|
||||
if (vehicle == SpeedType.any)
|
||||
return 1;
|
||||
const double roadPriorityFactor = 1;
|
||||
const double roadSpeedFactor = 0.0001;
|
||||
Region r = regionManager.GetRegion(current.coordinates)!;
|
||||
|
||||
double roadPriority = GetPriorityVehicleRoad(edge, vehicle, r) * roadPriorityFactor;
|
||||
double roadSpeed = regionManager.GetSpeedForEdge(current, edge.wayId, vehicle) * roadSpeedFactor;
|
||||
const double roadPriorityFactor = 3.5;
|
||||
const double roadSpeedFactor = 1.4;
|
||||
const double roadNameChangeFactor = 1.4;
|
||||
const double distanceDeltaFactor = 0.17;
|
||||
Region r = regionManager.GetRegion(currentNode.coordinates)!;
|
||||
|
||||
if (vehicle == Tag.SpeedType.pedestrian)
|
||||
return current.directDistanceToGoal / roadPriority;
|
||||
double distanceDelta = 0;
|
||||
if(previousNode is not null)
|
||||
distanceDelta = (previousNode.directDistanceToGoal - currentNode.directDistanceToGoal) * distanceDeltaFactor;
|
||||
double roadPriority = GetPriorityVehicleRoad(currentEdge, vehicle, r) * roadPriorityFactor;
|
||||
double roadSpeed = regionManager.GetSpeedForEdge(currentNode, currentEdge.wayId, vehicle) * 0.1 * roadSpeedFactor;
|
||||
|
||||
if (vehicle == SpeedType.pedestrian)
|
||||
return currentNode.directDistanceToGoal / (roadPriority + roadSpeed + distanceDelta);
|
||||
|
||||
if (vehicle == Tag.SpeedType.car)
|
||||
return current.directDistanceToGoal / roadPriority;
|
||||
double wayChange = 0;
|
||||
if (previousNode is not null && previousNode.edges.Count > 0)
|
||||
{
|
||||
OsmEdge? pEdge = previousNode.edges.FirstOrDefault(e => e.neighborId.Equals(currentNode.nodeId));
|
||||
if (pEdge is not null)
|
||||
{
|
||||
TagManager? prevTags = regionManager.GetRegion(previousNode.coordinates)?.tagManager;
|
||||
if (prevTags is not null)
|
||||
{
|
||||
TagManager curTags = r.tagManager;
|
||||
|
||||
bool sameName = false;
|
||||
string? curName = (string?)curTags.GetTag(currentEdge.wayId, TagType.name);
|
||||
if (curName is not null && (string?)prevTags.GetTag(pEdge.wayId, TagType.name) == curName)
|
||||
sameName = true;
|
||||
|
||||
bool sameRef = false;
|
||||
string? curRef = (string?)curTags.GetTag(currentEdge.wayId, TagType.tagref);
|
||||
if (curRef is not null && (string?)prevTags.GetTag(pEdge.wayId, TagType.tagref) == curRef)
|
||||
sameRef = true;
|
||||
|
||||
wayChange = (sameRef || sameName ? 1 : 0) * roadNameChangeFactor;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
double div = (roadPriority + wayChange + roadSpeed + (distanceDelta > 0 ? distanceDelta : 0)) + 1;
|
||||
double prio = currentNode.directDistanceToGoal / div;
|
||||
Console.WriteLine($"{currentNode.directDistanceToGoal:000000.00}/{div:+00.00;-00.00;000.00}={prio:+00000.00;-00000.00;000000.00} Type{roadPriority:00.00} name{wayChange:00.00} speed{roadSpeed:00.00} distance{distanceDelta:+00.00;-00.00;0}");
|
||||
if (vehicle == SpeedType.car)
|
||||
return prio;
|
||||
|
||||
return double.MaxValue;
|
||||
}
|
||||
|
||||
private static double GetPriorityVehicleRoad(OsmEdge edge, Tag.SpeedType vehicle, Region region)
|
||||
private static double GetPriorityVehicleRoad(OsmEdge edge, SpeedType vehicle, Region region)
|
||||
{
|
||||
if (vehicle == Tag.SpeedType.any)
|
||||
if (vehicle == SpeedType.any)
|
||||
return 1;
|
||||
WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, Tag.TagType.highway);
|
||||
WayType? wayType = (WayType?)region.tagManager.GetTag(edge.wayId, TagType.highway);
|
||||
if(wayType is null)
|
||||
return double.MaxValue;
|
||||
if (vehicle == Tag.SpeedType.car)
|
||||
if (vehicle == SpeedType.car)
|
||||
{
|
||||
switch (wayType)
|
||||
{
|
||||
@ -70,24 +103,25 @@ public static partial class Pathfinder
|
||||
case WayType.trunk_link:
|
||||
case WayType.primary:
|
||||
case WayType.primary_link:
|
||||
return 7;
|
||||
return 8;
|
||||
case WayType.secondary:
|
||||
case WayType.secondary_link:
|
||||
case WayType.tertiary:
|
||||
case WayType.tertiary_link:
|
||||
return 4;
|
||||
return 5;
|
||||
case WayType.unclassified:
|
||||
case WayType.residential:
|
||||
case WayType.road:
|
||||
case WayType.living_street:
|
||||
return 3;
|
||||
case WayType.service:
|
||||
case WayType.track:
|
||||
return 2;
|
||||
return 0.01;
|
||||
default:
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
if (vehicle == Tag.SpeedType.pedestrian)
|
||||
if (vehicle == SpeedType.pedestrian)
|
||||
{
|
||||
switch (wayType)
|
||||
{
|
||||
@ -115,7 +149,7 @@ public static partial class Pathfinder
|
||||
}
|
||||
}
|
||||
|
||||
return 100;
|
||||
return 0.01;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -21,7 +21,6 @@ public static partial class Pathfinder
|
||||
while (toVisit.Count > 0)
|
||||
{
|
||||
OsmNode currentNode = toVisit.Dequeue();
|
||||
Console.WriteLine($"{toVisit.Count} {currentNode.directDistanceToGoal}");
|
||||
|
||||
foreach (OsmEdge edge in currentNode.edges.Where(
|
||||
edge => regionManager.TestValidConnectionForType(currentNode, edge, vehicle)))
|
||||
@ -44,7 +43,7 @@ public static partial class Pathfinder
|
||||
{
|
||||
currentNode = neighbor;
|
||||
currentNode.currentPathLength = 0;
|
||||
while (currentNode != startAndEndNode.Item1 && currentNode.previousPathNode is not null)
|
||||
while (!currentNode.Equals(startAndEndNode.Item1) && currentNode.previousPathNode is not null)
|
||||
{
|
||||
currentNode.previousPathNode.currentPathLength = currentNode.currentPathLength +
|
||||
Utils.DistanceBetween(currentNode, currentNode.previousPathNode);
|
||||
@ -53,8 +52,8 @@ public static partial class Pathfinder
|
||||
|
||||
return GetRouteFromCalc(goalNode, regionManager);
|
||||
}
|
||||
//if (!toVisit.UnorderedItems.Any(item => item.Element.Equals(neighbor)))
|
||||
toVisit.Enqueue(neighbor, GetPriority(currentNode, edge, vehicle, regionManager));
|
||||
|
||||
toVisit.Enqueue(neighbor, GetPriority(currentNode, currentNode.previousPathNode, edge, vehicle, regionManager));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user