-- lua-tikz3dtools-matrix-math.lua

local mm = {}
mm.tau = 2*math.pi

local cos, sin = math.cos, math.sin

--- matrix multiplication
---
--- @param A table<table<number>> left matrix
--- @param B table<table<number>> right matrix
--- @return table<table<number>> the product
function mm.matrix_multiply(A, B)
   local rows_A = #A
   local columns_A = #A[1]
   local rows_B = #B
   local columns_B = #B[1]
   assert(
       columns_A == rows_B,
       string.format(
           [[
               Wrong size matrices for multiplication.
               Size A: %d,%d Size B: %d,%d
           ]],
           rows_A, columns_A,
           rows_B, columns_B
       )
   )
   local product = {}
   for row = 1, rows_A do
       product[row] = {}
       for column = 1, columns_B do
           product[row][column] = 0
           for dot_product_step = 1, columns_A do
               local a = A[row][dot_product_step]
               local b = B[dot_product_step][column]
               assert(type(a) == "number",
                   string.format("Expected number but got %s in A[%d][%d]", type(a), row, dot_product_step))
               assert(type(b) == "number",
                   string.format("Expected number but got %s in B[%d][%d]", type(b), dot_product_step, column))
               product[row][column] = product[row][column] + a * b
           end
       end
   end
   return product
end

function mm.reciprocate_by_homogenous(vector)
   local result = {}
   for i = 1, #vector do
       local row = vector[i]
       local w = row[4]
       if w == 0 then
           error("Cannot reciprocate row " .. i .. ": homogeneous coordinate w = 0")
       end
       --if w<0 then w=-w end
       result[i] = {
           row[1]/w,
           row[2]/w,
           row[3]/w,
           1
       }
   end
   return result
end

function  mm.transpose(A)
   local rows_A = #A
   local columns_A = #A[1]
   local result = {}
   for row = 1, columns_A, 1 do
       result[row] = {}
       for column = 1, rows_A, 1 do
           result[row][column] = A[column][row]
       end
   end
   return result
end

function mm.inverse(matrix)
   local rows = #matrix
   local columns = #matrix[1]
   assert(rows == columns, "You can only take the inverse of a square matrix.")
   local det = mm.det(matrix)
   assert(math.abs(math.abs(det)) > 0.00001, "You cannot take the inverse of a singular matrix.")

   local n = rows
   -- Build an augmented matrix [A | I]
   local augment = {}
   for i = 1, n do
       augment[i] = {}
       -- copy row i of A
       for j = 1, n do
           augment[i][j] = matrix[i][j]
       end
       -- append row i of I
       for j = 1, n do
           augment[i][n + j] = (i == j) and 1 or 0
       end
   end

   -- Gauss-Jordan elimination
   for i = 1, n do
       -- If pivot is zero (or very close), swap with a lower row that has a nonzero pivot
       if math.abs(augment[i][i]) < 1e-12 then
           local swapRow = nil
           for r = i + 1, n do
               if math.abs(augment[r][i]) > 1e-12 then
                   swapRow = r
                   break
               end
           end
           assert(swapRow, "Matrix is singular (zero pivot encountered).")
           augment[i], augment[swapRow] = augment[swapRow], augment[i]
       end

       -- Normalize row i so that augment[i][i] == 1
       local pivot = augment[i][i]
       for col = 1, 2 * n do
           augment[i][col] = augment[i][col] / pivot
       end

       -- Eliminate column i in all other rows
       for r = 1, n do
           if r ~= i then
               local factor = augment[r][i]
               for col = 1, 2 * n do
                   augment[r][col] = augment[r][col] - factor * augment[i][col]
               end
           end
       end
   end

   -- Extract the inverse matrix from the augmented result
   local inv = {}
   for i = 1, n do
       inv[i] = {}
       for j = 1, n do
           inv[i][j] = augment[i][n + j]
       end
   end

   return inv
end

