JSVD3D.hh

Go to the documentation of this file.

#ifndef __JMATH_JSVD3D__
#define __JMATH_JSVD3D__
 
#include <algorithm>
#include <cmath>
#include <array>
 
#include "JMath/JConstants.hh"
#include "JMath/JMatrix3D.hh"
#include "JMath/JMatrix3S.hh"
 
 
namespace JMATH {}
namespace JPP { using namespace JMATH; }
 
namespace JMATH {
 
  /**
   * Singular value decomposition.
   *
   * See:
   * "Computing the Singular Value Decomposition of 3x3 matrices 
   *  with minimal branching and elementary floating point operations",\n
   * A.\ McAdams, A.\ Selle, R.\ Tamstorf, J.\ Teran and E.\ Sifakis,\n
   * University of Wisconsin - Madison technical report TR1690, May 2011
   */
  class JSVD3D {
  public:
    /**
     * Default constructor.
     */
    JSVD3D()
    {}
 
 
    /**
     * Constructor.
     *
     * \param  A       matrix
     */
    JSVD3D(const JMatrix3D& A)
    {
      decompose(A);
    }
 
 
    /**
     * Decompose given matrix.
     *
     * \param  A       matrix
     */
    void decompose(const JMatrix3D& A)
    {
      using namespace std;
 
      // B = A^T * A
 
      B.a00  =  A.a00 * A.a00  +  A.a10 * A.a10  +  A.a20 * A.a20;
    
      B.a10  =  A.a01 * A.a00  +  A.a11 * A.a10  +  A.a21 * A.a20;
      B.a11  =  A.a01 * A.a01  +  A.a11 * A.a11  +  A.a21 * A.a21;
 
      B.a20  =  A.a02 * A.a00  +  A.a12 * A.a10  +  A.a22 * A.a20;
      B.a21  =  A.a02 * A.a01  +  A.a12 * A.a11  +  A.a22 * A.a21;
      B.a22  =  A.a02 * A.a02  +  A.a12 * A.a12  +  A.a22 * A.a22;
 
      B.a01  =  B.a10;
      B.a02  =  B.a20;
      B.a12  =  B.a21;
 
 
      // symmetric eigen alysis
 
      const JQuaternion q(B);
 
      const double w  = q[3];
      const double x  = q[0];
      const double y  = q[1];
      const double z  = q[2];
 
      const double xx = x*x;
      const double yy = y*y;
      const double zz = z*z;
      const double xz = x*z;
      const double xy = x*y;
      const double yz = y*z;
      const double wx = w*x;
      const double wy = w*y;
      const double wz = w*z;
 
      V.a00 = 1.0 - 2.0*(yy + zz);  V.a01 = 2.0*(xy - wz);        V.a02 = 2.0*(xz + wy);
      V.a10 = 2.0*(xy + wz);        V.a11 = 1.0 - 2.0*(xx + zz);  V.a12 = 2.0*(yz - wx);
      V.a20 = 2.0*(xz - wy);        V.a21 = 2.0*(yz + wx);        V.a22 = 1.0 - 2.0*(xx + yy);
 
 
      B.mul(A, V);
 
 
      // sort singular values.
 
      double rho1 = getLengthSquared(B.a00, B.a10, B.a20);
      double rho2 = getLengthSquared(B.a01, B.a11, B.a21);
      double rho3 = getLengthSquared(B.a02, B.a12, B.a22);
 
      if (rho1 < rho2) {
        swap(B.a00,B.a01);   B.a01 = -B.a01;   swap(V.a00,V.a01);   V.a01 = -V.a01;
        swap(B.a10,B.a11);   B.a11 = -B.a11;   swap(V.a10,V.a11);   V.a11 = -V.a11;
        swap(B.a20,B.a21);   B.a21 = -B.a21;   swap(V.a20,V.a21);   V.a21 = -V.a21;
        swap(rho1,rho2);  
      }
 
      if (rho1 < rho3) {
        swap(B.a00,B.a02);   B.a02 = -B.a02;   swap(V.a00,V.a02);   V.a02 = -V.a02;
        swap(B.a10,B.a12);   B.a12 = -B.a12;   swap(V.a10,V.a12);   V.a12 = -V.a12;
        swap(B.a20,B.a22);   B.a22 = -B.a22;   swap(V.a20,V.a22);   V.a22 = -V.a22;
        swap(rho1,rho3);  
      }
 
      if (rho2 < rho3) {
        swap(B.a01,B.a02);   B.a02 = -B.a02;   swap(V.a01,V.a02);   V.a02 = -V.a02;
        swap(B.a11,B.a12);   B.a12 = -B.a12;   swap(V.a11,V.a12);   V.a12 = -V.a12;
        swap(B.a21,B.a22);   B.a22 = -B.a22;   swap(V.a21,V.a22);   V.a22 = -V.a22;
      }
 
 
      // QR decomposition
 
      JMatrix3D& Q = U;
      JMatrix3D& R = S;
 
      double a, b;
    
      const JGivens q1(B.a00, B.a10);      // 1st Givens rotation (ch,0,0,sh)
 
      a = q1.geta();
      b = q1.getb();
 
      // apply B = Q' * B
 
      R.a00 =  a*B.a00 + b*B.a10;  R.a01 =  a*B.a01 + b*B.a11;  R.a02 =  a*B.a02 + b*B.a12;
      R.a10 = -b*B.a00 + a*B.a10;  R.a11 = -b*B.a01 + a*B.a11;  R.a12 = -b*B.a02 + a*B.a12;
      R.a20 =  B.a20;              R.a21 =  B.a21;              R.a22 =  B.a22;
  
      const JGivens q2(R.a00, R.a20);      // 2nd Givens rotation (ch,0,-sh,0)
 
      a = q2.geta();
      b = q2.getb();
 
      // apply B = Q' * B;
 
      B.a00 =  a*R.a00 + b*R.a20;  B.a01 =  a*R.a01 + b*R.a21;  B.a02 =  a*R.a02 + b*R.a22;
      B.a10 =  R.a10;              B.a11 =  R.a11;              B.a12 =  R.a12;
      B.a20 = -b*R.a00 + a*R.a20;  B.a21 = -b*R.a01 + a*R.a21;  B.a22 = -b*R.a02 + a*R.a22;
 
      const JGivens q3(B.a11, B.a21);      // 3rd Givens rotation (ch,sh,0,0)
 
      a = q3.geta();
      b = q3.getb();
 
      // R is now set to desired value
 
      R.a00 =  B.a00;              R.a01 =  B.a01;              R.a02 =  B.a02;
      R.a10 =  a*B.a10+b*B.a20;    R.a11 =  a*B.a11 + b*B.a21;  R.a12 =  a*B.a12 + b*B.a22;
      R.a20 = -b*B.a10+a*B.a20;    R.a21 = -b*B.a11 + a*B.a21;  R.a22 = -b*B.a12 + a*B.a22;
 
      // Q = Q1 * Q2 * Q3
 
      const double sp1 = -1.0 + 2.0*q1.sh*q1.sh;
      const double sp2 = -1.0 + 2.0*q2.sh*q2.sh;
      const double sp3 = -1.0 + 2.0*q3.sh*q3.sh;
 
      Q.a00 =  sp1*sp2; 
      Q.a01 =  4.0*q2.ch*q3.ch*sp1*q2.sh*q3.sh  +  2.0*q1.ch*q1.sh*sp3; 
      Q.a02 =  4.0*q1.ch*q3.ch*q1.sh*q3.sh      -  2.0*q2.ch*sp1*q2.sh*sp3;
 
      Q.a10 = -2.0*q1.ch*q1.sh*sp2;
      Q.a11 = -8.0*q1.ch*q2.ch*q3.ch*q1.sh*q2.sh*q3.sh  +  sp1*sp3; 
      Q.a12 = -2.0*q3.ch*q3.sh                          +  4.0*q1.sh*(q3.ch*q1.sh*q3.sh + q1.ch*q2.ch*q2.sh*sp3);
    
      Q.a20 =  2.0*q2.ch*q2.sh; 
      Q.a21 = -2.0*q3.ch*sp2*q3.sh; 
      Q.a22 =  sp2*sp3;
    }
 
 
    /**
     * Get inverted matrix.
     *
     * \param  precision   precision
     * \return             inverted matrix
     */
    const JMatrix3D& invert(const double precision = 1.0e-12) const
    {
      double w = fabs(S.a00);
 
      if (fabs(S.a11) > w) { w = fabs(S.a11); }
      if (fabs(S.a22) > w) { w = fabs(S.a22); }
 
      w *= precision;
 
      const double s00 = (fabs(S.a00) >= w ? 1.0 / S.a00 : 0.0);
      const double s11 = (fabs(S.a11) >= w ? 1.0 / S.a11 : 0.0);
      const double s22 = (fabs(S.a22) >= w ? 1.0 / S.a22 : 0.0);
 
      // B = V * S^-1
 
      B.a00 = V.a00*s00;  B.a01 = V.a01*s11;  B.a02 = V.a02*s22;
      B.a10 = V.a10*s00;  B.a11 = V.a11*s11;  B.a12 = V.a12*s22;
      B.a20 = V.a20*s00;  B.a21 = V.a21*s11;  B.a22 = V.a22*s22;
 
      U.transpose();
 
      // B = V * S^-1 * U^T
 
      B.mul(U);
 
      U.transpose();
 
      return B;
    }
 
