polar.h

#pragma once
 
#include <cgv/math/mat.h>
#include <cgv/math/inv.h>
#include <cgv/math/constants.h>
#include <limits>
#include <algorithm>
 
 
namespace cgv {
namespace math {
 
 
 
 
template <typename T>
void polar(const mat<T> &c, mat<T> &r, mat<T> &a,int num_iter=15)
{
    r = c;
    for(int i =0; i < num_iter;i++)
        r = ((T)0.5)*(r+transpose(inv(r)));
 
    a = inv(r)*c;
    /*
    * Alternative way using svd:
    * c = u*d*v^t
    * r = u*v^t,  a=v*d*v^t 
    */
}
 
 
template <typename T>
bool decompose_rotation(const cgv::math::mat<T>& R,
        cgv::math::vec<T>& axis,
        T& angle)
{
    assert(R.nrows() == 3 && R.ncols() == 3);
    
    
    mat<T> A = (T)0.5*(R - transpose(R));
    mat<T> S = (T)0.5*(R + transpose(R));
 
    
    T abssina = (T)sqrt(0.5*(double)(A.frobenius_norm()*A.frobenius_norm()));
    T cosa = 0.5*(trace(S)-1.0);
    
        
 
    angle =(T)(cgv::math::rad2deg(asin(abssina))));
    if(cosa < 0)
        angle = (T)180.0-angle;
        
    if(angle == 0 || angle == 180.0)
         return false;
    
    axis = (T)1.0/abssina*cgv::math::vec<T>(A(2,1),A(0,2),A(1,0));
 
    return true;
}
 
 
 
 
}
 
}