public void Init(MatrixMap matrixMap, Enemy enemy, Action onEnemyPassed, EPathFormula formula, float moveSpeed, float x, float y, int targetX, int targetY)
{
_matrixMap = matrixMap;
_enemy = enemy;
_onEnemyPassed = onEnemyPassed;
_formula = formula;
_moveSpeed = moveSpeed;
_startGridPosition = new GridPosition(x, y);
_endGridPosition = new GridPosition(targetX, targetY);
InitializePosition();
UpdatePath();
}
protected virtual double NeighborDistance(PathNode inStart, PathNode inEnd, EPathFormula formula)
{
var diffX = Math.Abs(inStart.X - inEnd.X);
var diffY = Math.Abs(inStart.Y - inEnd.Y);
switch (diffX + diffY)
{
case 1: return(1);
case 2: return(Math.Sqrt(2));
case 0: return(0);
default:
throw new ApplicationException();
}
}
protected override double Heuristic(PathNode inStart, PathNode inEnd, EPathFormula formula)
{
var dx = Math.Abs(inStart.X - inEnd.X);
var dy = Math.Abs(inStart.Y - inEnd.Y);
switch (formula)
{
case EPathFormula.Eucledian:
return(Math.Sqrt(dx * dx + dy * dy));
case EPathFormula.EucledianSquared:
return(dx * dx + dy * dy);
case EPathFormula.Diagonal:
return(Math.Min(dx, dy));
case EPathFormula.Manhatten:
return(dx * dy + dx + dy);
}
return(Math.Abs(inStart.X - inEnd.X) + Math.Abs(inStart.Y - inEnd.Y));
}
protected override double NeighborDistance(PathNode inStart, PathNode inEnd, EPathFormula formula)
{
return(Heuristic(inStart, inEnd, formula));
}
/// <summary>
/// Returns null, if no path is found. Start- and End-node are included in returned path. The user context
/// is passed to IsWalkable().
/// </summary>
public LinkedList <TPathNode> Search(Vector2 inStartNode, Vector2 inEndNode, TUserContext inUserContext, EPathFormula formula, bool flying)
{
var startNode = _searchSpace[(int)inStartNode.x, (int)inStartNode.y];
var endNode = _searchSpace[(int)inEndNode.x, (int)inEndNode.y];
if (startNode == endNode)
{
return(new LinkedList <TPathNode>(new[] { startNode.UserContext }));
}
var neighborNodes = new PathNode[4];
_closedSet.Clear();
_openSet.Clear();
_runtimeGrid.Clear();
_orderedOpenSet.Clear();
for (var x = 0; x < Width; x++)
{
for (var y = 0; y < Height; y++)
{
_cameFrom[x, y] = null;
}
}
startNode.G = 0;
startNode.H = Heuristic(startNode, endNode, formula);
startNode.F = startNode.H;
_openSet.Add(startNode);
_orderedOpenSet.Push(startNode);
_runtimeGrid.Add(startNode);
while (!_openSet.IsEmpty)
{
var x = _orderedOpenSet.Pop();
if (x == endNode)
{
var result = ReconstructPath(_cameFrom, _cameFrom[endNode.X, endNode.Y]);
result.AddLast(endNode.UserContext);
return(result);
}
_openSet.Remove(x);
_closedSet.Add(x);
StoreNeighborNodes(x, neighborNodes);
foreach (var y in neighborNodes)
{
bool tentativeIsBetter;
if (y == null)
{
continue;
}
if (!flying)
{
if (!y.UserContext.IsWalkable(inUserContext))
{
continue;
}
}
if (_closedSet.Contains(y))
{
continue;
}
var tentativeGScore = _runtimeGrid[x].G + NeighborDistance(x, y, formula);
var wasAdded = false;
if (!_openSet.Contains(y))
{
_openSet.Add(y);
tentativeIsBetter = true;
wasAdded = true;
}
else if (tentativeGScore < _runtimeGrid[y].G)
{
tentativeIsBetter = true;
}
else
{
tentativeIsBetter = false;
}
if (tentativeIsBetter)
{
_cameFrom[y.X, y.Y] = x;
if (!_runtimeGrid.Contains(y))
{
_runtimeGrid.Add(y);
}
_runtimeGrid[y].G = tentativeGScore;
_runtimeGrid[y].H = Heuristic(y, endNode, formula);
_runtimeGrid[y].F = _runtimeGrid[y].G + _runtimeGrid[y].H;
if (wasAdded)
{
_orderedOpenSet.Push(y);
}
else
{
_orderedOpenSet.Update(y);
}
}
}
}
return(null);
}
protected virtual double Heuristic(PathNode inStart, PathNode inEnd, EPathFormula formula)
{
return(Math.Sqrt((inStart.X - inEnd.X) * (inStart.X - inEnd.X) + (inStart.Y - inEnd.Y) * (inStart.Y - inEnd.Y)));
}
