diff --git a/src/__private__/__m_rotation.scad b/src/__private__/__m_rotation.scad new file mode 100644 index 00000000..4ce85d6a --- /dev/null +++ b/src/__private__/__m_rotation.scad @@ -0,0 +1,69 @@ +function __m_rotation_q_rotation(a, v) = + let( + half_a = a / 2, + axis = v / norm(v), + s = sin(half_a), + x = s * axis[0], + y = s * axis[1], + z = s * axis[2], + w = cos(half_a), + + 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, yx - wz, zx + wy, 0], + [yx + wz, 1 - xx - zz, zy - wx, 0], + [zx - wy, zy + wx, 1 - xx - yy, 0], + [0, 0, 0, 1] + ]; + +function __m_rotation_xRotation(a) = + let(c = cos(a), s = sin(a)) + [ + [1, 0, 0, 0], + [0, c, -s, 0], + [0, s, c, 0], + [0, 0, 0, 1] + ]; + +function __m_rotation_yRotation(a) = + let(c = cos(a), s = sin(a)) + [ + [c, 0, s, 0], + [0, 1, 0, 0], + [-s, 0, c, 0], + [0, 0, 0, 1] + ]; + +function __m_rotation_zRotation(a) = + let(c = cos(a), s = sin(a)) + [ + [c, -s, 0, 0], + [s, c, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1] + ]; + +function __m_rotation_xyz_rotation(a) = + let(ang = __to_ang_vect(a)) + __m_rotation_zRotation(ang[2]) * __m_rotation_yRotation(ang[1]) * __m_rotation_xRotation(ang[0]); + +function __m_rotation(a, v) = + (a == 0 || a == [0, 0, 0] || a == [0] || a == [0, 0]) ? [ + [1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1] + ] : (is_undef(v) ? __m_rotation_xyz_rotation(a) : __m_rotation_q_rotation(a, v)); \ No newline at end of file diff --git a/src/m_rotation.scad b/src/m_rotation.scad index 37a7b166..cd4e4bce 100644 --- a/src/m_rotation.scad +++ b/src/m_rotation.scad @@ -9,73 +9,6 @@ **/ include <__private__/__to_ang_vect.scad>; +include <__private__/__m_rotation.scad>; -function _q_rotation(a, v) = - let( - half_a = a / 2, - axis = v / norm(v), - s = sin(half_a), - x = s * axis[0], - y = s * axis[1], - z = s * axis[2], - w = cos(half_a), - - 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, yx - wz, zx + wy, 0], - [yx + wz, 1 - xx - zz, zy - wx, 0], - [zx - wy, zy + wx, 1 - xx - yy, 0], - [0, 0, 0, 1] - ]; - -function _m_xRotation(a) = - let(c = cos(a), s = sin(a)) - [ - [1, 0, 0, 0], - [0, c, -s, 0], - [0, s, c, 0], - [0, 0, 0, 1] - ]; - -function _m_yRotation(a) = - let(c = cos(a), s = sin(a)) - [ - [c, 0, s, 0], - [0, 1, 0, 0], - [-s, 0, c, 0], - [0, 0, 0, 1] - ]; - -function _m_zRotation(a) = - let(c = cos(a), s = sin(a)) - [ - [c, -s, 0, 0], - [s, c, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]; - -function _xyz_rotation(a) = - let(ang = __to_ang_vect(a)) - _m_zRotation(ang[2]) * _m_yRotation(ang[1]) * _m_xRotation(ang[0]); - -function m_rotation(a, v) = - (a == 0 || a == [0, 0, 0] || a == [0] || a == [0, 0]) ? [ - [1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1] - ] : (is_undef(v) ? _xyz_rotation(a) : _q_rotation(a, v)); \ No newline at end of file +function m_rotation(a, v) = __m_rotation(a, v); \ No newline at end of file