Milena (Olena)
User documentation 2.0a Id
|
00001 // Copyright (C) 2008, 2009, 2011 EPITA Research and Development 00002 // Laboratory (LRDE) 00003 // 00004 // This file is part of Olena. 00005 // 00006 // Olena is free software: you can redistribute it and/or modify it under 00007 // the terms of the GNU General Public License as published by the Free 00008 // Software Foundation, version 2 of the License. 00009 // 00010 // Olena is distributed in the hope that it will be useful, 00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00013 // General Public License for more details. 00014 // 00015 // You should have received a copy of the GNU General Public License 00016 // along with Olena. If not, see <http://www.gnu.org/licenses/>. 00017 // 00018 // As a special exception, you may use this file as part of a free 00019 // software project without restriction. Specifically, if other files 00020 // instantiate templates or use macros or inline functions from this 00021 // file, or you compile this file and link it with other files to produce 00022 // an executable, this file does not by itself cause the resulting 00023 // executable to be covered by the GNU General Public License. This 00024 // exception does not however invalidate any other reasons why the 00025 // executable file might be covered by the GNU General Public License. 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 // from num. rec. in C 00038 // FIXME: what about numrec licence? 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 // FIXME: nD ? 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 { // 1e-12 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)) // unsafe? 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 // DO *NOT* remove these semicolons!! 00138 // rotateJacobi is a macro with 4 function calls. 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 } // end of namespace math 00170 00171 } // end of namespace mln 00172 00173 00174 #endif // ! MLN_MATH_JACOBI_HH