diff --git a/README.md b/README.md index 8fdd4e3..6445101 100644 --- a/README.md +++ b/README.md @@ -1,81 +1,376 @@ -scad-utils -========== +# scad-utils -Utility libraries for OpenSCAD +Utility libraries for OpenSCAD. +This collection provides reusable math, geometry, and transformation tools for parametric modeling. +All modules are documented, consistently structured, and tested via dedicated `.scad` test files. +A list of projects making use of these utilities can be viewed in the [projects](#projects-using-scad-utils) section below. -Morphology ----------- +--- -contains basic 2D morphology operations +## Modules - inset(d=1) - creates a polygon at an offset d inside a 2D shape - outset(d=1) - creates a polygon at an offset d outside a 2D shape - fillet(r=1) - adds fillets of radius r to all concave corners of a 2D shape - rounding(r=1) - adds rounding to all convex corners of a 2D shape - shell(d,center=false) - makes a shell of width d along the edge of a 2D shape - - positive values of d places the shell on the outside - - negative values of d places the shell on the inside - - center=true and positive d places the shell centered on the edge - - -### Examples +### `hull.scad`: Convex Hull Utilities (2D and 3D) + +Computes convex hulls for 2D or 3D point sets with special handling for collinear cases. + +**Functions:** + +- `hull(points)` - Main entry point, automatically detects 2D/3D and handles edge cases +- Returns polygon vertex indices `[i1, i2, i3, ...]` for 2D +- Returns triangular face indices `[[i1,i2,i3], [i2,i3,i4], ...]` for 3D +- Returns two extreme endpoints `[i_min, i_max]` for collinear points + +**Features:** + +- Uses optimized algorithms with incremental construction +- Handles degenerate cases (collinear, coplanar points) +- Includes utility functions for spherical/cartesian conversions +- Compatible with both clockwise and counter-clockwise orientations + +### `linalg.scad`: Linear Algebra Utilities + +Essential vector and matrix operations for 3D transformations. + +**Vector Functions:** + +- `vec3(p)` - Ensure vector is 3D (pads with 0) +- `vec4(p)` - Ensure vector is 4D homogeneous (pads with 1) +- `unit(v)` - Normalize vector to unit length +- `take3(v)` - Extract first 3 elements +- `tail3(v)` - Extract elements 3,4,5 (for 6-vectors) + +**Matrix Functions:** + +- `identity3()`, `identity4()` - Identity matrices +- `transpose_3(m)`, `transpose_4(m)` - Matrix transpose +- `rotation_part(m)`, `translation_part(m)` - Extract parts from 4×4 transform +- `rot_trace(m)`, `rot_cos_angle(m)` - Rotation analysis +- `invert_rt(m)` - Invert rigid transform (rotation + translation) +- `construct_Rt(R, t)` - Build 4×4 transform from 3×3 rotation and translation +- `hadamard(a, b)` - Element-wise multiplication (works recursively) + +### `lists.scad`: List Manipulation Utilities + +Functional programming helpers for array operations. + +**Functions:** + +- `flatten(list)` - Flatten nested arrays one level: `[[0,1],[2,3]] → [0,1,2,3]` +- `range(r)` - Convert range to list: `[0:2:6] → [0,2,4,6]` +- `reverse(list)` - Reverse element order: `[1,2,3] → [3,2,1]` +- `subarray(list, begin, end)` - Extract slice (end=-1 means full length) +- `set(list, i, x)` - Return copy with element at index i replaced by x +- `remove(list, i)` - Return copy with element at index i removed + +### `mirror.scad`: Axis Mirroring Modules + +Simple mirroring operations that duplicate and reflect geometry. + +**Modules:** + +- `mirror_x([col])` - Mirror across YZ-plane (flip X coordinate) +- `mirror_y([col])` - Mirror across XZ-plane (flip Y coordinate) +- `mirror_z([col])` - Mirror across XY-plane (flip Z coordinate) + +**Parameters:** + +- `col` - Optional color name to tint the mirrored copy (e.g., "red", "teal") + +Each module creates a union of the original children plus the mirrored copy. + +### `morphology.scad`: 2D Morphology Operations + +Advanced 2D shape modification operations for polygon processing. + +**Core Operations:** + +- `outset(d=1)` - Grow shape outward by distance d (Minkowski sum with circle) +- `inset(d=1)` - Shrink shape inward by distance d (inverse of outset) +- `fillet(r=1)` - Add rounded fillets to concave (inward) corners +- `rounding(r=1)` - Round convex (outward) corners +- `shell(d, center=false)` - Create shell/band around shape edge + +**Shell Parameters:** + +- `d > 0, center=false`: shell extends outward +- `d < 0, center=false`: shell extends inward +- `center=true`: shell straddles the original edge + +**Compatibility:** + +- Works around OpenSCAD version differences in Minkowski operations +- Uses `render()` and `projection()` fallbacks for older versions + +### `se3.scad`: SE(3) Lie Group Utilities + +Exponential and logarithm maps for rigid body transformations (translation + rotation). + +**Functions:** + +- `se3_exp(mu)` - Convert 6D twist vector `[tx,ty,tz, rx,ry,rz]` to 4×4 transform matrix +- `se3_ln(m)` - Convert 4×4 transform matrix back to 6D twist vector + +**Features:** + +- Handles small-angle approximations with Taylor series (1st, 2nd, 3rd order) +- Rotation angles specified in degrees (converted internally) +- Combines SO(3) rotations with translation using proper Lie algebra +- Numerical stability for near-identity transforms + +**Dependencies:** `linalg.scad`, `so3.scad` + +### `shapes.scad`: 2D Shape Generators + +Parametric generators for common 2D shapes returned as point arrays. + +**Functions:** + +- `square(size)` - Centered square with edge length `size` +- `circle(r)` - Circle with radius `r` (uses global `$fn` for resolution) +- `regular(r, n)` - Regular n-sided polygon with circumradius `r` +- `rectangle_profile(size=[w,h])` - Rectangle with anchor at `[w/2, 0]` + +**Output:** All functions return arrays of 2D points suitable for `polygon()`. + +### `so3.scad`: SO(3) Lie Group Utilities + +Exponential and logarithm maps for 3D rotations using Rodrigues formula. + +**Functions:** + +- `so3_exp(w)` - Convert axis-angle vector (degrees) to 3×3 rotation matrix +- `so3_ln(m)` - Convert 3×3 rotation matrix back to axis-angle vector (degrees) +- `so3_exp_rad(w)`, `so3_ln_rad(m)` - Radian versions for internal use + +**Features:** + +- Taylor expansions for small angles (1st, 2nd, 3rd order approximations) +- Handles near-π rotations with symmetric matrix decomposition +- Rodrigues formula implementation: `R = I + sin(θ)K + (1-cos(θ))K²` +- Numerical stability across full rotation range + +**Dependencies:** `linalg.scad` + +### `spline.scad`: Cubic Spline and Bezier Utilities + +Comprehensive curve generation and evaluation with Frenet frame support. + +**Spline Functions:** + +- `spline_args(points, closed=false, v1=undef, v2=undef)` - Generate spline coefficients +- `spline(s, t)` - Evaluate position at parameter t +- `spline_tan(s, t)` - Evaluate tangent vector +- `spline_tan_unit(s, t)` - Unit tangent vector +- `spline_d2(s, t)` - Second derivative (curvature) +- `spline_normal_unit(s, t)`, `spline_binormal_unit(s, t)` - Frenet frame vectors +- `spline_transform(s, t)` - SE(3) transform aligned to curve + +**Bezier Functions:** + +- `bezier3_args(points, symmetric=false)` - Generate cubic Bezier coefficients + +**Features:** + +- Supports open and closed splines with customizable end conditions +- Frenet frame calculations for swept surfaces and path following +- Matrix-based coefficient computation with automatic boundary conditions +- Parameter t scales with curve segments (t=0..1 first segment, t=1..2 second, etc.) + +**Dependencies:** `linalg.scad`, `lists.scad` + +### `trajectory.scad`: SE(3) Twist Vector Construction + +Intuitive interface for building 6D motion vectors from directional parameters. + +**Functions:** + +- `trajectory(left/right, up/down, forward/backward, translation, pitch, yaw, roll, rotation)` +- `rotationv(pitch, yaw, roll, rotation)` - Build rotation component +- `translationv(...)` - Build translation component +- `rotationm(...)` - Convert angles to 3×3 rotation matrix + +**Direction Parameters:** + +- Translation: `left/right`, `up/down`, `forward/backward` OR explicit `translation=[x,y,z]` +- Rotation: `pitch`, `yaw`, `roll` (degrees) OR explicit `rotation=[rx,ry,rz]` + +**Output:** Returns `[tx,ty,tz, yaw,pitch,roll]` 6D twist vector suitable for `se3_exp()` + +**Dependencies:** `so3.scad` + +### `trajectory_path.scad`: Multi-Segment Path Quantization + +Convert trajectory sequences into discrete transformation samples. + +**Functions:** + +- `quantize_trajectory(trajectory, step/steps, start_position)` - Sample single 6D twist +- `quantize_trajectories(trajectories, step/steps, start_position, loop)` - Sample path sequence +- `close_trajectory_loop(trajectories)` - Add segment to close loop +- `trajectories_length(trajectories)` - Compute total path length +- `trajectories_end_position(trajectories)` - Final accumulated transform + +**Parameters:** + +- `step`: Physical step length (units of translation norm) +- `steps`: Fixed number of uniform samples across entire path +- `start_position`: Arc-length offset for starting point +- `loop=true`: Automatically close path back to start + +**Output:** Arrays of 4×4 transformation matrices for each sample point + +**Dependencies:** `linalg.scad`, `se3.scad` + +### `transformations.scad`: Geometric Transformation Matrices + +High-level constructors for common 3D transformations. + +**Matrix Constructors:** + +- `rotation(xyz=[rx,ry,rz])` - Euler angles (Rz·Ry·Rx order) +- `rotation(axis=[x,y,z])` - Axis-angle rotation +- `scaling([sx,sy,sz])` - Non-uniform scaling +- `translation([tx,ty,tz])` - Translation matrix + +**Utility Functions:** + +- `project(x)` - Convert homogeneous to Cartesian coordinates +- `transform(m, points)` - Apply matrix to point list +- `to_3d(points)` - Ensure points are 3D vectors + +**Usage:** Matrices can be multiplied for composition: `T * R * S` applies scaling, then rotation, then translation. + +**Dependencies:** `se3.scad`, `linalg.scad`, `lists.scad` + +## Examples + +### Morphology Operations With a basic sample polygon shape, - module shape() { - polygon([[0,0],[1,0],[1.5,1],[2.5,1],[2,-1],[0,-1]]); - } +```scad +module shape() { + polygon([[0,0],[1,0],[1.5,1],[2.5,1],[2,-1],[0,-1]]); +} +$fn = 32; +``` + +- `inset(d=0.3) shape();` + +![Inset Morphology Example](http://oskarlinde.github.io/scad-utils/img/morph-0.png) + +- `outset(d=0.3) shape();` + +![Outset Morphology Example](http://oskarlinde.github.io/scad-utils/img/morph-1.png) + +- `rounding(r=0.3) shape();` + +![Rounding Morphology Example](http://oskarlinde.github.io/scad-utils/img/morph-2.png) + +- `fillet(r=0.3) shape();` + +![Fillet Morphology Example](http://oskarlinde.github.io/scad-utils/img/morph-3.png) + +- `shell(d=0.3) shape();` + +![Shell Morphology Example Positive](http://oskarlinde.github.io/scad-utils/img/morph-4.png) + +- `shell(d=-0.3) shape();` + +![Shell Morphology Example Negative](http://oskarlinde.github.io/scad-utils/img/morph-5.png) + +- `shell(d=0.3,center=true) shape();` -and `$fn=32;`. +![Shell Morphology Example Centered](http://oskarlinde.github.io/scad-utils/img/morph-6.png) +### Mirror Operations -* `inset(d=0.3) shape();` +```scad +use -![](http://oskarlinde.github.io/scad-utils/img/morph-0.png) +module arrow(l=1, w=0.6, t=0.15) { + mirror_y("orange") + polygon([[0,0], [l,0], [l-w/2,w/2], + [l-w/2-sqrt(2)*t,w/2], + [l-t/2-sqrt(2)*t,t/2], [0,t/2]]); +} +arrow(l=20, w=10, t=2); +``` -* `outset(d=0.3) shape();` +### Convex Hull -![](http://oskarlinde.github.io/scad-utils/img/morph-1.png) +```scad +use +points_3d = [[0,0,0], [10,0,0], [5,10,0], [0,0,10]]; +faces = hull(points_3d); +polyhedron(points=points_3d, faces=faces); +``` -* `rounding(r=0.3) shape();` +### Spline Curves -![](http://oskarlinde.github.io/scad-utils/img/morph-2.png) +```scad +use +points = [[0,0,0], [10,10,0], [20,0,5], [30,10,0]]; +spline_data = spline_args(points, closed=true); -* `fillet(r=0.3) shape();` +for (t = [0:0.1:len(spline_data)]) + translate(spline(spline_data, t)) + sphere(r=0.5); +``` -![](http://oskarlinde.github.io/scad-utils/img/morph-3.png) +### SE(3) Transformations +```scad +use +use -*`shell(d=0.3) shape();` +// Define a 6D twist: translate [10,0,5] + rotate 45° about Z +twist = trajectory(forward=10, up=5, yaw=45); +transform_matrix = se3_exp(twist); -![](http://oskarlinde.github.io/scad-utils/img/morph-4.png) +multmatrix(transform_matrix) + cube([2,2,2]); +``` +### Multi-Segment Paths -*`shell(d=-0.3) shape();` +```scad +use -![](http://oskarlinde.github.io/scad-utils/img/morph-5.png) +// Define path segments +path = [ + [20, 0, 0, 0, 0, 30], // forward + yaw + [0, 15, 0, 0, 45, 0], // right + pitch + [0, 0, 10, 0, 0, -60] // up + yaw back +]; +// Sample every 5 units +poses = quantize_trajectories(path, step=5); -*`shell(d=0.3,center=true) shape();` +for (T = poses) + multmatrix(T) + cube([1,1,1]); +``` -![](http://oskarlinde.github.io/scad-utils/img/morph-6.png) +## `tests/` Directory +Each module includes comprehensive test files in the `tests/` directory: -Mirror ------- +**Test Coverage:** -contains simple mirroring functions +- **Regression Tests:** Echo-based validation of mathematical properties (e.g., `so3_ln(so3_exp(w)) ≈ w`) +- **Visual Tests:** Geometric demonstrations with color-coded results +- **Edge Cases:** Boundary conditions, degenerate inputs, numerical stability +- **Integration Tests:** Multi-module workflows (splines with SE(3), trajectory chains) - mirror_x() - mirror_y() - mirror_z() - -example: +**Running Tests:** Load test files directly in OpenSCAD to see both console output and visual results. - module arrow(l=1,w=.6,t=0.15) { - mirror_y() polygon([[0,0],[l,0],[l-w/2,w/2],[l-w/2-sqrt(2)*t,w/2],[l-t/2-sqrt(2)*t,t/2],[0,t/2]]); - } +## Projects using `scad-utils` +- [openscad/list-comprehension-demos](https://github.com/openscad/list-comprehension-demos) uses `lists.scad`, `linalg.scad`, `transformations.scad`, `shape.scad`, `trajectory.scad`, and `trajectory_path.scad` +- [adrianschlatter/threadlib](https://github.com/adrianschlatter/threadlib) uses `lists.scad` and `transformations.scad` through [MisterHW/IoP-satellite](https://github.com/MisterHW/IoP-satellite) +- [likeablob/parametric-stackable-box](https://github.com/likeablob/parametric-stackable-box) uses `morphology.scad` and `mirror.scad` +- [likeablob/misc-printable-accessories](https://github.com/likeablob/misc-printable-accessories) uses `morphology.scad` and `mirror.scad` diff --git a/hull.scad b/hull.scad index 5e0302e..3278b49 100644 --- a/hull.scad +++ b/hull.scad @@ -1,324 +1,216 @@ - -// NOTE: this code uses -// * experimental let() syntax -// * experimental list comprehension syntax -// * search() bugfix and feature addition -// * vector min()/max() - -// Calculates the convex hull of a set of points. -// The result is expressed in point indices. -// If the points are collinear (or 2d), the result is a convex -// polygon [i1,i2,i3,...], otherwise a triangular -// polyhedron [[i1,i2,i3],[i2,i3,i4],...] - -function hull(points) = - !(len(points) > 0) ? [] : - len(points[0]) == 2 ? convexhull2d(points) : - len(points[0]) == 3 ? convexhull3d(points) : []; +// ============================================================================ +// Convex Hull Utilities (2D and 3D) +// ---------------------------------------------------------------------------- +// Computes convex hulls for 2D or 3D point sets. +// +// - 2D: returns polygon vertex indices [i1, i2, i3, ...] +// - 3D: returns triangular face indices [[i1,i2,i3], [i2,i3,i4], ...] +// - Collinear: returns the two extreme endpoints [i_min, i_max] +// +// Notes: +// * Uses let() and list comprehensions +// * Relies on bug-fixed search() +// * Assumes vector min()/max() are available +// ============================================================================ epsilon = 1e-9; -// 2d version -function convexhull2d(points) = -len(points) < 3 ? [] : let( - a=0, b=1, - - c = find_first_noncollinear([a,b], points, 2) - -) c == len(points) ? convexhull_collinear(points) : let( - - remaining = [ for (i = [2:len(points)-1]) if (i != c) i ], - - polygon = area_2d(points[a], points[b], points[c]) > 0 ? [a,b,c] : [b,a,c] - -) convex_hull_iterative_2d(points, polygon, remaining); - - -// Adds the remaining points one by one to the convex hull -function convex_hull_iterative_2d(points, polygon, remaining, i_=0) = i_ >= len(remaining) ? polygon : - let ( - // pick a point - i = remaining[i_], - - // find the segments that are in conflict with the point (point not inside) - conflicts = find_conflicting_segments(points, polygon, points[i]) - - // no conflicts, skip point and move on - ) len(conflicts) == 0 ? convex_hull_iterative_2d(points, polygon, remaining, i_+1) : let( - - // find the first conflicting segment and the first not conflicting - // conflict will be sorted, if not wrapping around, do it the easy way - polygon = remove_conflicts_and_insert_point(polygon, conflicts, i) - ) convex_hull_iterative_2d( - points, - polygon, - remaining, - i_+1 - ); - -function find_conflicting_segments(points, polygon, point) = [ - for (i = [0:len(polygon)-1]) let(j = (i+1) % len(polygon)) - if (area_2d(points[polygon[i]], points[polygon[j]], point) < 0) - i -]; - -// remove the conflicting segments from the polygon -function remove_conflicts_and_insert_point(polygon, conflicts, point) = - conflicts[0] == 0 ? let( - nonconflicting = [ for(i = [0:len(polygon)-1]) if (!contains(conflicts, i)) i ], - new_indices = concat(nonconflicting, (nonconflicting[len(nonconflicting)-1]+1) % len(polygon)), - polygon = concat([ for (i = new_indices) polygon[i] ], point) - ) polygon : let( - prior_to_first_conflict = [ for(i = [0:1:min(conflicts)]) polygon[i] ], - after_last_conflict = [ for(i = [max(conflicts)+1:1:len(polygon)-1]) polygon[i] ], - polygon = concat(prior_to_first_conflict, point, after_last_conflict) - ) polygon; - - -// 3d version -function convexhull3d(points) = -len(points) < 3 ? [ for(i = [0:1:len(points)-1]) i ] : let ( - - // start with a single triangle - a=0, b=1, c=2, - plane = plane(points,a,b,c), - - d = find_first_noncoplanar(plane, points, 3) - -) d == len(points) ? /* all coplanar*/ let ( - - pts2d = [ for (p = points) plane_project(p, points[a], points[b], points[c]) ], - hull2d = convexhull2d(pts2d) - -) hull2d : let( - - remaining = [for (i = [3:len(points)-1]) if (i != d) i], - - // Build an initial tetrahedron - - // swap b,c if d is in front of triangle t - bc = in_front(plane, points[d]) ? [c,b] : [b,c], - b = bc[0], c = bc[1], - - triangles = [ - [a,b,c], - [d,b,a], - [c,d,a], - [b,d,c], - ], - - // calculate the plane equations - planes = [ for (t = triangles) plane(points, t[0], t[1], t[2]) ] - -) convex_hull_iterative(points, triangles, planes, remaining); - -// A plane equation (normal, offset) -function plane(points, a, b, c) = let( - normal = unit(cross(points[c]-points[a], points[b]-points[a])) -) [ - normal, - normal * points[a] -]; - -// Adds the remaining points one by one to the convex hull -function convex_hull_iterative(points, triangles, planes, remaining, i_=0) = i_ >= len(remaining) ? triangles : - let ( - // pick a point - i = remaining[i_], - - // find the triangles that are in conflict with the point (point not inside) - conflicts = find_conflicts(points[i], planes), - - // for all triangles that are in conflict, collect their halfedges - halfedges = [ - for(c = conflicts) - for(i = [0:2]) let(j = (i+1)%3) - [triangles[c][i], triangles[c][j]] - ], - - // find the outer perimeter of the set of conflicting triangles - horizon = remove_internal_edges(halfedges), - - // generate a new triangle for each horizon halfedge together with the picked point i - new_triangles = [ for (h = horizon) concat(h,i) ], - - // calculate the corresponding plane equations - new_planes = [ for (t = new_triangles) plane(points, t[0], t[1], t[2]) ] - - ) convex_hull_iterative( - points, - // remove the conflicting triangles and add the new ones - concat(remove_elements(triangles, conflicts), new_triangles), - concat(remove_elements(planes, conflicts), new_planes), - remaining, - i_+1 - ); - -function convexhull_collinear(points) = let( - n = points[1] - points[0], - a = points[0], - points1d = [ for(p = points) (p-a)*n ], - min_i = min_index(points1d), - max_i = max_index(points1d) -) [ min_i, max_i ]; - -function min_index(values,min_,min_i_,i_) = - i_ == undef ? min_index(values,values[0],0,1) : - i_ >= len(values) ? min_i_ : - values[i_] < min_ ? min_index(values,values[i_],i_,i_+1) - : min_index(values,min_,min_i_,i_+1); - -function max_index(values,max_,max_i_,i_) = - i_ == undef ? max_index(values,values[0],0,1) : - i_ >= len(values) ? max_i_ : - values[i_] > max_ ? max_index(values,values[i_],i_,i_+1) - : max_index(values,max_,max_i_,i_+1); +// --- Entry Point ------------------------------------------------------------ +function hull(points) = + (len(points) == 0) ? [] + : (len(points[0]) == 2) ? convexhull2d(points) + : (len(points[0]) == 3) ? convexhull3d(points) : []; -function remove_elements(array, elements) = [ - for (i = [0:len(array)-1]) - if (!search(i, elements)) - array[i] -]; - -function remove_internal_edges(halfedges) = [ - for (h = halfedges) - if (!contains(halfedges, reverse(h))) - h -]; - -function plane_project(point, a, b, c) = let( - u = b-a, - v = c-a, - n = cross(u,v), - w = cross(n,u), - relpoint = point-a -) [relpoint * u, relpoint * w]; - -function plane_unproject(point, a, b, c) = let( - u = b-a, - v = c-a, - n = cross(u,v), - w = cross(n,u) -) a + point[0] * u + point[1] * w; - -function reverse(arr) = [ for (i = [len(arr)-1:-1:0]) arr[i] ]; - -function contains(arr, element) = search([element],arr)[0] != [] ? true : false; - -function find_conflicts(point, planes) = [ - for (i = [0:len(planes)-1]) - if (in_front(planes[i], point)) - i -]; - -function find_first_noncollinear(line, points, i) = - i >= len(points) ? len(points) : - collinear(points[line[0]], - points[line[1]], - points[i]) ? find_first_noncollinear(line, points, i+1) - : i; - -function find_first_noncoplanar(plane, points, i) = - i >= len(points) ? len(points) : - coplanar(plane, points[i]) ? find_first_noncoplanar(plane, points, i+1) - : i; - -function distance(plane, point) = plane[0] * point - plane[1]; - -function in_front(plane, point) = distance(plane, point) > epsilon; - -function coplanar(plane, point) = abs(distance(plane,point)) <= epsilon; - -function unit(v) = v/norm(v); - -function area_2d(a,b,c) = ( - a[0] * (b[1] - c[1]) + - b[0] * (c[1] - a[1]) + - c[0] * (a[1] - b[1])) / 2; - -function collinear(a,b,c) = abs(area_2d(a,b,c)) < epsilon; - -function spherical(cartesian) = [ - atan2(cartesian[1], cartesian[0]), - asin(cartesian[2]) -]; - -function cartesian(spherical) = [ - cos(spherical[1]) * cos(spherical[0]), - cos(spherical[1]) * sin(spherical[0]), - sin(spherical[1]) -]; - - -/// TESTCODE - - -phi = 1.618033988749895; - -testpoints_on_sphere = [ for(p = - [ - [1,phi,0], [-1,phi,0], [1,-phi,0], [-1,-phi,0], - [0,1,phi], [0,-1,phi], [0,1,-phi], [0,-1,-phi], - [phi,0,1], [-phi,0,1], [phi,0,-1], [-phi,0,-1] - ]) - unit(p) -]; - -testpoints_spherical = [ for(p = testpoints_on_sphere) spherical(p) ]; -testpoints_circular = [ for(a = [0:15:360-epsilon]) [cos(a),sin(a)] ]; - -testpoints_coplanar = let(u = unit([1,3,7]), v = unit([-2,1,-2])) [ for(i = [1:10]) rands(-1,1,1)[0] * u + rands(-1,1,1)[0] * v ]; - -testpoints_collinear_2d = let(u = unit([5,3])) [ for(i = [1:20]) rands(-1,1,1)[0] * u ]; -testpoints_collinear_3d = let(u = unit([5,3,-5])) [ for(i = [1:20]) rands(-1,1,1)[0] * u ]; - -testpoints2d = 20 * [for (i = [1:10]) concat(rands(-1,1,2))]; -testpoints3d = 20 * [for (i = [1:50]) concat(rands(-1,1,3))]; - -// All points are on the sphere, no point should be red -translate([-50,0]) visualize_hull(20*testpoints_on_sphere); - -// 2D points -translate([50,0]) visualize_hull(testpoints2d); - -// All points on a circle, no point should be red -translate([0,50]) visualize_hull(20*testpoints_circular); - -// All points 3d but collinear -translate([0,-50]) visualize_hull(20*testpoints_coplanar); - -// Collinear -translate([50,50]) visualize_hull(20*testpoints_collinear_2d); - -// Collinear -translate([-50,50]) visualize_hull(20*testpoints_collinear_3d); - -// 3D points -visualize_hull(testpoints3d); - - -module visualize_hull(points) { - - hull = hull(points); - - %if (len(hull) > 0 && len(hull[0]) > 0) - polyhedron(points=points, faces = hull); - else - polyhedron(points=points, faces = [hull]); - - for (i = [0:len(points)-1]) assign(p = points[i], $fn = 16) { - translate(p) { - if (hull_contains_index(hull,i)) { - color("blue") sphere(1); - } else { - color("red") sphere(1); - } - } - } - - function hull_contains_index(hull, index) = - search(index,hull,1,0) || - search(index,hull,1,1) || - search(index,hull,1,2); - -} +// --- 2D Convex Hull --------------------------------------------------------- +function convexhull2d(points) = + (len(points) < 3) ? [] + : let ( + a = 0, + b = 1, + c = find_first_noncollinear([a, b], points, 2) + ) (c == len(points)) ? convexhull_collinear(points) + : let ( + remaining = [for (i = [2:len(points) - 1]) if (i != c) i], + polygon = (area_2d(points[a], points[b], points[c]) > 0) ? [a, b, c] : [b, a, c] + ) convex_hull_iterative_2d(points, polygon, remaining); + +function convex_hull_iterative_2d(points, polygon, remaining, i_ = 0) = + (i_ >= len(remaining)) ? polygon + : let ( + i = remaining[i_], + conflicts = find_conflicting_segments(points, polygon, points[i]) + ) (len(conflicts) == 0) ? + convex_hull_iterative_2d(points, polygon, remaining, i_ + 1) + : let ( + polygon = remove_conflicts_and_insert_point(polygon, conflicts, i) + ) convex_hull_iterative_2d(points, polygon, remaining, i_ + 1); + +function find_conflicting_segments(points, polygon, point) = + [ + for (i = [0:1:len(polygon) - 1]) let (j = (i + 1) % len(polygon)) if (area_2d(points[polygon[i]], points[polygon[j]], point) < 0) i, + ]; + +function remove_conflicts_and_insert_point(polygon, conflicts, point) = + (conflicts[0] == 0) ? + let ( + nonconf = [for (i = [0:1:len(polygon) - 1]) if (!contains(conflicts, i)) i], + indices = concat(nonconf, (nonconf[len(nonconf) - 1] + 1) % len(polygon)) + ) concat([for (i = indices) polygon[i]], point) + : let ( + before = [for (i = [0:1:min(conflicts)]) polygon[i]], + after = [for (i = [max(conflicts) + 1:1:len(polygon) - 1]) polygon[i]] + ) concat(before, point, after); + +// --- 3D Convex Hull --------------------------------------------------------- +function convexhull3d(points) = + (len(points) < 3) ? [for (i = [0:1:len(points) - 1]) i] + : is_collinear3(points) ? + convexhull_collinear(points) + : let ( + a = 0, + b = 1, + c = find_first_noncollinear3([a, b], points, 2), + pl = plane(points, a, b, c), + d = find_first_noncoplanar(pl, points, 0) + ) (d == len(points)) ? + // all coplanar → project to 2D + let (pts2d = [for (p = points) plane_project(p, points[a], points[b], points[c])]) convexhull2d(pts2d) + : let ( + // build initial tetrahedron + remaining = [for (i = [0:1:len(points) - 1]) if (i != a && i != b && i != c && i != d) i], + bc = in_front(pl, points[d]) ? [c, b] : [b, c], + b2 = bc[0], + c2 = bc[1], + tris = [[a, b2, c2], [d, b2, a], [c2, d, a], [b2, d, c2]], + planes = [for (t = tris) plane(points, t[0], t[1], t[2])] + ) convex_hull_iterative(points, tris, planes, remaining); + +// --- Helpers for 3D Collinearity ------------------------------------------- +function is_collinear3(pts) = + (len(pts) < 3) ? true + : let (a = pts[0], dir = pts[1] - a) _all_col3(pts, a, dir, 2); + +function _all_col3(pts, a, dir, i) = + (i >= len(pts)) ? true + : (norm(cross(pts[i] - a, dir)) <= epsilon) ? + _all_col3(pts, a, dir, i + 1) + : false; + +function find_first_noncollinear3(line, pts, i) = + (i >= len(pts)) ? len(pts) + : (norm(cross(pts[line[1]] - pts[line[0]], pts[i] - pts[line[0]])) <= epsilon) ? + find_first_noncollinear3(line, pts, i + 1) + : i; + +function plane(points, a, b, c) = + let (n = unit(cross(points[c] - points[a], points[b] - points[a]))) [n, n * points[a]]; + +function convex_hull_iterative(points, triangles, planes, remaining, i_ = 0) = + (i_ >= len(remaining)) ? triangles + : let ( + idx = remaining[i_], + conflicts = find_conflicts(points[idx], planes), + halfedges = [ + for (c = conflicts) for (k = [0:2]) let (j = (k + 1) % 3) [triangles[c][k], triangles[c][j]], + ], + horizon = remove_internal_edges(halfedges), + new_tris = [for (h = horizon) concat(h, idx)], + new_pls = [for (t = new_tris) plane(points, t[0], t[1], t[2])] + ) convex_hull_iterative( + points, + concat(remove_elements(triangles, conflicts), new_tris), + concat(remove_elements(planes, conflicts), new_pls), + remaining, + i_ + 1 + ); + +// --- Collinear Special Case ------------------------------------------------- +function convexhull_collinear(points) = + let ( + n = points[1] - points[0], + a = points[0], + pts1d = [for (p = points) (p - a) * n], + min_i = min_index(pts1d), + max_i = max_index(pts1d) + ) [min_i, max_i]; + +// --- Utility Functions ------------------------------------------------------ +function min_index(values, min_ = undef, idx_min = undef, i_ = 0) = + (i_ == 0) ? min_index(values, values[0], 0, 1) + : (i_ >= len(values)) ? idx_min + : (values[i_] < min_) ? + min_index(values, values[i_], i_, i_ + 1) + : min_index(values, min_, idx_min, i_ + 1); + +function max_index(values, max_ = undef, idx_max = undef, i_ = 0) = + (i_ == 0) ? max_index(values, values[0], 0, 1) + : (i_ >= len(values)) ? idx_max + : (values[i_] > max_) ? + max_index(values, values[i_], i_, i_ + 1) + : max_index(values, max_, idx_max, i_ + 1); + +function remove_elements(arr, to_remove) = + [for (i = [0:1:len(arr) - 1]) if (!search(i, to_remove)) arr[i]]; + +function remove_internal_edges(edges) = + [for (h = edges) if (!contains(edges, reverse(h))) h]; + +function plane_project(p, a, b, c) = + let ( + u = b - a, + v = c - a, + n = cross(u, v), + w = cross(n, u), + rel = p - a + ) [rel * u, rel * w]; + +function plane_unproject(p, a, b, c) = + let ( + u = b - a, + v = c - a, + n = cross(u, v), + w = cross(n, u) + ) a + p[0] * u + p[1] * w; + +function reverse(arr) = [for (i = [len(arr) - 1:-1:0]) arr[i]]; + +function contains(arr, el) = (search([el], arr)[0] != []) ? true : false; + +function find_conflicts(p, planes) = + [for (i = [0:1:len(planes) - 1]) if (in_front(planes[i], p)) i]; + +function find_first_noncollinear(line, pts, i) = + (i >= len(pts)) ? len(pts) + : collinear(pts[line[0]], pts[line[1]], pts[i]) ? + find_first_noncollinear(line, pts, i + 1) + : i; + +function find_first_noncoplanar(pl, pts, i) = + (i >= len(pts)) ? len(pts) + : coplanar(pl, pts[i]) ? + find_first_noncoplanar(pl, pts, i + 1) + : i; + +function distance(pl, p) = pl[0] * p - pl[1]; +function in_front(pl, p) = distance(pl, p) > epsilon; +function coplanar(pl, p) = abs(distance(pl, p)) <= epsilon; + +function unit(v) = v / norm(v); + +function area_2d(a, b, c) = + ( + a[0] * (b[1] - c[1]) + b[0] * (c[1] - a[1]) + c[0] * (a[1] - b[1]) + ) / 2; + +function collinear(a, b, c) = + abs(area_2d(a, b, c)) < epsilon; + +function spherical(cart) = + [atan2(cart[1], cart[0]), asin(cart[2])]; + +function cartesian(sph) = + [ + cos(sph[1]) * cos(sph[0]), + cos(sph[1]) * sin(sph[0]), + sin(sph[1]), + ]; diff --git a/linalg.scad b/linalg.scad index 959a38e..1db0640 100644 --- a/linalg.scad +++ b/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 -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; diff --git a/lists.scad b/lists.scad index 0d8e2b4..ee3febd 100644 --- a/lists.scad +++ 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]]; diff --git a/mirror.scad b/mirror.scad index d7665d6..53945c2 100644 --- a/mirror.scad +++ b/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(); + } } diff --git a/morphology.scad b/morphology.scad index 4bdd28b..a376249 100644 --- a/morphology.scad +++ b/morphology.scad @@ -1,109 +1,92 @@ -// Copyright (c) 2013 Oskar Linde. All rights reserved. -// License: BSD +// ============================================================================ +// Morphology Utilities (2D) +// ---------------------------------------------------------------------------- +// Provides basic 2D morphological operations: +// - outset(d=1): offset polygon outward +// - inset(d=1): offset polygon inward +// - fillet(r=1): fillet concave corners +// - rounding(r=1): round convex corners +// - shell(d, center=false): create a band (shell) around a polygon // -// This library contains basic 2D morphology operations -// -// outset(d=1) - creates a polygon at an offset d outside a 2D shape -// inset(d=1) - creates a polygon at an offset d inside a 2D shape -// fillet(r=1) - adds fillets of radius r to all concave corners of a 2D shape -// rounding(r=1) - adds rounding to all convex corners of a 2D shape -// shell(d,center=false) - makes a shell of width d along the edge of a 2D shape -// - positive values of d places the shell on the outside -// - negative values of d places the shell on the inside -// - center=true and positive d places the shell centered on the edge - -module outset(d=1) { - // Bug workaround for older OpenSCAD versions - if (version_num() < 20130424) render() outset_extruded(d) children(); - else minkowski() { - circle(r=d); - children(); - } -} - -module outset_extruded(d=1) { - projection(cut=true) minkowski() { - cylinder(r=d); - linear_extrude(center=true) children(); - } -} - -module inset(d=1) { - render() inverse() outset(d=d) inverse() children(); +// Notes: +// - Works around version-specific issues with minkowski/projection +// - Internal `_inverse()` helper used for inset/boolean operations +// ============================================================================ + +// --- Outset / Inset --------------------------------------------------------- + +// Outset: grows a shape outward by distance d +module outset(d = 1) { + // Bug workaround for older OpenSCAD versions + if (version_num() < 20130424) + render() outset_extruded(d) children(); + else + minkowski() { + circle(r=d); + children(); + } } -module fillet(r=1) { - inset(d=r) render() outset(d=r) children(); +// Helper for older OpenSCAD: emulate outset by extrusion + projection +module outset_extruded(d = 1) { + projection(cut=true) + minkowski() { + cylinder(r=d); + linear_extrude(center=true) children(); + } } -module rounding(r=1) { - outset(d=r) inset(d=r) children(); +// Inset: shrinks a shape inward by distance d +module inset(d = 1) { + render() _inverse() outset(d=d) _inverse() children(); } -module shell(d,center=false) { - if (center && d > 0) { - difference() { - outset(d=d/2) children(); - inset(d=d/2) children(); - } - } - if (!center && d > 0) { - difference() { - outset(d=d) children(); - children(); - } - } - if (!center && d < 0) { - difference() { - children(); - inset(d=-d) children(); - } - } - if (d == 0) children(); -} - - -// Below are for internal use only +// --- Corner Modifiers ------------------------------------------------------- -module inverse() { - difference() { - square(1e5,center=true); - children(); - } +// Fillet: adds arcs of radius r to concave corners +module fillet(r = 1) { + inset(d=r) render() outset(d=r) children(); } - -// TEST CODE - -use - -module arrow(l=1,w=.6,t=0.15) { - mirror_y() polygon([[0,0],[l,0],[l-w/2,w/2],[l-w/2-sqrt(2)*t,w/2],[l-t/2-sqrt(2)*t,t/2],[0,t/2]]); +// Rounding: rounds convex corners with arcs of radius r +module rounding(r = 1) { + outset(d=r) inset(d=r) children(); } -module shape() { - polygon([[0,0],[1,0],[1.5,1],[2.5,1],[2,-1],[0,-1]]); +// --- Shells ---------------------------------------------------------------- + +// Shell: creates a band of width d around the polygon edge +// - d > 0, center=false: shell on the outside +// - d < 0, center=false: shell on the inside +// - center=true: shell straddles the edge +module shell(d, center = false) { + if (center && d > 0) { + difference() { + outset(d=d / 2) children(); + inset(d=d / 2) children(); + } + } + if (!center && d > 0) { + difference() { + outset(d=d) children(); + children(); + } + } + if (!center && d < 0) { + difference() { + children(); + inset(d=-d) children(); + } + } + if (d == 0) children(); } -if(0) assign($fn=32) { +// --- Internal Helpers ------------------------------------------------------- - for (p = [0:10*3-1]) assign(o=floor(p/3)) { - translate([(p%3)*2.5,-o*3]) { - //%if (p % 3 == 1) translate([0,0,1]) shape(); - if (p % 3 == 0) shape(); - if (p % 3 == 1) translate([0.6,0]) arrow(); - if (p % 3 == 2) { - if (o == 0) inset(d=0.3) shape(); - if (o == 1) outset(d=0.3) shape(); - if (o == 2) rounding(r=0.3) shape(); - if (o == 3) fillet(r=0.3) shape(); - if (o == 4) shell(d=0.3) shape(); - if (o == 5) shell(d=-0.3) shape(); - if (o == 6) shell(d=0.3,center=true) shape(); - if (o == 7) rounding(r=0.3) fillet(r=0.3) shape(); - if (o == 8) shell(d=0.3,center=true) fillet(r=0.3) rounding(r=0.3) shape(); - if (o == 9) shell(d=-0.3) fillet(r=0.3) rounding(r=0.3) shape(); - } - } - } +// Inverse: covers the plane and subtracts children (used in inset) +module _inverse() { + difference() { + square(1e5, center=true); + children(); + } } diff --git a/se3.scad b/se3.scad index 48b59e6..1493a01 100644 --- a/se3.scad +++ b/se3.scad @@ -1,60 +1,120 @@ +// ============================================================================ +// SE(3) Utilities (Rigid Body Transformations in 3D) +// ---------------------------------------------------------------------------- +// Provides exponential and logarithm maps for SE(3) using so3 + translation. +// - se3_exp(mu): exponential map of 6-vector [t, w] (t=translation, w=rotation) +// - se3_ln(m): logarithm map from 4x4 rigid transform matrix to 6-vector +// ---------------------------------------------------------------------------- +// Dependencies: +// - linalg.scad (matrix utilities) +// - so3.scad (SO(3) exponential/logarithm maps) +// ============================================================================ + use use -function combine_se3_exp(w, ABt) = construct_Rt(rodrigues_so3_exp(w, ABt[0], ABt[1]), ABt[2]); +// --- Core Helpers ----------------------------------------------------------- + +// Combine rotation (Rodrigues) with translated ABt components +function combine_se3_exp(w, ABt) = + construct_Rt( + rodrigues_so3_exp(w, ABt[0], ABt[1]), + ABt[2] + ); + +// --- Exponential Map (Taylor Approximations) -------------------------------- + +// Small-angle approx (1st order) +function se3_exp_1(t, w) = + concat( + so3_exp_1(w * w), + [t + 0.5 * cross(w, t)] + ); + +// 2nd order Taylor expansion +function se3_exp_2(t, w) = se3_exp_2_0(t, w, w * w); + +function se3_exp_2_0(t, w, theta_sq) = + se3_exp_23( + so3_exp_2(theta_sq), + C=(1.0 - theta_sq / 20) / 6, + t=t, + w=w + ); -// [A,B,t] -function se3_exp_1(t,w) = concat( - so3_exp_1(w*w), - [t + 0.5 * cross(w,t)] -); +// 3rd order approximation +function se3_exp_3(t, w) = + se3_exp_3_0( + t, w, + theta_deg=sqrt(w * w) * 180 / PI, + inv_theta=1 / sqrt(w * w) + ); -function se3_exp_2(t,w) = se3_exp_2_0(t,w,w*w); -function se3_exp_2_0(t,w,theta_sq) = -se3_exp_23( - so3_exp_2(theta_sq), - C = (1.0 - theta_sq/20) / 6, - t=t,w=w); +function se3_exp_3_0(t, w, theta_deg, inv_theta) = + se3_exp_23( + so3_exp_3_0(theta_deg=theta_deg, inv_theta=inv_theta), + C=(1 - sin(theta_deg) * inv_theta) * (inv_theta * inv_theta), + t=t, + w=w + ); -function se3_exp_3(t,w) = se3_exp_3_0(t,w,sqrt(w*w)*180/PI,1/sqrt(w*w)); +// Shared expansion for orders 2–3 +function se3_exp_23(AB, C, t, w) = + [AB[0], AB[1], t + AB[1] * cross(w, t) + C * cross(w, cross(w, t))]; -function se3_exp_3_0(t,w,theta_deg,inv_theta) = -se3_exp_23( - so3_exp_3_0(theta_deg = theta_deg, inv_theta = inv_theta), - C = (1 - sin(theta_deg) * inv_theta) * (inv_theta * inv_theta), - t=t,w=w); +// --- Exponential Map Wrapper ------------------------------------------------ -function se3_exp_23(AB,C,t,w) = -[AB[0], AB[1], t + AB[1] * cross(w,t) + C * cross(w,cross(w,t)) ]; +// se3_exp: exponential map from 6-vector μ = [t, w] +// Converts w from degrees to radians internally +function se3_exp(mu) = + se3_exp_0( + t=take3(mu), + w=tail3(mu) / 180 * PI + ); -function se3_exp(mu) = se3_exp_0(t=take3(mu),w=tail3(mu)/180*PI); +function se3_exp_0(t, w) = + combine_se3_exp( + w, + (w * w < 1e-8) ? se3_exp_1(t, w) + : (w * w < 1e-6) ? se3_exp_2(t, w) + : se3_exp_3(t, w) + ); -function se3_exp_0(t,w) = -combine_se3_exp(w, -// Evaluate by Taylor expansion when near 0 - w*w < 1e-8 - ? se3_exp_1(t,w) - : w*w < 1e-6 - ? se3_exp_2(t,w) - : se3_exp_3(t,w) -); +// --- Logarithm Map ---------------------------------------------------------- +// Logarithm: returns [t, w] in degrees function se3_ln(m) = se3_ln_to_deg(se3_ln_rad(m)); -function se3_ln_to_deg(v) = concat(take3(v),tail3(v)*180/PI); - -function se3_ln_rad(m) = se3_ln_0(m, - rot = so3_ln_rad(rotation_part(m))); -function se3_ln_0(m,rot) = se3_ln_1(m,rot, - theta = sqrt(rot*rot)); -function se3_ln_1(m,rot,theta) = se3_ln_2(m,rot,theta, - shtot = theta > 0.00001 ? sin(theta/2*180/PI)/theta : 0.5, - halfrotator = so3_exp_rad(rot * -.5)); -function se3_ln_2(m,rot,theta,shtot,halfrotator) = -concat( (halfrotator * translation_part(m) - - (theta > 0.001 - ? rot * ((translation_part(m) * rot) * (1-2*shtot) / (rot*rot)) - : rot * ((translation_part(m) * rot)/24) - )) / (2 * shtot), rot); - -__se3_test = [20,-40,60,-80,100,-120]; -echo(UNITTEST_se3=norm(__se3_test-se3_ln(se3_exp(__se3_test))) < 1e-8); + +function se3_ln_to_deg(v) = concat(take3(v), tail3(v) * 180 / PI); + +// Internal: logarithm in radians +function se3_ln_rad(m) = + se3_ln_0( + m, + rot=so3_ln_rad(rotation_part(m)) + ); + +function se3_ln_0(m, rot) = + se3_ln_1( + m, rot, + theta=sqrt(rot * rot) + ); + +function se3_ln_1(m, rot, theta) = + se3_ln_2( + m, rot, theta, + shtot=(theta > 1e-5) ? sin(theta / 2 * 180 / PI) / theta : 0.5, + halfrotator=so3_exp_rad(rot * -0.5) + ); + +function se3_ln_2(m, rot, theta, shtot, halfrotator) = + concat( + ( + halfrotator * translation_part(m) - ( + (theta > 1e-3) ? + rot * ( (translation_part(m) * rot) * (1 - 2 * shtot) / (rot * rot)) + : rot * ( (translation_part(m) * rot) / 24) + ) + ) / (2 * shtot), + rot + ); diff --git a/shapes.scad b/shapes.scad index 6d33d8a..07d19e3 100644 --- a/shapes.scad +++ b/shapes.scad @@ -1,16 +1,43 @@ -function square(size) = [[-size,-size], [-size,size], [size,size], [size,-size]] / 2; +// ============================================================================ +// Shape Generators (2D) +// ---------------------------------------------------------------------------- +// Provides simple parametric 2D shape generators: +// - square(size): centered square polygon +// - circle(r): circle polygon using current $fn +// - regular(r, n): regular n-gon (circle with fixed $fn) +// - rectangle_profile(size=[w,h]): rectangular profile with anchor at [w/2,0] +// ---------------------------------------------------------------------------- +// Notes: +// - All outputs are polygon point lists (suitable for polygon(), etc.) +// - `circle` and `regular` depend on $fn resolution +// ============================================================================ -function circle(r) = [for (i=[0:$fn-1]) let (a=i*360/$fn) r * [cos(a), sin(a)]]; +// --- Square ----------------------------------------------------------------- +// size: length of edge +function square(size) = + [[-size, -size], [-size, size], [size, size], [size, -size]] / 2; +// --- Circle ----------------------------------------------------------------- +// r: radius +// Uses global $fn for resolution +function circle(r) = + [for (i = [0:$fn - 1]) let (a = i * 360 / $fn) r * [cos(a), sin(a)]]; + +// --- Regular Polygon -------------------------------------------------------- +// r: circumradius +// n: number of sides function regular(r, n) = circle(r, $fn=n); -function rectangle_profile(size=[1,1]) = [ - // The first point is the anchor point, put it on the point corresponding to [cos(0),sin(0)] - [ size[0]/2, 0], - [ size[0]/2, size[1]/2], - [-size[0]/2, size[1]/2], - [-size[0]/2, -size[1]/2], - [ size[0]/2, -size[1]/2], -]; +// --- Rectangle Profile ------------------------------------------------------ +// size = [w, h] +// Anchor point is [w/2, 0] +function rectangle_profile(size = [1, 1]) = + [ + [size[0] / 2, 0], + [size[0] / 2, size[1] / 2], + [-size[0] / 2, size[1] / 2], + [-size[0] / 2, -size[1] / 2], + [size[0] / 2, -size[1] / 2], + ]; -// FIXME: Move rectangle and rounded rectangle from extrusion \ No newline at end of file +// FIXME: Move rectangle and rounded rectangle from extrusion diff --git a/so3.scad b/so3.scad index 83308aa..f33ee85 100644 --- a/so3.scad +++ b/so3.scad @@ -1,82 +1,117 @@ -// so3 +// ============================================================================ +// SO(3) Utilities (3D Rotation Group) +// ---------------------------------------------------------------------------- +// Provides exponential and logarithm maps for SO(3). +// - so3_exp(w): exponential map from axis-angle (deg) +// - so3_ln(m): logarithm map from rotation matrix → axis-angle (deg) +// ---------------------------------------------------------------------------- +// Dependencies: +// - linalg.scad +// ============================================================================ use -function rodrigues_so3_exp(w, A, B) = [ -[1.0 - B*(w[1]*w[1] + w[2]*w[2]), B*(w[0]*w[1]) - A*w[2], B*(w[0]*w[2]) + A*w[1]], -[B*(w[0]*w[1]) + A*w[2], 1.0 - B*(w[0]*w[0] + w[2]*w[2]), B*(w[1]*w[2]) - A*w[0]], -[B*(w[0]*w[2]) - A*w[1], B*(w[1]*w[2]) + A*w[0], 1.0 - B*(w[0]*w[0] + w[1]*w[1])] -]; +// --- Rodrigues Formula (core exp) ------------------------------------------ -function so3_exp(w) = so3_exp_rad(w/180*PI); +// Build rotation matrix given axis w and coefficients A, B +function rodrigues_so3_exp(w, A, B) = + [ + [1.0 - B * (w[1] * w[1] + w[2] * w[2]), B * (w[0] * w[1]) - A * w[2], B * (w[0] * w[2]) + A * w[1]], + [B * (w[0] * w[1]) + A * w[2], 1.0 - B * (w[0] * w[0] + w[2] * w[2]), B * (w[1] * w[2]) - A * w[0]], + [B * (w[0] * w[2]) - A * w[1], B * (w[1] * w[2]) + A * w[0], 1.0 - B * (w[0] * w[0] + w[1] * w[1])], + ]; + +// --- Exponential Map -------------------------------------------------------- + +// Wrapper: exponential map with input in degrees +function so3_exp(w) = so3_exp_rad(w / 180 * PI); + +// Exponential map in radians, uses Taylor expansions for small angles function so3_exp_rad(w) = -combine_so3_exp(w, - w*w < 1e-8 - ? so3_exp_1(w*w) - : w*w < 1e-6 - ? so3_exp_2(w*w) - : so3_exp_3(w*w)); - -function combine_so3_exp(w,AB) = rodrigues_so3_exp(w,AB[0],AB[1]); - -// Taylor series expansions close to 0 -function so3_exp_1(theta_sq) = [ - 1 - 1/6*theta_sq, - 0.5 -]; - -function so3_exp_2(theta_sq) = [ - 1.0 - theta_sq * (1.0 - theta_sq/20) / 6, - 0.5 - 0.25/6 * theta_sq -]; - -function so3_exp_3_0(theta_deg, inv_theta) = [ - sin(theta_deg) * inv_theta, - (1 - cos(theta_deg)) * (inv_theta * inv_theta) -]; - -function so3_exp_3(theta_sq) = so3_exp_3_0(sqrt(theta_sq)*180/PI, 1/sqrt(theta_sq)); - - -function rot_axis_part(m) = [m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1]]*0.5; - -function so3_ln(m) = 180/PI*so3_ln_rad(m); -function so3_ln_rad(m) = so3_ln_0(m, - cos_angle = rot_cos_angle(m), - preliminary_result = rot_axis_part(m)); - -function so3_ln_0(m, cos_angle, preliminary_result) = -so3_ln_1(m, cos_angle, preliminary_result, - sin_angle_abs = sqrt(preliminary_result*preliminary_result)); - -function so3_ln_1(m, cos_angle, preliminary_result, sin_angle_abs) = - cos_angle > sqrt(1/2) - ? sin_angle_abs > 0 - ? preliminary_result * asin(sin_angle_abs)*PI/180 / sin_angle_abs - : preliminary_result - : cos_angle > -sqrt(1/2) - ? preliminary_result * acos(cos_angle)*PI/180 / sin_angle_abs - : so3_get_symmetric_part_rotation( - preliminary_result, - m, - angle = PI - asin(sin_angle_abs)*PI/180, - d0 = m[0][0] - cos_angle, - d1 = m[1][1] - cos_angle, - d2 = m[2][2] - cos_angle - ); + combine_so3_exp( + w, + (w * w < 1e-8) ? so3_exp_1(w * w) + : (w * w < 1e-6) ? so3_exp_2(w * w) + : so3_exp_3(w * w) + ); + +function combine_so3_exp(w, AB) = + rodrigues_so3_exp(w, AB[0], AB[1]); + +// --- Taylor Expansions (near 0) -------------------------------------------- + +function so3_exp_1(theta_sq) = [1 - theta_sq / 6, 0.5]; + +function so3_exp_2(theta_sq) = + [ + 1.0 - theta_sq * (1.0 - theta_sq / 20) / 6, + 0.5 - 0.25 / 6 * theta_sq, + ]; + +function so3_exp_3_0(theta_deg, inv_theta) = + [ + sin(theta_deg) * inv_theta, + (1 - cos(theta_deg)) * (inv_theta * inv_theta), + ]; + +function so3_exp_3(theta_sq) = + so3_exp_3_0(sqrt(theta_sq) * 180 / PI, 1 / sqrt(theta_sq)); + +// --- Logarithm Map ---------------------------------------------------------- + +// Axis part from skew-symmetric difference +function rot_axis_part(m) = + [m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1]] * 0.5; + +// Logarithm: returns axis-angle in degrees +function so3_ln(m) = 180 / PI * so3_ln_rad(m); + +// Logarithm in radians +function so3_ln_rad(m) = + so3_ln_0( + m, + cos_angle=rot_cos_angle(m), + preliminary_result=rot_axis_part(m) + ); + +function so3_ln_0(m, cos_angle, preliminary_result) = + so3_ln_1( + m, cos_angle, preliminary_result, + sin_angle_abs=sqrt(preliminary_result * preliminary_result) + ); + +function so3_ln_1(m, cos_angle, preliminary_result, sin_angle_abs) = + (cos_angle > sqrt(0.5)) ? + ( + sin_angle_abs > 0 ? + preliminary_result * asin(sin_angle_abs) * PI / 180 / sin_angle_abs + : preliminary_result + ) + : (cos_angle > -sqrt(0.5)) ? + preliminary_result * acos(cos_angle) * PI / 180 / sin_angle_abs + : so3_get_symmetric_part_rotation( + preliminary_result, m, + angle=PI - asin(sin_angle_abs) * PI / 180, + d0=m[0][0] - cos_angle, + d1=m[1][1] - cos_angle, + d2=m[2][2] - cos_angle + ); + +// --- Symmetric Part Handling (for near-π cases) ----------------------------- function so3_get_symmetric_part_rotation(preliminary_result, m, angle, d0, d1, d2) = -so3_get_symmetric_part_rotation_0(preliminary_result,angle,so3_largest_column(m, d0, d1, d2)); + so3_get_symmetric_part_rotation_0( + preliminary_result, + angle, + so3_largest_column(m, d0, d1, d2) + ); function so3_get_symmetric_part_rotation_0(preliminary_result, angle, c_max) = - angle * unit(c_max * preliminary_result < 0 ? -c_max : c_max); + angle * unit((c_max * preliminary_result < 0) ? -c_max : c_max); function so3_largest_column(m, d0, d1, d2) = - d0*d0 > d1*d1 && d0*d0 > d2*d2 - ? [d0, (m[1][0]+m[0][1])/2, (m[0][2]+m[2][0])/2] - : d1*d1 > d2*d2 - ? [(m[1][0]+m[0][1])/2, d1, (m[2][1]+m[1][2])/2] - : [(m[0][2]+m[2][0])/2, (m[2][1]+m[1][2])/2, d2]; - -__so3_test = [12,-125,110]; -echo(UNITTEST_so3=norm(__so3_test-so3_ln(so3_exp(__so3_test))) < 1e-8); + (d0 * d0 > d1 * d1 && d0 * d0 > d2 * d2) ? + [d0, (m[1][0] + m[0][1]) / 2, (m[0][2] + m[2][0]) / 2] + : (d1 * d1 > d2 * d2) ? + [(m[1][0] + m[0][1]) / 2, d1, (m[2][1] + m[1][2]) / 2] + : [(m[0][2] + m[2][0]) / 2, (m[2][1] + m[1][2]) / 2, d2]; diff --git a/spline.scad b/spline.scad index dbf487b..71247d7 100644 --- a/spline.scad +++ b/spline.scad @@ -1,113 +1,143 @@ -// Spline module for scad-util library -// Author Sergei Kuzmin, 2014. - -// For n+1 given point and hense n intervals returns the spline coefficient matrix. -// param p defines the anchor points. -// File defines two functions: spline_args and spline. -// example usage: -// spl1 = spline_args(point, v1=[0,1,0], closed=false); -// interpolated_points = [for(t=[0:0.1:len(point)-1]) spline(spl1, t)] +// ============================================================================ +// Spline Utilities +// ---------------------------------------------------------------------------- +// Provides cubic spline and Bezier curve utilities for OpenSCAD. +// +// Functions: +// - spline_args(p, closed=false, v1=undef, v2=undef) +// → compute spline coefficient matrices for given control points +// - spline(s, t) +// → evaluate spline at parameter t +// - spline_tan(), spline_tan_unit() +// → evaluate tangent vector +// - spline_d2(), spline_binormal_unit(), spline_normal_unit() +// → evaluate higher derivatives and Frenet frame +// - spline_transform() +// → construct SE3 transform aligned to spline +// - bezier3_args(p, symmetric=false) +// → construct cubic Bezier coefficient matrices +// +// Internal helpers implement determinant, matrix inversion, etc. +// +// Author: Sergei Kuzmin, 2014 +// License: BSD +// ============================================================================ use -use - -q1=[[1,0,0,0],[1,1,1,1],[0,1,2,3],[0,0,1,3]]; -q1inv=[[1,0,0,0],[-3,3,-2,1],[3,-3,3,-2],[-1,1,-1,1]]; -q2=[[0,0,0,0],[0,0,0,0],[0,-1,0,0],[0,0,-1,0]]; -qn1i2=-q1inv*q2; -z3=[0,0,0]; -z4=[0,0,0,0]; - -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); - -function det(m) = let(r=[for(i=[0:1: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=n? u : u-q2*q1inv*spline_helper(i+1, n, p); +function spline_helper(i, n, p) = + let (u = [p[i], p[i + 1], z3, z3]) i + 3 >= n ? u + : u - q2 * q1inv * spline_helper(i + 1, n, p); +// Recursive calculation of segment coefficients // knowing s[j+1], calculate s[j]. Stop when found s[i] -function spline_si(i,n, p, sn) = i == n ? sn : q1inv*(spline_u(i,p)-q2*spline_si(i+1, n, p, sn)); +function spline_si(i, n, p, sn) = + i == n ? sn + : q1inv * (spline_u(i, p) - q2 * spline_si(i + 1, n, p, sn)); + +// --- Bezier Utilities ------------------------------------------------------- // Takes array of (3n+1) points or (2n + 2) points, if tangent segments are symmetric. -// For non-symmetric version input is: point0, normal0, neg_normal1, point1, normal1, ... neg_normal_n, point_n -// For symmetric version: point0, normal0, point1, normal1, ... , normal_n_sub_1, point_n +// For non-symmetric version input is: +// point0, normal0, neg_normal1, point1, normal1, ... neg_normal_n, point_n +// For symmetric version: +// point0, normal0, point1, normal1, ... , normal_n_sub_1, point_n // In the second case second tangent is constructed from the next tangent by symmetric map. -// I.e. if current points are p0,p1,p2 then anchor points are p0 and p2, first tangent defined by p1-p0, -// second tangent defined by p3-p2. -// Return array of coefficients accepted by spline(), spline_tan() and similar -function bezier3_args(p, symmetric=false) = let(step=symmetric?2:3) - [for(i=[0:step:len(p)-3]) [[1,0,0,0],[-3,3,0,0],[3,-6,3,0],[-1,3,-3,1]]* - (symmetric?[p[i],p[i]+p[i+1],p[i+2]-p[i+3],p[i+2]] : [p[i], p[i]+p[i+1], p[i+3]+p[i+2], p[i+3]])]; - +// I.e. if current points are p0,p1,p2 then anchor points are p0 and p2, first tangent +// defined by p1-p0, second tangent defined by p3-p2. +// Return array of coefficients accepted by spline(), spline_tan() and similar +function bezier3_args(p, symmetric = false) = + let (step = symmetric ? 2 : 3) [ + for (i = [0:step:len(p) - 3]) [[1, 0, 0, 0], [-3, 3, 0, 0], [3, -6, 3, 0], [-1, 3, -3, 1]] * ( + symmetric ? [p[i], p[i] + p[i + 1], p[i + 2] - p[i + 3], p[i + 2]] + : [p[i], p[i] + p[i + 1], p[i + 3] + p[i + 2], p[i + 3]] + ), + ]; + +// --- Evaluation ------------------------------------------------------------- +// Evaluate spline/Bezier and derivatives // s - spline arguments calculated by spline_args -// t - defines point on curve. each segment length is 1. I.e. t= 0..1 is first segment, t=1..2 - second. -function spline(s, t)= let(i=t>=len(s)?len(s)-1: floor(t), t2=t-i) [1,t2,t2*t2,t2*t2*t2]*s[i]; - -function spline_tan(s, t)= let(i=t>=len(s)?len(s)-1: floor(t), t2=t-i) [0,1,2*t2,3*t2*t2]*s[i]; -function spline_tan_unit(s, t)= unit(spline_tan(s,t)); -function spline_d2(s,t)= let(i=t>=len(s)?len(s)-1: floor(t), t2=t-i) [0,0,2,6*t2]*s[i]; -function spline_binormal_unit(s,t)= unit(cross(spline_tan(s, t), spline_d2(s,t))); -function spline_normal_unit(s,t)= unit(cross(spline_tan(s, t), spline_binormal_unit(s,t))); - -function spline_transform(s, t)= - construct_Rt(transpose_3([spline_normal_unit(s,t), spline_binormal_unit(s,t), spline_tan_unit(s,t)]), spline(s,t)); - -// Unit tests -__s = spline_args([[0,10,0], [10,0,0],[0,-5,2]], v1=[0,1,0], v2=[-1,0,0], closed=true); -for(t=[0:0.01:len(__s)]) translate(spline(__s, t)) - cube([0.2,0.2,0.2], center=true); - -__s1=spline_args([[0,0,0],[0,0,15], [26,0,26+15]], /*v1=[0,0,100],*/ v2=[40,0,0]); -for(t=[0:0.01:len(s1)]) translate(spline(__s1, t)) - cube([0.2,0.2,0.2], center=true); - -__s2=bezier3_args([[0,0,0],[0,0,10],[0,0,15],[0,0,26*0.552284],[26,0,41],[26*0.552284,0,0]],symmetric=true); -echo(__s2); -for(t=[0:0.01:len(__s2)]) translate(spline(__s2, t)) - cube([0.2,0.2,0.2], center=true); - -// Rotation methods taken from list-comprehension-demos/sweep.scad to demonstrate normal and binormal -// Normally spline_transform is more convenient -function __rotation_from_axis(x,y,z) = [[x[0],y[0],z[0]],[x[1],y[1],z[1]],[x[2],y[2],z[2]]]; -function __rotate_from_to(a,b,_axis=[]) = - len(_axis) == 0 - ? __rotate_from_to(a,b,unit(cross(a,b))) - : _axis*_axis >= 0.99 ? __rotation_from_axis(unit(b),_axis,cross(_axis,unit(b))) * - transpose_3(__rotation_from_axis(unit(a),_axis,cross(_axis,unit(a)))) : identity3(); - -__s3 = spline_args([[0,10,0], [6,6,0], [10,0,0],[0,-5,4]], v1=[0,1,0], v2=[-1,0,0], closed=true); -for(t=[0:0.05:len(__s3)]) translate(spline(__s3, t)) { - translate([0,0,3]) multmatrix(m=__rotate_from_to([0,0,1],spline_normal_unit(__s3,t))) - cylinder(r1=0.1, r2=0, h=1, $fn=3); - translate([0,0,6]) multmatrix(m=__rotate_from_to([0,0,1],spline_binormal_unit(__s3,t))) - cylinder(r1=0.1, r2=0, h=1, $fn=3); -} - -translate([0,0,9]) for(t=[0:0.025:len(__s3)]) - multmatrix(spline_transform(__s3,t)) cube([1,1,0.1],center=true); - - - +// t - defines point on curve. Each segment length is 1. +// i.e. t= 0..1 is first segment, t=1..2 - second. +function spline(s, t) = + let ( + i = t >= len(s) ? len(s) - 1 : floor(t), + t2 = t - i + ) [1, t2, t2 * t2, t2 * t2 * t2] * s[i]; + +function spline_tan(s, t) = + let ( + i = t >= len(s) ? len(s) - 1 : floor(t), + t2 = t - i + ) [0, 1, 2 * t2, 3 * t2 * t2] * s[i]; + +function spline_tan_unit(s, t) = unit(spline_tan(s, t)); + +function spline_d2(s, t) = + let ( + i = t >= len(s) ? len(s) - 1 : floor(t), + t2 = t - i + ) [0, 0, 2, 6 * t2] * s[i]; + +function spline_binormal_unit(s, t) = unit(cross(spline_tan(s, t), spline_d2(s, t))); +function spline_normal_unit(s, t) = unit(cross(spline_tan(s, t), spline_binormal_unit(s, t))); + +function spline_transform(s, t) = + construct_Rt( + transpose_3( + [ + spline_normal_unit(s, t), + spline_binormal_unit(s, t), + spline_tan_unit(s, t), + ] + ), + spline(s, t) + ); diff --git a/tests/test_hull.scad b/tests/test_hull.scad new file mode 100644 index 0000000..e24bd7a --- /dev/null +++ b/tests/test_hull.scad @@ -0,0 +1,78 @@ +include <../hull.scad> + +// --- Test Data -------------------------------------------------------------- +phi = 1.618033988749895; + +testpoints_on_sphere = [ + for ( + p = [ + [1, phi, 0], + [-1, phi, 0], + [1, -phi, 0], + [-1, -phi, 0], + [0, 1, phi], + [0, -1, phi], + [0, 1, -phi], + [0, -1, -phi], + [phi, 0, 1], + [-phi, 0, 1], + [phi, 0, -1], + [-phi, 0, -1], + ] + ) unit(p), +]; +testpoints_spherical = [for (p = testpoints_on_sphere) spherical(p)]; +testpoints_circular = [for (a = [0:15:360 - epsilon]) [cos(a), sin(a)]]; +testpoints_coplanar = let (u = unit([1, 3, 7]), v = unit([-2, 1, -2])) [for (i = [1:10]) rands(-1, 1, 1)[0] * u + rands(-1, 1, 1)[0] * v]; +testpoints_collinear_2d = let (u = unit([5, 3])) [for (i = [1:20]) rands(-1, 1, 1)[0] * u]; +testpoints_collinear_3d = let (u = unit([5, 3, -5])) [for (i = [1:20]) rands(-1, 1, 1)[0] * u]; +testpoints2d = 20 * [for (i = [1:10]) concat(rands(-1, 1, 2))]; +testpoints3d = 20 * [for (i = [1:50]) concat(rands(-1, 1, 3))]; + +// --- Visualization ---------------------------------------------------------- +echo("Test points 3d"); +visualize_hull(testpoints3d); +echo("Test points on sphere"); +translate([-50, 0]) visualize_hull(20 * testpoints_on_sphere); +echo("Test points on 2d"); +translate([50, 0]) visualize_hull(testpoints2d); +echo("Test points on circular"); +translate([0, 50]) visualize_hull(20 * testpoints_circular); +echo("Test points coplanar"); +translate([0, -50]) visualize_hull(20 * testpoints_coplanar); +echo("Test points collinear 2d"); +translate([50, 50]) visualize_hull(20 * testpoints_collinear_2d); +echo("Test points collinear 3d"); +translate([-50, 50]) visualize_hull(20 * testpoints_collinear_3d); + +// --- Visualization Helpers -------------------------------------------------- +module visualize_hull(points) { + let (faces = hull(points)) { + echo("Faces: ", faces); + if (len(faces) == 0) { + // nothing + } else if (is_list(faces[0])) { + %polyhedron(points=points, faces=faces); // 3D hull + } else if (len(faces) >= 3) { + %polyhedron(points=points, faces=[faces]); // 2D hull + } else if (len(faces) == 2) { + // collinear case: show a rod between endpoints + p0 = points[faces[0]]; + p1 = points[faces[1]]; + hull() { + translate(p0) sphere(r=0.8, $fn=24); + translate(p1) sphere(r=0.8, $fn=24); + } + } + + // Draw points: blue if on hull, red otherwise + for (i = [0:len(points) - 1]) { + translate(points[i]) + color(hull_contains_index(faces, i) ? "blue" : "red") + sphere(r=1, $fn=16); + } + } +} + +function hull_contains_index(faces, idx) = + search(idx, faces, 1, 0) || search(idx, faces, 1, 1) || search(idx, faces, 1, 2); diff --git a/tests/test_linalg.scad b/tests/test_linalg.scad new file mode 100644 index 0000000..d42e463 --- /dev/null +++ b/tests/test_linalg.scad @@ -0,0 +1,65 @@ +include <../linalg.scad>; + +// --- Vector constructors ---------------------------------------------------- +echo("vec3([1,2]) =", vec3([1, 2])); // expect [1,2,0] +echo("vec4([1,2,3]) =", vec4([1, 2, 3])); // expect [1,2,3,1] +echo("unit([3,0,0]) =", unit([3, 0, 0])); // expect [1,0,0] + +// --- Identity matrices ------------------------------------------------------ +echo("identity3() =", identity3()); // expect 3x3 identity +echo("identity4() =", identity4()); // expect 4x4 identity + +// --- Vector access ---------------------------------------------------------- +echo("take3([9,8,7,6]) =", take3([9, 8, 7, 6])); // expect [9,8,7] +echo("tail3([0,1,2,3,4,5]) =", tail3([0, 1, 2, 3, 4, 5])); // expect [3,4,5] + +// --- Matrix parts ----------------------------------------------------------- +M = [ + [1, 0, 0, 5], + [0, 1, 0, 6], + [0, 0, 1, 7], + [0, 0, 0, 1], +]; +echo("rotation_part(M) =", rotation_part(M)); // expect identity3() +echo("translation_part(M) =", translation_part(M)); // expect [5,6,7] + +// --- Rotation metrics ------------------------------------------------------- +R = identity3(); +echo("rot_trace(R) =", rot_trace(R)); // expect 3 +echo("rot_cos_angle(R) =", rot_cos_angle(R)); // expect 1 + +// --- Transpose -------------------------------------------------------------- +A3 = [[1, 2, 3], [4, 5, 6], [7, 8, 9]]; +echo("transpose_3(A3) =", transpose_3(A3)); // expect [[1,4,7],[2,5,8],[3,6,9]] + +A4 = identity4(); +echo("transpose_4(identity4) =", transpose_4(A4)); // expect identity4() + +// --- Rigid transform inverse/construct ------------------------------------- +echo("invert_rt(M) =", invert_rt(M)); +// expect transform with R=I, t=[-5,-6,-7] + +R2 = [[0, -1, 0], [1, 0, 0], [0, 0, 1]]; // 90° rot about Z +t2 = [10, 0, 0]; +Rt = construct_Rt(R2, t2); +echo("construct_Rt(R2,t2) =", Rt); + +// --- Hadamard product ------------------------------------------------------ +echo("hadamard([1,2,3],[4,5,6]) =", hadamard([1, 2, 3], [4, 5, 6])); +// expect [4,10,18] + +echo( + "hadamard([[1,2],[3,4]], [[5,6],[7,8]]) =", + hadamard([[1, 2], [3, 4]], [[5, 6], [7, 8]]) +); +// expect [[5,12],[21,32]] + +// --- Matrix utilities ------------------------------------------------------- +B = [[2, 0], [0, 3]]; +echo("matrix_power(B,0) =", matrix_power(B, 0)); // expect identity2 (approximated as [[1,0],[0,1]]) +echo("matrix_power(B,2) =", matrix_power(B, 2)); // expect [[4,0],[0,9]] + +C = [[1, 2], [3, 4]]; +echo("det(C) =", det(C)); // expect -2 +echo("matrix_invert(C) =", matrix_invert(C)); +// expect [[-2,1],[1.5,-0.5]] diff --git a/tests/test_lists.scad b/tests/test_lists.scad new file mode 100644 index 0000000..977ffdf --- /dev/null +++ b/tests/test_lists.scad @@ -0,0 +1,34 @@ +include <../lists.scad>; + +// --- Flatten --------------------------------------------------------------- +echo("flatten([[0,1],[2,3]]) =", flatten([[0, 1], [2, 3]])); +// expect [0,1,2,3] + +// --- Range ----------------------------------------------------------------- +echo("range([0:2:6]) =", range([0:2:6])); +// expect [0,2,4,6] + +// --- Reverse --------------------------------------------------------------- +echo("reverse([1,2,3]) =", reverse([1, 2, 3])); +// expect [3,2,1] + +// --- Subarray -------------------------------------------------------------- +echo("subarray([1,2,3,4],1,3) =", subarray([1, 2, 3, 4], 1, 3)); +// expect [2,3] + +echo("subarray([1,2,3,4],0,-1) =", subarray([1, 2, 3, 4], 0, -1)); +// expect [1,2,3,4] + +// --- Set ------------------------------------------------------------------- +echo("set([1,2,3,4],2,5) =", set([1, 2, 3, 4], 2, 5)); +// expect [1,2,5,4] + +// --- Remove --------------------------------------------------------------- +echo("remove([4,3,2,1],1) =", remove([4, 3, 2, 1], 1)); +// expect [4,2,1] + +echo("remove([10,20,30,40,50],0) =", remove([10, 20, 30, 40, 50], 0)); +// expect [20,30,40,50] + +echo("remove([10,20,30,40,50],4) =", remove([10, 20, 30, 40, 50], 4)); +// expect [10,20,30,40] diff --git a/tests/test_mirror.scad b/tests/test_mirror.scad new file mode 100644 index 0000000..4993c26 --- /dev/null +++ b/tests/test_mirror.scad @@ -0,0 +1,82 @@ +include <../mirror.scad> + +// --------------------------------------------------------------------------- +// Sample reference shape +// --------------------------------------------------------------------------- +module sample_shape() { + translate([20, 10, 5]) { + cube([10, 6, 6], center=false); + translate([10, 3, 6]) sphere(r=4, $fn=24); + } +} + +// --------------------------------------------------------------------------- +// Axes helper for orientation +// --------------------------------------------------------------------------- +module axes(len = 25, thick = 0.8) { + // +X axis (red) + color("red") + translate([len / 2, 0, 0]) + cube([len, thick, thick], center=true); + + // +Y axis (green) + color("green") + translate([0, len / 2, 0]) + cube([thick, len, thick], center=true); + + // +Z axis (blue) + color("blue") + translate([0, 0, len / 2]) + cube([thick, thick, len], center=true); +} + +// --------------------------------------------------------------------------- +// Demonstrations +// --------------------------------------------------------------------------- + +// Mirror across X-axis +translate([-60, 0, 0]) { + axes(); + mirror_x("teal") sample_shape(); +} + +// Mirror across Y-axis +translate([0, 0, 0]) { + axes(); + mirror_y("indigo") sample_shape(); +} + +// Mirror across Z-axis +translate([60, 0, 0]) { + axes(); + mirror_z("salmon") sample_shape(); +} + +// Compare with a plain built-in mirror +translate([-60, -40, 0]) { + axes(); + union() { + sample_shape(); + mirror([1, 0, 0]) color("MediumAquamarine") sample_shape(); + } +} + +// 2D arrow example using mirror_y +module arrow(l = 1, w = 0.6, t = 0.15) { + mirror_y("orange") + polygon( + [ + [0, 0], + [l, 0], + [l - w / 2, w / 2], + [l - w / 2 - sqrt(2) * t, w / 2], + [l - t / 2 - sqrt(2) * t, t / 2], + [0, t / 2], + ] + ); +} + +translate([60, -40, 0]) { + axes(); + arrow(l=20, w=10, t=2); +} diff --git a/tests/test_morphology.scad b/tests/test_morphology.scad new file mode 100644 index 0000000..299eb53 --- /dev/null +++ b/tests/test_morphology.scad @@ -0,0 +1,47 @@ +use <../morphology.scad> +use // for arrow() + +// --------------------------------------------------------------------------- +// Base test shape +// --------------------------------------------------------------------------- +module shape() { + polygon( + [ + [0, 0], + [1, 0], + [1.5, 1], + [2.5, 1], + [2, -1], + [0, -1], + ] + ); +} + +debug = true; + +if (debug) { + $fn = 32; + + // 10 groups of 3 columns: [original, arrow, transformed] + for (p = [0:10 * 3 - 1]) { + o = floor(p / 3); // row index (operation group) + + translate([(p % 3) * 2.5, -o * 3]) { + if (p % 3 == 0) shape(); // original + if (p % 3 == 1) translate([0.6, 0]) color("grey") arrow(); // arrow + if (p % 3 == 2) { + // transformed + if (o == 0) inset(d=0.3) shape(); + if (o == 1) outset(d=0.3) shape(); + if (o == 2) rounding(r=0.3) shape(); + if (o == 3) fillet(r=0.3) shape(); + if (o == 4) rounding(r=0.3) fillet(r=0.3) shape(); + if (o == 5) shell(d=0.3) shape(); + if (o == 6) shell(d=-0.3) shape(); + if (o == 7) shell(d=0.3, center=true) shape(); + if (o == 8) shell(d=0.3, center=true) fillet(r=0.3) rounding(r=0.3) shape(); + if (o == 9) shell(d=-0.3) fillet(r=0.3) rounding(r=0.3) shape(); + } + } + } +} diff --git a/tests/test_se3.scad b/tests/test_se3.scad new file mode 100644 index 0000000..7c380bf --- /dev/null +++ b/tests/test_se3.scad @@ -0,0 +1,12 @@ +use <../se3.scad> + +// --- Unit Test for SE(3) --------------------------------------------------- +// Verify se3_ln(se3_exp(mu)) ≈ mu + +mu_test = [20, -40, 60, -80, 100, -120]; +result = se3_ln(se3_exp(mu_test)); + +echo("mu_test =", mu_test); +echo("se3_ln(se3_exp(mu_test)) =", result); +echo("Error norm =", norm(mu_test - result)); +echo("PASS =", norm(mu_test - result) < 1e-8); diff --git a/tests/test_shapes.scad b/tests/test_shapes.scad new file mode 100644 index 0000000..987b941 --- /dev/null +++ b/tests/test_shapes.scad @@ -0,0 +1,14 @@ +include <../shapes.scad> + +$fn = 16; + +echo("square(2) =", square(2)); +echo("circle(r=5) =", circle(5)); +echo("regular(r=5, n=6) =", regular(5, 6)); +echo("rectangle_profile([4,2]) =", rectangle_profile([4, 2])); + +// Quick visualization +translate([-15, 0, 0]) polygon(square(10)); +translate([0, 0, 0]) polygon(circle(5)); +translate([15, 0, 0]) polygon(regular(5, 6)); +translate([30, 0, 0]) polygon(rectangle_profile([8, 4])); diff --git a/tests/test_so3.scad b/tests/test_so3.scad new file mode 100644 index 0000000..5ffc804 --- /dev/null +++ b/tests/test_so3.scad @@ -0,0 +1,12 @@ +use <../so3.scad> + +// --- Unit Test for SO(3) --------------------------------------------------- +// Verify so3_ln(so3_exp(w)) ≈ w + +w_test = [12, -125, 110]; +result = so3_ln(so3_exp(w_test)); + +echo("w_test =", w_test); +echo("so3_ln(so3_exp(w_test)) =", result); +echo("Error norm =", norm(w_test - result)); +echo("PASS =", norm(w_test - result) < 1e-8); diff --git a/tests/test_spline.scad b/tests/test_spline.scad new file mode 100644 index 0000000..211d660 --- /dev/null +++ b/tests/test_spline.scad @@ -0,0 +1,91 @@ +include <../spline.scad> +include <../se3.scad> + +$fn = 24; + +// ============================================================================ +// Test Suite for spline.scad +// ---------------------------------------------------------------------------- +// Covers: +// - spline_args with open/closed curves +// - tangent / binormal / normal evaluation +// - Bezier curve generation +// - Frenet frame transform visualization +// - Regression consistency checks +// ============================================================================ + +// --- Basic Spline Example (closed curve) ----------------------------------- +p1 = [[0, 10, 0], [10, 0, 0], [0, -5, 2]]; +s1 = spline_args(p1, v1=[0, 1, 0], v2=[-1, 0, 0], closed=true); + +// Visualize spline points +for (t = [0:0.01:len(s1)]) + translate(spline(s1, t)) + color("red") sphere(r=0.1); + +// Tangent demo +for (t = [0:0.5:len(s1)]) + let (pt = spline(s1, t)) + color("blue") + translate(pt) cylinder(r=0.1, h=2, center=true, $fn=12); + +// --- Open Spline Example --------------------------------------------------- +p2 = [[0, 0, 0], [0, 0, 15], [26, 0, 41]]; +s2 = spline_args(p2, v2=[40, 0, 0]); + +for (t = [0:0.01:len(s2)]) + translate(spline(s2, t)) + color("indigo") sphere(r=0.1); + +// --- Bezier Curve Example -------------------------------------------------- +p3 = [ + [0, 0, 0], + [0, 0, 10], + [0, 0, 15], + [0, 0, 26 * 0.552], + [26, 0, 41], + [26 * 0.552, 0, 0], +]; +s3 = bezier3_args(p3, symmetric=true); + +echo("Bezier coefficients =", s3); + +for (t = [0:0.01:len(s3)]) + translate(spline(s3, t)) + color("green") sphere(r=0.1); + +// --- Frenet Frame Demo ----------------------------------------------------- +// Rotation methods (ported from list-comprehension-demos/sweep.scad) +function __rotation_from_axis(x, y, z) = + [[x[0], y[0], z[0]], [x[1], y[1], z[1]], [x[2], y[2], z[2]]]; + +function __rotate_from_to(a, b, _axis = []) = + len(_axis) == 0 ? __rotate_from_to(a, b, unit(cross(a, b))) + : _axis * _axis >= 0.99 ? __rotation_from_axis(unit(b), _axis, cross(_axis, unit(b))) * transpose_3(__rotation_from_axis(unit(a), _axis, cross(_axis, unit(a)))) + : identity3(); + +p4 = [[0, 10, 0], [6, 6, 0], [10, 0, 0], [0, -5, 4]]; +s4 = spline_args(p4, v1=[0, 1, 0], v2=[-1, 0, 0], closed=true); + +// Normal/binormal markers +for (t = [0:0.05:len(s4)]) + translate(spline(s4, t)) { + translate([0, 0, 3]) + multmatrix(m=__rotate_from_to([0, 0, 1], spline_normal_unit(s4, t))) + color("teal") + cylinder(r1=0.1, r2=0, h=1, $fn=3); + translate([0, 0, 6]) + multmatrix(m=__rotate_from_to([0, 0, 1], spline_binormal_unit(s4, t))) + color("brown") + cylinder(r1=0.1, r2=0, h=1, $fn=3); + } + +// Spline transform demo (Frenet-aligned cubes) +translate([0, 0, 9])for (t = [0:0.025:len(s4)]) + multmatrix(spline_transform(s4, t)) + color("orange") + cube([1, 1, 0.1], center=true); + +// --- Consistency Check ----------------------------------------------------- +__test = [20, -40, 60, -80, 100, -120]; +echo("SE3 consistency =", norm(__test - se3_ln(se3_exp(__test))) < 1e-8); diff --git a/tests/test_trajectory.scad b/tests/test_trajectory.scad new file mode 100644 index 0000000..172638e --- /dev/null +++ b/tests/test_trajectory.scad @@ -0,0 +1,36 @@ +include <../trajectory.scad> +include <../so3.scad> + +$fn = 24; + +// --- Regression / Echo Tests ------------------------------------------------ +echo("Forward 10 =", trajectory(forward=10)); // expect [0,0,10, 0,0,0] +echo("Up 5, Left 3 =", trajectory(up=5, left=3)); // expect [-3,5,0, 0,0,0] +echo("Yaw 45 =", rotationv(yaw=45)); // expect [45,0,0] +echo("Pitch 90 matrix =", rotationm(pitch=90)); + +// Combined twist +twist1 = trajectory(forward=20, yaw=45); +echo("Combined twist =", twist1); + +// --- Visual Demonstrations -------------------------------------------------- + +// Helper: apply trajectory to a cube +module demo_traj(traj, col = "lightblue") { + T = concat(take3(traj), [0]); // translation + R = rotationm(rotation=tail3(traj)); + multmatrix( + [ + [R[0][0], R[0][1], R[0][2], T[0]], + [R[1][0], R[1][1], R[1][2], T[1]], + [R[2][0], R[2][1], R[2][2], T[2]], + [0, 0, 0, 1], + ] + ) + color(col) cube([5, 5, 5], center=true); +} + +// Show original then two trajectory examples side by side +translate([0, 0, 0]) demo_traj(trajectory(up=0), "lightblue"); +translate([-20, 0, 0]) demo_traj(trajectory(right=10, up=5, yaw=30), "red"); +translate([20, 0, 0]) demo_traj(trajectory(translation=[5, 5, 5], rotation=[0, 90, 0]), "green"); diff --git a/tests/test_trajectory_path.scad b/tests/test_trajectory_path.scad new file mode 100644 index 0000000..6d78caa --- /dev/null +++ b/tests/test_trajectory_path.scad @@ -0,0 +1,56 @@ +include <../trajectory_path.scad> +include <../linalg.scad> +include <../se3.scad> + +$fn = 24; + +// Simple frame visualizer ---------------------------------------------------- +module frame(T, s = 2) { + multmatrix(T) { + color("red") cube([s, .2, .2], center=false); + color("green") cube([.2, s, .2], center=false); + color("blue") cube([.2, .2, s], center=false); + color("gray") translate([0, 0, 0]) sphere(r=.4); + } +} + +// Example trajectories (6D twists) ------------------------------------------ +// Move +X 60, slight +Z arc; then yaw+translate; then small upward arc. +traj = [ + [60, 0, 0, 0, 0, 30], // translate 60 along X while yawing 30° + [20, 0, 0, 0, 0, -45], // short turn back + [0, 0, 30, 0, 90, 0], // arc up in pitch + [0, 0, 20, 0, 0, 0], // short straight up (relative forward to base frame) +]; + +// --- Sampling with a physical step (units of translation norm) ------------- +poses_step = quantize_trajectories(traj, step=5, start_position=0, loop=false); + +// --- Sampling with a fixed number of steps across whole path ---------------- +poses_steps = quantize_trajectories(traj, steps=30, start_position=0, loop=false); + +// --- Looping path (returns to start) --------------------------------------- +poses_loop = quantize_trajectories(traj, step=5, loop=true); + +// Render -------------------------------------------------------------------- +translate([0, -70, 0]) { + // By fixed step length + for (T = poses_step) frame(T, s=3); + echo("poses_step count =", len(poses_step)); +} + +translate([0, 0, 0]) { + // By fixed number of steps + for (T = poses_steps) frame(T, s=3); + echo("poses_steps count =", len(poses_steps)); +} + +translate([0, 70, 0]) { + // Looping variant + for (T = poses_loop) frame(T, s=3); + echo("poses_loop count =", len(poses_loop)); +} + +// Show overall end pose to verify loop closure visually +echo("End pose (open) =", trajectories_end_position(traj)); +echo("End pose (loop) =", trajectories_end_position(close_trajectory_loop(traj))); diff --git a/tests/test_transformations.scad b/tests/test_transformations.scad new file mode 100644 index 0000000..84748b7 --- /dev/null +++ b/tests/test_transformations.scad @@ -0,0 +1,40 @@ +include <../transformations.scad> + +$fn = 24; + +// ============================================================================ +// Test Suite for transformations.scad +// ---------------------------------------------------------------------------- +// Covers: +// - rotation (Euler and axis-angle) +// - scaling +// - translation +// - project / transform / to_3d +// ============================================================================ + +// --- Sample Shape ----------------------------------------------------------- +points = [[0, 0, 0], [10, 0, 0], [10, 10, 0], [0, 10, 0]]; + +// --- Rotation Tests --------------------------------------------------------- +Rz45 = rotation(axis=[0, 0, 45]); +echo("Rotation Z45 applied =", transform(Rz45, points)); + +Rx90 = rotation(xyz=[90, 0, 0]); +echo("Rotation X90 applied =", transform(Rx90, [[0, 0, 1]])); + +// --- Scaling Tests ---------------------------------------------------------- +S = scaling([2, 1, 1]); +echo("Scaling [2,1,1] =", transform(S, points)); + +// --- Translation Tests ------------------------------------------------------ +T = translation([5, 5, 0]); +echo("Translation [5,5,0] =", transform(T, points)); + +// --- Combined Transform ----------------------------------------------------- +M = T * Rz45 * S; +echo("Combined TRS =", transform(M, points)); + +// --- Project / to_3d Tests -------------------------------------------------- +p_h = [3, 4, 5, 1]; +echo("Project [3,4,5,1] =", project(p_h)); +echo("to_3d([ [1,2], [3,4,5] ]) =", to_3d([[1, 2], [3, 4, 5]])); diff --git a/trajectory.scad b/trajectory.scad index a7bcf81..37616c8 100644 --- a/trajectory.scad +++ b/trajectory.scad @@ -1,43 +1,109 @@ +// ============================================================================ +// Trajectory Utilities +// ---------------------------------------------------------------------------- +// Purpose : Provide a clean way to construct SE(3) “twist” vectors +// (translation + rotation) from intuitive parameters. +// Features: +// - Flexible specification of translation (left/right, up/down, fwd/back) +// - Flexible specification of rotation (pitch, yaw, roll) or direct vector +// - Helpers for dealing with undef inputs and selection logic +// - trajectory(...) returns [tx,ty,tz, yaw,pitch,roll] 6D vector +// - rotationm(...) builds a 3x3 rotation matrix from angles +// ---------------------------------------------------------------------------- +// Dependencies: so3.scad +// ============================================================================ + use -function val(a=undef,default=undef) = a == undef ? default : a; -function vec_is_undef(x,index_=0) = index_ >= len(x) ? true : -is_undef_or_oob(x[index_]) && vec_is_undef(x,index_+1); +// --- Helpers ---------------------------------------------------------------- + +// Return `a` unless it is undef, then use `default`. +function val(a = undef, default = undef) = + is_undef(a) ? default : a; + +// Check if vector/list is fully undef or out-of-bounds (recursively). +function vec_is_undef(x, index_ = 0) = + index_ >= len(x) ? true + : is_undef_or_oob(x[index_]) && vec_is_undef(x, index_ + 1); + +// Treat scalars, lists, or undef consistently. +function is_undef_or_oob(x) = + is_undef(x) ? true + : is_list(x) ? vec_is_undef(x) + : false; -function is_undef_or_oob(x) = is_undef(x) ? true : is_list(x) ? vec_is_undef(x) : false; -// Either a or b, but not both -function either(a,b,default=undef) = is_undef_or_oob(a) ? (is_undef_or_oob(b) ? default : b) : is_undef_or_oob(b) ? a : undef; +// Either return a or b (whichever is valid). If both valid → undef. +// If both invalid → default. +function either(a, b, default = undef) = + is_undef_or_oob(a) ? (is_undef_or_oob(b) ? default : b) + : is_undef_or_oob(b) ? a + : undef; -function translationv(left=undef,right=undef,up=undef,down=undef,forward=undef,backward=undef,translation=undef) = -translationv_2( - x = either(up,is_undef(down) ? down : -down), - y = either(right,is_undef(left) ? left : -left), - z = either(forward,is_undef(backward) ? backward : -backward), - translation = translation); +// --- Translation builders --------------------------------------------------- -function translationv_2(x,y,z,translation) = - x == undef && y == undef && z == undef ? translation : - is_undef_or_oob(translation) ? [val(x,0),val(y,0),val(z,0)] - : undef; +// Accepts directional keywords (left/right, up/down, forward/backward) +// or an explicit `translation` vector. Returns [x,y,z] or undef. +function translationv( + left = undef, + right = undef, + up = undef, + down = undef, + forward = undef, + backward = undef, + translation = undef +) = + translationv_2( + // X axis points "up" (OpenSCAD’s Z is height, but we’re flexible here) + x=either(up, is_undef(down) ? down : -down), + y=either(right, is_undef(left) ? left : -left), + z=either(forward, is_undef(backward) ? backward : -backward), + translation=translation + ); -function rotationv(pitch=undef,yaw=undef,roll=undef,rotation=undef) = - rotation == undef ? [val(yaw,0),val(pitch,0),val(roll,0)] : - pitch == undef && yaw == undef && roll == undef ? rotation : - undef; +// Internal helper to finalize translation vector. +function translationv_2(x, y, z, translation) = + (is_undef(x) && is_undef(y) && is_undef(z)) ? translation + : (is_undef_or_oob(translation) ? [val(x, 0), val(y, 0), val(z, 0)] : undef); +// --- Rotation builders ------------------------------------------------------ + +// Accepts yaw/pitch/roll angles (deg), or an explicit `rotation` vector. +// Returns [yaw, pitch, roll] vector or undef. +function rotationv( + pitch = undef, + yaw = undef, + roll = undef, + rotation = undef +) = + is_undef(rotation) ? [val(yaw, 0), val(pitch, 0), val(roll, 0)] + : (is_undef(pitch) && is_undef(yaw) && is_undef(roll)) ? rotation + : undef; + +// --- Main API --------------------------------------------------------------- + +// trajectory(...) → build a 6D twist vector = [tx,ty,tz, yaw,pitch,roll] function trajectory( - left=undef, right=undef, - up=undef, down=undef, - forward=undef, backward=undef, - translation=undef, - - pitch=undef, - yaw=undef, - roll=undef, - rotation=undef -) = concat( - translationv(left=left,right=right,up=up,down=down,forward=forward,backward=backward,translation=translation), - rotationv(pitch=pitch,yaw=yaw,roll=roll,rotation=rotation) -); - -function rotationm(rotation=undef,pitch=undef,yaw=undef,roll=undef) = so3_exp(rotationv(rotation=rotation,pitch=pitch,yaw=yaw,roll=roll)); + left = undef, + right = undef, + up = undef, + down = undef, + forward = undef, + backward = undef, + translation = undef, + pitch = undef, + yaw = undef, + roll = undef, + rotation = undef +) = + concat( + translationv( + left=left, right=right, up=up, down=down, + forward=forward, backward=backward, + translation=translation + ), + rotationv(pitch=pitch, yaw=yaw, roll=roll, rotation=rotation) + ); + +// rotationm(...) → build 3x3 rotation matrix from angles +function rotationm(rotation = undef, pitch = undef, yaw = undef, roll = undef) = + so3_exp(rotationv(rotation=rotation, pitch=pitch, yaw=yaw, roll=roll)); diff --git a/trajectory_path.scad b/trajectory_path.scad index f9fffe3..580eafc 100644 --- a/trajectory_path.scad +++ b/trajectory_path.scad @@ -1,89 +1,186 @@ +// ============================================================================ +// Trajectory Path Utilities +// ---------------------------------------------------------------------------- +// Purpose : Quantize SE(3) “twists” (6D vectors) into discrete transforms, +// compose segments, and optionally close loops. +// Key ideas +// - A single trajectory is a 6D vector mu = [tx,ty,tz, wx,wy,wz] (angles in deg) +// interpreted via se3_exp(mu) -> 4x4 homogeneous transform. +// - A list of trajectories is concatenated piecewise in the given order. +// - Quantization can be specified either by: +// * step: physical step length along translation part, OR +// * steps: fixed number of samples for the *entire* path +// - start_position allows skipping an initial arc-length offset. +// ---------------------------------------------------------------------------- +// Dependencies: linalg.scad, se3.scad +// ============================================================================ + use use -function left_multiply(a,bs,i_=0) = i_ >= len(bs) ? [] : - concat([ - a * bs[i_] - ], left_multiply(a,bs,i_+1)); - - -function right_multiply(as,b,i_=0) = i_ >= len(as) ? [] : - concat([ - as[i_] * b - ], right_multiply(as,b,i_+1)); - -function quantize_trajectory(trajectory,step=undef,start_position=0,steps=undef,i_=0,length_=undef) = - length_ == undef ? quantize_trajectory( - trajectory=trajectory, - start_position=(step==undef?norm(take3(trajectory))/steps*start_position:start_position), - length_=norm(take3(trajectory)), - step=step,steps=steps,i_=i_) : - (steps==undef?start_position > length_:i_>=steps) ? [] : - concat([ - // if steps is defined, ignore start_position - se3_exp(trajectory*(steps==undef ? start_position/length_ - : i_/(steps>1?steps-1:1))) - ], quantize_trajectory(trajectory=trajectory,step=step,start_position=(steps==undef?start_position+step:start_position),steps=steps,i_=i_+1,length_=length_)); - -function close_trajectory_loop(trajectories) = concat(trajectories,[se3_ln(invert_rt(trajectories_end_position(trajectories)))]); +// --- Small helpers ---------------------------------------------------------- -function quantize_trajectories(trajectories,step=undef,start_position=0,steps=undef,loop=false,last_=identity4(),i_=0,current_length_=undef,j_=0) = - // due to quantization differences, the last step may be missed. In that case, add it: - loop==true ? quantize_trajectories( - trajectories=close_trajectory_loop(trajectories), - step=step, - start_position = start_position, - steps=steps, - loop=false, - last_=last_, - i_=i_, - current_length_=current_length_, - j_=j_) : - i_ >= len(trajectories) ? (j_ < steps ? [last_] : []) : - current_length_ == undef ? - quantize_trajectories( - trajectories=trajectories, - step = (step == undef ? trajectories_length(trajectories) / steps : step), - start_position = (step == undef ? start_position * trajectories_length(trajectories) / steps : start_position), - steps=steps, - loop=loop, - last_=last_, - i_=i_, - current_length_=norm(take3(trajectories[i_])), - j_=j_) : - concat( - left_multiply(last_,quantize_trajectory( - trajectory=trajectories[i_], - start_position=start_position, - step=step)), - quantize_trajectories( - trajectories=trajectories, - step=step, - start_position = start_position > current_length_ - ? start_position - current_length_ - : step - ((current_length_-start_position) % step), - steps=steps, - loop=loop, - last_=last_ * se3_exp(trajectories[i_]), - i_=i_+1, - current_length_ = undef, - j_=j_+len( +// Left-multiply a single transform `a` by each transform in array `bs`. +function left_multiply(a, bs, i_ = 0) = + (i_ >= len(bs)) ? [] + : concat([a * bs[i_]], left_multiply(a, bs, i_ + 1)); - quantize_trajectory( - trajectory=trajectories[i_], - start_position=start_position, - step=step +// Right-multiply each transform in array `as` by a single transform `b`. +function right_multiply(as, b, i_ = 0) = + (i_ >= len(as)) ? [] + : concat([as[i_] * b], right_multiply(as, b, i_ + 1)); - )) - )) -; +// --- Single-trajectory quantization ---------------------------------------- +// quantize_trajectory: produces an array of 4x4 transforms sampled along one 6D twist. +// If `steps` is provided, it overrides `step` and distributes uniformly. +// If `step` is provided, samples start at `start_position` and advance by `step`. +// `start_position` and `step` are in the units of the translation norm. +// +// Notes: +// - Length is computed from the translational part only: norm(take3(trajectory)) +// - For steps==1, returns the transform at the path end (t=1). +function quantize_trajectory( + trajectory, + step = undef, + start_position = 0, + steps = undef, + i_ = 0, + length_ = undef +) = + // Bootstrap total length if not provided + is_undef(length_) ? + quantize_trajectory( + trajectory=trajectory, + start_position=is_undef(step) ? (norm(take3(trajectory)) / steps) * start_position + : start_position, + length_=norm(take3(trajectory)), + step=step, + steps=steps, + i_=i_ + ) + // Termination: either finished by count, or past end by distance + : ( + is_undef(steps) ? (start_position > length_) + : (i_ >= steps) + ) ? [] + // Emit current sample and recurse + : concat( + [ + se3_exp( + trajectory * ( + is_undef(steps) ? start_position / length_ + : i_ / (steps > 1 ? (steps - 1) : 1) + ) + ), + ], + quantize_trajectory( + trajectory=trajectory, + step=step, + start_position=is_undef(steps) ? (start_position + step) : start_position, + steps=steps, + i_=i_ + 1, + length_=length_ + ) + ); +// --- Multi-trajectory helpers ---------------------------------------------- -function trajectories_length(trajectories, i_=0) = i_ >= len(trajectories) ? 0 - : norm(take3(trajectories[i_])) + trajectories_length(trajectories,i_+1); +// Append a final segment that closes the loop back to identity when applied +// after the whole chain (computed in SE(3) log space). +function close_trajectory_loop(trajectories) = + concat( + trajectories, + [se3_ln(invert_rt(trajectories_end_position(trajectories)))] + ); +// Total translational arc-length of a list of 6D twists. +function trajectories_length(trajectories, i_ = 0) = + (i_ >= len(trajectories)) ? 0 + : norm(take3(trajectories[i_])) + trajectories_length(trajectories, i_ + 1); -function trajectories_end_position(rt,i_=0,last_=identity4()) = - i_ >= len(rt) ? last_ : - trajectories_end_position(rt, i_+1, last_ * se3_exp(rt[i_])); +// End pose after chaining all twists (via se3_exp and left-to-right product). +function trajectories_end_position(rt, i_ = 0, last_ = identity4()) = + (i_ >= len(rt)) ? last_ + : trajectories_end_position(rt, i_ + 1, last_ * se3_exp(rt[i_])); +// --- Multi-trajectory quantization ----------------------------------------- +// Quantize a *list* of twists into an array of transforms along the whole path. +// Parameters mirror quantize_trajectory; additionally: +// - loop=true closes the path with an extra segment that returns to start +// Behavior: +// - If `steps` is given, we derive a uniform `step` for the *whole* path. +// - Returns an array of transforms (absolute poses), starting from identity. +function quantize_trajectories( + trajectories, + step = undef, + start_position = 0, + steps = undef, + loop = false, + last_ = identity4(), + i_ = 0, + current_length_ = undef, + j_ = 0 +) = + // Optionally close the loop by appending a final corrective twist + loop ? + quantize_trajectories( + trajectories=close_trajectory_loop(trajectories), + step=step, + start_position=start_position, + steps=steps, + loop=false, + last_=last_, + i_=i_, + current_length_=current_length_, + j_=j_ + ) + // End when all segments consumed. If steps was given but rounding skipped + // the final sample, return the last pose once more. + : (i_ >= len(trajectories)) ? ( (!is_undef(steps) && (j_ < steps)) ? [last_] : []) + // Initialize global step/offset once we know total length + : is_undef(current_length_) ? + quantize_trajectories( + trajectories=trajectories, + step=is_undef(step) ? (trajectories_length(trajectories) / steps) : step, + start_position=is_undef(step) ? (start_position * trajectories_length(trajectories) / steps) + : start_position, + steps=steps, + loop=loop, + last_=last_, + i_=i_, + current_length_=norm(take3(trajectories[i_])), + j_=j_ + ) + // Emit samples for current segment (absolute poses), then continue + : concat( + // Sample the i_-th local twist, then left-multiply by `last_` to get + // absolute poses for this segment. + left_multiply( + last_, + quantize_trajectory( + trajectory=trajectories[i_], + start_position=start_position, + step=step + ) + ), + // Recurse to next segment: + quantize_trajectories( + trajectories=trajectories, + step=step, + // Advance start_position into the next segment: + start_position=(start_position > current_length_) ? (start_position - current_length_) + : (step - ( (current_length_ - start_position) % step)), + steps=steps, + loop=loop, + last_=last_ * se3_exp(trajectories[i_]), + i_=i_ + 1, + current_length_=undef, + j_=j_ + len( + quantize_trajectory( + trajectory=trajectories[i_], + start_position=start_position, + step=step + ) + ) + ) + ); diff --git a/transformations.scad b/transformations.scad index 89edff6..e6b55f1 100644 --- a/transformations.scad +++ b/transformations.scad @@ -1,43 +1,89 @@ +// ============================================================================ +// Transformations Utilities +// ---------------------------------------------------------------------------- +// Provides matrix constructors and helpers for geometric transformations: +// - rotation(xyz, axis): create rotation from Euler angles or axis-angle +// - scaling(v): scaling matrix +// - translation(v): translation matrix +// - project(x): cartesian from homogeneous coordinates +// - transform(m, list): apply matrix to list of points +// - to_3d(list): ensure vectors are 3D +// +// Notes: +// - Euler convention: R = Rz * Ry * Rx +// - Axis rotation uses se3 exponential map +// ============================================================================ + use use use -/*! - Creates a rotation matrix +// --- Rotation --------------------------------------------------------------- +// Creates a rotation matrix +// Options: +// - xyz = Euler angles (applied as Rz * Ry * Rx) +// - axis = axis-angle vector (axis * angle) +// Examples: +// rotation(xyz=[90,0,0]) // rotate 90° about X +// rotation(axis=[0,0,45]) // rotate 45° about Z +function rotation(xyz = undef, axis = undef) = + // disallow both forms together + (!is_undef(xyz) && !is_undef(axis)) ? + undef + : + // pure axis-angle exponential form + (is_undef(xyz) && !is_undef(axis)) ? + se3_exp([0, 0, 0, axis[0], axis[1], axis[2]]) + : + // shorthand for single-angle rotation about Z + (is_undef(axis) && !is_undef(xyz) && !is_list(xyz)) ? + rotation(axis=[0, 0, xyz]) + : + // full Euler xyz case + (is_undef(axis) && is_list(xyz)) ? + ( + len(xyz) >= 3 ? + rotation(axis=[0, 0, xyz[2]]) + : identity4() + ) * ( + len(xyz) >= 2 ? + rotation(axis=[0, xyz[1], 0]) + : identity4() + ) * ( + len(xyz) >= 1 ? + rotation(axis=[xyz[0], 0, 0]) + : identity4() + ) + : + // fallback + identity4(); - xyz = euler angles = rz * ry * rx - axis = rotation_axis * rotation_angle -*/ -function rotation(xyz=undef, axis=undef) = - xyz != undef && axis != undef ? undef : - xyz == undef ? se3_exp([0,0,0,axis[0],axis[1],axis[2]]) : - len(xyz) == undef ? rotation(axis=[0,0,xyz]) : - (len(xyz) >= 3 ? rotation(axis=[0,0,xyz[2]]) : identity4()) * - (len(xyz) >= 2 ? rotation(axis=[0,xyz[1],0]) : identity4()) * - (len(xyz) >= 1 ? rotation(axis=[xyz[0],0,0]) : identity4()); +// --- Scaling ---------------------------------------------------------------- +// Creates a scaling matrix: scaling([sx, sy, sz]) +function scaling(v) = + [ + [v[0], 0, 0, 0], + [0, v[1], 0, 0], + [0, 0, v[2], 0], + [0, 0, 0, 1], + ]; -/*! - Creates a scaling matrix -*/ -function scaling(v) = [ - [v[0],0,0,0], - [0,v[1],0,0], - [0,0,v[2],0], - [0,0,0,1], -]; +// --- Translation ------------------------------------------------------------ +// Creates a translation matrix: translation([tx, ty, tz]) +function translation(v) = + [ + [1, 0, 0, v[0]], + [0, 1, 0, v[1]], + [0, 0, 1, v[2]], + [0, 0, 0, 1], + ]; -/*! - Creates a translation matrix -*/ -function translation(v) = [ - [1,0,0,v[0]], - [0,1,0,v[1]], - [0,0,1,v[2]], - [0,0,0,1], -]; +// --- Coordinate Conversion -------------------------------------------------- +// Converts from homogeneous to cartesian coordinates +function project(x) = subarray(x, end=len(x) - 1) / x[len(x) - 1]; -// Convert between cartesian and homogenous coordinates -function project(x) = subarray(x,end=len(x)-1) / x[len(x)-1]; +// Applies matrix `m` to a list of points +function transform(m, list) = [for (p = list) project(m * vec4(p))]; -function transform(m, list) = [for (p=list) project(m * vec4(p))]; -function to_3d(list) = [ for(v = list) vec3(v) ]; +// Ensures points are represented as 3D vectors +function to_3d(list) = [for (v = list) vec3(v)];