71 lines
1.2 KiB
C++
71 lines
1.2 KiB
C++
#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 {
|
|
|
|
|
|
|
|
|
|
///polar decomposition of matrix c=r*a
|
|
///r orthonormal matrix
|
|
///a positive semi-definite matrix
|
|
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
|
|
*/
|
|
}
|
|
|
|
|
|
/// extract axis and angle from rotation matrix
|
|
///returns true if successful problematic cases are angle == 0° and angle == 180°
|
|
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;
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|