$extrastylesheet
Olena  User documentation 2.1
An Image Processing Platform
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
jacobi.hh
1 // Copyright (C) 2008, 2009, 2011, 2012 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 
50  algebra::quat
51  jacobi(algebra::mat<4u,4u,float> a);
52 
53 
54 # ifndef MLN_INCLUDE_ONLY
55 
56  // FIXME: nD ?
57 #define rotateJacobi(a,i,j,k,l) g=a(i,j);h=a(k,l);a(i,j)=g-s*(h+g*tau); \
58  a(k,l)=h+s*(g-h*tau);
59 
60 
61  inline
62  algebra::quat
63  jacobi(algebra::mat<4u,4u,float> a)
64  {
65  float dd, d[4];
66  algebra::mat < 4, 4, float >v(literal::zero);
67  int j, iq, ip, i = 0;
68  float tresh, theta, tau, t, sm, s, h, g, c, b[4], z[4];
69  for (ip = 0; ip < 4; ip++)
70  {
71  for (iq = 0; iq < 4; iq++)
72  v(ip, iq) = 0.0f;
73  v(ip, ip) = 1.0f;
74  }
75  for (ip = 0; ip < 4; ip++)
76  {
77  b[ip] = d[ip] = a(ip, ip);
78  z[ip] = 0.0f;
79  }
80  while (1)
81  {
82  sm = 0.0f;
83  for (ip = 0; ip < 3; ip++)
84  {
85  for (iq = ip + 1; iq < 4; iq++)
86  sm += std::fabs(a(ip, iq));
87  }
88  if (sm < 1e-12f)
89  { // 1e-12
90  dd = d[0];
91  iq = 0;
92  for (ip = 1; ip < 4; ip++)
93  if (d[ip] > dd)
94  {
95  iq = ip;
96  dd = d[ip];
97  }
98  algebra::quat q(v(0, iq), v(1, iq), v(2, iq), v(3, iq));
99  q.set_unit();
100  return q;
101  }
102  if (i < 4)
103  {
104  i++;
105  tresh = 0.0125f * sm;
106  }
107  else
108  tresh = 0.0;
109  for (ip = 0; ip < 3; ip++)
110  {
111  for (iq = ip + 1; iq < 4; iq++)
112  {
113  g = 100.0f * std::fabs(a(ip, iq));
114  if (i > 4 && (float)(std::fabs(d[ip]) + g) == (float)std::fabs(d[ip])
115  && (float)(std::fabs(d[iq]) + g) == (float)std::fabs(d[iq]))
116  a(ip, iq) = 0.0f;
117  else if (std::fabs(a(ip, iq)) > tresh)
118  {
119  h = d[iq] - d[ip];
120  if ((float)(std::fabs(h) + g) == (float)std::fabs(h)) // unsafe?
121  t = (a(ip, iq)) / h;
122  else
123  {
124  theta = 0.5f * h / (a(ip, iq));
125  t = 1.0f / (std::fabs(theta) + std::sqrt(1.0f +
126  theta * theta));
127  if (theta < 0.0f)
128  t = -t;
129  }
130  c = 1.0f / std::sqrt(1 + t * t);
131  s = t * c;
132  tau = s / (1.0f + c);
133  h = t * a(ip, iq);
134  z[ip] -= h;
135  z[iq] += h;
136  d[ip] -= h;
137  d[iq] += h;
138  a(ip, iq) = 0.0;
139 
140  // DO *NOT* remove these semicolons!!
141  // rotateJacobi is a macro with 4 function calls.
142  for (j = 0; j <= ip - 1; j++)
143  {
144  rotateJacobi(a, j, ip, j, iq);
145  }
146  for (j = ip + 1; j <= iq - 1; j++)
147  {
148  rotateJacobi(a, ip, j, j, iq);
149  }
150  for (j = iq + 1; j < 4; j++)
151  {
152  rotateJacobi(a, ip, j, iq, j);
153  }
154  for (j = 0; j < 4; j++)
155  {
156  rotateJacobi(v, j, ip, j, iq);
157  }
158  }
159  }
160  }
161  for (ip = 0; ip < 4; ip++)
162  {
163  b[ip] += z[ip];
164  d[ip] = b[ip];
165  z[ip] = 0.0f;
166  }
167  }
168  }
169 
170 # endif // ! MLN_INCLUDE_ONLY
171 
172  } // end of namespace math
173 
174 } // end of namespace mln
175 
176 
177 #endif // ! MLN_MATH_JACOBI_HH