public AStarPosition(Position position, AStarPosition previousPosition, int cost, int distance)
{
Position = position;
PreviousPosition = previousPosition;
Cost = cost;
Distance = distance;
}
/// <summary>
/// <paramref name="targetPosition"/>と隣接する追加可能のノードを<see cref="_openList"/>に追加します。
/// </summary>
private void OpenSurroundingPoints(AStarPosition targetPosition)
{
int targetPositionX = targetPosition.Position.axisX,
targetPositionY = targetPosition.Position.axisY;
// 隣接する追加可能なノードを_openListsに追加します。
var nextPoint = new Position(targetPositionX - 1, targetPositionY);
if (targetPositionX > 0 && !_obstacles.Contains(nextPoint))
{
AddOpenPoint(nextPoint, targetPosition);
}
nextPoint = new Position(targetPositionX, targetPositionY + 1);
if (targetPositionY < _height - 1 && !_obstacles.Contains(nextPoint))
{
AddOpenPoint(nextPoint, targetPosition);
}
nextPoint = new Position(targetPositionX + 1, targetPositionY);
if (targetPositionX < _width - 1 && !_obstacles.Contains(nextPoint))
{
AddOpenPoint(nextPoint, targetPosition);
}
nextPoint = new Position(targetPositionX, targetPositionY - 1);
if (targetPositionY > 0 && !_obstacles.Contains(nextPoint))
{
AddOpenPoint(nextPoint, targetPosition);
}
}
/// <summary>
/// 経路を算出します。
/// </summary>
private IEnumerable <Position> FindPath()
{
AStarPosition currentPosition = null;
// 毎回f値が一番小さいノードを_closeListに追加して、隣接するノードを_openListに追加します。
while (!_closeList.Any(point => point.Position == _endPoint) && _openList.Count != 0)
{
currentPosition = GetMinLengthPosition();
_closeList.Add(currentPosition);
_openList.Remove(currentPosition);
OpenSurroundingPoints(currentPosition);
}
if (currentPosition == null || !_closeList.Any(point => point.Position == _endPoint))
{
// 経路が見つからなかった場合、nullを返します。
return(null);
}
else
{
// 経路を返します。
var path = new List <Position>();
path.Add(currentPosition.Position);
while (currentPosition.PreviousPosition != null)
{
currentPosition = currentPosition.PreviousPosition;
path.Add(currentPosition.Position);
}
path.Reverse();
return(path);
}
AStarPosition GetMinLengthPosition()
{
var minLengthPosition = _openList.First();
int minLength = minLengthPosition.TotalLength;
foreach (var openPoint in _openList)
{
if (openPoint.TotalLength < minLength)
{
minLength = openPoint.TotalLength;
minLengthPosition = openPoint;
}
}
return(minLengthPosition);
}
}
/// <summary>
/// 対象位置にあるノードを<see cref="_openList"/>に追加します。
/// </summary>
private void AddOpenPoint(Position targetPosition, AStarPosition previousPosition)
{
int cost = (previousPosition?.Cost ?? 0) + 1,
distance = GetDistance(targetPosition, _endPoint),
totalLength = cost + distance;
var nextAstarPosition = new AStarPosition(targetPosition, previousPosition, cost, distance);
var existedClosePosition = _closeList.FirstOrDefault(position => position.Position == nextAstarPosition.Position);
if (existedClosePosition != null)
{
// _closeListに存在し、f値が既存のより小さい場合、_closeListからノードを削除し、後ほど_openListに追加します。
if (existedClosePosition.TotalLength > totalLength)
{
_closeList.Remove(existedClosePosition);
}
else
{
return;
}
}
var existedOpenPosition = _openList.FirstOrDefault(position => position.Position == nextAstarPosition.Position);
if (existedOpenPosition != null)
{
// _openListに存在し、f値が既存のより小さい場合、既存のノードを置き換えます。
if (existedOpenPosition.TotalLength > totalLength)
{
existedOpenPosition.Cost = cost;
existedOpenPosition.Distance = distance;
existedOpenPosition.PreviousPosition = previousPosition;
}
else
{
return;
}
}
else
{
_openList.Add(nextAstarPosition);
}
}
