Milena (Olena)  User documentation 2.0a Id
jacobi.hh
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
 All Classes Namespaces Functions Variables Typedefs Enumerator