122 lines
3.7 KiB
GDScript
122 lines
3.7 KiB
GDScript
extends Node3D
|
|
class_name VehicleNode
|
|
|
|
var UUID = preload('res://addons/uuid/uuid.gd')
|
|
|
|
"""
|
|
4 | 1 byte | bool | peer provides datetime
|
|
5 | 1 byte | bool | peer provides gps
|
|
6 | 1 byte | bool | peer provides accelerometer
|
|
7 | 1 byte | bool | peer provides gyroscope
|
|
8 | 1 byte | bool | peer provides magnetometer
|
|
"""
|
|
|
|
var host_id: String = ''
|
|
@export var host: bool = true
|
|
var axis_scene_template: PackedScene = preload("res://axis/Axis.tscn")
|
|
var sensor_suites = {}
|
|
var sensor_classes = {
|
|
"PNT": [],
|
|
"ACCEL": [],
|
|
"GYRO": [],
|
|
"MAG": []
|
|
}
|
|
var datetime_sensors = {}
|
|
var gps_sensors = {}
|
|
var accelerometer_sensors = {}
|
|
var gyro_sensors = {}
|
|
var magnet_sensors = {}
|
|
|
|
var current_timestamp = 0.
|
|
var lat_rad: float = 0.
|
|
var long_rad: float = 0.
|
|
var alt_km: float = 0.
|
|
|
|
var position_3d: Vector3 = Vector3.ZERO
|
|
|
|
var heading_rad: float = 0.
|
|
|
|
|
|
# Called when the node enters the scene tree for the first time.
|
|
func _ready() -> void:
|
|
if host:
|
|
_load_id()
|
|
Signals.position_updated.connect(_on_pnt_position_updated)
|
|
print('VehicleNode ready')
|
|
#var axis_scene = axis_scene_template.instantiate()
|
|
#axis_scene.axis_length = 2.
|
|
#if not has_node('Axis'):
|
|
# add_child(axis_scene)
|
|
|
|
# Called every frame. 'delta' is the elapsed time since the previous frame.
|
|
func _process(delta: float) -> void:
|
|
var time_dict = Time.get_time_dict_from_system(true)
|
|
var unix_time = Time.get_unix_time_from_system()
|
|
current_timestamp = time_dict["hour"] * 3600 + time_dict["minute"] * 60 + time_dict["second"]
|
|
|
|
func _physics_process(delta: float) -> void:
|
|
pass
|
|
#_update_position()
|
|
#_update_rotation()
|
|
|
|
func _load_id():
|
|
var file_path = "user://host_id.idf"
|
|
if not FileAccess.file_exists(file_path):
|
|
host_id = _generate_id()
|
|
var id_file = FileAccess.open(file_path, FileAccess.WRITE)
|
|
id_file.store_line(host_id)
|
|
id_file.close()
|
|
var id_file = FileAccess.open(file_path, FileAccess.READ)
|
|
host_id = id_file.get_line()
|
|
|
|
func _generate_id():
|
|
return UUID.new().as_string()
|
|
|
|
func _update_position():
|
|
var time_offset = 0.
|
|
var sensor_data_array = []
|
|
for sensor_id in gps_sensors:
|
|
var sensor = sensor_suites[sensor_id]
|
|
var sensor_time_offset = Constants.calc_time_diff(sensor.gps_timestamp, current_timestamp)
|
|
time_offset += sensor_time_offset
|
|
sensor_data_array.append([sensor_time_offset, sensor.gps_position])
|
|
var avg_position = Vector3.ZERO
|
|
for sensor_data in sensor_data_array:
|
|
avg_position += (sensor_data[0]/time_offset) * sensor_data[1]
|
|
position = avg_position
|
|
#print(position)
|
|
#print(position.length())
|
|
var camera: Camera3D = get_node("/root/Main/Node3D/Camera3D")
|
|
camera.position = position + position.normalized() * 1
|
|
if camera.position != position:
|
|
camera.look_at(position, Vector3(0, 1, 0))
|
|
|
|
func _update_rotation():
|
|
if position.length() == 0:
|
|
return
|
|
var down_vec = -1 * position.normalized()
|
|
var up_axis = Vector3(0, 1, 0)
|
|
up_axis = (up_axis * quaternion).normalized()
|
|
var rot_axis = down_vec.cross(up_axis).normalized()
|
|
var rot_deg = up_axis.angle_to(down_vec)
|
|
var rot_quat = Quaternion(rot_axis, rot_deg).normalized()
|
|
quaternion = (quaternion * rot_quat).normalized()
|
|
|
|
func _on_pnt_position_updated(sensor_ip: String, sensor_position_3d: Vector3):
|
|
if sensor_ip not in sensor_suites:
|
|
return
|
|
if sensor_ip not in sensor_classes["PNT"]:
|
|
return
|
|
position_3d = sensor_position_3d
|
|
|
|
func add_sensor_node(sensor_node: SensorSuiteNode):
|
|
sensor_suites[sensor_node.ip_address] = sensor_node
|
|
if sensor_node.provides_pnt:
|
|
sensor_classes["PNT"].append(sensor_node.ip_address)
|
|
if sensor_node.provides_accel:
|
|
sensor_classes["ACCEL"].append(sensor_node.ip_address)
|
|
if sensor_node.provides_gyro:
|
|
sensor_classes["GYRO"].append(sensor_node.ip_address)
|
|
if sensor_node.provides_mag:
|
|
sensor_classes["MAG"].append(sensor_node.ip_address)
|