Milena (Olena)  User documentation 2.0a Id
 All Classes Namespaces Functions Variables Typedefs Enumerator Groups Pages
jacobi.hh
1 // Copyright (C) 2008, 2009, 2011 EPITA Research and Development
2 // Laboratory (LRDE)
3 //
4 // This file is part of Olena.
5 //
6 // Olena is free software: you can redistribute it and/or modify it under
7 // the terms of the GNU General Public License as published by the Free
8 // Software Foundation, version 2 of the License.
9 //
10 // Olena is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Public License for more details.
14 //
15 // You should have received a copy of the GNU General Public License
16 // along with Olena. If not, see <http://www.gnu.org/licenses/>.
17 //
18 // As a special exception, you may use this file as part of a free
19 // software project without restriction. Specifically, if other files
20 // instantiate templates or use macros or inline functions from this
21 // file, or you compile this file and link it with other files to produce
22 // an executable, this file does not by itself cause the resulting
23 // executable to be covered by the GNU General Public License. This
24 // exception does not however invalidate any other reasons why the
25 // executable file might be covered by the GNU General Public License.
26 
27 #ifndef MLN_MATH_JACOBI_HH
28 # define MLN_MATH_JACOBI_HH
29 
31 
32 # include <cmath>
33 
34 # include <mln/algebra/quat.hh>
35 # include <mln/algebra/mat.hh>
36 
37 // from num. rec. in C
38 // FIXME: what about numrec licence?
39 
40 
41 namespace mln
42 {
43 
44  namespace math
45  {
46 
47  algebra::quat
48  jacobi(algebra::mat<4u,4u,float> a);
49 
50 
51 # ifndef MLN_INCLUDE_ONLY
52 
53  // FIXME: nD ?
54 #define rotateJacobi(a,i,j,k,l) g=a(i,j);h=a(k,l);a(i,j)=g-s*(h+g*tau); \
55  a(k,l)=h+s*(g-h*tau);
56 
57 
58  inline
59  algebra::quat
60  jacobi(algebra::mat<4u,4u,float> a)
61  {
62  float dd, d[4];
63  algebra::mat < 4, 4, float >v(literal::zero);
64  int j, iq, ip, i = 0;
65  float tresh, theta, tau, t, sm, s, h, g, c, b[4], z[4];
66  for (ip = 0; ip < 4; ip++)
67  {
68  for (iq = 0; iq < 4; iq++)
69  v(ip, iq) = 0.0f;
70  v(ip, ip) = 1.0f;
71  }
72  for (ip = 0; ip < 4; ip++)
73  {
74  b[ip] = d[ip] = a(ip, ip);
75  z[ip] = 0.0f;
76  }
77  while (1)
78  {
79  sm = 0.0f;
80  for (ip = 0; ip < 3; ip++)
81  {
82  for (iq = ip + 1; iq < 4; iq++)
83  sm += std::fabs(a(ip, iq));
84  }
85  if (sm < 1e-12f)
86  { // 1e-12
87  dd = d[0];
88  iq = 0;
89  for (ip = 1; ip < 4; ip++)
90  if (d[ip] > dd)
91  {
92  iq = ip;
93  dd = d[ip];
94  }
95  algebra::quat q(v(0, iq), v(1, iq), v(2, iq), v(3, iq));
96  q.set_unit();
97  return q;
98  }
99  if (i < 4)
100  {
101  i++;
102  tresh = 0.0125f * sm;
103  }
104  else
105  tresh = 0.0;
106  for (ip = 0; ip < 3; ip++)
107  {
108  for (iq = ip + 1; iq < 4; iq++)
109  {
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]))
113  a(ip, iq) = 0.0f;
114  else if (std::fabs(a(ip, iq)) > tresh)
115  {
116  h = d[iq] - d[ip];
117  if ((float)(std::fabs(h) + g) == (float)std::fabs(h)) // unsafe?
118  t = (a(ip, iq)) / h;
119  else
120  {
121  theta = 0.5f * h / (a(ip, iq));
122  t = 1.0f / (std::fabs(theta) + std::sqrt(1.0f +
123  theta * theta));
124  if (theta < 0.0f)
125  t = -t;
126  }
127  c = 1.0f / std::sqrt(1 + t * t);
128  s = t * c;
129  tau = s / (1.0f + c);
130  h = t * a(ip, iq);
131  z[ip] -= h;
132  z[iq] += h;
133  d[ip] -= h;
134  d[iq] += h;
135  a(ip, iq) = 0.0;
136 
137  // DO *NOT* remove these semicolons!!
138  // rotateJacobi is a macro with 4 function calls.
139  for (j = 0; j <= ip - 1; j++)
140  {
141  rotateJacobi(a, j, ip, j, iq);
142  }
143  for (j = ip + 1; j <= iq - 1; j++)
144  {
145  rotateJacobi(a, ip, j, j, iq);
146  }
147  for (j = iq + 1; j < 4; j++)
148  {
149  rotateJacobi(a, ip, j, iq, j);
150  }
151  for (j = 0; j < 4; j++)
152  {
153  rotateJacobi(v, j, ip, j, iq);
154  }
155  }
156  }
157  }
158  for (ip = 0; ip < 4; ip++)
159  {
160  b[ip] += z[ip];
161  d[ip] = b[ip];
162  z[ip] = 0.0f;
163  }
164  }
165  }
166 
167 # endif // ! MLN_INCLUDE_ONLY
168 
169  } // end of namespace math
170 
171 } // end of namespace mln
172 
173 
174 #endif // ! MLN_MATH_JACOBI_HH