/// <summary>
/// Calculates path in current thread and returns the result
/// </summary>
/// <param name="start"></param>
/// <param name="isGoal">allows to stop search at the alternative goal point</param>
/// <param name="getSuccessors"></param>
/// <param name="estimate"></param>
public Path3D CalculateCustomPath(Vector3I start, Predicate <AStarNodeFunc3D> isGoal, Action <AStarNodeFunc3D, List <AStarNodeFunc3D> > getSuccessors, Func <AStarNodeFunc3D, double> estimate)
{
AStarFunc <AStarNodeFunc3D> calculator = null;
lock (_pathPool)
{
if (_pathPool.Count > 0)
{
calculator = _pathPool.Dequeue();
}
}
if (calculator == null)
{
calculator = new AStarFunc <AStarNodeFunc3D>();
}
var startNode = new AStarNodeFunc3D(GetCursor(start), null, 1);
#if DEBUG
var sw = Stopwatch.StartNew();
#endif
calculator.FindPath(startNode, isGoal, getSuccessors, estimate);
#if DEBUG
sw.Stop();
#endif
var path = new Path3D {
Start = start
};
#if DEBUG
path.PathFindTime = sw.Elapsed.TotalMilliseconds;
path.IterationsPerformed = calculator.Iterations;
#endif
if (calculator.Solution != null)
{
var list = calculator.Solution.Select(node3D => node3D.Cursor.GlobalPosition).ToList();
path.Points = list;
path.Goal = list.LastOrDefault();
}
lock (_pathPool)
{
_pathPool.Enqueue(calculator);
}
return(path);
}
/// <summary>
/// Calculates path in current thread and returns the result
/// </summary>
/// <param name="start"></param>
/// <param name="goal"></param>
/// <returns></returns>
public Path3D CalculatePath(Vector3I start, Vector3I goal)
{
var goalNode = new AStarNodeFunc3D(GetCursor(goal), null, 1);
return(CalculateCustomPath(start, n => n.IsSameState(goalNode), AStarNodeFunc3D.GetSuccessors, n => Vector3I.Distance(n.Cursor.GlobalPosition, goal)));
}