89 lines
2.8 KiB
GDScript
89 lines
2.8 KiB
GDScript
|
extends Node3D
|
||
|
|
class_name SatelliteWidget3dNode
|
||
|
|
|
||
|
|
var sensor_class: String
|
||
|
|
var used_sensors: Dictionary = {}
|
||
|
|
|
||
|
|
var host_vehicle: VehicleNode
|
||
|
|
var vehicle_3d_node: Node3D
|
||
|
|
var vehicle_position: Vector3 = Vector3.ZERO
|
||
|
|
var sat_scene: PackedScene = preload("res://widgets/satellites/satellite.tscn")
|
||
|
|
var sat_nodes = {}
|
||
|
|
var need_sat_update: bool = true
|
||
|
|
var sat_update_timer: float = 0
|
||
|
|
var sat_update_rate: float = 60.
|
||
|
|
|
||
|
|
var lines = []
|
||
|
|
|
||
|
|
var GPS_ALT_KM = 20_200
|
||
|
|
|
||
|
|
# Called when the node enters the scene tree for the first time.
|
||
|
|
func _ready() -> void:
|
||
|
|
GPS_ALT_KM += Constants.EARTH_RADIUS_KM
|
||
|
|
Signals.vehicle_added.connect(_on_vehicle_added)
|
||
|
|
Signals.requested_host_vehicle.emit(self)
|
||
|
|
Signals.sat_position_updated.connect(_on_sat_position_updated)
|
||
|
|
Signals.position_updated.connect(_on_position_updated)
|
||
|
|
|
||
|
|
func _process(delta: float):
|
||
|
|
sat_update_timer += delta
|
||
|
|
if sat_update_timer >= sat_update_rate:
|
||
|
|
sat_update_timer = 0
|
||
|
|
need_sat_update = true
|
||
|
|
if host_vehicle != null:
|
||
|
|
return
|
||
|
|
Signals.requested_host_vehicle.emit(self)
|
||
|
|
|
||
|
|
func _on_vehicle_added(new_vehicle):
|
||
|
|
host_vehicle = new_vehicle
|
||
|
|
|
||
|
|
func set_host_vehicle(new_vehicle):
|
||
|
|
host_vehicle = new_vehicle
|
||
|
|
vehicle_3d_node = Node3D.new()
|
||
|
|
add_child(vehicle_3d_node)
|
||
|
|
|
||
|
|
func _on_sat_position_updated(sensor_ip: String, sat_position_data: Array):
|
||
|
|
if vehicle_3d_node == null:
|
||
|
|
return
|
||
|
|
if not need_sat_update:
|
||
|
|
return
|
||
|
|
need_sat_update = false
|
||
|
|
var current_sat_ids = []
|
||
|
|
for sat_data in sat_position_data:
|
||
|
|
var sat_id = sat_data[0]
|
||
|
|
var el_rad = deg_to_rad(sat_data[1])
|
||
|
|
var az_rad = deg_to_rad(sat_data[2])
|
||
|
|
current_sat_ids.append(sat_id)
|
||
|
|
if sat_id not in sat_nodes:
|
||
|
|
_create_sat_node(sat_id)
|
||
|
|
_update_sat_position(sat_id, el_rad, az_rad)
|
||
|
|
for old_sat_id in sat_nodes.keys():
|
||
|
|
if old_sat_id not in current_sat_ids:
|
||
|
|
vehicle_3d_node.remove_child(sat_nodes[old_sat_id])
|
||
|
|
sat_nodes[old_sat_id].queue_free()
|
||
|
|
sat_nodes.erase(old_sat_id)
|
||
|
|
|
||
|
|
func _create_sat_node(sat_id: String):
|
||
|
|
var new_sat_node = sat_scene.instantiate()
|
||
|
|
sat_nodes[sat_id] = new_sat_node
|
||
|
|
vehicle_3d_node.add_child(new_sat_node)
|
||
|
|
|
||
|
|
func _update_sat_position(sat_id, el_rad, az_rad):
|
||
|
|
# vehicle node is rotated to SEZ.
|
||
|
|
# Az is rotated clockwise from north
|
||
|
|
# 0 el is horizon. 90 is zenith
|
||
|
|
var x = sin(el_rad) * cos(az_rad + PI)
|
||
|
|
var y = sin(el_rad) * sin(az_rad + PI)
|
||
|
|
var z = cos(el_rad)
|
||
|
|
var sat_position = Vector3(x, y, z)
|
||
|
|
var global_sat_position = ((vehicle_position.normalized() + sat_position) * GPS_ALT_KM)
|
||
|
|
var local_sat_position = global_sat_position - vehicle_position
|
||
|
|
sat_nodes[sat_id].position = Vector3(local_sat_position.x, local_sat_position.z, local_sat_position.y)
|
||
|
|
|
||
|
|
func _on_position_updated(sensor_ip: String, physics_position: Vector3):
|
||
|
|
if vehicle_3d_node == null:
|
||
|
|
return
|
||
|
|
vehicle_position = physics_position
|
||
|
|
vehicle_3d_node.position = Vector3(physics_position.x, physics_position.z, physics_position.y)
|
||
|
|
|