189 lines
7.5 KiB
GDScript
189 lines
7.5 KiB
GDScript
@tool
|
|
extends Node3D
|
|
|
|
@export_tool_button("Generate", "Reload") var action: Callable = generate
|
|
@export var nav_point_offset: float = 0.5
|
|
@export var debug_duration: float = 50.0
|
|
|
|
@export var collider_shape: Shape3D = SphereShape3D.new()
|
|
|
|
@onready var grid_map: GridMap = get_parent() as GridMap
|
|
|
|
var grid: AStar3D = AStar3D.new()
|
|
|
|
var cell_nav_point_ids: Dictionary[Vector3i, int] = {}
|
|
var item_orient_nav_point_positions: Dictionary[int,Vector3] = {}
|
|
var item_orient_nav_point_normals: Dictionary[int, Vector3] = {}
|
|
@export var item_pair_traversability_map: Dictionary[int, bool] = {}
|
|
|
|
var cell_neighbours: Array[Vector3i] = []
|
|
|
|
func get_item_orientation_id(tile: int, orient: int) -> int:
|
|
if tile == -1:
|
|
return 0
|
|
var ret: int = 0
|
|
ret += (tile+1) & 0b11111111
|
|
ret += (orient & 0b11111) << 8
|
|
return ret
|
|
|
|
func get_pair_id(tile_a: int, orient_a: int, tile_b: int, orient_b: int, relative_pos: Vector3i) -> int:
|
|
# tile_a: 8 bits
|
|
# orient_a: 5 bits
|
|
# tile_b: 8 bits
|
|
# orient_b: 5 bits
|
|
# relative_pos: 5 bits
|
|
|
|
var ret: int = 0
|
|
ret |= get_item_orientation_id(tile_a, orient_a)
|
|
ret |= get_item_orientation_id(tile_b, orient_b) << 13
|
|
|
|
relative_pos += Vector3i(1,1,1)
|
|
var relative_pos_id: int = relative_pos.x + (relative_pos.y * 3) + (relative_pos.z * 9)
|
|
|
|
ret |= (relative_pos_id & 0b11111) << 26
|
|
|
|
return ret
|
|
|
|
func get_quad_id(item_orient_a: int, item_orient_b: int, item_orient_c: int, item_orient_d: int, relative_pos: Vector3i) -> int:
|
|
relative_pos += Vector3i(1,1,1)
|
|
var relative_pos_id: int = relative_pos.x + (relative_pos.y * 3) + (relative_pos.z * 9)
|
|
|
|
var ret: int = 0
|
|
ret |= item_orient_a
|
|
ret |= item_orient_b << 13
|
|
ret |= (relative_pos_id & 0b11111) << 26
|
|
ret |= item_orient_c << 31
|
|
ret |= item_orient_d << 44
|
|
|
|
return ret
|
|
|
|
func find_nav_point(item: int, orientation: int, global_pos: Vector3) -> Array[Vector3]:
|
|
var id: int = get_item_orientation_id(item, orientation)
|
|
#print("Finding nav point for item %d orientation %d (ID:%d) at location %s" % [item, orientation, id, global_pos])
|
|
if item_orient_nav_point_positions.has(id):
|
|
var relative_pos: Vector3 = item_orient_nav_point_positions[id]
|
|
var normal: Vector3 = item_orient_nav_point_normals[id]
|
|
#print("cached nav point for item %d: %s" % [item, relative_pos])
|
|
return [global_pos + relative_pos + (normal * nav_point_offset),normal]
|
|
else:
|
|
var params: PhysicsRayQueryParameters3D = PhysicsRayQueryParameters3D.new()
|
|
params.from = global_pos + Vector3(0,2,0)
|
|
params.to = global_pos - Vector3(0,2,0)
|
|
#params.shape = collider_shape
|
|
var result: Dictionary = get_world_3d().direct_space_state.intersect_ray(params)
|
|
TowerUtil.draw_raycast_hit(params, result, debug_duration)
|
|
if result.has("position"):
|
|
var relative_pos: Vector3 = result["position"] - global_pos
|
|
item_orient_nav_point_positions.set(id,relative_pos)
|
|
item_orient_nav_point_normals.set(id,result["normal"])
|
|
#print("new nav point for item %d: %s" % [id, relative_pos])
|
|
return [result["position"] + (result["normal"] * nav_point_offset), result["normal"]]
|
|
return []
|
|
|
|
func check_traversability(a: Vector3, b: Vector3) -> bool:
|
|
#print("Testing traversability between %s and %s" % [a,b])
|
|
var params: PhysicsShapeQueryParameters3D = PhysicsShapeQueryParameters3D.new()
|
|
params.shape = collider_shape
|
|
params.transform = Transform3D(Basis(),a)
|
|
params.motion = b - a
|
|
var result: PackedFloat32Array = get_world_3d().direct_space_state.cast_motion(params)
|
|
|
|
var hit: bool = !is_equal_approx(1.0, result[0])
|
|
|
|
DebugDraw3D.draw_line(a,b,Color.YELLOW if hit else Color.MAGENTA, debug_duration)
|
|
return !hit
|
|
#TowerUtil.draw_raycast_hit(params, result, debug_duration)
|
|
#return !result.has("position")
|
|
|
|
func update_nav_point(id: int, cell: Vector3i) -> bool:
|
|
var cell_above: Vector3i = cell + Vector3i(0,1,0)
|
|
if grid_map.get_cell_item(cell_above) != -1:
|
|
return false # skip cells with things on top
|
|
var item: int = grid_map.get_cell_item(cell)
|
|
var orientation: int = grid_map.get_cell_item_orientation(cell)
|
|
var global_pos: Vector3 = grid_map.to_global(grid_map.map_to_local(cell))
|
|
var nav_point: Array[Vector3] = find_nav_point(item, orientation, global_pos)
|
|
grid.add_point(id, nav_point[0])
|
|
cell_nav_point_ids.set(cell, id)
|
|
#print("Nav ID for cell %s: %d" % [cell, id])
|
|
DebugDraw3D.draw_sphere(nav_point[0], 0.1, Color.GREEN, debug_duration)
|
|
DebugDraw3D.draw_arrow_ray(nav_point[0], nav_point[1], 1.0, Color.GREEN, 0.5, false, debug_duration)
|
|
return true
|
|
|
|
func update_connections(cell: Vector3i) -> void:
|
|
var nav_id: int = cell_nav_point_ids.get(cell, -1)
|
|
#print("Nav ID for cell %s: %d" % [cell, nav_id])
|
|
if nav_id == -1:
|
|
return
|
|
var global_pos: Vector3 = grid.get_point_position(nav_id)
|
|
var item: int = grid_map.get_cell_item(cell)
|
|
var orientation: int = grid_map.get_cell_item_orientation(cell)
|
|
for rel_pos in cell_neighbours:
|
|
var diagonal = abs(rel_pos.x + rel_pos.z) != 1
|
|
|
|
var neighbour: Vector3i = cell + rel_pos
|
|
#print("Testing traversability between %s and %s" % [cell, neighbour])
|
|
var neighbour_item: int = grid_map.get_cell_item(neighbour)
|
|
if neighbour_item == -1:
|
|
continue
|
|
var neighbour_orientation: int = grid_map.get_cell_item_orientation(neighbour)
|
|
var neighbour_nav_id: int = cell_nav_point_ids.get(neighbour, -1)
|
|
if neighbour_nav_id == -1:
|
|
continue
|
|
#print("\tNav IDs: %d and %d" % [nav_id, neighbour_nav_id])
|
|
if grid.are_points_connected(nav_id, neighbour_nav_id, true):
|
|
continue
|
|
var neighbour_global_pos: Vector3 = grid.get_point_position(neighbour_nav_id)
|
|
|
|
var quad_id: int = 0
|
|
if diagonal:
|
|
var diag_obstacle_y: int = neighbour.y + 1
|
|
var cell_c: Vector3i = Vector3i(cell.x, diag_obstacle_y, neighbour.z)
|
|
var cell_d: Vector3i = Vector3i(neighbour.x, diag_obstacle_y, cell.z)
|
|
#print("Testing diagonal link from a:%s to b:%s; considering c:%s and d:%s" % [cell, neighbour, cell_c, cell_d])
|
|
var tile_orient_c: int = get_item_orientation_id(grid_map.get_cell_item(cell_c),grid_map.get_cell_item_orientation(cell_c))
|
|
var tile_orient_d: int = get_item_orientation_id(grid_map.get_cell_item(cell_d),grid_map.get_cell_item_orientation(cell_d))
|
|
quad_id = get_quad_id(get_item_orientation_id(item,orientation),get_item_orientation_id(neighbour_item,neighbour_orientation),tile_orient_c,tile_orient_d,rel_pos)
|
|
else:
|
|
quad_id = get_pair_id(item, orientation, neighbour_item, neighbour_orientation, rel_pos)
|
|
|
|
DebugDraw3D.draw_text(global_pos.lerp(neighbour_global_pos,0.5), "%x"%quad_id, 32, Color.RED, debug_duration)
|
|
|
|
#var pair_id: int
|
|
var traversable: bool = true
|
|
if item_pair_traversability_map.has(quad_id):
|
|
traversable = item_pair_traversability_map.get(quad_id)
|
|
else:
|
|
traversable = check_traversability(global_pos, neighbour_global_pos)
|
|
item_pair_traversability_map.set(quad_id, traversable)
|
|
#print("New traversability pair: ID=%x Traversable=%s" % [quad_id, traversable])
|
|
#print("\tTraversable: %s" % traversable)
|
|
if traversable:
|
|
grid.connect_points(nav_id, neighbour_nav_id)
|
|
#print("Connected points %d and %d" % [nav_id, neighbour_nav_id])
|
|
DebugDraw3D.draw_line(global_pos, neighbour_global_pos, Color.NAVY_BLUE, debug_duration)
|
|
|
|
func generate() -> void:
|
|
cell_neighbours.clear()
|
|
for x in range(-1,2):
|
|
for y in range(-1,2):
|
|
for z in range(-1,2):
|
|
if x != 0 or z != 0:
|
|
cell_neighbours.append(Vector3i(x,y,z))
|
|
print(cell_neighbours)
|
|
|
|
cell_nav_point_ids.clear()
|
|
item_orient_nav_point_positions.clear()
|
|
item_pair_traversability_map.clear()
|
|
grid.clear()
|
|
|
|
var id: int = 0
|
|
|
|
for cell in grid_map.get_used_cells():
|
|
var added: bool = update_nav_point(id, cell)
|
|
if added:
|
|
id += 1
|
|
|
|
for cell in grid_map.get_used_cells():
|
|
update_connections(cell)
|