67 lines
3.1 KiB
GDScript
67 lines
3.1 KiB
GDScript
extends Node3D
|
|
|
|
signal use_fallback(active: bool)
|
|
|
|
## Rotation of cyclic stick model for full control deflection, in degrees
|
|
@export var range_cyclic: float = 30
|
|
## Main rotor speed, in revolutions per minute
|
|
@export var rotor_rpm: float = 500
|
|
@export var track_platform: bool = false
|
|
|
|
## Current angle of rotor rotation
|
|
var _rotor_azimuth: float = 0
|
|
|
|
@onready var connector: MarshConnector = $MarshConnector
|
|
@onready var skeleton: Skeleton3D = $"Mi-2/Armature/Skeleton3D"
|
|
@onready var attitude_root: Node3D = $AttitudeRoot
|
|
@onready var tracker_mount_chair: Node3D = $AttitudeRoot/PilotEyes/PilotFloor/TrackerMountChair
|
|
@onready var tracker_mount_platform: Node3D = $AttitudeRoot/PilotEyes/PilotFloor/TrackerMountPlatform
|
|
@onready var xr_origin: XROrigin3D = $AttitudeRoot/PilotEyes/PilotFloor/XROrigin3D
|
|
@onready var tracker: XRController3D = $AttitudeRoot/PilotEyes/PilotFloor/XROrigin3D/ViveTracker
|
|
@onready var xr_camera: XRCamera3D = $AttitudeRoot/PilotEyes/PilotFloor/XROrigin3D/XRCamera3D
|
|
@onready var recent_tracking: Timer = $AttitudeRoot/PilotEyes/PilotFloor/XROrigin3D/RecentTracking
|
|
|
|
@onready var bone_cg: int = skeleton.find_bone("BodyCG")
|
|
@onready var bone_cyclic: int = skeleton.find_bone("Cyclic")
|
|
@onready var bone_rotor: int = skeleton.find_bone("Rotor")
|
|
|
|
func _process(delta: float) -> void:
|
|
var target: Transform3D = connector.get_aircraft()
|
|
position = target.origin
|
|
|
|
# Add the rotation to the correct bone for rotation around CG
|
|
var rest: Quaternion = skeleton.get_bone_rest(bone_cg).basis.get_rotation_quaternion()
|
|
var attitude: Quaternion = target.basis.get_rotation_quaternion()
|
|
skeleton.set_bone_pose_rotation(bone_cg, attitude * rest)
|
|
|
|
# Rotate other children (not using BoneAttachment3D due to jitter)
|
|
attitude_root.rotation = target.basis.get_euler()
|
|
|
|
# Rotate the cyclic stick bone
|
|
var cyclic: Vector2 = connector.get_cyclic()
|
|
var cyc_rest: Quaternion= skeleton.get_bone_rest(bone_cyclic).basis.get_rotation_quaternion()
|
|
var cyc_angles_rad: Vector2 = cyclic * deg_to_rad(range_cyclic)
|
|
var cyc_att := Quaternion.from_euler(Vector3(cyc_angles_rad.y, cyc_angles_rad.x, 0))
|
|
skeleton.set_bone_pose_rotation(bone_cyclic, cyc_att * cyc_rest)
|
|
|
|
# Spin the rotor only when receiving flight data
|
|
if connector.get_model_connected():
|
|
_rotor_azimuth += rotor_rpm * 2 * PI / 60 * delta
|
|
var rotor_rest: Quaternion = skeleton.get_bone_rest(bone_rotor).basis.get_rotation_quaternion()
|
|
var rotor_att := Quaternion.from_euler(Vector3(0, _rotor_azimuth, 0))
|
|
# Note that this is reverse order, to rotate in local bone axes
|
|
skeleton.set_bone_pose_rotation(bone_rotor, rotor_rest * rotor_att)
|
|
|
|
# Move the XR origin in such way that the tracker is in reference position
|
|
if tracker.get_has_tracking_data():
|
|
var tracker_mount: Node3D = tracker_mount_platform if track_platform else tracker_mount_chair
|
|
xr_origin.transform = tracker_mount.transform * tracker.transform.affine_inverse()
|
|
recent_tracking.start()
|
|
|
|
# Only reset the position when tracking is lost for some time
|
|
func _on_recent_tracking_timeout() -> void:
|
|
xr_origin.transform = Transform3D.IDENTITY
|
|
|
|
func _on_vr_setup_failed() -> void:
|
|
xr_camera.current = false
|
|
use_fallback.emit(true)
|