00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef MLN_MATH_JACOBI_HH
00027 # define MLN_MATH_JACOBI_HH
00028
00030
00031 # include <cmath>
00032
00033 # include <mln/algebra/quat.hh>
00034 # include <mln/algebra/mat.hh>
00035
00036
00037
00038
00039
00040 namespace mln
00041 {
00042
00043 namespace math
00044 {
00045
00046 algebra::quat
00047 jacobi(algebra::mat<4u,4u,float> a);
00048
00049
00050 # ifndef MLN_INCLUDE_ONLY
00051
00052
00053 #define rotateJacobi(a,i,j,k,l) g=a(i,j);h=a(k,l);a(i,j)=g-s*(h+g*tau); \
00054 a(k,l)=h+s*(g-h*tau);
00055
00056
00057 algebra::quat
00058 jacobi(algebra::mat<4u,4u,float> a)
00059 {
00060 float dd, d[4];
00061 algebra::mat < 4, 4, float >v(literal::zero);
00062 int j, iq, ip, i = 0;
00063 float tresh, theta, tau, t, sm, s, h, g, c, b[4], z[4];
00064 for (ip = 0; ip < 4; ip++)
00065 {
00066 for (iq = 0; iq < 4; iq++)
00067 v(ip, iq) = 0.0f;
00068 v(ip, ip) = 1.0f;
00069 }
00070 for (ip = 0; ip < 4; ip++)
00071 {
00072 b[ip] = d[ip] = a(ip, ip);
00073 z[ip] = 0.0f;
00074 }
00075 while (1)
00076 {
00077 sm = 0.0f;
00078 for (ip = 0; ip < 3; ip++)
00079 {
00080 for (iq = ip + 1; iq < 4; iq++)
00081 sm += std::fabs(a(ip, iq));
00082 }
00083 if (sm < 1e-12f)
00084 {
00085 dd = d[0];
00086 iq = 0;
00087 for (ip = 1; ip < 4; ip++)
00088 if (d[ip] > dd)
00089 {
00090 iq = ip;
00091 dd = d[ip];
00092 }
00093 algebra::quat q(v(0, iq), v(1, iq), v(2, iq), v(3, iq));
00094 q.set_unit();
00095 return q;
00096 }
00097 if (i < 4)
00098 {
00099 i++;
00100 tresh = 0.0125f * sm;
00101 }
00102 else
00103 tresh = 0.0;
00104 for (ip = 0; ip < 3; ip++)
00105 {
00106 for (iq = ip + 1; iq < 4; iq++)
00107 {
00108 g = 100.0f * std::fabs(a(ip, iq));
00109 if (i > 4 && (float)(std::fabs(d[ip]) + g) == (float)std::fabs(d[ip])
00110 && (float)(std::fabs(d[iq]) + g) == (float)std::fabs(d[iq]))
00111 a(ip, iq) = 0.0f;
00112 else if (std::fabs(a(ip, iq)) > tresh)
00113 {
00114 h = d[iq] - d[ip];
00115 if ((float)(std::fabs(h) + g) == (float)std::fabs(h))
00116 t = (a(ip, iq)) / h;
00117 else
00118 {
00119 theta = 0.5f * h / (a(ip, iq));
00120 t = 1.0f / (std::fabs(theta) + std::sqrt(1.0f +
00121 theta * theta));
00122 if (theta < 0.0f)
00123 t = -t;
00124 }
00125 c = 1.0f / std::sqrt(1 + t * t);
00126 s = t * c;
00127 tau = s / (1.0f + c);
00128 h = t * a(ip, iq);
00129 z[ip] -= h;
00130 z[iq] += h;
00131 d[ip] -= h;
00132 d[iq] += h;
00133 a(ip, iq) = 0.0;
00134
00135
00136
00137 for (j = 0; j <= ip - 1; j++)
00138 {
00139 rotateJacobi(a, j, ip, j, iq);
00140 }
00141 for (j = ip + 1; j <= iq - 1; j++)
00142 {
00143 rotateJacobi(a, ip, j, j, iq);
00144 }
00145 for (j = iq + 1; j < 4; j++)
00146 {
00147 rotateJacobi(a, ip, j, iq, j);
00148 }
00149 for (j = 0; j < 4; j++)
00150 {
00151 rotateJacobi(v, j, ip, j, iq);
00152 }
00153 }
00154 }
00155 }
00156 for (ip = 0; ip < 4; ip++)
00157 {
00158 b[ip] += z[ip];
00159 d[ip] = b[ip];
00160 z[ip] = 0.0f;
00161 }
00162 }
00163 }
00164
00165 # endif // ! MLN_INCLUDE_ONLY
00166
00167 }
00168
00169 }
00170
00171
00172 #endif // ! MLN_MATH_JACOBI_HH