You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
62 lines
1.4 KiB
62 lines
1.4 KiB
use <../vector/to_ang_vec.scad>
|
|
|
|
FINAL_ROW = [0, 0, 0, 1];
|
|
function __m_rotation_q_rotation(a, v) =
|
|
let(
|
|
uv = v / norm(v),
|
|
s = sin(a / 2) * uv,
|
|
w = sin(a) * uv,
|
|
|
|
xx = 2 * s.x ^ 2,
|
|
yy = 2 * s.y ^ 2,
|
|
zz = 2 * s.z ^ 2,
|
|
|
|
xy = 2 * s.x * s.y,
|
|
xz = 2 * s.x * s.z,
|
|
yz = 2 * s.y * s.z
|
|
)
|
|
[
|
|
[1 - yy - zz, xy - w.z, xz + w.y, 0],
|
|
[xy + w.z, 1 - xx - zz, yz - w.x, 0],
|
|
[xz - w.y, yz + w.x, 1 - xx - yy, 0],
|
|
FINAL_ROW
|
|
];
|
|
|
|
function __m_rotation_xRotation(a) =
|
|
let(c = cos(a), s = sin(a))
|
|
[
|
|
[1, 0, 0, 0],
|
|
[0, c, -s, 0],
|
|
[0, s, c, 0],
|
|
FINAL_ROW
|
|
];
|
|
|
|
function __m_rotation_yRotation(a) =
|
|
let(c = cos(a), s = sin(a))
|
|
[
|
|
[c, 0, s, 0],
|
|
[0, 1, 0, 0],
|
|
[-s, 0, c, 0],
|
|
FINAL_ROW
|
|
];
|
|
|
|
function __m_rotation_zRotation(a) =
|
|
let(c = cos(a), s = sin(a))
|
|
[
|
|
[c, -s, 0, 0],
|
|
[s, c, 0, 0],
|
|
[0, 0, 1, 0],
|
|
FINAL_ROW
|
|
];
|
|
|
|
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_impl(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],
|
|
FINAL_ROW
|
|
] : (is_undef(v) ? __m_rotation_xyz_rotation(a) : __m_rotation_q_rotation(a, v));
|