mirror of
https://github.com/BelfrySCAD/BOSL2.git
synced 2024-12-29 08:19:43 +00:00
Added rot_decode to decode rotation matrices and matrix_trace,
supporting function, and regression tests for both.
This commit is contained in:
parent
dcc7e9faaa
commit
c80c7c558a
4 changed files with 88 additions and 0 deletions
42
affine.scad
42
affine.scad
|
@ -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
|
// vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap
|
||||||
|
|
10
math.scad
10
math.scad
|
@ -904,6 +904,16 @@ function norm_fro(A) =
|
||||||
norm(flatten(A));
|
norm(flatten(A));
|
||||||
|
|
||||||
|
|
||||||
|
// Function: matrix_trace()
|
||||||
|
// Usage:
|
||||||
|
// matrix_trace(M)
|
||||||
|
// Description:
|
||||||
|
// Computes the trace of a square matrix, the sum of the entries on the diagonal.
|
||||||
|
function matrix_trace(M) =
|
||||||
|
assert(is_matrix(M,square=true), "Input to trace must be a square matrix")
|
||||||
|
[for(i=[0:1:len(M)-1])1] * [for(i=[0:1:len(M)-1]) M[i][i]];
|
||||||
|
|
||||||
|
|
||||||
// Section: Comparisons and Logic
|
// Section: Comparisons and Logic
|
||||||
|
|
||||||
// Function: all_zero()
|
// Function: all_zero()
|
||||||
|
|
|
@ -252,5 +252,34 @@ module test_apply_list() {
|
||||||
test_apply_list();
|
test_apply_list();
|
||||||
|
|
||||||
|
|
||||||
|
module test_rot_decode() {
|
||||||
|
Tlist = [
|
||||||
|
rot(37),
|
||||||
|
xrot(49),
|
||||||
|
yrot(88),
|
||||||
|
rot(37,v=[1,3,3]),
|
||||||
|
rot(41,v=[2,-3,4]),
|
||||||
|
rot(180),
|
||||||
|
xrot(180),
|
||||||
|
yrot(180),
|
||||||
|
rot(180, v=[3,2,-5], cp=[3,5,18]),
|
||||||
|
rot(0.1, v=[1,2,3]),
|
||||||
|
rot(-47,v=[3,4,5],cp=[9,3,4]),
|
||||||
|
rot(197,v=[13,4,5],cp=[9,-3,4]),
|
||||||
|
move([3,4,5]),
|
||||||
|
move([3,4,5]) * rot(a=56, v=[5,3,-3], cp=[2,3,4]),
|
||||||
|
ident(4)
|
||||||
|
];
|
||||||
|
errlist = [for(T = Tlist)
|
||||||
|
let(
|
||||||
|
parm = rot_decode(T),
|
||||||
|
restore = move(parm[3])*rot(a=parm[0],v=parm[1],cp=parm[2])
|
||||||
|
)
|
||||||
|
norm_fro(restore-T)];
|
||||||
|
assert(max(errlist)<1e-13);
|
||||||
|
}
|
||||||
|
test_rot_decode();
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap
|
// vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap
|
||||||
|
|
|
@ -550,6 +550,13 @@ module test_determinant() {
|
||||||
test_determinant();
|
test_determinant();
|
||||||
|
|
||||||
|
|
||||||
|
module test_matrix_trace() {
|
||||||
|
M = [ [6,4,-2,9], [1,-2,8,3], [1,5,7,6], [4,2,5,1] ];
|
||||||
|
assert_equal(matrix_trace(M), 6-2+7+1);
|
||||||
|
}
|
||||||
|
test_matrix_trace();
|
||||||
|
|
||||||
|
|
||||||
// Logic
|
// Logic
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in a new issue