public Node(Node parentNode, Node goalNode, int gCost, HeuristicFormula heuristicFormula, int x, int y)
{
this.x = x;
this.y = y;
this.gCost = gCost;
this.goalNode = goalNode;
this.parentNode = parentNode;
this.formula = heuristicFormula;
this.InitNode();
}
public PathFinder(byte[,] grid, PathFinderOptions pathFinderOptions = null)
{
if (grid == null)
{
throw new Exception("Grid cannot be null");
}
_grid = grid;
_gridX = (ushort)(_grid.GetUpperBound(0) + 1);
_gridY = (ushort)(_grid.GetUpperBound(1) + 1);
// ReSharper disable CompareOfFloatsByEqualityOperator
if (Math.Log(_gridX, 2) != (int)Math.Log(_gridX, 2) || Math.Log(_gridY, 2) != (int)Math.Log(_gridY, 2))
{
throw new Exception("Invalid Grid, size in X and Y must be power of 2");
}
// ReSharper restore CompareOfFloatsByEqualityOperator
_gridXMinus1 = (ushort)(_gridX - 1);
_gridYLog2 = (ushort)Math.Log(_gridY, 2);
if (_mCalcGrid == null || _mCalcGrid.Length != (_gridX * _gridY))
{
_mCalcGrid = new PathFinderNodeFast[_gridX * _gridY];
}
_open = new PriorityQueueB <int>(new ComparePfNodeMatrix(_mCalcGrid));
//set options
if (pathFinderOptions == null)
{
pathFinderOptions = new PathFinderOptions();
}
_formula = pathFinderOptions.Formula;
_heavyDiagonals = pathFinderOptions.HeavyDiagonals;
_heuristicEstimate = pathFinderOptions.HeuristicEstimate;
_punishChangeDirection = pathFinderOptions.PunishChangeDirection;
_tieBreaker = pathFinderOptions.TieBreaker;
_searchLimit = pathFinderOptions.SearchLimit;
_diagonals = pathFinderOptions.Diagonals;
_direction = pathFinderOptions.Diagonals
? new sbyte[, ] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
}
: new sbyte[, ] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }
};
}
public static int DetermineH(HeuristicFormula heuristicFormula, TilePosition end, int heuristicEstimate, int newLocationY, int newLocationX)
{
int h;
switch (heuristicFormula)
{
case HeuristicFormula.MaxDXDY:
h = heuristicEstimate * (Math.Max(Math.Abs(newLocationX - end.X), Math.Abs(newLocationY - end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
var hDiagonal = Math.Min(Math.Abs(newLocationX - end.X),
Math.Abs(newLocationY - end.Y));
var hStraight = (Math.Abs(newLocationX - end.X) + Math.Abs(newLocationY - end.Y));
h = (heuristicEstimate * 2) * hDiagonal + heuristicEstimate * (hStraight - 2 * hDiagonal);
break;
case HeuristicFormula.Euclidean:
h = (int)(heuristicEstimate * Math.Sqrt(Math.Pow((newLocationY - end.X), 2) + Math.Pow((newLocationY - end.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
h = (int)(heuristicEstimate * (Math.Pow((newLocationX - end.X), 2) + Math.Pow((newLocationY - end.Y), 2)));
break;
case HeuristicFormula.Custom1:
var dxy = new TilePosition(Math.Abs(end.X - newLocationX), Math.Abs(end.Y - newLocationY));
var Orthogonal = Math.Abs(dxy.X - dxy.Y);
var Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
h = heuristicEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
break;
// ReSharper disable once RedundantCaseLabel
case HeuristicFormula.Manhattan:
default:
h = heuristicEstimate * (Math.Abs(newLocationX - end.X) + Math.Abs(newLocationY - end.Y));
break;
}
return(h);
}
public PathFinder(Movable _mov, PathSettings p, Point Start, Point End, ManualResetEvent _m)
{
mov = _mov;
_doneEvent = _m;
formula = p.formula;
maxSearch = p.maxSearch;
useDiagonals = p.useDiagonals;
heavyDiagonals = p.heavyDiagonals;
tieBreaker = p.tieBreaker;
mHEstimate = p.mHEstimate;
diagonalNeighbors = p.diagonalNeighbors;
startX = Start.x;
startY = Start.y;
endX = End.x;
endY = End.y;
}
/// <summary>
/// Determines an estimated H value.
/// </summary>
/// <param name="heuristicFormula">The formula to use.</param>
/// <param name="end">The destination to work towards.</param>
/// <param name="heuristicEstimate">The weight to apply to the heuristic (default 1).</param>
/// <param name="newLocationY">The starting location.</param>
/// <param name="newLocationX">The ending location.</param>
/// <returns>An estimate to the distance to the end.</returns>
public static int DetermineH(HeuristicFormula heuristicFormula, Point end, int heuristicEstimate, int newLocationY, int newLocationX)
{
int h;
switch (heuristicFormula)
{
case HeuristicFormula.MaxDXDY:
h = heuristicEstimate * Math.Max(Math.Abs(newLocationX - end.X), Math.Abs(newLocationY - end.Y));
break;
case HeuristicFormula.DiagonalShortCut:
var hDiagonal = Math.Min(
Math.Abs(newLocationX - end.X),
Math.Abs(newLocationY - end.Y));
var hStraight = Math.Abs(newLocationX - end.X) + Math.Abs(newLocationY - end.Y);
h = (heuristicEstimate * 2 * hDiagonal) + (heuristicEstimate * (hStraight - (2 * hDiagonal)));
break;
case HeuristicFormula.Euclidean:
h = (int)(heuristicEstimate * Math.Sqrt(Math.Pow(newLocationY - end.X, 2) + Math.Pow(newLocationY - end.Y, 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
h = (int)(heuristicEstimate * (Math.Pow(newLocationX - end.X, 2) + Math.Pow(newLocationY - end.Y, 2)));
break;
case HeuristicFormula.Manhattan:
h = heuristicEstimate * (Math.Abs(newLocationX - end.X) + Math.Abs(newLocationY - end.Y));
break;
default:
throw new InvalidOperationException($"Unrecognized pathfinding heuristic {heuristicFormula}.");
}
return(h);
}
