From 2cd6f38b738dad4b29c1eb1bd727226903df33b9 Mon Sep 17 00:00:00 2001
From: Fat Mimir <fatmimir@bitidsoft.com>
Date: Tue, 30 May 2023 03:11:15 -0600
Subject: [PATCH] stabilization on k2

---
 follower.gd              | 2 +-
 main.tscn                | 2 ++
 project.godot            | 1 +
 second_order_dynamics.gd | 6 ++++--
 4 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/follower.gd b/follower.gd
index 47dcd59..7866384 100644
--- a/follower.gd
+++ b/follower.gd
@@ -43,7 +43,7 @@ func _input(event):
 		second_order_dynamics = SecondOrderDynamics.new(f, z, r, global_position)
 
 func _process(delta):
-	global_position = second_order_dynamics.update(delta, target_node.global_position, Vector3.ZERO)
+	global_position = second_order_dynamics.update(delta, target_node.global_position)
 	
 	if f_label_node:
 		f_label_node.text = "f = %.3f" % [f]
diff --git a/main.tscn b/main.tscn
index f045eba..7c18b85 100644
--- a/main.tscn
+++ b/main.tscn
@@ -68,6 +68,8 @@ transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 3.5, 0)
 mesh = SubResource("SphereMesh_w43c3")
 surface_material_override/0 = SubResource("StandardMaterial3D_txxch")
 script = ExtResource("3_4rocj")
+f = 2.0
+r = -2.0
 target_path = NodePath("../Target")
 f_label_path = NodePath("../Help/FLabel")
 z_label_path = NodePath("../Help/ZLabel")
diff --git a/project.godot b/project.godot
index 651b61e..1328183 100644
--- a/project.godot
+++ b/project.godot
@@ -13,6 +13,7 @@ config_version=5
 config/name="Interpolation Curves"
 run/main_scene="res://main.tscn"
 config/features=PackedStringArray("4.0", "Forward Plus")
+run/max_fps=120
 config/icon="res://icon.svg"
 
 [input]
diff --git a/second_order_dynamics.gd b/second_order_dynamics.gd
index a9bfd80..4d268a4 100644
--- a/second_order_dynamics.gd
+++ b/second_order_dynamics.gd
@@ -12,15 +12,17 @@ func _init(f: float, z: float, r: float, x0: Vector3):
 	k1 = z / PI * f
 	k2 = 1 / ((2 * PI * f) * (2 * PI * f))
 	k3 = r * z / (2 * PI * f)
+	
 	xp = x0
 	y = x0
 	yd = Vector3.ZERO
 	
-func update(t: float, x: Vector3, xd: Vector3) -> Vector3:
+func update(t: float, x: Vector3, xd: Vector3 = Vector3.ZERO) -> Vector3:
 	if xd.is_zero_approx():
 		xd = (x - xp) / t
 		xp = x
 	
+	var k2_stable = max(k2, 1.1 * (t * t/4 + t * k1/2))
 	y = y + t * yd
-	yd = yd + t * (x + k3*xd - y - k1*yd) / k2
+	yd = yd + t * (x + k3*xd - y - k1*yd) / k2_stable
 	return y