Refactor: Standardize refactor + add test suite

linalg.scad

@@ -1,32 +1,125 @@
-// very minimal set of linalg functions needed by so3, se3 etc.
+// ============================================================================
+// Minimal Linear Algebra Utilities (for so3, se3, etc.)
+// ----------------------------------------------------------------------------
+// Provides basic vector and matrix operations needed for transformations.
+// - Vector constructors and normalization
+// - Identity matrices
+// - Rotation/translation parts extraction
+// - Transpose and inverse (rigid transform)
+// - Hadamard (elementwise) product
+// ============================================================================
 
-// cross and norm are builtins
-//function cross(x,y) = [x[1]*y[2]-x[2]*y[1], x[2]*y[0]-x[0]*y[2], x[0]*y[1]-x[1]*y[0]];
-//function norm(v) = sqrt(v*v);
+use <lists.scad>
 
-function vec3(p) = len(p) < 3 ? concat(p,0) : p;
-function vec4(p) = let (v3=vec3(p)) len(v3) < 4 ? concat(v3,1) : v3;
-function unit(v) = v/norm(v);
+epsilon = 1e-9;
 
-function identity3()=[[1,0,0],[0,1,0],[0,0,1]]; 
-function identity4()=[[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]];
+// --- Vector Constructors ----------------------------------------------------
+function vec3(p) = (len(p) < 3) ? concat(p, 0) : p;
 
+function vec4(p) =
+  let (v3 = vec3(p)) (len(v3) < 4) ? concat(v3, 1) : v3;
 
-function take3(v) = [v[0],v[1],v[2]];
-function tail3(v) = [v[3],v[4],v[5]];
-function rotation_part(m) = [take3(m[0]),take3(m[1]),take3(m[2])];
+function unit(v) = v / norm(v);
+
+// --- Identity Matrices ------------------------------------------------------
+function identity3() =
+  [
+    [1, 0, 0],
+    [0, 1, 0],
+    [0, 0, 1],
+  ];
+
+function identity4() =
+  [
+    [1, 0, 0, 0],
+    [0, 1, 0, 0],
+    [0, 0, 1, 0],
+    [0, 0, 0, 1],
+  ];
+
+// --- Vector Access Helpers --------------------------------------------------
+function take3(v) = [v[0], v[1], v[2]];
+
+function tail3(v) = [v[3], v[4], v[5]];
+
+// --- Matrix Part Extraction -------------------------------------------------
+function rotation_part(m) =
+  [
+    take3(m[0]),
+    take3(m[1]),
+    take3(m[2]),
+  ];
+
+function translation_part(m) = [m[0][3], m[1][3], m[2][3]];
+
+// --- Matrix Utilities -------------------------------------------------------
+
+// Compute matrix power
+function matrix_power(m, n) =
+  n == 0 ? (len(m) == 3 ? identity3() : identity4())
+  : n == 1 ? m
+  : (n % 2 == 1) ? matrix_power(m * m, floor(n / 2)) * m
+  : matrix_power(m * m, n / 2);
+
+// Determinant (recursive Laplace expansion)
+function det(m) =
+  let (r = [for (i = [0:len(m) - 1]) i]) det_help(m, 0, r);
+
+// Construction indices list is inefficient, but currently there is no way to
+// imperatively assign to a list element
+function det_help(m, i, r) =
+  len(r) == 0 ? 1
+  : m[len(m) - len(r)][r[i]] * det_help(m, 0, remove(r, i)) - (i + 1 < len(r) ? det_help(m, i + 1, r) : 0);
+
+// Matrix inversion (adjugate method)
+function matrix_invert(m) =
+  let (r = [for (i = [0:len(m) - 1]) i]) [
+    for (i = r) [
+      for (j = r) ( (i + j) % 2 == 0 ? 1 : -1) * matrix_minor(m, 0, remove(r, j), remove(r, i)),
+    ],
+  ] / det(m);
+
+function matrix_minor(m, k, ri, rj) =
+  let (len_r = len(ri)) len_r == 0 ? 1
+  : m[ri[0]][rj[k]] * matrix_minor(m, 0, remove(ri, 0), remove(rj, k)) - (k + 1 < len_r ? matrix_minor(m, k + 1, ri, rj) : 0);
+
+// --- Rotation Metrics -------------------------------------------------------
 function rot_trace(m) = m[0][0] + m[1][1] + m[2][2];
