Second order dynamics

SecondOrderSystem.gd

class_name SecondOrderSystem
extends Node3D

@export var frequency := 1.0 :
  set(value):
    frequency = value
    _update_k_values()
    
@export var damping := 0.5 :
  set(value):
    damping = value
    _update_k_values()
    
@export var response := 2.0 :
  set(value):
    response = value
    _update_k_values()
    
@export var target:NodePath
var _target:Node3D

var target_position := Vector2.ZERO
var target_velocity := Vector2.ZERO

var previous_position := Vector2.ZERO

# constants for second order dynamics
var k1 : float
var k2 : float
var k3 : float

func _ready():
  _update_k_values()
  _target = get_node(target)
  
func _process(delta):
  _target.global_position = Vector3(
    target_position.x,
    _target.global_position.y,
    target_position.y
  )

func _physics_process(delta):
  var k2_stable = max(
    k2,
    pow(delta, 2)/2 + delta * k1/2,
    delta * k1
  )
  
  # move
  target_position += delta * target_velocity
  var position_2d = Vector2(global_position.x, global_position.z)

  # estimate velocity
  var velocity = position_2d - previous_position
  previous_position = position_2d

  # accelerate
  target_velocity += delta * (
    position_2d + k3 * velocity - target_position - k1 * target_velocity
  ) / k2_stable

func _update_k_values() -> void:
  var period = TAU * frequency
  
  k1 = damping / (period / 2.0)
  k2 = 1 / pow(period, 2)
  k3 = response * damping / period
  
  return