extends Node3D
class_name AxisNode


var axis_length: float = 1.


# Called when the node enters the scene tree for the first time.
func _ready() -> void:
	_setup_axis()


# Called every frame. 'delta' is the elapsed time since the previous frame.
func _process(delta: float) -> void:
	pass

func _setup_axis():
	var x_basis = Vector3(1, 0, 0)
	var y_basis = Vector3(0, 1, 0)
	var z_basis = Vector3(0, 0, 1)
	var x_node: MeshInstance3D = get_node("x_axis")
	x_node.scale = Vector3(0.1, axis_length/2, 0.1)
	x_node.quaternion = _get_quaternion_rotation(Vector3(0,1,0), x_basis)
	x_node.position = Vector3(axis_length/2, 0, 0)
	var y_node = get_node("y_axis")
	y_node.scale = Vector3(0.1, axis_length/2, 0.1)
	y_node.quaternion = _get_quaternion_rotation(Vector3(0,1,0), y_basis)
	y_node.position = Vector3(0, axis_length/2, 0)
	var z_node = get_node("z_axis")
	z_node.scale = Vector3(0.1, axis_length/2, 0.1)
	z_node.quaternion = _get_quaternion_rotation(Vector3(0,1,0), z_basis)
	z_node.position = Vector3(0, 0, axis_length/2)

func _get_quaternion_rotation(start_vector: Vector3, end_vector: Vector3):
	var rot_axis: Vector3 = end_vector.cross(start_vector).normalized()
	if rot_axis.length_squared() == 0.:
		return Quaternion.IDENTITY
	var rot_angle: float = end_vector.angle_to(start_vector)
	var quat = Quaternion(rot_axis, rot_angle).normalized()
	return quat
