second_order_system_2d.gd

class_name SecondOrderSystem
extends Node2D

@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 : Node2D

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() -> void:
  _update_k_values()
  _target = get_node(target)


func _process(_delta: float) -> void:
  _target.global_position = Vector2(
    target_position.x,
    target_position.y
  )


func _physics_process(delta: float) -> void:
  var k2_stable : float = max(
    k2,
    pow(delta, 2)/2 + delta * k1/2,
    delta * k1
  )
  
  # move
  target_position += delta * target_velocity

  # estimate velocity
  var velocity := global_position - previous_position
  previous_position = global_position

  # accelerate
  target_velocity += delta * (
    global_position + 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