// Created 2024 by Ryan A. Colyer.
// This work is released with CC0 into the public domain.
// https://creativecommons.org/publicdomain/zero/1.0/


// Perform an affine transformation of matrix M on coordinate v.
//
// [Scale X]          [Shear X along Y] [Shear X along Z] [Translate X]
// [Shear Y along X]  [Scale Y]         [Shear Y along Z] [Translate Y]
// [Shear Z along X]  [Shear Z along Y] [Scale Z]         [Translate Z]
// or rotation matrix [[cos,-sin],[sin,cos]] in the 2 axes for a plane.
function Affine(M, v) = M * [v[0], v[1], v[2], 1];


// Combine a list of affine transformation matrices into one.
function AffMerge(Mlist) = let(
    AffMergeRec = function(Mlist, i) i >= len(Mlist) ?
      [[1,0,0,0],[0,1,0,0],[0,0,1,0]] :
      let (
        rest = AffMergeRec(Mlist, i+1),
        prod = Mlist[i] * [rest[0], rest[1], rest[2], [0,0,0,1]]
      )
      [prod[0], prod[1], prod[2]]
  )
  AffMergeRec(Mlist, 0);


// Prepare a matrix to rotate around the x-axis.
function RotX(a) =
  [[     1,      0,       0, 0],
   [     0, cos(a), -sin(a), 0],
   [     0, sin(a),  cos(a), 0]];

// Prepare a matrix to rotate around the y-axis.
function RotY(a) =
  [[ cos(a), 0, sin(a), 0],
   [     0,  1,      0, 0],
   [-sin(a), 0, cos(a), 0]];

// Prepare a matrix to rotate around the z-axis.
function RotZ(a) =
  [[cos(a), -sin(a), 0, 0],
   [sin(a),  cos(a), 0, 0],
   [     0,       0, 1, 0]];

// Prepare a matrix to rotate around x, then y, then z.
function Rotate(rotvec) =
  AffMerge([RotZ(rotvec[0]), RotY(rotvec[1]), RotX(rotvec[2])]);

// Prepare a matrix to translate by vector v.
function Translate(v) =
  [[1, 0, 0, v[0]],
   [0, 1, 0, v[1]],
   [0, 0, 1, v[2]]];

// Prepare a matrix to scale by vector v.
function Scale(v) =
  [[v[0],    0,    0, 0],
   [   0, v[1],    0, 0],
   [   0,    0, v[2], 0]];


// Cross-product
function CP(v1, v2) =
  [v1[1]*v2[2]-v1[2]*v2[1], v1[2]*v2[0]-v1[0]*v2[2], v1[0]*v2[1]-v1[1]*v2[0]];


// Find normal vector.
function NormVect(points) = let(
    nv_rec = function(ps, i) let(
        norm_v = CP(ps[1]-ps[0], ps[i]-ps[1])
      )
      norm_v != [0,0,0] ? norm_v/norm(norm_v) :
      i >= len(ps)-1 ? [0,0,0] : nv_rec(ps, i+1)
  )
  nv_rec(points, 2);


// Center of mass
function COM(points) = let(
    sum_rec = function(ps, i, sum=[0,0,0]) i >= len(ps) ? sum :
      sum_rec(ps, i+1, sum+ps[i])
  )
  sum_rec(points, 0)/len(points);


// Rotate around center of mass
function RotCOM(points, rv) = let(com = COM(points))
  [for (p=points)
    Affine(AffMerge([Translate(com), Rotate(rv), Translate(-com)]), p)];

// Test these functions:

vertices = [[2,3,4], [20,5,6], [20,5,17], [2,3,17]];

for (p=vertices) {
  color("blue") translate(p) sphere(1);
}

echo(COM(vertices));

rotated = RotCOM(vertices, [30, 10, 25]);

for (p=rotated) {
  color("red") translate(p) sphere(1);
}

vcom = COM(vertices);
vnorm = NormVect(vertices);
echo(vnorm);
for (i=[0:10]) {
  color("blue") translate(vcom + i*vnorm) cube(1);
}

rcom = COM(rotated);
rnorm = NormVect(rotated);
echo(rnorm);
for (i=[0:10]) {
  color("red") translate(rcom + i*rnorm) cube(1);
}