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); } }