Pathfind

# I made changes to original code. The original code is here:
# https://github.com/GDquest/Godot-engine-tutorial-demos/blob/master/2018/03-30-astar-pathfinding/pathfind_astar.gd
# **MIT License**
#
# Copyright (c) 2017 Nathan Lovato
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
extends Object
class_name SEnemyGridAStarPathfind

var astar : AStar setget , get_astar
var _astar : AStar
var _map_size : Vector2
var _point_path : PoolVector3Array = PoolVector3Array()
var _tile_map : TileMap
var _obstacle_cells_id: int
var _half_cell_size : Vector2


func get_astar() -> AStar:
	if _astar == null:
		_astar = AStar.new()
	return _astar


func _init(p_tile_map: TileMap, p_obstacle_cells_id: int -> void:
	_tile_map = p_tile_map
	_map_size = _tile_map.get_used_rect().size
	_half_cell_size = _tile_map.cell_size * 0.5
	_obstacle_cells_id = p_obstacle_cells_id
	set_obstacles(_tile_map.get_used_cells_by_id(_obstacle_cells_id))


func set_obstacles(p_obstacle_positions: PoolVector2Array) -> void:
	var walkable_cells_list : PoolVector2Array = astar_add_walkable_cells(p_obstacle_positions)
	astar_connect_walkable_cells(walkable_cells_list)


# Loops through all cells within the map's bounds and
# adds all points to the _astar, except the obstacles
func astar_add_walkable_cells(p_obstacle_positions: PoolVector2Array) -> PoolVector2Array:
	var points_array: PoolVector2Array = PoolVector2Array()
	for y in range(_map_size.y):
		for x in range(_map_size.x):
			var point = Vector2(x, y)
			if point in p_obstacle_positions:
				continue

			points_array.append(point)
			# The AStar class references points with indices
			# Using a function to calculate the index from a point's coordinates
			# ensures we always get the same index with the same input point
			var point_index = calculate_point_index(point)
			# AStar works for both 2d and 3d, so we have to convert the point
			# coordinates from and to Vector3s
			self.astar.add_point(point_index, Vector3(point.x, point.y, 0))
	return points_array


func astar_connect_walkable_cells(p_points_array: PoolVector2Array) -> void:
	for point in p_points_array:
		var point_index = calculate_point_index(point)
		# For every cell in the map, we check the one to the top, right.
		# left and bottom of it. If it's in the map and not an obstalce,
		# We connect the current point with it
		var points_relative = PoolVector2Array([
			Vector2(point.x + 1, point.y),
			Vector2(point.x - 1, point.y),
			Vector2(point.x, point.y + 1),
			Vector2(point.x, point.y - 1)])
		for point_relative in points_relative:
			var point_relative_index = calculate_point_index(point_relative)

			if is_outside_map_bounds(point_relative):
				continue
			if not self.astar.has_point(point_relative_index):
				continue
			# Note the 3rd argument. It tells the self.astar that we want the
			# connection to be bilateral: from point A to B and B to A
			# If you set this value to false, it becomes a one-way path
			# As we loop through all points we can set it to false
			self.astar.connect_points(point_index, point_relative_index, false)


func is_outside_map_bounds(p_point: Vector2) -> bool:
	var r : bool = p_point.x < 0 or p_point.y < 0 or p_point.x >= _map_size.x or p_point.y >= _map_size.y
	return r


func calculate_point_index(p_point: Vector2) -> int:
	var r: int = p_point.x + _map_size.x * p_point.y
	return r


func get_path(p_start_position: Vector2, p_end_position: Vector2) -> PoolVector2Array:
	_recalculate_path(p_start_position, p_end_position)
	var path_world : PoolVector2Array = PoolVector2Array()
	for point in _point_path:
		var point_world = _tile_map.map_to_world(Vector2(point.x, point.y)) + _half_cell_size
		path_world.append(point_world)
	return path_world


func _recalculate_path(p_start_position: Vector2, p_end_position: Vector2) -> void:

	# WRONG LINES
	#var start_point_index: = calculate_point_index(p_start_position)
    #var end_point_index: = calculate_point_index(p_end_position)

	# FIXED LINES
	var start_point_index: = calculate_point_index(_tile_map.world_to_map(p_start_position))
	var end_point_index: = calculate_point_index(_tile_map.world_to_map(p_end_position))

	# This method gives us an array of points. Note you need the start and end
	# points' indices as input
	_point_path = self.astar.get_point_path(start_point_index, end_point_index)