function mm.det(matrix)
   local rows = #matrix
   local columns = #matrix[1]
   assert(rows > 0, "Matrix must have at least one row to take determinant.")
   assert(columns > 0, "Matrix must have at least one column to take determinant.")
   assert(rows == columns, "You can only take the determinant of a square matrix.")
   if rows == 1 then
       return matrix[1][1]
   elseif rows == 2 then
       -- return a*d - b*c
       return matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1]
   end
   -- We will do a cofactor expansion on the first row.
   local det = 0
   local minor
   local new_row
   for element = 1, columns, 1 do
       minor = {}
       for row = 2, rows, 1 do
           new_row = {}
           for column = 1, columns, 1 do
               if column ~= element then
                   table.insert(new_row, matrix[row][column])
               end
           end
           table.insert(minor,new_row)
       end
       det = det + matrix[1][element] * (-1)^(element+1) * mm.det(minor)
   end
   return det
end

function mm.yrotation(angle)
   local c = cos(angle)
   local s = sin(angle)
   return {
       {c,0,-s,0}
       ,{0,1,0,0}
       ,{s,0,c,0}
       ,{0,0,0,1}
   }
end

function mm.translate(x,y,z)
   return {
       {1,0,0,0}
       ,{0,1,0,0}
       ,{0,0,1,0}
       ,{x,y,z,1}
   }
end

function mm.xscale(scale)
   return {
       {scale,0,0,0}
       ,{0,1,0,0}
       ,{0,0,1,0}
       ,{0,0,0,1}
   }
end

function mm.yscale(scale)
   return {
       {1,0,0,0}
       ,{0,scale,0,0}
       ,{0,0,1,0}
       ,{0,0,0,1}
   }
end

function mm.zscale(scale)
   return {
       {1,0,0,0}
       ,{0,1,0,0}
       ,{0,0,scale,0}
       ,{0,0,0,1}
   }
end

function mm.scale(scale)
   return {
       {scale,0,0,0}
       ,{0,scale,0,0}
       ,{0,0,scale,0}
       ,{0,0,0,1}
   }
end

function mm.xrotation(angle)
   return {
       {1,0,0,0}
       ,{0,math.cos(angle),math.sin(angle),0}
       ,{0,-math.sin(angle),math.cos(angle),0}
       ,{0,0,0,1}
   }
end


function mm.zrotation(angle)
   local c = cos(angle)
   local s = sin(angle)
   return {
       {c,s,0,0}
       ,{-s,c,0,0}
       ,{0,0,1,0}
       ,{0,0,0,1}
   }
end

function mm.euler(alpha,beta,gamma)
   return mm.matrix_multiply(
       mm.zrotation(gamma)
       ,mm.matrix_multiply(
           mm.yrotation(beta)
           ,mm.zrotation(alpha)
       )
   )
end

function mm.sphere(longitude,latitude)
   local s = sin(latitude)
   return {{
       s * cos(longitude)
       ,s * sin(longitude)
       ,cos(latitude)
       ,1
   }}
end

function mm.matrix_add(A, B)
   local rows_A = #A
   local columns_A = #A[1]
   local rows_B = #B
   local columns_B = #B[1]
   assert(rows_A == rows_B and columns_A == columns_B, "Wrong size matrices for addition.")
   local sum = {}
   for row = 1, rows_A, 1 do
       sum[row] = {}
       for column = 1, columns_A, 1 do
           sum[row][column] = A[row][column] + B[row][column]
       end
   end
   return sum
end

function mm.matrix_subtract(A,B)
   local rows_A = #A
   local columns_A = #A[1]
   local rows_B = #B
   local columns_B = #B[1]
   assert(rows_A == rows_B and columns_A == columns_B, "Wrong size matrices for subtraction.")
   local sum = {}
   for row = 1, rows_A, 1 do
       sum[row] = {}
       for column = 1, columns_A, 1 do
           sum[row][column] = A[row][column] - B[row][column]
       end
   end
   return sum
