From c80c7c558a5048a308a8b4ace392f883cf151010 Mon Sep 17 00:00:00 2001
From: Adrian Mariano <avm4@cornell.edu>
Date: Tue, 20 Oct 2020 16:26:11 -0400
Subject: [PATCH] Added rot_decode to decode rotation matrices and
 matrix_trace, supporting function, and regression tests for both.

---
 affine.scad            | 42 ++++++++++++++++++++++++++++++++++++++++++
 math.scad              | 10 ++++++++++
 tests/test_affine.scad | 29 +++++++++++++++++++++++++++++
 tests/test_math.scad   |  7 +++++++
 4 files changed, 88 insertions(+)

diff --git a/affine.scad b/affine.scad
index 5696cc6..462ae1f 100644
--- a/affine.scad
+++ b/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
diff --git a/math.scad b/math.scad
index 0c847b8..c0ac6ab 100644
--- a/math.scad
+++ b/math.scad
@@ -904,6 +904,16 @@ function norm_fro(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
 
 // Function: all_zero()
diff --git a/tests/test_affine.scad b/tests/test_affine.scad
index a759b56..b9a4e49 100644
--- a/tests/test_affine.scad
+++ b/tests/test_affine.scad
@@ -252,5 +252,34 @@ module 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
diff --git a/tests/test_math.scad b/tests/test_math.scad
index 9fb7c65..824098a 100644
--- a/tests/test_math.scad
+++ b/tests/test_math.scad
@@ -550,6 +550,13 @@ module 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