【Godot】AStar2Dを使用した経路探索の実装方法

extends TileMap

# 障害物のマップチップID
const CHIP_OBSTACLE = 0

# タイルマップのサイズ
export(Vector2) var map_size = Vector2(16, 9) # 16x9

# 経路の開始地点と終了地点
var path_start_position = Vector2() setget _set_path_start_position
var path_end_position = Vector2() setget _set_path_end_position


# AStarノードを生成
onready var astar_node = AStar2D.new()

# 障害物の位置リスト
onready var obstacles:PoolVector2Array = get_used_cells_by_id(CHIP_OBSTACLE)

# グリッドの半分のサイズ (左上を中心に移動させるために必要)
onready var _HALF_CELL_SIZE = cell_size / 2

func _ready() -> void:
	# 移動可能な位置のリストを作成する
	var walkable_cells_list = _get_walkable_cells(obstacles)
	
	# 移動可能な位置をAStarに登録する
	for p in walkable_cells_list:
		# 位置をインデックスに変換する
		var index = _point_to_index(p)
		# 登録
		astar_node.add_point(index, p)
	
	# AStarにノード同士の接続情報を登録する
	_astar_connect_walkable_cells(walkable_cells_list)
	
# 移動可能な位置のリストを作成する
func _get_walkable_cells(_obstacles = []) -> PoolVector2Array:
	
	# 結果
	var result := PoolVector2Array()
	
	# マップのすべての要素をチェックする
	for j in range(map_size.y):
		for i in range(map_size.x):
			
			# ポイントを作成する
			var p = Vector2(i, j)
			if p in _obstacles:
				continue # 障害物は含めない
			
			# 移動できるので結果に登録する
			result.append(p)
			
	return result

# AStarにノード同士の接続情報を登録する
func _astar_connect_walkable_cells(points_array:PoolVector2Array) -> void:
	for p in points_array:
		# インデックスに変換する
		var index = _point_to_index(p)
		
		# 上下左右に接続する
		var points_ralative := PoolVector2Array([
			Vector2(p.x + 1, p.y), # 右
			Vector2(p.x - 1, p.y), # 左
			Vector2(p.x, p.y + 1), # 下
			Vector2(p.x, p.y - 1)]) # 上
		
		# 上下左右を調べる
		for p_relative in points_ralative:
			# インデックスに変換
			var relative_index = _point_to_index(p_relative)
			
			if is_outside_map_bounds(p_relative):
				continue # 領域外なので接続できない
			if not astar_node.has_point(relative_index):
				continue # 移動不可なので接続できない
			
			# インデックス同士を接続する
			# 第3引数がfalseなので、index -> relative_index への一方通行を許可
			astar_node.connect_points(index, relative_index, false)

# AStarにノード同士の接続情報を登録する (斜め移動を許可する)
func _astar_connect_walkable_cells_diagonal(points_array:PoolVector2Array) -> void:
	for p in points_array:
		var index = _point_to_index(p)
		# 3x3の9方向探索
		for local_y in range(3):
			for local_x in range(3):
				var point_relative = Vector2(p.x + local_x - 1, p.y + local_y - 1)
				var relative_index = _point_to_index(point_relative)

				if point_relative == p or is_outside_map_bounds(point_relative):
					continue # 領域外
				if not astar_node.has_point(relative_index):
					continue # 障害物
				astar_node.connect_points(index, relative_index, true)

# 位置をインデックスに変換する
func _point_to_index(p:Vector2) -> int:
	return p.x + (p.y * map_size.x)

# ワールド座標をグリッド座標系に変換する
func _world_to_point(v:Vector2) -> Vector2:
	return Vector2(
		int(v.x/cell_size.x),
		int(v.y/cell_size.y))

# ワールド座標をインデックスに変換する
func _world_to_index(v:Vector2) -> int:
	var p := _world_to_point(v)	
	return _point_to_index(p)

# インデックスをワールド座標に変換する
func _index_to_world_position(index:int) -> Vector2:
	var i = index%map_size.x
	var j = int(index/map_size.x)
	return Vector2(
		i * cell_size.x,
		j * cell_size.y)

# 指定の位置がマップの領域外かどうか
func is_outside_map_bounds(p:Vector2) -> bool:
	if p.x < 0 or p.y < 0 or p.x >= map_size.x or p.y >= map_size.y:
		return true # 領域外
	
	# 有効なマップの範囲内
	return false

# AStarによる経路探索を実行する
func recalculate_path() -> PoolVector2Array:
	var start_index = _point_to_index(path_start_position)
	var end_index = _point_to_index(path_end_position)
	
	var point_list = astar_node.get_point_path(start_index, end_index)
	
	# ワールド座標に変換する
	for i in range(point_list.size()):
		point_list[i].x *= cell_size.x	
		point_list[i].y *= cell_size.y
		
		# 左上座標を中心に移動させる
		point_list[i].x += _HALF_CELL_SIZE.x
		point_list[i].y += _HALF_CELL_SIZE.y
	
	return point_list

