text: /** file:        rodirgues.c
   ** author:      Andrea Vedaldi
   ** description: Rodrigues formulas
   **/
  
  /* AUTORIGHTS
  Copyright 2007 (c) Andrea Vedaldi and Brian Fulkerson
  
  This file is part of VLFeat, available in the terms of the GNU
  General Public License version 2.
  */
  
  #include "generic.h"
  #include "mathop.h"
  #include "rodrigues.h"
  
  #include <math.h>
  
  /** @brief Rodrigues' formula
   **
   ** @param R_pt  3x3 matrix - array of 9 double (in) .
   ** @param dR_pt 9x3 matrix - array of 27 double (in).
   ** @param om_pt 3   vector - array of 3 dobule (out).
   **/
  VL_EXPORT
  void 
  vl_rodrigues(double* R_pt, double* dR_pt, const double* om_pt)
  {
    /*
      Let
       
         th = |om|,  r=w/th,  
         sth=sin(th),  cth=cos(th), 
         ^om = hat(om)
  
      Then the rodrigues formula is an expansion of the exponential
      function:
       
       rodrigues(om) = exp ^om = I + ^r sth + ^r^2 (1 - cth).
  
      The derivative can be computed by elementary means and
      results:
  
      d(vec rodrigues(om))    sth  d ^r    1 - cth  d (^r)^2
      -------------------- =  ---- ----- + -------  -------- +
            d om^T             th  d r^T     th      d r^T
                    
                            sth                     1 - cth
            + vec^r (cth - -----) + vec^r^2 (sth - 2-------)r^T
                            th                         th
    */
  
  #define OM(i)   om_pt[(i)]
  #define R(i,j)  R_pt[(i)+3*(j)]
  #define DR(i,j) dR_pt[(i)+9*(j)]
  
    const double small = 1e-6 ;
  
    double th = sqrt( OM(0)*OM(0) + 
                      OM(1)*OM(1) +
                      OM(2)*OM(2) ) ;
    
    if( th < small ) {
      R(0,0) = 1.0 ; R(0,1) = 0.0 ; R(0,2) = 0.0 ;
      R(1,0) = 0.0 ; R(1,1) = 1.0 ; R(1,2) = 0.0 ;
      R(2,0) = 0.0 ; R(2,1) = 0.0 ; R(2,2) = 1.0 ;
      
      if(dR_pt) {
        DR(0,0) = 0  ; DR(0,1) = 0   ; DR(0,2) = 0 ;
        DR(1,0) = 0  ; DR(1,1) = 0   ; DR(1,2) = 1 ;
        DR(2,0) = 0  ; DR(2,1) = -1  ; DR(2,2) = 0 ;
  
        DR(3,0) = 0  ; DR(3,1) = 0   ; DR(3,2) = -1 ;
        DR(4,0) = 0  ; DR(4,1) = 0   ; DR(4,2) = 0 ;
        DR(5,0) = 1  ; DR(5,1) = 0   ; DR(5,2) = 0 ;
  
        DR(6,0) = 0  ; DR(6,1) = 1   ; DR(6,2) = 0 ;
        DR(7,0) = -1 ; DR(7,1) = 0   ; DR(7,2) = 0 ;
        DR(8,0) = 0  ; DR(8,1) = 0   ; DR(8,2) = 0 ;
      }
      return ;
    }
  
    {
      double x = OM(0) / th ;
      double y = OM(1) / th ;
      double z = OM(2) / th ;
      
      double xx = x*x ;
      double xy = x*y ;
      double xz = x*z ;
      double yy = y*y ;
      double yz = y*z ;
      double zz = z*z ;
      
      const double yx = xy ;
      const double zx = xz ;
      const double zy = yz ;
      
      double sth  = sin(th) ;
      double cth  = cos(th) ;
      double mcth = 1.0 - cth ; 
      
      R(0,0) = 1          - mcth * (yy+zz) ;
      R(1,0) =     sth*z  + mcth * xy ;
      R(2,0) =   - sth*y  + mcth * xz ;
      
      R(0,1) =   - sth*z  + mcth * yx ;
      R(1,1) = 1          - mcth * (zz+xx) ;
      R(2,1) =     sth*x  + mcth * yz ;
      
      R(0,2) =     sth*y  + mcth * xz ;
      R(1,2) =   - sth*x  + mcth * yz ;
      R(2,2) = 1          - mcth * (xx+yy) ;
      
      if(dR_pt) {
        double a =  sth / th ;
        double b = mcth / th ;
        double c = cth - a ;
        double d = sth - 2*b ;
        
        DR(0,0) =                         - d * (yy+zz) * x ;
        DR(1,0) =        b*y   + c * zx   + d * xy      * x ;
        DR(2,0) =        b*z   - c * yx   + d * xz      * x ;
        
        DR(3,0) =        b*y   - c * zx   + d * xy      * x ;
        DR(4,0) =     -2*b*x              - d * (zz+xx) * x ;
        DR(5,0) =  a           + c * xx   + d * yz      * x ;
        
        DR(6,0) =        b*z   + c * yx   + d * zx      * x ;
        DR(7,0) = -a           - c * xx   + d * zy      * x ;
        DR(8,0) =     -2*b*x              - d * (yy+xx) * x ;
        
        DR(0,1) =     -2*b*y              - d * (yy+zz) * y ;
        DR(1,1) =        b*x   + c * zy   + d * xy      * y ;
        DR(2,1) = -a           - c * yy   + d * xz      * y ;
        
        DR(3,1) =        b*x   - c * zy   + d * xy      * y ;
        DR(4,1) =                         - d * (zz+xx) * y ;
        DR(5,1) =        b*z   + c * xy   + d * yz      * y ;
        
        DR(6,1) = a            + c * yy   + d * zx      * y ;
        DR(7,1) =        b*z   - c * xy   + d * zy      * y ;
        DR(8,1) =     -2*b*y              - d * (yy+xx) * y ;
        
        DR(0,2) =     -2*b*z              - d * (yy+zz) * z ;
        DR(1,2) =  a           + c * zz   + d * xy      * z ;
        DR(2,2) =        b*x   - c * yz   + d * xz      * z ;
        
        DR(3,2) =  -a          - c * zz   + d * xy      * z ;
        DR(4,2) =     -2*b*z              - d * (zz+xx) * z ;
        DR(5,2) =        b*y   + c * xz   + d * yz      * z ;
        
        DR(6,2) =        b*x   + c * yz   + d * zx      * z ;
        DR(7,2) =        b*y   - c * xz   + d * zy      * z ;
        DR(8,2) =                         - d * (yy+xx) * z ;
      }
    }
  
  #undef OM
  #undef R
  #undef DR
  
  }
  
  /** @brief Inverse Rodrigues formula
   *
   ** @param om_pt  3    vector - array of 3   dobule (out).
   ** @param dom_pt 3x9  matrix - array of 3x9 dobule (out).
   ** @param R_pt   3x3  matrix - array of 9   double (in).
   **
   ** This function computes the Rodrigues formula of the argument @a
   ** om_pt. The result is stored int the matrix @a R_pt. If @a dR_pt is
   ** non null, then the derivative of the Rodrigues formula is computed
   ** and stored into the matrix @a dR_pt.
   **/
  
  VL_EXPORT
  void vl_irodrigues(double* om_pt, double* dom_pt, const double* R_pt)
  {
    /*
                      tr R - 1          1    [ R32 - R23 ]
        th = cos^{-1} --------,  r =  ------ [ R13 - R31 ], w = th r.
                          2           2 sth  [ R12 - R21 ]
       
        sth = sin(th)
       
         dw    th*cth-sth      dw     th   [di3 dj2 - di2 dj3]
        ---- = ---------- r,  ---- = ----- [di1 dj3 - di3 dj1].
        dRii     2 sth^2      dRij   2 sth [di1 dj2 - di2 dj1]
       
        trace(A) < -1 only for small num. errors.
    */
  
  #define OM(i)    om_pt[(i)]
  #define DOM(i,j) dom_pt[(i)+3*(j)]
  #define R(i,j)   R_pt[(i)+3*(j)]
  #define W(i,j)   W_pt[(i)+3*(j)]
  
    const double small = 1e-6 ;
  
    double th = acos
      (0.5*(VL_MAX(R(0,0)+R(1,1)+R(2,2),-1.0) - 1.0)) ;
    
    double sth = sin(th) ;
    double cth = cos(th) ;
  
    if(fabs(sth) < small && cth < 0) {
      /*
        we have this singularity when the rotation  is about pi (or -pi)
        we use the fact that in this case
  
        hat( sqrt(1-cth) * r )^2 = W = (0.5*(R+R') - eye(3)) 
        
        which gives
  
        (1-cth) rx^2 = 0.5 * (W(1,1)-W(2,2)-W(3,3))
        (1-cth) ry^2 = 0.5 * (W(2,2)-W(3,3)-W(1,1)) 
        (1-cth) rz^2 = 0.5 * (W(3,3)-W(1,1)-W(2,2)) 
      */
      
      double W_pt [9], x, y, z ;
      W_pt[0] = 0.5*( R(0,0) + R(0,0) ) - 1.0 ;    
      W_pt[0] = 0.5*( R(1,0) + R(0,1) ) ;
      W_pt[0] = 0.5*( R(2,0) + R(0,2) );
      
      W_pt[0] = 0.5*( R(0,1) + R(1,0) );
      W_pt[0] = 0.5*( R(1,1) + R(1,1) ) - 1.0;
      W_pt[0] = 0.5*( R(2,1) + R(1,2) );
    
      W_pt[0] =  0.5*( R(0,2) + R(2,0) ) ;
      W_pt[0] =  0.5*( R(1,2) + R(2,1) ) ;
      W_pt[0] =  0.5*( R(2,2) + R(2,2) ) - 1.0 ;
  
      /* these are only absolute values */
      x = sqrt( 0.5 * (W(0,0)-W(1,1)-W(2,2)) ) ;
      y = sqrt( 0.5 * (W(1,1)-W(2,2)-W(0,0)) ) ;
      z = sqrt( 0.5 * (W(2,2)-W(0,0)-W(1,1)) ) ;
      
      /* set the biggest component to + and use the element of the
      ** matrix W to determine the sign of the other components
      ** then the solution is either (x,y,z) or its opposite */
      if( x >= y && x >= z ) {
        y = (W(1,0) >=0) ? y : -y ;
        z = (W(2,0) >=0) ? z : -z ;      
      } else if( y >= x && y >= z ) {
        z = (W(2,1) >=0) ? z : -z ;
        x = (W(1,0) >=0) ? x : -x ;
      } else {
        x = (W(2,0) >=0) ? x : -x ;
        y = (W(2,1) >=0) ? y : -y ;
      }
  
      /* we are left to chose between (x,y,z) and (-x,-y,-z)
      ** unfortunately we cannot (as the rotation is too close to pi) and
      ** we just keep what we have. */
      {
        double scale = th / sqrt( 1 - cth ) ; 
        OM(0) = scale * x ;
        OM(1) = scale * y ;
        OM(2) = scale * z ;
        
        if( dom_pt ) {
          int k ;
          for(k=0; k<3*9; ++k)
            dom_pt [k] = VL_NAN_D ;
        }
        return ;
      }
  
    } else {
      double a = (fabs(sth) < small) ? 1 : th/sin(th) ;
      double b ;
      OM(0) = 0.5*a*(R(2,1) - R(1,2)) ;
      OM(1) = 0.5*a*(R(0,2) - R(2,0)) ;
      OM(2) = 0.5*a*(R(1,0) - R(0,1)) ;
      
      if( dom_pt ) {
        if( fabs(sth) < small ) {
          a = 0.5 ;
          b = 0 ;
        } else {
          a = th/(2*sth) ;
          b = (th*cth - sth)/(2*sth*sth)/th ;
        }
  
        DOM(0,0) = b*OM(0) ;
        DOM(1,0) = b*OM(1) ;
        DOM(2,0) = b*OM(2) ;
          
        DOM(0,1) = 0 ;
        DOM(1,1) = 0 ;
        DOM(2,1) = a ;
  
        DOM(0,2) = 0 ;
        DOM(1,2) = -a ;
        DOM(2,2) = 0 ;
  
        DOM(0,3) = 0 ;
        DOM(1,3) = 0 ;
        DOM(2,3) = -a ;
        
        DOM(0,4) = b*OM(0) ;
        DOM(1,4) = b*OM(1) ;
        DOM(2,4) = b*OM(2) ;
          
        DOM(0,5) = a ;
        DOM(1,5) = 0 ;
        DOM(2,5) = 0 ;
  
        DOM(0,6) = 0 ;
        DOM(1,6) = a ;
        DOM(2,6) = 0 ;
  
        DOM(0,7) = -a ;
        DOM(1,7) = 0 ;
        DOM(2,7) = 0 ;
  
        DOM(0,8) = b*OM(0) ;
        DOM(1,8) = b*OM(1) ;
        DOM(2,8) = b*OM(2) ;
      }
    }
  
  #undef OM
  #undef DOM
  #undef R
  #undef W
  }
  