-function rot_cos_angle(m) = (rot_trace(m)-1)/2;
-
-function rotation_part(m) = [take3(m[0]),take3(m[1]),take3(m[2])];
-function translation_part(m) = [m[0][3],m[1][3],m[2][3]];
-function transpose_3(m) = [[m[0][0],m[1][0],m[2][0]],[m[0][1],m[1][1],m[2][1]],[m[0][2],m[1][2],m[2][2]]];
-function transpose_4(m) = [[m[0][0],m[1][0],m[2][0],m[3][0]],
-                           [m[0][1],m[1][1],m[2][1],m[3][1]],
-                           [m[0][2],m[1][2],m[2][2],m[3][2]],
-                           [m[0][3],m[1][3],m[2][3],m[3][3]]]; 
-function invert_rt(m) = construct_Rt(transpose_3(rotation_part(m)), -(transpose_3(rotation_part(m)) * translation_part(m)));
-function construct_Rt(R,t) = [concat(R[0],t[0]),concat(R[1],t[1]),concat(R[2],t[2]),[0,0,0,1]];
-
-// Hadamard product of n-dimensional arrays
-function hadamard(a,b) = !(len(a)>0) ? a*b : [ for(i = [0:len(a)-1]) hadamard(a[i],b[i]) ];
+
+function rot_cos_angle(m) = (rot_trace(m) - 1) / 2;
+
+// --- Matrix Transpose -------------------------------------------------------
+function transpose_3(m) =
+  [
+    [m[0][0], m[1][0], m[2][0]],
+    [m[0][1], m[1][1], m[2][1]],
+    [m[0][2], m[1][2], m[2][2]],
+  ];
+
+function transpose_4(m) =
+  [
+    [m[0][0], m[1][0], m[2][0], m[3][0]],
+    [m[0][1], m[1][1], m[2][1], m[3][1]],
+    [m[0][2], m[1][2], m[2][2], m[3][2]],
+    [m[0][3], m[1][3], m[2][3], m[3][3]],
+  ];
+
+// --- Rigid Transform Utilities ---------------------------------------------
+function invert_rt(m) =
+  let (
+    R = transpose_3(rotation_part(m)),
+    t = -(R * translation_part(m))
+  ) construct_Rt(R, t);
+
+function construct_Rt(R, t) =
+  [
+    concat(R[0], t[0]),
+    concat(R[1], t[1]),
+    concat(R[2], t[2]),
+    [0, 0, 0, 1],
+  ];
+
+// --- Elementwise Operations -------------------------------------------------
+// Hadamard product: works recursively on arrays, multiplies scalars directly.
+function hadamard(a, b) =
+  is_list(a) ? [for (i = [0:1:len(a) - 1]) hadamard(a[i], b[i])] : a * b;

lists.scad

