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