# -------------------------------------------
# setter
# -------------------------------------------
# 開始地点の設定
func _set_path_start_position(v:Vector2):
	
	# グリッド座標系に変換する
	var p := _world_to_point(v)
	
	if p in obstacles:
		return # 障害物の位置には設定できない
	if is_outside_map_bounds(p):
		return # マップ領域外には設定できない
	
	path_start_position = p

# 終了地点の設定
func _set_path_end_position(v:Vector2):

	# グリッド座標系に変換する
	var p := _world_to_point(v)
	
	if p in obstacles:
		return # 障害物の位置には設定できない
	if is_outside_map_bounds(p):
		return # マップ領域外には設定できない
	
	path_end_position = p

# タイルマップのサイズ
export(Vector2) var map_size = Vector2(16, 9) # 16x9

	# マップのすべての要素をチェックする
	for j in range(map_size.y):
		for i in range(map_size.x):
			
			# ポイントを作成する
			var p = Vector2(i, j)
			if p in _obstacles:
				continue # 障害物は含めない
			
			# 移動できるので結果に登録する
			result.append(p)

# 障害物のマップチップID
const CHIP_OBSTACLE = 0

# 障害物の位置リスト
onready var obstacles:PoolVector2Array = get_used_cells_by_id(CHIP_OBSTACLE)

	# 移動可能な位置をAStarに登録する
	for p in walkable_cells_list:
		# 位置をインデックスに変換する
		var index = _point_to_index(p)
		# 登録
		astar_node.add_point(index, p)

# AStarにノード同士の接続情報を登録する
func _astar_connect_walkable_cells(points_array:PoolVector2Array) -> void:
	for p in points_array:
		# インデックスに変換する
		var index = _point_to_index(p)
		
		# 上下左右に接続する
		var points_ralative := PoolVector2Array([
			Vector2(p.x + 1, p.y), # 右
			Vector2(p.x - 1, p.y), # 左
			Vector2(p.x, p.y + 1), # 下
			Vector2(p.x, p.y - 1)]) # 上
		
		# 上下左右を調べる
		for p_relative in points_ralative:
			# インデックスに変換
			var relative_index = _point_to_index(p_relative)
			
			if is_outside_map_bounds(p_relative):
				continue # 領域外なので接続できない
			if not astar_node.has_point(relative_index):
				continue # 移動不可なので接続できない
			
			# インデックス同士を接続する
			# 第3引数がfalseなので、index -> relative_index への一方通行を許可
			astar_node.connect_points(index, relative_index, false)

# -------------------------------------------
# setter
# -------------------------------------------
# 開始地点の設定
func _set_path_start_position(v:Vector2):
	
	# グリッド座標系に変換する
	var p := _world_to_point(v)
	
	if p in obstacles:
		return # 障害物の位置には設定できない
	if is_outside_map_bounds(p):
		return # マップ領域外には設定できない
	
	path_start_position = p

# 終了地点の設定
func _set_path_end_position(v:Vector2):

	# グリッド座標系に変換する
	var p := _world_to_point(v)
	
	if p in obstacles:
		return # 障害物の位置には設定できない
	if is_outside_map_bounds(p):
		return # マップ領域外には設定できない
	
	path_end_position = p

# AStarによる経路探索を実行する
func recalculate_path() -> PoolVector2Array:
	var start_index = _point_to_index(path_start_position)
	var end_index = _point_to_index(path_end_position)
	
	var point_list = astar_node.get_point_path(start_index, end_index)
	
	# ワールド座標に変換する
	for i in range(point_list.size()):
		point_list[i].x *= cell_size.x	
		point_list[i].y *= cell_size.y
		
		# 左上座標を中心に移動させる
		point_list[i].x += _HALF_CELL_SIZE.x
		point_list[i].y += _HALF_CELL_SIZE.y
	
	return point_list

extends Node2D

# タイルマップ
onready var tile_map := $TileMap

# 経路を視覚的に表示する線
onready var line_2d := $Line2D

# キャラクター
onready var godot := $Godot

# 左マウスクリックを処理する
func _unhandled_input(event: InputEvent) -> void:
	if not event is InputEventMouseButton:
		return # マウスイベントでない
	if event.button_index != BUTTON_LEFT or not event.pressed:
		return # 左クリックでない
	
	# キャラクターから左クリックした位置までの経路を求める
	tile_map.path_start_position = godot.position
	tile_map.path_end_position = event.position
	var new_path = tile_map.recalculate_path()
	
	# 経路を線で視覚的に表示する
	line_2d.points = new_path