@@ -1,48 +1,46 @@
-// List helpers
-
-/*!
-  Flattens a list one level:
-
-  flatten([[0,1],[2,3]]) => [0,1,2,3]
-*/
-function flatten(list) = [ for (i = list, v = i) v ];
-
-
-/*!
-  Creates a list from a range:
-
-  range([0:2:6]) => [0,2,4,6]
-*/
-function range(r) = [ for(x=r) x ];
-
-/*!
-  Reverses a list:
-
-  reverse([1,2,3]) => [3,2,1]
-*/
-function reverse(list) = [for (i = [len(list)-1:-1:0]) list[i]];
-
-/*!
-  Extracts a subarray from index begin (inclusive) to end (exclusive)
-  FIXME: Change name to use list instead of array?
-
-  subarray([1,2,3,4], 1, 2) => [2,3]
-*/
-function subarray(list,begin=0,end=-1) = [
-    let(end = end < 0 ? len(list) : end)
-      for (i = [begin : 1 : end-1])
-        list[i]
-];
-
-/*!
-  Returns a copy of a list with the element at index i set to x
-
-  set([1,2,3,4], 2, 5) => [1,2,5,4]
-*/
-function set(list, i, x) = [for (i_=[0:len(list)-1]) i == i_ ? x : list[i_]];
-
-/*!
-  Remove element from the list by index.
-  remove([4,3,2,1],1) => [4,2,1]
-*/
-function remove(list, i) = [for (i_=[0:1:len(list)-2]) list[i_ < i ? i_ : i_ + 1]];
+// ============================================================================
+// List Utilities
+// ----------------------------------------------------------------------------
+// Provides small helper functions for working with lists:
+// - Flattening
+// - Ranges
+// - Reversals and subarrays
+// - Element updates and removals
+// ============================================================================
+
+// --- Flatten ---------------------------------------------------------------
+// Flatten a list one level.
+// Example: flatten([[0,1],[2,3]]) => [0,1,2,3]
+function flatten(list) = [for (sub = list, v = sub) v];
+
+// --- Range -----------------------------------------------------------------
+// Create a list from a range.
+// Example: range([0:2:6]) => [0,2,4,6]
+function range(r) = [for (x = r) x];
+
+// --- Reverse ---------------------------------------------------------------
+// Reverse the order of elements.
+// Example: reverse([1,2,3]) => [3,2,1]
+function reverse(list) =
+  [for (i = [len(list) - 1:-1:0]) list[i]];
+
+// --- Subarray --------------------------------------------------------------
+// Extract a subarray from index `begin` (inclusive) to `end` (exclusive).
+// Notes:
+//   - If `end < 0`, uses `len(list)`.
+//   - FIXME: Consider renaming to `sublist` for clarity.
+// Example: subarray([1,2,3,4], 1, 3) => [2,3]
+function subarray(list, begin = 0, end = -1) =
+  let (end = (end < 0) ? len(list) : end) [for (i = [begin:end - 1]) list[i]];
+
+// --- Set -------------------------------------------------------------------
+// Return a copy of a list with the element at index `i` replaced by `x`.
+// Example: set([1,2,3,4], 2, 5) => [1,2,5,4]
+function set(list, i, x) =
+  [for (j = [0:1:len(list) - 1]) (i == j) ? x : list[j]];
+
+// --- Remove ---------------------------------------------------------------
+// Remove the element at index `i`.
+// Example: remove([4,3,2,1], 1) => [4,2,1]
+function remove(list, i) =
+  [for (j = [0:1:len(list) - 2]) list[ (j < i) ? j : j + 1]];

mirror.scad

@@ -1,30 +1,43 @@
-// Copyright (c) 2013 Oskar Linde. All rights reserved.
-// License: BSD
-//
-// This library contains simple mirroring functions
-//
-// mirror_x()
-// mirror_y()
-// mirror_z()
+// ============================================================================
+// Mirror Utilities
+// ----------------------------------------------------------------------------
+// Provides simple mirroring modules around the X, Y, and Z axes.
+// Each module duplicates its children and adds a mirrored copy.
+// Optional: pass `col="colorname"` to tint the mirrored copy.
+// - mirror_x([col]): mirror across the YZ-plane (flip X)
+// - mirror_y([col]): mirror across the XZ-plane (flip Y)
+// - mirror_z([col]): mirror across the XY-plane (flip Z)
+// ============================================================================
 
-
-module mirror_x() {
-	union() {
-		children();
-		scale([-1,1,1]) children();
-	}
+// --- Mirror across X-axis ---------------------------------------------------
+module mirror_x(col = undef) {
+  union() {
+    children();
+    if (is_undef(col))
+      scale([-1, 1, 1]) children();
+    else
+      color(col) scale([-1, 1, 1]) children();
+  }
 }
 
-module mirror_y() {
-	union() {
-		children();
-		scale([1,-1,1]) children();
-	}
+// --- Mirror across Y-axis ---------------------------------------------------
+module mirror_y(col = undef) {
+  union() {
+    children();
+    if (is_undef(col))
+      scale([1, -1, 1]) children();
+    else
+      color(col) scale([1, -1, 1]) children();
+  }
 }
 
-module mirror_z() {
-	union() {
-		children();
-		scale([1,1,-1]) children();
-	}
+// --- Mirror across Z-axis ---------------------------------------------------
+module mirror_z(col = undef) {
+  union() {
+    children();
+    if (is_undef(col))
+      scale([1, 1, -1]) children();
+    else
+      color(col) scale([1, 1, -1]) children();
+  }
 }