From 58733064c11a3502944e5de83f82d73f0962c394 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Sun, 8 Sep 2024 21:11:02 +0200
Subject: [PATCH] Use spring damper for player geometry orientation.

---
 objects/player.gd     |  5 ++++-
 utils/SpringDamper.gd | 45 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 49 insertions(+), 1 deletion(-)
 create mode 100644 utils/SpringDamper.gd

diff --git a/objects/player.gd b/objects/player.gd
index 58b85c3..0cec1c8 100644
--- a/objects/player.gd
+++ b/objects/player.gd
@@ -13,6 +13,7 @@ var gravity = ProjectSettings.get_setting("physics/3d/default_gravity")
 var inventory:Inventory = Inventory.new()
 
 var look_direction:Vector3 = Vector3.BACK
+var look_direction_damper:SpringDamper = SpringDamper.new(Vector3.ZERO)
 
 signal trigger_message(message:String)
 
@@ -51,9 +52,11 @@ func _physics_process(delta):
 	elif direction.length_squared() > 0.1 * 0.1:
 		look_direction = direction
 
+	var damped_look_direction = look_direction_damper.calc(geometry.global_basis.z, look_direction, delta)
+
 	animation_tree.set("parameters/conditions/running", is_moving)
 	animation_tree.set("parameters/conditions/idle", not is_moving)
-	geometry.look_at(position - look_direction, Vector3.UP)
+	geometry.look_at(position - damped_look_direction, Vector3.UP)
 
 
 func on_item_picked_up(item:Item):
diff --git a/utils/SpringDamper.gd b/utils/SpringDamper.gd
new file mode 100644
index 0000000..a5cb720
--- /dev/null
+++ b/utils/SpringDamper.gd
@@ -0,0 +1,45 @@
+class_name SpringDamper
+extends Object
+
+# Based on: allenchou.net/2015/04/game-math-precise-control-over-numeric-springing/
+
+var omega
+var zeta
+
+var v:Variant = null
+
+# Called when the node enters the scene tree for the first time.
+func _ready():
+	pass # Replace with function body.
+
+func _init(v0:Variant, osc_freq:float = 4, osc_red:float = 0.003, osc_red_h:float = 0.003):
+	assert (osc_red > 0.001 and osc_red < 0.999)
+	omega = osc_freq * 2 * PI
+	zeta = log(1.0 - osc_red) / (-omega * osc_red_h)
+	print ("omega: ", omega, " zeta: ", zeta)
+	v = v0
+	
+func calc(x, xt, h:float):
+	var f = 1.0 + 2.0 * h * zeta * omega
+	var oo = omega * omega
+	var hoo = oo * h
+	var hhoo = hoo * h
+	var det_inv = 1.0 / (f + hhoo)
+	var det_x = f * x + h * v + hhoo * xt
+	var det_v = v + hoo * (xt - x)
+	x = det_x * det_inv
+	v = det_v * det_inv
+	return x
+
+func calc_clamped_speed(x, xt, h:float, s_max:float):
+	var x_old = x
+	var v_old = v
+	
+	var x_new = calc(x, xt, h)
+	var vel_new = (x_new - x_old) / h
+	var speed_new = vel_new.length()
+	if speed_new > s_max:
+		vel_new = (vel_new / speed_new) * s_max
+		x = x_old + vel_new * h
+	
+	return x