# Based on: allenchou.net/2015/04/game-math-precise-control-over-numeric-springing/

extends Object

class_name SpringDamper

var omega
var zeta

# Called when the node enters the scene tree for the first time.
func _ready():
	pass # Replace with function body.

func _init(osc_freq:float = 1.0, osc_red:float = 0.1, osc_red_h:float = 1.0):
	assert (osc_red > 0.001 and osc_red < 0.999)
	omega = osc_freq * 2 * PI
	zeta = log(1.0 - osc_red) / (-omega * osc_red_h)
	print ("omega: ", omega, " zeta: ", zeta)
	
func calc(x, v, xt, h:float):
	var f = 1.0 + 2.0 * h * zeta * omega
	var oo = omega * omega
	var hoo = oo * h
	var hhoo = hoo * h
	var det_inv = 1.0 / (f + hhoo)
	var det_x = f * x + h * v + hhoo * xt
	var det_v = v + hoo * (xt - x)
	return [det_x * det_inv, det_v * det_inv]