openscad/BOSL2
1
0
Fork 0
mirror of https://github.com/revarbat/BOSL2.git synced 2025-08-16 22:33:57 +02:00
Code Issues Projects Releases Wiki Activity

Added rot_decode to decode rotation matrices and matrix_trace,

Browse Source 
supporting function, and regression tests for both.
This commit is contained in:
Adrian Mariano
2020-10-20 16:26:11 -04:00
parent dcc7e9faaa
commit c80c7c558a
4 changed files with 88 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
affine.scad
View File

@@ -467,6 +467,48 @@ function is_2d_transform(t) = // z-parameters are zero, except we allow t[2][
// Function: rot_decode()
// Usage:
// [angle,axis,cp,translation] = rot_decode(rotation)
// Description:
// Given an input 3d rigid transformation operator (one composed of just rotations and translations)
// represented as a 4x4 matrix, compute the rotation and translation parameters of the operator.
// Returns a list of the four parameters, the angle, in the interval [0,180], the rotation axis
// as a unit vector, a centerpoint for the rotation, and a translation. If you set `parms=rot_decode(rotation)`
// then the transformation can be reconstructed from parms as `move(parms[3])*rot(a=parms[0],v=parms[1],cp=parms[2])`.
// This decomposition makes it possible to perform interpolation. If you construct a transformation using `rot`
// the decoding may flip the axis (if you gave an angle outside of [0,180]). The returned axis will be a unit vector,
// and the centerpoint lies on the plane through the origin that is perpendicular to the axis. It may be different
// than the centerpoint you used to construct the transformation.
// Example:
// rot_decode(rot(45)); // Returns [45,[0,0,1], [0,0,0], [0,0,0]]
// rot_decode(rot(a=37, v=[1,2,3], cp=[4,3,-7]))); // Returns [37, [0.26, 0.53, 0.80], [4.8, 4.6, -4.6], [0,0,0]]
// rot_decode(left(12)*xrot(-33)); // Returns [33, [-1,0,0], [0,0,0], [-12,0,0]]
// rot_decode(translate([3,4,5])); // Returns [0, [0,0,1], [0,0,0], [3,4,5]]
function rot_decode(M) =
assert(is_matrix(M,4,4) && M[3]==[0,0,0,1], "Input matrix must be a 4x4 matrix representing a 3d transformation")
let(R = submatrix(M,[0:2],[0:2]))
assert(approx(det3(R),1) && approx(norm_fro(R * transpose(R)-ident(3)),0),"Input matrix is not a rotation")
let(
translation = [for(row=[0:2]) M[row][3]], // translation vector
largest = max_index([R[0][0], R[1][1], R[2][2]]),
axis_matrix = R + transpose(R) - (matrix_trace(R)-1)*ident(3), // Each row is on the rotational axis
// Construct quaternion q = c * [x sin(theta/2), y sin(theta/2), z sin(theta/2), cos(theta/2)]
q_im = axis_matrix[largest],
q_re = R[(largest+2)%3][(largest+1)%3] - R[(largest+1)%3][(largest+2)%3],
c_sin = norm(q_im), // c * sin(theta/2) for some c
c_cos = abs(q_re) // c * cos(theta/2)
)
approx(c_sin,0) ? [0,[0,0,1],[0,0,0],translation] :
let(
angle = 2*atan2(c_sin, c_cos), // This is supposed to be more accurate than acos or asin
axis = (q_re>=0 ? 1:-1)*q_im/c_sin,
tproj = translation - (translation*axis)*axis, // Translation perpendicular to axis determines centerpoint
cp = (tproj + cross(axis,tproj)*c_cos/c_sin)/2
)
[angle, axis, cp, (translation*axis)*axis];
// vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap