Added rot_decode to decode rotation matrices and matrix_trace,

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

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 // vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap

View file

@ -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()

View file

@ -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

View file

@ -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