You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
29 lines
562 B
GDScript
29 lines
562 B
GDScript
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
|