27 #ifndef MLN_MATH_JACOBI_HH
28 # define MLN_MATH_JACOBI_HH
34 # include <mln/algebra/quat.hh>
35 # include <mln/algebra/mat.hh>
48 jacobi(algebra::mat<4u,4u,float> a);
51 # ifndef MLN_INCLUDE_ONLY
54 #define rotateJacobi(a,i,j,k,l) g=a(i,j);h=a(k,l);a(i,j)=g-s*(h+g*tau); \
60 jacobi(algebra::mat<4u,4u,float> a)
65 float tresh, theta, tau, t, sm, s, h, g, c, b[4], z[4];
66 for (ip = 0; ip < 4; ip++)
68 for (iq = 0; iq < 4; iq++)
72 for (ip = 0; ip < 4; ip++)
74 b[ip] = d[ip] = a(ip, ip);
80 for (ip = 0; ip < 3; ip++)
82 for (iq = ip + 1; iq < 4; iq++)
83 sm += std::fabs(a(ip, iq));
89 for (ip = 1; ip < 4; ip++)
95 algebra::quat q(v(0, iq), v(1, iq), v(2, iq), v(3, iq));
102 tresh = 0.0125f * sm;
106 for (ip = 0; ip < 3; ip++)
108 for (iq = ip + 1; iq < 4; iq++)
110 g = 100.0f * std::fabs(a(ip, iq));
111 if (i > 4 && (
float)(std::fabs(d[ip]) + g) == (
float)std::fabs(d[ip])
112 && (
float)(std::fabs(d[iq]) + g) == (
float)std::fabs(d[iq]))
114 else if (std::fabs(a(ip, iq)) > tresh)
117 if ((
float)(std::fabs(h) + g) == (
float)std::fabs(h))
121 theta = 0.5f * h / (a(ip, iq));
122 t = 1.0f / (std::fabs(theta) + std::sqrt(1.0f +
127 c = 1.0f / std::sqrt(1 + t * t);
129 tau = s / (1.0f + c);
139 for (j = 0; j <= ip - 1; j++)
141 rotateJacobi(a, j, ip, j, iq);
143 for (j = ip + 1; j <= iq - 1; j++)
145 rotateJacobi(a, ip, j, j, iq);
147 for (j = iq + 1; j < 4; j++)
149 rotateJacobi(a, ip, j, iq, j);
151 for (j = 0; j < 4; j++)
153 rotateJacobi(v, j, ip, j, iq);
158 for (ip = 0; ip < 4; ip++)
167 # endif // ! MLN_INCLUDE_ONLY
174 #endif // ! MLN_MATH_JACOBI_HH