54 lines
1000 B
GDScript
54 lines
1000 B
GDScript
# based on the work of t3ssel8r
|
|
# https://www.youtube.com/watch?v=KPoeNZZ6H4s
|
|
class_name SecondOrderDynamics
|
|
|
|
var y: Vector3
|
|
|
|
var _k1: float
|
|
var _k2: float
|
|
var _k3: float
|
|
|
|
var _w: float
|
|
var _z: float
|
|
var _d: float
|
|
|
|
var _xp: Vector3
|
|
var _yd: Vector3
|
|
|
|
|
|
func _init(fzr: Vector3, x0: Vector3) -> void:
|
|
var f := fzr.x
|
|
var z := fzr.y
|
|
var r := fzr.z
|
|
|
|
_w = 2 * PI * f
|
|
_z = z
|
|
_d = _w * sqrt(absf(z * z - 1))
|
|
|
|
_k1 = z / (PI * f)
|
|
_k2 = 1 / (_w * _w)
|
|
_k3 = r * z / _w
|
|
|
|
_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: float
|
|
if _w * t < _z:
|
|
k2_stable = max(_k2, t * t / 2 + t * _k1 / 2, t * _k1)
|
|
else:
|
|
var t1 := exp(-_z * _w * t)
|
|
var alpha := 2 * t1 * (cos(t * _d) if _z <= 1 else cosh(t * _d))
|
|
var beta := t1 * t1
|
|
var t2 := t / (1 + beta - alpha)
|
|
k2_stable = t * t2
|
|
y = y + t * _yd
|
|
_yd = _yd + t * (x + _k3 * xd - y - _k1 * _yd) / k2_stable
|
|
return y
|
|
|