From d16f9b97a3d86ab360b487919c617f9b98f73b60 Mon Sep 17 00:00:00 2001
From: teatov <hlebpoimenigleb@gmail.com>
Date: Tue, 29 Jul 2025 15:11:04 +1000
Subject: [PATCH] add sod pole matching for extra stability

---
 scenes/game_key.tscn             |  3 +++
 scripts/second_order_dynamics.gd | 23 +++++++++++++++++++----
 2 files changed, 22 insertions(+), 4 deletions(-)

diff --git a/scenes/game_key.tscn b/scenes/game_key.tscn
index 82a32b8..b747cb6 100644
--- a/scenes/game_key.tscn
+++ b/scenes/game_key.tscn
@@ -51,6 +51,9 @@ _lower_right_label = NodePath("LowerRightLabel")
 _center_label = NodePath("CenterLabel")
 _press_light = NodePath("PressLight")
 _sfx_player = NodePath("SFXPlayer")
+_idle_frequency = 20.0
+_idle_amplitude = 1.46
+_idle_offset = 2.11
 _press_sfx = SubResource("AudioStreamRandomizer_agrko")
 _release_sfx = SubResource("AudioStreamRandomizer_ch32x")
 
diff --git a/scripts/second_order_dynamics.gd b/scripts/second_order_dynamics.gd
index dabe971..bf3a474 100644
--- a/scripts/second_order_dynamics.gd
+++ b/scripts/second_order_dynamics.gd
@@ -5,6 +5,10 @@ var _k1: float
 var _k2: float
 var _k3: float
 
+var _w: float
+var _z: float
+var _d: float
+
 var _xp: Vector3
 var _y: Vector3
 var _yd: Vector3
@@ -15,9 +19,13 @@ func _init(fzr: Vector3, x0: Vector3) -> void:
 	var z := fzr.y
 	var r := fzr.z
 
+	_w = 2 * PI * f
+	_z = z
+	_d = _w * sqrt(absf(z * z - 1))
+
 	_k1 = z / (PI * f)
-	_k2 = 1 / ((2 * PI * f) * (2 * PI * f))
-	_k3 = r * z / (2 * PI * f)
+	_k2 = 1 / (_w * _w)
+	_k3 = r * z / _w
 
 	_xp = x0
 	_y = x0
@@ -28,8 +36,15 @@ func process(t: float, x: Vector3, xd: Vector3 = Vector3.INF) -> Vector3:
 	if xd == Vector3.INF:
 		xd = (x - _xp) / t
 		_xp = x
-	# var k2_stable := maxf(_k2, 1.1 * (t * t / 4 + t * _k1 / 2))
-	var k2_stable := max(_k2, t * t / 2 + t * _k1 / 2, t * _k1) as float
+	var k2_stable: float
+	if _w * t < _z:
+		k2_stable = max(_k2, t * t / 2 + t * _k1 / 2, t * _k1)
+	else:
+		var t1 := exp(-_z * _w * t)
+		var alpha := 2 * t1 * (cos(t * _d) if _z <= 1 else cosh(t * _d))
+		var beta := t1 * t1
+		var t2 := t / (1 + beta - alpha)
+		k2_stable = t * t2
 	_y = _y + t * _yd
 	_yd = _yd + t * (x + _k3 * xd - _y - _k1 * _yd) / k2_stable
 	return _y