end

function mm.matrix_scale(factor, A)
   local rows = #A
   local cols = #A[1]
   local result = {}
   for i = 1, rows do
       result[i] = {}
       for j = 1, cols do
           result[i][j] = A[i][j] * factor
       end
   end
   return result
end

function mm.sign(number)
   if number >= 0 then return "positive" end
   return "negative"
end

function mm.dot_product(u,v)
   local result = u[1][1]*v[1][1] + u[1][2]*v[1][2] + u[1][3]*v[1][3]
   return result
end

function mm.cross_product(u,v)
   local x = u[1][2]*v[1][3]-u[1][3]*v[1][2]
   local y = u[1][3]*v[1][1]-u[1][1]*v[1][3]
   local z = u[1][1]*v[1][2]-u[1][2]*v[1][1]
   local result = {{x,y,z,1}}
   return result
end

function mm.norm(u)
   local result = math.sqrt((u[1][1])^2 + (u[1][2])^2 + (u[1][3])^2)
   return result
end

function mm.normalize(vector)
   local len = mm.norm(vector)
   return {{
       vector[1][1]/len
       ,vector[1][2]/len
       ,vector[1][3]/len
       ,1
   }}
end

function mm.identity_matrix()
   local I = {}
   for i = 1, 4 do
       I[i] = {}
       for j = 1, 4 do
           I[i][j] = (i == j) and 1 or 0
       end
   end
   return I
end

function mm.midpoint(triangle)
   local P,Q,R = table.unpack(triangle)
   local x = (P[1]+Q[1]+R[1])/3
   local y = (P[2]+Q[2]+R[2])/3
   local z = (P[3]+Q[3]+R[3])/3
   return {{x,y,z,1}}
end

function mm.orthogonal_vector(u)
   local v
   if (u[1][1]~=0 and u[1][2]==0 and u[1][3]==0) then
       v = mm.cross_product(u,{{0,1,0,1}})
   else
       v = mm.cross_product(u,{{1,0,0,1}})
   end
   return v
end

function mm.get_observer_plane_basis(observer)
   local origin = {{0,0,0,1}}
   local basis_i = mm.orthogonal_vector(observer)
   basis_i = mm.normalize(basis_i)
   local basis_j = mm.cross_product(observer,basis_i)
   basis_j = mm.normalize(basis_j)
   return {origin,basis_i,basis_j}
end

function mm.orthogonal_vector_projection(base_vector,projected_vector)
   local scale = (
       mm.dot_product(base_vector,projected_vector) /
       mm.dot_product(base_vector,base_vector)
   )
   return {{base_vector[1][1]*scale,base_vector[1][2]*scale,base_vector[1][3]*scale,1}}
end

function mm.project_point_onto_basis(point,basis)
   local normal = mm.cross_product(basis[2],basis[3])
   normal = mm.normalize(normal)
   local vector_from_plane = mm.orthogonal_vector_projection(point,normal)
   local result = {{
       point[1][1]-vector_from_plane[1][1]
       ,point[1][2]-vector_from_plane[1][2]
       ,point[1][3]-vector_from_plane[1][3]
       ,1
   }}
   return result
end

function mm.stereographic_projection(point)
   local x = point[1][1]
   local y = point[1][2]
   local z = point[1][3]
   return {{x / (1 - z), y / (1 - z), 0, 1}}
end

