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)(asin(abssina)*180.0/PI);
        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;
}
 
 
 
 
}
 
}