stabilization on k2

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]

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")

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]

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