ldlt.luau

--!strict
-- Matrices are in row-major order
-- magicoal_nerb :3

local function entry(a: number, b: number, n: number): number
	return (a-1)*n + b
end

local function forwardSubstitution(L: { number }, b: { number }): { number }
	-- Let A = LU. Ax = b <=> LUx = b
	-- Ux = L^-1 b. Let c = L^-1 b
	-- <=> Lc = b. To solve:
	--   c_i = (b_i - sum j=0,i-1 L_ij*c_j)/L_ii
	local n = math.sqrt(#L)
	local c = table.create(n, 0)
	
	for i = 1, n do
		-- L_11 c_1 = b_1
		-- L_21 c_1 + L_22 c_2 = b_2
		-- L_31 c_1 + L_32 c_2 + ... + L_n c_n = b_n
		-- Get the general c_n
		
		local sum = b[i]
		for j = 1, i - 1 do
			sum -= L[entry(i, j, n)] * c[j]
		end
		
		c[i] = sum / L[entry(i, i, n)]
	end
	
	return c
end

local function backwardSubstitution(U: { number }, c: { number }): { number }
	-- Let A = LU. Ax = b <=> LUx = b
	-- From doing forward substitution, we get:
	-- <=> Ux = c. To solve:
	--   x_i = (c_i - sum j=i+1,n U_ij*c_j)/U_ii
	local n = math.sqrt(#U)
	local x = table.create(n, 0)
	
	for i = 1, n do
		-- U_nn x_n = b_n
		-- U_(n-1)(n-1) x_(n-1) + U_nn x_n = b_n ; find x_(n-1)
		-- This happens recursively. Same as forward substitution
		-- but the order is flipped (bottom-up)
		
		local k = n - i + 1
		local sum = c[k]
		for j = 1, i - 1 do
			sum -= U[entry(k, n-j+1, n)] * x[n-j+1]
		end
		
		x[k] = sum / U[entry(k, k, n)]
	end
	
	return x
end

local function lu(a: { number }): ({ number }, { number })
	local n = math.sqrt(#a)
	
	local l = table.create(#a, 0)
	local u = table.clone(a)
	
	-- Do a LU decomposition where L is a lower
	-- triangular matrix and U is an upper triangular matrix.
	
	-- Let E be an elimination matrix. Then, find EA = U, where U is
	-- upper triangular. This is done by simply doing gaussian elimination
	-- below the diagonal element in the matrix.
	for i = 1, n do
		-- Lower triangular matrix must have
		-- 1s along the diagonal
		l[entry(i, i, n)] = 1
		
		for j = i + 1, n do
			-- lambda = u_ji/u_ii
			-- E_ij = -lambda ; but note that
			--                  (E^-1 = L)_ij = lambda
			-- L_ij = lambda
			-- r_j -= r_i * lambda
			local lambda = u[entry(j, i, n)] / u[entry(i, i, n)]
			l[entry(j, i, n)] = lambda
			
			for k = 1, n do
				u[entry(j, k, n)] -= lambda * u[entry(i, k, n)]
			end
		end
	end
	
	return l, u
end

local function directSubstitution(L: { number }, D: { number }, b: { number }): { number }
	-- LDL^T x = b
	-- DL^T x = L^-1 b ; def c <=> Lc = b
	local c = forwardSubstitution(L, b)
	local n = #D
	
	local z = table.create(n, 0)
	for i = 1, n do
		-- L^T x = D^-1 * c ; def z <=> Dz = c 
		z[i] = c[i] / D[i]
	end
	
	-- Let A = L^T U. Ax = b <=> L^T Ux = b
	-- From doing forward substitution, we get:
	-- <=> Ux = c. To solve:
	--   x_i = (c_i - sum j=i+1,n U_ij*c_j)/U_ii
	local x = table.create(n, 0)
	for i = 1, n do
		-- This happens recursively. Same as backwards substitution
		-- but the entry order is flipped due to the transpose
		local k = n - i + 1
		local sum = z[k]
		for j = 1, i - 1 do
			sum -= L[entry(n-j+1, k, n)] * x[n-j+1]
		end

		x[k] = sum / L[entry(k, k, n)]
	end
	
	return x
end

local function ldlt(a: { number }): ({ number }, { number })
	-- Assume A is a symmetric definite matrix
	local n = math.sqrt(#a)

	local l = table.create(#a, 0)
	local d = table.create(n, 0)
	local u = table.clone(a)

	-- Do a LDL^T decomposition where L is a lower
	-- triangular matrix and D is a diagonal matrix.
	-- L is the same as in LU(A). flops ~ O(1/3 n^3).
	for i = 1, n do
		-- Lower triangular matrix must have
		-- 1s along the diagonal
		l[entry(i, i, n)] = 1

		-- D_11 = A_11 => D_22 = A_22 - D_11 * L_21^2
		-- D_nn = A_nn - sum k=1,n L_(n-1)k^2 * d_k
		local dii = a[entry(i, i, n)]
		for j = 1, i - 1 do
			local Ljk = l[entry(i, j, n)]
			dii -= Ljk*Ljk * d[j]
		end

		for j = i + 1, n do
			-- lambda = u_ji/u_ii
			-- E_ij = -lambda ; but note that
			--                  (E^-1 = L)_ij = lambda
			-- L_ij = lambda
			-- r_j -= r_i * lambda
			local lambda = u[entry(j, i, n)] / u[entry(i, i, n)]
			l[entry(j, i, n)] = lambda

			for k = 1, j do
				u[entry(j, k, n)] -= lambda * u[entry(i, k, n)]
			end
		end

		d[i] = dii
	end

	return l, d
end

solve-exact.luau

--!strict
-- Explicitly solves the matrices (for N<=4) in the form Ax=b
-- magicoal_nerb :3

local function solveMat2(
	a11: number, a21: number, a22: number,
	b1: number, b2: number
): (number, number)
	-- Solves a 2x2 matrix with Ax = b
	local invDet = 1.0 / (a11*a22 - a21*a21)
	local r1 = (a22*b1 - a21*b2)*invDet
	local r2 = (a11*b2 - a21*b1)*invDet
	
	return r1, r2
end

local function solveLdlt3(
	a11: number, a22: number, a33: number,
	a21: number, a32: number, a31: number,
	b1: number, b2: number, b3: number
): (number, number, number)
	--[[
		a11         b1
		a21 a22     b2
		a31 a32 a33 b3
	]]
	
	-- LDL^T x = b
	-- From doing the LDL^T by hand
	
	-- 6div, 14mul, 11sub, 
	local invA11 = 1.0 / a11
	local l21 = a21 * invA11
	local l31 = a31 * invA11
	
	-- Second elimination
	local d2 = a22 - l21*l21*a11
	local invD2 = 1.0 / d2
	
	local l32 = (a32 - l31 * l21 * a11) * invD2
	local d3 = a33 - l31*l31*a11 - l32*l32*d2
	
	-- DL^T x = L^-1 b <=> DL^T x = c
	-- Lc = b
	local c2 = b2 - l21*b1
	local c3 = b3 - l31*b1 - l32*c2
	
	-- L^T x = D^-1 c ; first term on LHS
	-- L^T x = e
	local r3 = (c3 / d3)
	local r2 = (c2 * invD2) - l32*r3
	local r1 = (b1 * invA11) - l31*r3 - l21*r2
	
	return r1, r2, r3
end

local function solveLdlt4(
	a11: number, a22: number, a33: number, a44: number,
	a21: number, a32: number, a43: number,
	a31: number, a42: number,
	a41: number,
	
	b1: number, b2: number, b3: number, b4: number
): (number, number, number, number)
	--[[
		a11             b1
		a21 a22         b2
		a31 a32 a33     b3
		a41 a42 a43 a44 b4
	]]
	
	-- First elimination
	local invA11 = 1/a11
	local l21 = a21*invA11
	local l31 = a31*invA11
	local l41 = a41*invA11
	
	-- Second elimination
	local d2 = a22 - l21*l21*a11
	local invD2 = 1.0 / d2
	local l32 = (a32 - l31*l21*a11)*invD2
	local l42 = (a42 - l41*l21*a11)*invD2
	
	-- Third elimination
	local d3 = a33 - l31*l31*a11 - l32*l32*d2
	local invD3 = 1.0 / d3
	
	local l43 = (a43 - l41*l31*a11 - l42*l32*d2) * invD3
	local d4 = a44 - l41*l41*a11 - l42*l42*d2 - l43*l43*d3

	-- DL^T x = L^-1 b <=> DL^T x = c
	-- Lc = b
	local c1 = b1
	local c2 = b2 - l21 * c1
	local c3 = b3 - l31 * c1 - l32 * c2
	local c4 = b4 - l41 * c1 - l42 * c2 - l43 * c3
	
	-- L^T x = D^-1 c <=> De = c
	-- Provided: e ; first term on LHS
	-- L^T x = e
	local r4 = (c4 / d4)
	local r3 = (c3 * invD3) - l43 * r4
	local r2 = (c2 * invD2) - l42 * r4 - l32 * r3
	local r1 = (c1 * invA11) - l41 * r4 - l31 * r3 - l21 * r2
	
	return r1, r2, r3, r4
end


local function ldlt(matrix: { number }, b: { number }): (Vector3, Vector3)
	-- Extract elements from lower triangle storage
	local A11, A21, A31, A41, A51, A61 = matrix[1], matrix[2], matrix[3], matrix[4], matrix[5], matrix[6]
	local A22, A32, A42, A52, A62 = matrix[7], matrix[8], matrix[9], matrix[10], matrix[11]
	local A33, A43, A53, A63 = matrix[12], matrix[13], matrix[14], matrix[15]
	local A44, A54, A64 = matrix[16], matrix[17], matrix[18]
	local A55, A65 = matrix[19], matrix[20]
	local A66 = matrix[21]
	
	-- LDL^T decomposition
	-- First elimination
	local L21 = A21 / A11
	local L31 = A31 / A11
	local L41 = A41 / A11
	local L51 = A51 / A11
	local L61 = A61 / A11

	local D1 = A11

	-- Second elimination
	local D2 = A22 - L21 * L21 * D1

	local L32 = (A32 - L21 * L31 * D1) / D2
	local L42 = (A42 - L21 * L41 * D1) / D2
	local L52 = (A52 - L21 * L51 * D1) / D2
	local L62 = (A62 - L21 * L61 * D1) / D2

	-- Third elimination
	local D3 = A33 - (L31 * L31 * D1 + L32 * L32 * D2)

	local L43 = (A43 - L31 * L41 * D1 - L32 * L42 * D2) / D3
	local L53 = (A53 - L31 * L51 * D1 - L32 * L52 * D2) / D3
	local L63 = (A63 - L31 * L61 * D1 - L32 * L62 * D2) / D3

	-- Fourth elimination
	local D4 = A44 - (L41 * L41 * D1 + L42 * L42 * D2 + L43 * L43 * D3)

	local L54 = (A54 - L41 * L51 * D1 - L42 * L52 * D2 - L43 * L53 * D3) / D4
	local L64 = (A64 - L41 * L61 * D1 - L42 * L62 * D2 - L43 * L63 * D3) / D4

	-- Fifth elimination
	local D5 = A55 - (L51 * L51 * D1 + L52 * L52 * D2 + L53 * L53 * D3 + L54 * L54 * D4)

	-- Sixth elimination
	local L65 = (A65 - L51 * L61 * D1 - L52 * L62 * D2 - L53 * L63 * D3 - L54 * L64 * D4) / D5

	local D6 = A66 - (L61 * L61 * D1 + L62 * L62 * D2 + L63 * L63 * D3 + L64 * L64 * D4 + L65 * L65 * D5)

	-- DL^T x = L^-1 b <=> DL^T x = y
	-- Ly = b
	local y1 = b[1]
	local y2 = b[2] - L21 * y1
	local y3 = b[3] - L31 * y1 - L32 * y2
	local y4 = b[4] - L41 * y1 - L42 * y2 - L43 * y3
	local y5 = b[5] - L51 * y1 - L52 * y2 - L53 * y3 - L54 * y4
	local y6 = b[6] - L61 * y1 - L62 * y2 - L63 * y3 - L64 * y4 - L65 * y5

	-- Pretty easy, Dz = y
	local z1 = y1 / D1
	local z2 = y2 / D2
	local z3 = y3 / D3
	local z4 = y4 / D4
	local z5 = y5 / D5
	local z6 = y6 / D6

	-- L^T x = D^-1 c <=> De = z
	-- Provided: e ; first term on LHS
	-- L^T x = z	
	local x6 = z6
	local x5 = z5 - L65 * x6
	local x4 = z4 - L54 * x5 - L64 * x6
	local x3 = z3 - L43 * x4 - L53 * x5 - L63 * x6
	local x2 = z2 - L32 * x3 - L42 * x4 - L52 * x5 - L62 * x6
	local x1 = z1 - L21 * x2 - L31 * x3 - L41 * x4 - L51 * x5 - L61 * x6
	
	return Vector3.new(x1, x2, x3), Vector3.new(x4, x5, x6)
end