function mm.clip_triangle_against_line(triangle, line)
   -- triangle: array of 3 points {{x,y,z}, {x,y,z}, {x,y,z}}
   -- line: two points defining a 3D line {{x,y,z}, {x,y,z}}

   local function point_on_side(p, line)
       -- Determine signed distance of point p to the infinite line
       -- using vector cross product magnitude with line direction vector

       local function vector_sub(a,b)
           return {a[1]-b[1], a[2]-b[2], a[3]-b[3]}
       end

       local function cross(u,v)
           return {
               u[2]*v[3] - u[3]*v[2],
               u[3]*v[1] - u[1]*v[3],
               u[1]*v[2] - u[2]*v[1],
           }
       end

       local function dot(u,v)
           return u[1]*v[1] + u[2]*v[2] + u[3]*v[3]
       end

       local function norm(v)
           return math.sqrt(dot(v,v))
       end

       local A = line[1]
       local B = line[2]
       local AB = vector_sub(B,A)
       local AP = vector_sub(p,A)

       local cross_vec = cross(AB, AP)
       local dist = norm(cross_vec) / norm(AB)

       -- Also need sign: find projection of AP onto AB perpendicular to AB
       -- sign = dot(cross(AB, AP), some reference vector).
       -- Here we pick a consistent reference: AB cross with vector perpendicular to AB and in plane of triangle
       -- But simpler: pick sign by dot product with cross(AB, AP) and cross(AB, normal)

       -- For simplicity here, let's get the sign by dot product of vector from point projected to line

       -- We'll just use a rough approach: calculate vector perpendicular to AB in triangle plane
       -- Triangle normal
       local normal = cross(
           vector_sub(triangle[2], triangle[1]),
           vector_sub(triangle[3], triangle[1])
       )
       -- Sign based on dot product of cross_vec and normal
       local sign_val = dot(cross_vec, normal)
       if sign_val >= 0 then
           return dist
       else
           return -dist
       end
   end

   -- Compute distances of each vertex to the line
   local d = {}
   for i=1,3 do
       d[i] = point_on_side(triangle[i], line)
   end

   -- Classify vertices by sign
   local positive = {}
   local negative = {}
   for i=1,3 do
       if d[i] >= 0 then
           table.insert(positive, i)
       else
           table.insert(negative, i)
       end
   end

   -- If all on one side, return original triangle
   if #positive == 0 or #negative == 0 then
       return {triangle}
   end

   -- Helper to interpolate between two points by ratio t
   local function interp(p1, p2, t)
       return {
           p1[1] + t*(p2[1] - p1[1]),
           p1[2] + t*(p2[2] - p1[2]),
           p1[3] + t*(p2[3] - p1[3]),
       }
   end

   -- Find intersection points on edges crossing the line (zero crossing of distance)
   local function intersect(i1, i2)
       local p1, p2 = triangle[i1], triangle[i2]
       local dist1, dist2 = d[i1], d[i2]
       local t = dist1 / (dist1 - dist2)
       return interp(p1, p2, t)
   end

   local new_triangles = {}

   if #positive == 2 and #negative == 1 then
       -- Two positive, one negative
       -- Split into two triangles
       local i_neg = negative[1]
       local i_pos1 = positive[1]
       local i_pos2 = positive[2]

       local p_int1 = intersect(i_neg, i_pos1)
       local p_int2 = intersect(i_neg, i_pos2)

       -- Triangle 1: positive1, positive2, p_int1
       table.insert(new_triangles, {triangle[i_pos1], triangle[i_pos2], p_int1})
       -- Triangle 2: positive2, p_int1, p_int2
       table.insert(new_triangles, {triangle[i_pos2], p_int1, p_int2})

   elseif #negative == 2 and #positive == 1 then
       -- Two negative, one positive
       -- Split into one smaller triangle
       local i_pos = positive[1]
       local i_neg1 = negative[1]
       local i_neg2 = negative[2]

       local p_int1 = intersect(i_pos, i_neg1)
       local p_int2 = intersect(i_pos, i_neg2)

       -- Triangle: positive, p_int1, p_int2
       table.insert(new_triangles, {triangle[i_pos], p_int1, p_int2})
   else
       -- Should not happen for triangles but just in case
       return {triangle}
   end

   return new_triangles
end



return mm

You are viewing proxied material from ftp.icm.edu.pl. The copyright of proxied material belongs to its original authors. Any comments or complaints in relation to proxied material should be directed to the original authors of the content concerned. Please see the disclaimer for more details.