initial commit

2026-07-04 18:22:23 -06:00
commit 55522fc6fc
272 changed files with 12391 additions and 0 deletions
@@ -0,0 +1,121 @@
+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)