Blender  V3.3
matrix_util.cpp
Go to the documentation of this file.
1 /* SPDX-License-Identifier: GPL-2.0-or-later
2  * The Original Code is:
3  * GXML/Graphite: Geometry and Graphics Programming Library + Utilities
4  * Copyright 2000 Bruno Levy <levy@loria.fr> */
5 
10 #include "matrix_util.h"
11 
12 #include "BLI_math.h"
13 
15 
16 static const double EPS = 0.00001;
17 static int MAX_ITER = 100;
18 
19 void semi_definite_symmetric_eigen(const double *mat, int n, double *eigen_vec, double *eigen_val)
20 {
21  double *a, *v;
22  double a_norm, a_normEPS, thr, thr_nn;
23  int nb_iter = 0;
24  int jj;
25  int i, j, k, ij, ik, l, m, lm, mq, lq, ll, mm, imv, im, iq, ilv, il, nn;
26  int *index;
27  double a_ij, a_lm, a_ll, a_mm, a_im, a_il;
28  double a_lm_2;
29  double v_ilv, v_imv;
30  double x;
31  double sinx, sinx_2, cosx, cosx_2, sincos;
32  double delta;
33 
34  // Number of entries in mat
35  nn = (n * (n + 1)) / 2;
36 
37  // Step 1: Copy mat to a
38  a = new double[nn];
39 
40  for (ij = 0; ij < nn; ij++) {
41  a[ij] = mat[ij];
42  }
43 
44  // Ugly Fortran-porting trick: indices for a are between 1 and n
45  a--;
46 
47  // Step 2 : Init diagonalization matrix as the unit matrix
48  v = new double[n * n];
49 
50  ij = 0;
51  for (i = 0; i < n; i++) {
52  for (j = 0; j < n; j++) {
53  if (i == j) {
54  v[ij++] = 1.0;
55  }
56  else {
57  v[ij++] = 0.0;
58  }
59  }
60  }
61 
62  // Ugly Fortran-porting trick: indices for v are between 1 and n
63  v--;
64 
65  // Step 3 : compute the weight of the non diagonal terms
66  ij = 1;
67  a_norm = 0.0;
68  for (i = 1; i <= n; i++) {
69  for (j = 1; j <= i; j++) {
70  if (i != j) {
71  a_ij = a[ij];
72  a_norm += a_ij * a_ij;
73  }
74  ij++;
75  }
76  }
77 
78  if (a_norm != 0.0) {
79  a_normEPS = a_norm * EPS;
80  thr = a_norm;
81 
82  // Step 4 : rotations
83  while (thr > a_normEPS && nb_iter < MAX_ITER) {
84  nb_iter++;
85  thr_nn = thr / nn;
86 
87  for (l = 1; l < n; l++) {
88  for (m = l + 1; m <= n; m++) {
89  // compute sinx and cosx
90  lq = (l * l - l) / 2;
91  mq = (m * m - m) / 2;
92 
93  lm = l + mq;
94  a_lm = a[lm];
95  a_lm_2 = a_lm * a_lm;
96 
97  if (a_lm_2 < thr_nn) {
98  continue;
99  }
100 
101  ll = l + lq;
102  mm = m + mq;
103  a_ll = a[ll];
104  a_mm = a[mm];
105 
106  delta = a_ll - a_mm;
107 
108  if (delta == 0.0) {
109  x = -M_PI_4;
110  }
111  else {
112  x = -atan((a_lm + a_lm) / delta) / 2.0;
113  }
114 
115  sinx = sin(x);
116  cosx = cos(x);
117  sinx_2 = sinx * sinx;
118  cosx_2 = cosx * cosx;
119  sincos = sinx * cosx;
120 
121  // rotate L and M columns
122  ilv = n * (l - 1);
123  imv = n * (m - 1);
124 
125  for (i = 1; i <= n; i++) {
126  if (!ELEM(i, l, m)) {
127  iq = (i * i - i) / 2;
128 
129  if (i < m) {
130  im = i + mq;
131  }
132  else {
133  im = m + iq;
134  }
135  a_im = a[im];
136 
137  if (i < l) {
138  il = i + lq;
139  }
140  else {
141  il = l + iq;
142  }
143  a_il = a[il];
144 
145  a[il] = a_il * cosx - a_im * sinx;
146  a[im] = a_il * sinx + a_im * cosx;
147  }
148 
149  ilv++;
150  imv++;
151 
152  v_ilv = v[ilv];
153  v_imv = v[imv];
154 
155  v[ilv] = cosx * v_ilv - sinx * v_imv;
156  v[imv] = sinx * v_ilv + cosx * v_imv;
157  }
158 
159  x = a_lm * sincos;
160  x += x;
161 
162  a[ll] = a_ll * cosx_2 + a_mm * sinx_2 - x;
163  a[mm] = a_ll * sinx_2 + a_mm * cosx_2 + x;
164  a[lm] = 0.0;
165 
166  thr = fabs(thr - a_lm_2);
167  }
168  }
169  }
170  }
171 
172  // Step 5: index conversion and copy eigen values
173 
174  // back from Fortran to C++
175  a++;
176 
177  for (i = 0; i < n; i++) {
178  k = i + (i * (i + 1)) / 2;
179  eigen_val[i] = a[k];
180  }
181 
182  delete[] a;
183 
184  // Step 6: sort the eigen values and eigen vectors
185 
186  index = new int[n];
187  for (i = 0; i < n; i++) {
188  index[i] = i;
189  }
190 
191  for (i = 0; i < (n - 1); i++) {
192  x = eigen_val[i];
193  k = i;
194 
195  for (j = i + 1; j < n; j++) {
196  if (x < eigen_val[j]) {
197  k = j;
198  x = eigen_val[j];
199  }
200  }
201 
202  eigen_val[k] = eigen_val[i];
203  eigen_val[i] = x;
204 
205  jj = index[k];
206  index[k] = index[i];
207  index[i] = jj;
208  }
209 
210  // Step 7: save the eigen vectors
211 
212  // back from Fortran to C++
213  v++;
214 
215  ij = 0;
216  for (k = 0; k < n; k++) {
217  ik = index[k] * n;
218  for (i = 0; i < n; i++) {
219  eigen_vec[ij++] = v[ik++];
220  }
221  }
222 
223  delete[] v;
224  delete[] index;
225 }
226 
227 //_________________________________________________________
228 
229 } // namespace Freestyle::OGF::MatrixUtil
#define M_PI_4
Definition: BLI_math_base.h:26
#define ELEM(...)
ATTR_WARN_UNUSED_RESULT const BMLoop * l
ATTR_WARN_UNUSED_RESULT const BMVert * v
ccl_device_inline float2 fabs(const float2 &a)
Definition: math_float2.h:222
static const double EPS
Definition: matrix_util.cpp:16
void semi_definite_symmetric_eigen(const double *mat, int n, double *eigen_vec, double *eigen_val)
Definition: matrix_util.cpp:19
static unsigned x[3]
Definition: RandGen.cpp:73
static unsigned a[3]
Definition: RandGen.cpp:78
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:319
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
Definition: rall1d.h:375
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:311