From 0b562f54afd7dbfe2ff0bbffb67f62eb7d003ecb Mon Sep 17 00:00:00 2001
From: Andrew Jewett <jewett.aij@gmail.com>
Date: Thu, 20 Feb 2020 19:26:07 -0800
Subject: [PATCH] the rotation quaternion is now available.  Like R, T, and c,
 the quaternion, q, is a public data member of the Superpose3D class.  From
 it, the rotation angle and rotation axis can easily be determined.

---
 README.md               | 15 +++++++++++++--
 include/superpose3d.hpp | 11 ++++++++++-
 2 files changed, 23 insertions(+), 3 deletions(-)

diff --git a/README.md b/README.md
index 174da02..59991e1 100644
--- a/README.md
+++ b/README.md
@@ -27,7 +27,7 @@ between corresponding points from either point cloud, where RMSD is defined as:
 ```
    T = a translation vector (a 1-D array containing x,y,z displacements),
    R = a rotation matrix    (a 3x3 array whose determinant = 1),
-   c = a scale factor       (a number, optional, 1 by default)
+   c = a scalar             (a number, optional, 1 by default)
 ```
 After invoking Superpose3D::Superpose(), the optimal translation, rotation and
 scale factor are stored in Superpose3D data members named
@@ -35,7 +35,7 @@ scale factor are stored in Superpose3D data members named
 (*T* is implemented as a C-style array, and
  *R* is implemented as a C-style 3x3 array in pointer-to-pointer format.)
 
-A *weighted* version of the RMSD minimization algorithm is also available
+A weighted version of the RMSD minimization algorithm is also available
 if the caller supplies an extra argument specifying the weight of every
 point in the cloud (*w<sub>n</sub>*).  In that case, RMSD is defined as:
 <img src="http://latex.codecogs.com/gif.latex?\large&space;RMSD=\sqrt\left\sum_{n=1}^N\,w_n\,\sum_{i=1}^3 \left|X_{ni}-\left(\sum_{j=1}^3 c R_{ij}x_{nj}+T_i\right)\right|^2\quad\middle/\quad\sum_{n=1}^N w_n}\right}"/>
@@ -94,6 +94,7 @@ double rmsd = superposer.Superpose(X, x);
 
 // Note: The optimal rotation, translation, and scale factor will be stored in
 //       superposer.R, superposer.T, and superposer.c, respectively.
+//       (A quaternion describing the rotation is stored in superpose.p.)
 ```
 *(A complete working example can be found [here](tests/test_superpose3d.cpp).)*
 
@@ -103,6 +104,16 @@ If you want to allow scale transformations, then use:
 superposer.Superpose(X, x, true);
 ```
 
+#### Rotation angle, axis, and quaternion
+If the corresponding rotation angle and rotation axis are also needed, they
+can be inferred from the ***q*** data member ("*superposer.q*" in the
+example above). After invoking Superpose(), the *q* member will store the
+[quaternion corresponding to rotation *R*](https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation).
+The first element of *q* will store *cos(θ/2)* (where *θ* is the
+rotation angle).  The remaining 3 elements of *q* will store the
+axis of rotation (with length *sin(θ/2)*).
+
+#### Weighted RMSD
 By default point in the point cloud will be given equal weights when
 calculating RMSD.  If you want to specify different weights for each point
 (ie. *w<sub>n</sub>* in the formula above), then see the following example:
diff --git a/include/superpose3d.hpp b/include/superpose3d.hpp
index f3f1a9b..a4c0c8e 100644
--- a/include/superpose3d.hpp
+++ b/include/superpose3d.hpp
@@ -41,6 +41,10 @@ class Superpose3D {
   Scalar **R;  //!< store optimal rotation here (this is a 3x3 array).
   Scalar T[3]; //!< store optimal translation here
   Scalar c;  //!< store optimal scale (typically 1 unless requested by the user)
+  Scalar q[4]; //!< quaternion corresponding to the rotation stored in R.
+               //   The first entry of q is cos(θ/2).  The remaining 3 entries
+               //   of q are the axis of rotation (with length sin(θ/2)).
+               // (Note: This is not the same as "p" from Diamond's 1988 paper.)
 
   Superpose3D(size_t N = 0);  //!< N=number of points in both point clouds
 
@@ -217,8 +221,8 @@ Superpose(ConstArrayOfCoords aaXf, // coords for the "frozen" object
   P[3][2] = V[2];
   P[3][3] = 0.0;
 
-  // The vector "p" will contain the optimal rotation (in quaternion format)
   Scalar p[4];
+  // The vector "p" will contain the optimal rotation (in quaternion format)
   Scalar eval_max = eigen_calc.PrincipalEigen(P, p, true);
 
   // Now normalize p
@@ -242,6 +246,11 @@ Superpose(ConstArrayOfCoords aaXf, // coords for the "frozen" object
   R[0][2] = 2*(p[0]*p[2] + p[1]*p[3]);
   R[2][0] = 2*(p[0]*p[2] - p[1]*p[3]);
 
+  q[0] = p[3];  // Note: The "p" variable is not a quaternion in the
+  q[1] = p[0];  //       conventional sense because its elements
+  q[2] = p[1];  //       are in the wrong order.  I correct for that here.
+  q[3] = p[2];  //       "q" is the quaternion correspond to rotation R.
+
   Scalar pPp = eval_max;
 
   // Optional: Decide the scale factor, c