CGII/framework/include/cgv/math/inv.h

281 lines
7.6 KiB
C
Raw Normal View History

2018-05-17 14:01:02 +00:00
#pragma once
#include <cgv/math/lin_solve.h>
#include <cgv/math/fmat.h>
namespace cgv {
namespace math {
//returns the inverse of a diagonal matrix
template <typename T>
diag_mat<T> inv(const diag_mat<T>& m)
{
diag_mat<T> im(m.size());
for(unsigned i = 0; i < m.nrows();i++)
im(i) = (T)(1.0/m(i));
return im;
}
//returns the inverse of a lower triangular matrix
template <typename T>
low_tri_mat<T> inv(const low_tri_mat<T>& m)
{
unsigned N = m.nrows();
low_tri_mat<T> im(N,(T)0.0);
T sum;
for(unsigned k = 0; k < N;k++)
{
for(unsigned i = k; i < N;i++)
{
sum =0;
for(unsigned j = k;j < i; j++)
sum += m(i,j)*im(j,k);
assert(m(i,i) != 0);// not invertible
if(i == k)
im(i,k) = (1 - sum)/m(i,i);
else
im(i,k) = ( -sum)/m(i,i);
}
}
return im;
}
//returns the inverse of an upper triangular matrix
template <typename T>
up_tri_mat<T> inv(const up_tri_mat<T>& m)
{
up_tri_mat<T> im(m.nrows(),(T)0.0);
unsigned N = m.nrows();
vec<double> x;
vec<double> b(N);
x.resize(N);
T sum;
for(unsigned k = 0; k < N;k++)
{
for(int i = k; i >= 0;i--)
{
sum =0;
for(unsigned j = i+1;j <= k; j++)
sum += m(i,j)*im(j,k);
assert(m(i,i) != 0);
if(k == i)
im(i,k) = ((T)1.0 - sum)/m(i,i);
else
im(i,k) = ( - sum)/m(i,i);
}
}
return im;
}
/// return the inverse of a square matrix
template <typename T, cgv::type::uint32_type N>
fmat<T,N,N> inv(const fmat<T,N,N>& m)
{
2018-05-17 13:50:03 +00:00
mat<T> M(N,N,&m(0,0));
return fmat<T,N,N>(&(inv(M)(0,0)));
2018-05-17 14:01:02 +00:00
}
template <typename T>
mat<T> inv(const mat<T>& m)
{
assert(m.nrows() == m.ncols());
mat<T> im(m.nrows(),m.nrows());
solve(m,identity<T>(m.ncols()),im);
return im;
}
///compute inverse of 2x2 matrix
template <typename T>
mat<T> inv_22(const mat<T>& m)
{
mat<T> im(2, 2);
T t4 = 1.0 / (-m(0, 0) * m(1, 1) + m(0, 1) * m(1, 0));
im(0, 0) = -m(1, 1) * t4;
im(1, 0) = m(1, 0) * t4;
im(0, 1) = m(0, 1) * t4;
im(1, 1) = -m(0, 0) * t4;
return im;
}
///compute inverse of 2x2 matrix
template <typename T>
fmat<T,2,2> inv_22(const fmat<T,3,3>& m)
{
fmat<T,2,2> im;
T t4 = 1.0 / (-m(0, 0) * m(1, 1) + m(0, 1) * m(1, 0));
im(0, 0) = -m(1, 1) * t4;
im(1, 0) = m(1, 0) * t4;
im(0, 1) = m(0, 1) * t4;
im(1, 1) = -m(0, 0) * t4;
return im;
}
///compute inverse of 3x3 matrix
template <typename T>
mat<T> inv_33(const mat<T>& m)
{
mat<T> im(3, 3);
T t4 = m(2, 0) * m(0, 1);
T t6 = m(2, 0) * m(0, 2);
T t8 = m(1, 0) * m(0, 1);
T t10 = m(1, 0) * m(0, 2);
T t12 = m(0, 0) * m(1, 1);
T t14 = m(0, 0) * m(1, 2);
T t17 = (T)1.0 / (t4 * m(1, 2) - t6 * m(1, 1) - t8 * m(2, 2) + t10 * m(2, 1) + t12 * m(2, 2) - t14 * m(2, 1));
im(0, 0) = (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) * t17;
im(0, 1) = -(m(0, 1) * m(2, 2) - m(0, 2) * m(2, 1)) * t17;
im(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * t17;
im(1, 0) = -(-m(2, 0) * m(1, 2) + m(1, 0) * m(2, 2)) * t17;
im(1, 1) = (-t6 + m(0, 0) * m(2, 2)) * t17;
im(1, 2) = -(-t10 + t14) * t17;
im(2, 0) = (-m(2, 0) * m(1, 1) + m(1, 0) * m(2, 1)) * t17;
im(2, 1) = -(-t4 + m(0, 0) * m(2, 1)) * t17;
im(2, 2) = (-t8 + t12) * t17;
return im;
}
///compute inverse of 3x3 matrix
template <typename T>
fmat<T,3,3> inv_33(const fmat<T,3,3>& m)
{
fmat<T,3,3> im;
T t4 = m(2, 0) * m(0, 1);
T t6 = m(2, 0) * m(0, 2);
T t8 = m(1, 0) * m(0, 1);
T t10 = m(1, 0) * m(0, 2);
T t12 = m(0, 0) * m(1, 1);
T t14 = m(0, 0) * m(1, 2);
T t17 = (T)1.0 / (t4 * m(1, 2) - t6 * m(1, 1) - t8 * m(2, 2) + t10 * m(2, 1) + t12 * m(2, 2) - t14 * m(2, 1));
im(0, 0) = (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) * t17;
im(0, 1) = -(m(0, 1) * m(2, 2) - m(0, 2) * m(2, 1)) * t17;
im(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * t17;
im(1, 0) = -(-m(2, 0) * m(1, 2) + m(1, 0) * m(2, 2)) * t17;
im(1, 1) = (-t6 + m(0, 0) * m(2, 2)) * t17;
im(1, 2) = -(-t10 + t14) * t17;
im(2, 0) = (-m(2, 0) * m(1, 1) + m(1, 0) * m(2, 1)) * t17;
im(2, 1) = -(-t4 + m(0, 0) * m(2, 1)) * t17;
im(2, 2) = (-t8 + t12) * t17;
return im;
}
//compute inverse of 4x4 matrix
template <typename T>
mat<T> inv_44(const mat<T>& m)
{
mat<T> im(4,4);
2018-05-17 13:50:03 +00:00
T t1 = m(3,3) * m(1,1);
T t3 = m(3,2) * m(1,1);
T t7 = m(3,1) * m(1,2);
T t9 = m(3,1) * m(1,3);
T t11 = m(3,2) * m(2,1);
T t14 = m(0,0) * m(1,1);
T t19 = m(0,0) * m(3,3);
T t20 = m(1,2) * m(2,1);
T t22 = m(3,1) * m(0,0);
T t23 = m(1,2) * m(2,3);
T t25 = m(1,3) * m(2,2);
T t27 = m(3,2) * m(0,0);
T t28 = m(2,1) * m(1,3);
T t30 = m(1,1) * m(3,0);
T t31 = m(0,3) * m(2,2);
T t33 = m(2,0) * m(0,3);
T t35 = m(0,2) * m(2,3);
T t37 = m(2,0) * m(0,2);
T t39 = m(3,0) * m(0,2);
T t41 = m(3,1) * m(1,0);
T t43 = t14 * m(3,3) * m(2,2) - t14 * m(3,2) * m(2,3) - t19 * t20 +
t22 * t23 - t22 * t25 + t27 * t28 - t30 * t31 + t3 * t33 + t30 * t35
- t1 * t37 - t39 * t28 - t41 * t35;
T t45 = m(3,0) * m(0,1);
T t47 = m(1,0) * m(3,3);
T t50 = m(2,0) * m(3,3);
T t51 = m(0,1) * m(1,2);
T t53 = m(3,2) * m(1,0);
T t56 = m(0,2) * m(2,1);
T t58 = m(3,0) * m(0,3);
T t63 = m(3,2) * m(2,0);
T t64 = m(0,1) * m(1,3);
T t66 = m(1,0) * m(0,3);
T t68 = -t7 * t33 - t45 * t23 - t47 * m(0,1) * m(2,2) + t50 * t51 + t53 *
m(0,1) * m(2,3) + t47 * t56 + t58 * t20 + t9 * t37 + t41 * t31 + t45 *
t25 - t63 * t64 - t11 * t66;
T t70 = (T)1.0 / (t43 + t68);
T t72 = m(3,3) * m(0,1);
T t74 = m(3,2) * m(0,1);
T t78 = m(0,3) * m(3,1);
T t108 = m(2,0) * m(1,2);
T t111 = m(1,3) * m(3,0);
T t131 = m(0,0) * m(1,2);
T t135 = m(1,0) * m(0,2);
T t148 = m(3,1) * m(2,0);
T t150 = m(1,0) * m(2,1);
T t156 = m(0,0) * m(2,1);
T t158 = m(0,0) * m(2,3);
T t161 = m(2,0) * m(0,1);
im(0,0) = (t1 * m(2,2) - t3 * m(2,3) - m(3,3) * m(1,2) * m(2,1) +
t7 * m(2,3) - t9 * m(2,2) + t11 * m(1,3)) * t70;
im(0,1) = -(t72 * m(2,2) - t74 * m(2,3) - t56 * m(3,3) + t35 * m(3,1) -
t78 * m(2,2) + m(0,3) * m(3,2) * m(2,1)) * t70;
im(0,2) = (t72 * m(1,2) - t74 * m(1,3) - t1 * m(0,2) + m(0,2) * m(3,1) *
m(1,3) + t3 * m(0,3) - t78 * m(1,2)) * t70;
im(0,3) = -(t51 * m(2,3) - t64 * m(2,2) - m(1,1) * m(0,2) * m(2,3) + t56 *
m(1,3) + m(1,1) * m(0,3) * m(2,2) - m(0,3) * m(1,2) * m(2,1)) * t70;
im(1,0) = -(t47 * m(2,2) - t53 * m(2,3) + m(1,3) * m(3,2) * m(2,0) - t108 *
m(3,3) + t23 * m(3,0) - t111 * m(2,2)) * t70;
im(1,1) = (t19 * m(2,2) - t27 * m(2,3) - t58 * m(2,2) + t63 * m(0,3) + t39 *
m(2,3) - t50 * m(0,2)) * t70;
im(1,2) = -(t19 * m(1,2) - t27 * m(1,3) - t47 * m(0,2) - t58 * m(1,2) + t111 *
m(0,2) + t66 * m(3,2)) * t70;
im(1,3) = (t131 * m(2,3) - m(0,0) * m(1,3) * m(2,2) - t135 * m(2,3) - t108 *
m(0,3) + m(1,3) * m(2,0) * m(0,2) + t66 * m(2,2)) * t70;
im(2,0) = (-m(1,1) * m(2,0) * m(3,3) + m(1,1) * m(2,3) * m(3,0) - t28 *
m(3,0) + t148 * m(1,3) + t150 * m(3,3) - m(2,3) * m(3,1) * m(1,0)) * t70;
im(2,1) = -(t156 * m(3,3) - t158 * m(3,1) + t33 * m(3,1) - t161 * m(3,3) - m(2,1) *
m(3,0) * m(0,3) + m(2,3) * m(3,0) * m(0,1)) * t70;
im(2,2) = (t19 * m(1,1) - t22 * m(1,3) - t58 * m(1,1) - t47 * m(0,1) + t41 *
m(0,3) + t45 * m(1,3)) * t70;
im(2,3) = -(-m(2,3) * m(1,0) * m(0,1) + t158 * m(1,1) - t33 * m(1,1) + t161 *
m(1,3) - t156 * m(1,3) + t150 * m(0,3)) * t70;
im(3,0) = -(-t3 * m(2,0) + t30 * m(2,2) + t11 * m(1,0) - m(3,0) * m(1,2) *
m(2,1) - t41 * m(2,2) + t7 * m(2,0)) * t70;
im(3,1) = (-t22 * m(2,2) + t27 * m(2,1) - t39 * m(2,1) + t148 * m(0,2) + t45 *
m(2,2) - t63 * m(0,1)) * t70;
im(3,2) = -(-t53 * m(0,1) + t27 * m(1,1) - t39 * m(1,1) + t41 * m(0,2) - t22 *
m(1,2) + t45 * m(1,2)) * t70;
im(3,3) = t70 * (t161 * m(1,2) - t37 * m(1,1) - m(1,0) * m(0,1) * m(2,2) + t135 *
m(2,1) + t14 * m(2,2) - t131 * m(2,1));
2018-05-17 14:01:02 +00:00
return im;
}
inline const perm_mat inv(const perm_mat &p)
{
perm_mat r=p;
r.transpose();
return r;
}
}
}