class_name SecondOrderDynamics

var k1: float
var k2: float
var k3: float

var xp: Vector3
var y: Vector3
var yd: Vector3

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.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_stable
	return y