Gyro Camera for Godot

angle_filter.gd

extends RefCounted
class_name AngleFilter
# Same as FloatFilter but with a workaround when angle loop back around 2pi

var array:Array[Vector2] = [];
var index:int = 0;
var size:int = 0;

func _init(count:int=5) -> void:
	array.resize(count);
	index = 0;
	size = count;

var value:float:
	get: 
		var angle = array.reduce(sum,Vector2.ZERO) / size;
		return atan2(angle.y, angle.x)
		
func sum(accum:Vector2, number:Vector2):
	return accum + number
	
func record(value:float):
	var vector_value = Vector2(cos(value),sin(value));
	array[index%size] = vector_value
	if (index<size):
		for i in range(index, size):
			array[i] =vector_value;
	index+=1;
	return 

gyro_camera.gd

extends Node3D

# Giro camera
# Make the node3d follow the rotation of the phone
# Make sure you have the magnetometer & gravity sensor enabled in your project setting
# We assume the phone stay in landscape mode 
# Because of the way phone magnetometer work they may get uncalibrated every once in a while. Check online on how to recalibrate it.
# Also there may be some more efficient and pretty way of doing this.

# The number here influence the noise removal filter. Adjust them to you need. Larger valeur are smoother but introduce more "lag" 
var gravity = VectorFilter.new(10);
var magneto = VectorFilter.new(20);
var pitch = AngleFilter.new(20);
var yaw = AngleFilter.new(20);

func _process(delta: float) -> void:
	# Apply some filtering to the raw sensor value, to remove the noisy input 
	gravity.record(Input.get_gravity())
	magneto.record(Input.get_magnetometer())
	var filtered_gravity = gravity.value.normalized();
	var filtered_magneto = magneto.value;
	# The inclination of the phone is calculated by how much the gravity is allong the forward axis of the phone
	var pitch_angle = acos(Vector3.FORWARD.dot(filtered_gravity))-PI/2
	
	# We compute the absolute direction of the magnetic north by compensating for the inclination of the phone
	# Basicaly, we compute what rotation it would take to align the gravity vector with DOWN and apply the same rotation to the magnetometer
	var absolute_magneto = filtered_magneto.rotated(filtered_gravity.cross(Vector3.DOWN).normalized(), filtered_gravity.angle_to(Vector3.DOWN));
	var yaw_angle = atan2(-absolute_magneto.z, absolute_magneto.x);
	
	# Because of error compound, even with our input filtering, the result is still very noisy. We apply a new 
	pitch.record(pitch_angle);
	yaw.record(yaw_angle)
	
	# Finaly we aply the rotation we calculated.
	basis = Basis.IDENTITY.rotated(Vector3.RIGHT, pitch.value).rotated(Vector3.DOWN, yaw.value);

vector_filter.gd

extends RefCounted
class_name VectorFilter

var array:Array[Vector3] = [];
var index:int = 0;
var size:int = 0;

func _init(count:int=5) -> void:
	array.resize(count);
	index = 0;
	size = count;

var value:Vector3:
	get: 
		return array.reduce(sum,Vector3.ZERO) / size;
		
func sum(accum:Vector3, number:Vector3):
	return accum + number
	
func record(value:Vector3):
	array[index%size] = value;
	if (index<size):
		for i in range(index, size):
			array[i] = value;
	index+=1;
	return