35 lines
683 B
GDScript
35 lines
683 B
GDScript
# https://www.youtube.com/watch?v=KPoeNZZ6H4s
|
|
class_name SecondOrderDynamics
|
|
|
|
var _k1: float
|
|
var _k2: float
|
|
var _k3: float
|
|
|
|
var _xp: Vector3
|
|
var _y: Vector3
|
|
var _yd: Vector3
|
|
|
|
|
|
func _init(fzr: Vector3, x0: Vector3) -> void:
|
|
var f := fzr.x
|
|
var z := fzr.y
|
|
var r := fzr.z
|
|
|
|
_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 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))
|
|
_y = _y + t * _yd
|
|
_yd = _yd + t * (x + _k3 * xd - _y - _k1 * _yd) / k2_stable
|
|
return _y
|