OLD | NEW |
(Empty) | |
| 1 /* |
| 2 * ***** BEGIN GPL LICENSE BLOCK ***** |
| 3 * |
| 4 * This program is free software; you can redistribute it and/or |
| 5 * modify it under the terms of the GNU General Public License |
| 6 * as published by the Free Software Foundation; either version 2 |
| 7 * of the License, or (at your option) any later version. |
| 8 * |
| 9 * This program is distributed in the hope that it will be useful, |
| 10 * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 12 * GNU General Public License for more details. |
| 13 * |
| 14 * You should have received a copy of the GNU General Public License |
| 15 * along with this program; if not, write to the Free Software Foundation, |
| 16 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. |
| 17 * |
| 18 * This Code is Copyright (C) 2010 Blender Foundation. |
| 19 * All rights reserved. |
| 20 * |
| 21 * The Original Code is: |
| 22 * GXML/Graphite: Geometry and Graphics Programming Library + Utilities |
| 23 * Copyright (C) 2000 Bruno Levy |
| 24 * Contact: Bruno Levy |
| 25 * levy@loria.fr |
| 26 * ISA Project |
| 27 * LORIA, INRIA Lorraine, |
| 28 * Campus Scientifique, BP 239 |
| 29 * 54506 VANDOEUVRE LES NANCY CEDEX |
| 30 * FRANCE |
| 31 * |
| 32 * Contributor(s): none yet. |
| 33 * |
| 34 * ***** END GPL LICENSE BLOCK ***** |
| 35 */ |
| 36 |
| 37 /** \file blender/freestyle/intern/geometry/matrix_util.cpp |
| 38 * \ingroup freestyle |
| 39 * \author Bruno Levy |
| 40 */ |
| 41 |
| 42 #include <math.h> |
| 43 |
| 44 #include "matrix_util.h" |
| 45 |
| 46 namespace OGF { |
| 47 |
| 48 namespace MatrixUtil { |
| 49 |
| 50 static const double EPS = 0.00001; |
| 51 static int MAX_ITER = 100; |
| 52 |
| 53 void semi_definite_symmetric_eigen(const double *mat, int n, double *eig
en_vec, double *eigen_val) |
| 54 { |
| 55 double *a, *v; |
| 56 double a_norm, a_normEPS, thr, thr_nn; |
| 57 int nb_iter = 0; |
| 58 int jj; |
| 59 int i, j, k, ij, ik, l, m, lm, mq, lq, ll, mm, imv, im, iq, ilv,
il, nn; |
| 60 int *index; |
| 61 double a_ij, a_lm, a_ll, a_mm, a_im, a_il; |
| 62 double a_lm_2; |
| 63 double v_ilv, v_imv; |
| 64 double x; |
| 65 double sinx, sinx_2, cosx, cosx_2, sincos; |
| 66 double delta; |
| 67 |
| 68 // Number of entries in mat |
| 69 nn = (n * (n + 1)) / 2; |
| 70 |
| 71 // Step 1: Copy mat to a |
| 72 a = new double[nn]; |
| 73 |
| 74 for (ij = 0; ij < nn; ij++) { |
| 75 a[ij] = mat[ij]; |
| 76 } |
| 77 |
| 78 // Ugly Fortran-porting trick: indices for a are between 1 and n |
| 79 a--; |
| 80 |
| 81 // Step 2 : Init diagonalization matrix as the unit matrix |
| 82 v = new double[n * n]; |
| 83 |
| 84 ij = 0; |
| 85 for (i = 0; i < n; i++) { |
| 86 for (j = 0; j < n; j++) { |
| 87 if (i == j) { |
| 88 v[ij++] = 1.0; |
| 89 } |
| 90 else { |
| 91 v[ij++] = 0.0; |
| 92 } |
| 93 } |
| 94 } |
| 95 |
| 96 // Ugly Fortran-porting trick: indices for v are between 1 and n |
| 97 v--; |
| 98 |
| 99 // Step 3 : compute the weight of the non diagonal terms· |
| 100 ij = 1; |
| 101 a_norm = 0.0; |
| 102 for (i = 1; i <= n; i++) { |
| 103 for (j = 1; j <= i; j++) { |
| 104 if (i != j) { |
| 105 a_ij = a[ij]; |
| 106 a_norm += a_ij * a_ij; |
| 107 } |
| 108 ij++; |
| 109 } |
| 110 } |
| 111 |
| 112 if (a_norm != 0.0) { |
| 113 a_normEPS = a_norm * EPS; |
| 114 thr = a_norm; |
| 115 |
| 116 // Step 4 : rotations |
| 117 while (thr > a_normEPS && nb_iter < MAX_ITER) { |
| 118 nb_iter++; |
| 119 thr_nn = thr / nn; |
| 120 |
| 121 for (l = 1; l < n; l++) { |
| 122 for (m = l + 1; m <= n; m++) { |
| 123 // compute sinx and cosx |
| 124 lq = (l * l - l) / 2; |
| 125 mq = (m * m - m) / 2; |
| 126 |
| 127 lm = l + mq; |
| 128 a_lm = a[lm]; |
| 129 a_lm_2 = a_lm * a_lm; |
| 130 |
| 131 if (a_lm_2 < thr_nn) { |
| 132 continue; |
| 133 } |
| 134 |
| 135 ll = l + lq; |
| 136 mm = m + mq; |
| 137 a_ll = a[ll]; |
| 138 a_mm = a[mm]; |
| 139 |
| 140 delta = a_ll - a_mm; |
| 141 |
| 142 if (delta == 0.0) { |
| 143 x = -M_PI / 4; |
| 144 } |
| 145 else { |
| 146 x = -atan((a_lm + a_lm)
/ delta) / 2.0; |
| 147 } |
| 148 |
| 149 sinx = sin(x); |
| 150 cosx = cos(x); |
| 151 sinx_2 = sinx * sinx; |
| 152 cosx_2 = cosx * cosx; |
| 153 sincos = sinx * cosx; |
| 154 |
| 155 // rotate L and M columns |
| 156 ilv = n * (l - 1); |
| 157 imv = n * (m - 1); |
| 158 |
| 159 for (i = 1; i <= n; i++) { |
| 160 if ((i != l) && (i != m)
) { |
| 161 iq = (i * i - i)
/ 2; |
| 162 |
| 163 if (i < m) { |
| 164 im = i +
mq; |
| 165 } |
| 166 else { |
| 167 im = m +
iq; |
| 168 } |
| 169 a_im = a[im]; |
| 170 |
| 171 if (i < l) { |
| 172 il = i +
lq; |
| 173 } |
| 174 else { |
| 175 il = l +
iq; |
| 176 } |
| 177 a_il = a[il]; |
| 178 |
| 179 a[il] = a_il * c
osx - a_im * sinx; |
| 180 a[im] = a_il * s
inx + a_im * cosx; |
| 181 } |
| 182 |
| 183 ilv++; |
| 184 imv++; |
| 185 |
| 186 v_ilv = v[ilv]; |
| 187 v_imv = v[imv]; |
| 188 |
| 189 v[ilv] = cosx * v_ilv -
sinx * v_imv; |
| 190 v[imv] = sinx * v_ilv +
cosx * v_imv; |
| 191 } |
| 192 |
| 193 x = a_lm * sincos; |
| 194 x += x; |
| 195 |
| 196 a[ll] = a_ll * cosx_2 + a_mm * s
inx_2 - x; |
| 197 a[mm] = a_ll * sinx_2 + a_mm * c
osx_2 + x; |
| 198 a[lm] = 0.0; |
| 199 |
| 200 thr = fabs(thr - a_lm_2); |
| 201 } |
| 202 } |
| 203 } |
| 204 } |
| 205 |
| 206 // Step 5: index conversion and copy eigen values |
| 207 |
| 208 // back from Fortran to C++ |
| 209 a++; |
| 210 |
| 211 for (i = 0; i < n; i++) { |
| 212 k = i + (i * (i + 1)) / 2; |
| 213 eigen_val[i] = a[k]; |
| 214 } |
| 215 |
| 216 delete[] a; |
| 217 |
| 218 // Step 6: sort the eigen values and eigen vectors |
| 219 |
| 220 index = new int[n]; |
| 221 for (i = 0; i < n; i++) { |
| 222 index[i] = i; |
| 223 } |
| 224 |
| 225 for (i = 0; i < (n - 1); i++) { |
| 226 x = eigen_val[i]; |
| 227 k = i; |
| 228 |
| 229 for (j = i + 1; j < n; j++) { |
| 230 if (x < eigen_val[j]) { |
| 231 k = j; |
| 232 x = eigen_val[j]; |
| 233 } |
| 234 } |
| 235 |
| 236 eigen_val[k] = eigen_val[i]; |
| 237 eigen_val[i] = x; |
| 238 |
| 239 jj = index[k]; |
| 240 index[k] = index[i]; |
| 241 index[i] = jj; |
| 242 } |
| 243 |
| 244 // Step 7: save the eigen vectors· |
| 245 |
| 246 // back from Fortran to to C++ |
| 247 v++; |
| 248 |
| 249 ij = 0; |
| 250 for (k = 0; k < n; k++) { |
| 251 ik = index[k] * n; |
| 252 for (i = 0; i < n; i++) { |
| 253 eigen_vec[ij++] = v[ik++]; |
| 254 } |
| 255 } |
| 256 |
| 257 delete[] v; |
| 258 delete[] index; |
| 259 return; |
| 260 } |
| 261 |
| 262 //_________________________________________________________ |
| 263 |
| 264 } // MatrixUtil namespace |
| 265 |
| 266 } // OGF namespace |
OLD | NEW |