80 lines
2.3 KiB
C#
80 lines
2.3 KiB
C#
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// D E R E L I C T
|
|
//
|
|
/// // (c) 2003..2025
|
|
|
|
using Godot;
|
|
using System.Collections.Generic;
|
|
|
|
public static class AStarSolver
|
|
{
|
|
public static Vector3[] FindPath( AStarGraph graph, Vector3 startPos, Vector3 endPos )
|
|
{
|
|
int startId = graph.GetClosestPoint( startPos );
|
|
int endId = graph.GetClosestPoint( endPos );
|
|
|
|
log.info( $"Finding path from {startPos.Log}->{endPos.Log} ({startId}->{endId})" );
|
|
|
|
if( startId == -1 || endId == -1 )
|
|
return new Vector3[0];
|
|
|
|
var openSet = new PriorityQueue<int, float>();
|
|
var cameFrom = new Dictionary<int, int>();
|
|
|
|
var gScore = new Dictionary<int, float>();
|
|
gScore[startId] = 0;
|
|
|
|
var fScore = new Dictionary<int, float>();
|
|
fScore[startId] = startPos.DistanceTo( endPos );
|
|
|
|
openSet.Enqueue( startId, fScore[startId] );
|
|
|
|
while( openSet.Count > 0 )
|
|
{
|
|
int currentId = openSet.Dequeue();
|
|
if( currentId == endId )
|
|
{
|
|
log.debug( $"Found path!" );
|
|
return ReconstructPath( startPos, endPos, cameFrom, currentId, graph );
|
|
}
|
|
|
|
foreach( int neighborId in graph.GetPointConnections( currentId ) )
|
|
{
|
|
Vector3 currentPos = graph.GetPointPosition( currentId );
|
|
Vector3 neighborPos = graph.GetPointPosition( neighborId );
|
|
float tentativeGScore = gScore.GetValueOrDefault( currentId, float.MaxValue ) + currentPos.DistanceTo( neighborPos );
|
|
|
|
if( tentativeGScore < gScore.GetValueOrDefault( neighborId, float.MaxValue ) )
|
|
{
|
|
cameFrom[neighborId] = currentId;
|
|
gScore[neighborId] = tentativeGScore;
|
|
fScore[neighborId] = tentativeGScore + neighborPos.DistanceTo( endPos );
|
|
|
|
openSet.Enqueue( neighborId, fScore[neighborId] );
|
|
}
|
|
}
|
|
}
|
|
|
|
return new Vector3[0]; // No path found
|
|
}
|
|
|
|
private static Vector3[] ReconstructPath( Vector3 startPos, Vector3 endPos, Dictionary<int, int> cameFrom, int currentId, AStarGraph graph )
|
|
{
|
|
var path = new List<Vector3> { endPos, graph.GetPointPosition( currentId ) };
|
|
while( cameFrom.ContainsKey( currentId ) )
|
|
{
|
|
currentId = cameFrom[currentId];
|
|
path.Add( graph.GetPointPosition( currentId ) );
|
|
}
|
|
path.Add( startPos );
|
|
|
|
path.Reverse();
|
|
|
|
log.debug( $"Path is {path.Count} long." );
|
|
|
|
|
|
return path.ToArray();
|
|
}
|
|
}
|