76 lines
2.1 KiB
GDScript
76 lines
2.1 KiB
GDScript
extends Module
|
|
|
|
var enabled := true
|
|
|
|
var pos_x_register_index:int
|
|
var pos_y_register_index:int
|
|
var pos_z_register_index:int
|
|
var index_register_index:int
|
|
|
|
var prev_index := 0
|
|
var target := Vector3i.ZERO
|
|
var path : Array[Vector3i]
|
|
|
|
func _ready() -> void:
|
|
if Terrain.instance == null:
|
|
print("No terrain to plan path on")
|
|
if cpu.get_parent() is not Robot:
|
|
print("Module not part of Robot")
|
|
|
|
func get_register_reservation_size()->int:
|
|
return 4
|
|
|
|
func set_reserved_registers(index:int)->void:
|
|
pos_x_register_index = index
|
|
pos_y_register_index = index+1
|
|
pos_z_register_index = index+2
|
|
index_register_index = index+3
|
|
|
|
func get_register_types()->Dictionary[int, String]:
|
|
return {
|
|
pos_x_register_index: "[X] Path",
|
|
pos_y_register_index: "[Y] Path",
|
|
pos_z_register_index: "[Z] Path",
|
|
index_register_index: "Idx Path"
|
|
}
|
|
|
|
func early_tick()->void:
|
|
var registers := cpu.cobor_vm.get_registers()
|
|
prev_index = registers[index_register_index]
|
|
|
|
func late_tick()->void:
|
|
var registers := cpu.cobor_vm.get_registers()
|
|
var index := registers[index_register_index]
|
|
if index < 0:
|
|
target = Vector3i(
|
|
registers[pos_x_register_index],
|
|
registers[pos_y_register_index],
|
|
registers[pos_z_register_index]
|
|
)
|
|
print("Target set: ", target)
|
|
else:
|
|
if index == prev_index:
|
|
print("index stays same")
|
|
return
|
|
if prev_index < 0:
|
|
var path_finder := VoxelAStarGrid3D.new()
|
|
path_finder.set_terrain(Terrain.instance)
|
|
path_finder.set_region(AABB(cpu.get_parent().global_position + Vector3.ONE * -100, Vector3.ONE * 200))
|
|
path = path_finder.find_path(cpu.get_parent().global_position, target)
|
|
if not path.is_empty():
|
|
path.append(target)
|
|
print("Recalculating path of size:", path.size(), " towards: ", target)
|
|
if index >= path.size():
|
|
index = path.size()-1
|
|
var step:Vector3i
|
|
if path.size() == 0:
|
|
step = Vector3i.ZERO
|
|
else:
|
|
step = path[index]
|
|
print("next step: ", step)
|
|
registers[pos_x_register_index] = step.x
|
|
registers[pos_y_register_index] = step.y
|
|
registers[pos_z_register_index] = step.z
|
|
registers[index_register_index] = index
|
|
cpu.cobor_vm.set_registers(cpu.register_size, registers)
|