diff --git a/src/rotate_p.scad b/src/rotate_p.scad index fb91f02d..ab04dd53 100644 --- a/src/rotate_p.scad +++ b/src/rotate_p.scad @@ -14,6 +14,36 @@ include <__private__/__to2d.scad>; include <__private__/__to3d.scad>; + +function _q_rotate_p_3d(p, a, v) = + let( + axis = v / norm(v), + s = sin(a / 2), + x = s * axis[0], + y = s * axis[1], + z = s * axis[2], + w = cos(a / 2), + + x2 = x + x, + y2 = y + y, + z2 = z + z, + + xx = x * x2, + yx = y * x2, + yy = y * y2, + zx = z * x2, + zy = z * y2, + zz = z * z2, + wx = w * x2, + wy = w * y2, + wz = w * z2 + ) + [ + (1 - yy - zz) * p[0] + (yx - wz) * p[1] + (zx + wy) * p[2], + (yx + wz) * p[0] + (1 - xx - zz) * p[1] + (zy - wx) * p[2], + (zx - wy) * p[0] + (zy + wx) * p[1] + (1 - xx - yy) * p[2] + ]; + function _rotx(pt, a) = let(cosa = cos(a), sina = sin(a)) [ @@ -48,10 +78,21 @@ function to_avect(a) = ) ); -function rotate_p(point, a) = +function _rotate_p(p, a) = let(angle = to_avect(a)) - len(point) == 3 ? - _rotate_p_3d(point, angle) : + len(p) == 3 ? + _rotate_p_3d(p, angle) : __to2d( - _rotate_p_3d(__to3d(point), angle) + _rotate_p_3d(__to3d(p), angle) ); + + +function _q_rotate_p(p, a, v) = + len(p) == 3 ? + _q_rotate_p_3d(p, a, v) : + __to2d( + _q_rotate_p_3d(__to3d(p), a, v) + ); + +function rotate_p(point, a, v) = + v == undef ? _rotate_p(point, a) : _q_rotate_p(p, a, v);