    mutable JMatrix3D U;
    mutable JMatrix3D S;
    mutable JMatrix3D V;
 
  private:
 
    mutable JMatrix3S B;     // work space
 
    /**
     * Auxiliary class for quaternion computation.
     */
    struct JQuaternion :
      public std::array<double, 4>
    {
      /**
       * Constructor.
       *
       * Finds transformation that diagonalizes the given symmetric matrix by using Jacobi eigen analysis.
       *
       * \param  S                 matrix
       */
      JQuaternion(JMatrix3S& S)
      {
        // follow same indexing convention as GLM
 
        (*this)[3] = 1.0; 
        (*this)[0] = 0.0; 
        (*this)[1] = 0.0; 
        (*this)[2] = 0.0;
 
        for (int i = 0; i != 4; ++i) {
 
          // eliminate maximum off-diagonal element on every iteration,
          // while cycling over all 3 possible rotations in fixed order.
          // (p,q) = (0,1), (1,2), (0,2) still retains asymptotic convergence.
 
          conjugate(0, 1, 2, S);  // (p,q) = (0,1)
          conjugate(1, 2, 0, S);  // (p,q) = (1,2)
          conjugate(2, 0, 1, S);  // (p,q) = (0,2)
        }
      }
 
    private:
 
      /**
       * Get conjugate of symmetric matrix for given order.
       *
       * \param  x                 1st index
       * \param  y                 2nd index
       * \param  z                 3rd index
       * \param  S                 matrix
       */
      inline void conjugate(const int  x, 
                            const int  y,
                            const int  z,
                            JMatrix3S& S)
      {
        static const double GAMMA  =  sqrt(8.0) + 3.0;
        static const double CSTAR  =  cos(PI/8.0);
        static const double SSTAR  =  sin(PI/8.0);
 
        // approximate Givens quaternion
 
        double ch = 2.0*(S.a00 - S.a11);
        double sh = S.a10;
 
        if (GAMMA*sh*sh < ch*ch) {
 
          const double w = 1.0 / sqrt(ch*ch+sh*sh);
 
          ch *= w;
          sh *= w;
 
        } else {
 
          ch  = CSTAR;
          sh  = SSTAR;
        }
 
        const double scale =  ch*ch + sh*sh;
        const double a     = (ch+sh)*(ch-sh) / scale;
        const double b     = (2.0*sh*ch)     / scale;
 
        // make temporary copy of S
 
        double s00 = S.a00;
        double s10 = S.a10;  double s11 = S.a11;
        double s20 = S.a20;  double s21 = S.a21;  double s22 = S.a22;
 
        // perform conjugation S = Q' * S * Q
        // where Q is already implicitly solved from a and b
 
        S.a00 =  a*( a*s00 + b*s10) + b*( a*s10 + b*s11);
        S.a10 =  a*(-b*s00 + a*s10) + b*(-b*s10 + a*s11);
        S.a11 = -b*(-b*s00 + a*s10) + a*(-b*s10 + a*s11);
        S.a20 =  a*s20 + b*s21;
        S.a21 = -b*s20 + a*s21;
        S.a22 =  s22;
 
        // update cumulative rotation
 
        double tmp[] = {
          (*this)[0]*sh,
          (*this)[1]*sh,
          (*this)[2]*sh
        };
 
        sh *= (*this)[3];
 
        (*this)[0] *= ch;
        (*this)[1] *= ch;
        (*this)[2] *= ch;
        (*this)[3] *= ch;
 
        // (x,y,z) corresponds to {(0,1,2), (1,2,0), (2,0,1)}
        // for (p,q) = {(0,1), (1,2), (0,2)}, respectively.
 
        (*this)[z] += sh;
        (*this)[3] -= tmp[z]; // w
        (*this)[x] += tmp[y];
        (*this)[y] -= tmp[x];
 
        // re-arrange matrix for next iteration
 
        s00   = S.a11;
        s10   = S.a21; s11   = S.a22;
        s20   = S.a10; s21   = S.a20; s22   = S.a00;
 
        S.a00 = s00;
        S.a10 = s10;   S.a11 = s11;
        S.a20 = s20;   S.a21 = s21;   S.a22 = s22;
      }
    };
 
 
    /**
     * Get length squared.
     *
     * \param  x             x
     * \param  y             y
     * \param  z             z
     * \return               square of length
     */
    static inline double getLengthSquared(const double x, const double y, const double z)
    {
      return x*x + y*y + z*z;
    }
 
 
    /**
     * Givens quaternion.
     */
    struct JGivens {
 
      static constexpr double EPSILON  =  1.0e-6;
 
      /**
       * Constructor.
       *
       * \param  a            pivot point on diagonal
       * \param  b            lower triangular entry we want to annihilate
       */
      JGivens(const double a,
              const double b)
      {
        using namespace std;
 
        const double rho = sqrt(a*a + b*b);
 
        sh = rho > EPSILON ? b : 0.0;
        ch = fabs(a) + max(rho,EPSILON);
 
        if (a < 0.0) {
          swap(sh,ch);
        }
 
        const double w = 1.0 / sqrt(ch*ch + sh*sh);
 
        ch *= w;
        sh *= w;
      }
 
      inline double geta() const { return 1.0 - 2.0*sh*sh; }  //!< get new a value
      inline double getb() const { return 2.0*ch*sh; }        //!< get new b value
 
      double ch;    //!< cosine rotation angle
      double sh;    //!< sine   rotation angle
    };
  };
}
 
#endif