local T={}do T.__index=T function T.new(s,b,k,p,C)local Y=p.toIntermediate(k)return setmetatable({d=s,f=b,g=Y,p=Y,v=table.create(#Y,0),typedat=p,rawTarget=C},T)end function T.setGoal(s,T)s.rawTarget=T s.g=s.typedat.toIntermediate(T)end function T.canSleep(T)if magnitudeSq(T.v)>SLEEP_VELOCITY_SQ_LIMIT then return false end if distanceSq(T.p,T.g)>SLEEP_OFFSET_SQ_LIMIT then return false end return true end function T.step(s,T)local b=s.d local k=(s.f*2)*pi local p=s.g local C=s.p local Y=s.v if b==1 then local s=exp(-k*T)local b=T*s local n=s+b*k local c=s-b*k local I=(b*k)*k for T=1,#C,1 do local s=C[T]-p[T]C[T]=(s*n+Y[T]*b)+p[T]Y[T]=Y[T]*c-s*I end elseif b<1 then local s=exp((-b*k)*T)local n=sqrt(1-b*b)local c=cos((T*k)*n)local I=sin((T*k)*n)local Z if n>EPS then Z=I/n else local s=T*k Z=s+(((((s*s)*(n*n))*(n*n))/20-n*n)*((s*s)*s))/6 end local i if k*n>EPS then i=I/(k*n)else local s=k*n i=T+(((((T*T)*(s*s))*(s*s))/20-s*s)*((T*T)*T))/6 end for T=1,#C,1 do local n=C[T]-p[T]C[T]=(n*(c+Z*b)+Y[T]*i)*s+p[T]Y[T]=(Y[T]*(c-Z*b)-n*(Z*k))*s end else local s=sqrt(b*b-1)local n=-k*(b-s)local c=-k*(b+s)local I=exp(n*T)local Z=exp(c*T)for T=1,#C,1 do local b=C[T]-p[T]local i=(Y[T]-b*n)/((2*k)*s)local W=I*(b-i)C[T]=(W+i*Z)+p[T]Y[T]=W*n+(i*Z)*c end end return s.typedat.fromIntermediate(s.p)end end

local LinearSpring = {} do
	LinearSpring.__index = LinearSpring

	function LinearSpring.new(dampingRatio, frequency, pos, typedat, rawTarget)
		local linearPos = typedat.toIntermediate(pos)
		return setmetatable(
			{
				d = dampingRatio,
				f = frequency,
				g = linearPos,
				p = linearPos,
				v = table.create(#linearPos, 0),
				typedat = typedat,
				rawTarget = rawTarget,
			},
			LinearSpring
		)
	end

	function LinearSpring:setGoal(goal)
		self.rawTarget = goal
		self.g = self.typedat.toIntermediate(goal)
	end

	function LinearSpring:canSleep()
		if magnitudeSq(self.v) > SLEEP_VELOCITY_SQ_LIMIT then
			return false
		end

		if distanceSq(self.p, self.g) > SLEEP_OFFSET_SQ_LIMIT then
			return false
		end

		return true
	end

	function LinearSpring:step(dt)
		-- Advance the spring simulation by dt seconds.
		-- Take the damped harmonic oscillator ODE:
		--    f^2*(X[t] - g) + 2*d*f*X'[t] + X''[t] = 0
		-- Where X[t] is position at time t, g is target position,
		-- f is undamped angular frequency, and d is damping ratio.
		-- Apply constant initial conditions:
		--    X[0] = p0
		--    X'[0] = v0
		-- Solve the IVP to get analytic expressions for X[t] and X'[t].
		-- The solution takes one of three forms for 0<=d<1, d=1, and d>1

		local d = self.d
		local f = self.f*2*pi -- Hz -> Rad/s
		local g = self.g
		local p = self.p
		local v = self.v

		if d == 1 then -- critically damped
			local q = exp(-f*dt)
			local w = dt*q

			local c0 = q + w*f
			local c2 = q - w*f
			local c3 = w*f*f

			for idx = 1, #p do
				local o = p[idx] - g[idx]
				p[idx] = o*c0 + v[idx]*w + g[idx]
				v[idx] = v[idx]*c2 - o*c3
			end

		elseif d < 1 then -- underdamped
			local q = exp(-d*f*dt)
			local c = sqrt(1 - d*d)

			local i = cos(dt*f*c)
			local j = sin(dt*f*c)

			-- Damping ratios approaching 1 can cause division by very small numbers.
			-- To mitigate that, group terms around z=j/c and find an approximation for z.
			-- Start with the definition of z:
			--    z = sin(dt*f*c)/c
			-- Substitute a=dt*f:
			--    z = sin(a*c)/c
			-- Take the Maclaurin expansion of z with respect to c:
			--    z = a - (a^3*c^2)/6 + (a^5*c^4)/120 + O(c^6)
			--    z ≈ a - (a^3*c^2)/6 + (a^5*c^4)/120
			-- Rewrite in Horner form:
			--    z ≈ a + ((a*a)*(c*c)*(c*c)/20 - c*c)*(a*a*a)/6

			local z
			if c > EPS then
				z = j/c
			else
				local a = dt*f
				z = a + ((a*a)*(c*c)*(c*c)/20 - c*c)*(a*a*a)/6
			end

			-- Frequencies approaching 0 present a similar problem.
			-- We want an approximation for y as f approaches 0, where:
			--    y = sin(dt*f*c)/(f*c)
			-- Substitute b=dt*c:
			--    y = sin(b*c)/b
			-- Now reapply the process from z.

			local y
			if f*c > EPS then
				y = j/(f*c)
			else
				local b = f*c
				y = dt + ((dt*dt)*(b*b)*(b*b)/20 - b*b)*(dt*dt*dt)/6
			end

			for idx = 1, #p do
				local o = p[idx] - g[idx]
				p[idx] = (o*(i + z*d) + v[idx]*y)*q + g[idx]
				v[idx] = (v[idx]*(i - z*d) - o*(z*f))*q
			end

		else -- overdamped
			local c = sqrt(d*d - 1)

			local r1 = -f*(d - c)
			local r2 = -f*(d + c)

			local ec1 = exp(r1*dt)
			local ec2 = exp(r2*dt)

			for idx = 1, #p do
				local o = p[idx] - g[idx]
				local co2 = (v[idx] - o*r1)/(2*f*c)
				local co1 = ec1*(o - co2)

				p[idx] = co1 + co2*ec2 + g[idx]
				v[idx] = co1*r1 + co2*ec2*r2
			end
		end

		return self.typedat.fromIntermediate(self.p)
	end
end
