include <../utils/core/core.scad>

use <maths.scad>

function transpose3(m) = [ [m[0].x, m[1].x, m[2].x],

[m[0].y, m[1].y, m[2].y],

[m[0].z, m[1].z, m[2].z] ];

function fs_frame(tangents) =

let(tangent = tangents[0],

normal = tangents[1] - tangents[0],

binormal = cross(tangent, normal),

z = unit(tangent),

x = assert(norm(binormal) > 0.00001, "first three points are colinear") unit(binormal),

y = unit(cross(z, x))

) [[x.x, y.x, z.x],

[x.y, y.y, z.y],

[x.z, y.z, z.z]];

function rotate_from_to(a, b) =

let(axis = unit(cross(a, b)))

axis * axis >= 0.99 ? transpose3([b, axis, cross(axis, b)]) * [a, axis, cross(axis, a)]

: a * b > 0 ? [[ 1, 0, 0], [0, 1, 0], [0, 0, 1]]

: [[-1, 0, 0], [0, 1, 0], [0, 0, -1]];

function calculate_twist(A, B) = let(D = transpose3(B) * A) atan2(D[1][0], D[0][0]);

function orientate(p, r) =

let(x = r[0], y = r[1], z = r[2])

[[x.x, y.x, z.x],

[x.y, y.y, z.y],

[x.z, y.z, z.z],

[p.x, p.y, p.z]];

function rot3_z(a) =

let(c = cos(a),

s = sin(a))

[ [ c, -s, 0],

[ s, c, 0],

[ 0, 0, 1] ];

function tangent(path, before, i, after) = unit(unit(path[i] - path[before]) + unit(path[after] - path[i]));

function helical_twist_per_segment(r, pitch, sides) = //! Calculate the twist around Z that rotate_from_to() introduces

let(step_angle = 360 / sides,

lt = 2 * r * sin(step_angle), // length of tangent between two facets

slope = atan(2 * pitch / sides / lt) // slope of tangents

) step_angle * sin(slope); // angle tangent should rotate around z projected onto axis rotate_from_to() uses

function skin_points(profile, path, loop, twist = 0) =

let(len = len(path),

last = len - 1,

profile4 = [for(p = profile) [p.x, p.y, p.z, 1]],

tangents = [tangent(path, loop ? last : 0, 0, 1),

for(i = [1 : last - 1]) tangent(path, i - 1, i, i + 1),

tangent(path, last - 1, last, loop ? 0 : last)],

rotations = [for(i = 0, rot = fs_frame(tangents);

i < len;

i = i + 1,

rot = i < len ? rotate_from_to(tangents[i - 1], tangents[i]) * rot : undef) rot],

missmatch = loop ? calculate_twist(rotations[0], rotations[last]) : 0,

rotation = missmatch + twist

)

[for(i = [0 : last])

let(za = rotation * i / last)

each profile4 * orientate(path[i], rotations[i] * rot3_z(za))

];

function cap(facets, segment = 0, end) = //! Create the mesh for an end cap

let(reverse = is_undef(end) ? segment : end)

[for(i = [0 : facets - 1]) facets * segment + (reverse ? i : facets - 1 - i)];

function quad(p, a, b, c, d) = norm(p[a] - p[c]) > norm(p[b] - p[d]) ? [[b, c, d], [b, d, a]] : [[a, b, c], [a, c, d]];

function skin_faces(points, npoints, facets, loop, offset = 0) = //! Create the mesh for the swept volume without end caps

[for(i = [0 : facets - 1], s = [0 : npoints - (loop ? 1 : 2)])

let(j = s + offset, k = loop ? (j + 1) % npoints : j + 1)

each quad(points,

j * facets + i,

j * facets + (i + 1) % facets,

k * facets + (i + 1) % facets,

k * facets + i)];

function sweep(path, profile, loop = false, twist = 0) = //! Generate the point list and face list of the swept volume

let(

npoints = len(path),

facets = len(profile),

points = skin_points(profile, path, loop, twist),

skin_faces = skin_faces(points, npoints, facets, loop),

faces = loop ? skin_faces : concat([cap(facets)], skin_faces, [cap(facets, npoints - 1)])

) [points, faces];

module sweep(path, profile, loop = false, twist = 0) { //! Draw a polyhedron that is the swept volume

mesh = sweep(path, profile, loop, twist);

polyhedron(points = mesh[0], faces = mesh[1]);

}

function path_length(path, i = 0, length = 0) = //! Calculated the length along a path

i >= len(path) - 1 ? length

: path_length(path, i + 1, length + norm(path[i + 1] - path[i]));

function circle_points(r = 1, z = 0, dir = -1) = //! Generate the points of a circle, setting z makes a single turn spiral

let(sides = r2sides(r))

[for(i = [0 : sides - 1]) let(a = dir * i * 360 / sides) [r * cos(a), r * sin(a), z * i / sides]];

function rectangle_points(w, h) = [[-w/2, -h/2, 0], [-w/2, h/2, 0], [w/2, h/2, 0], [w/2, -h/2, 0]]; //! Generate the points of a rectangle

function arc_points(r, a = [90, 0, 180], al = 90) = //! Generate the points of a circular arc

let(sides = ceil(r2sides(r) * al / 360), tf = rotate(a))

[for(i = [0 : sides]) let(t = i * al / sides) transform([r * sin(t), r * cos(t), 0], tf)];

function before(path1, path2) = //! Translate ```path1``` so its end meets the start of ```path2``` and then concatenate

let(end = len(path1) - 1, offset = path2[0] - path1[end])

concat([for(i = [0 : end - 1]) path1[i] + offset], path2);

function after(path1, path2) = //! Translate ```path2``` so its start meets the end of ```path1``` and then concatenate

let(end1 = len(path1) - 1, end2 = len(path2) - 1, offset = path1[end1] - path2[0])

concat(path1, [for(i = [1 : end2]) path2[i] + offset]);