| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Copyright (c) 2000-2022 Inria | ||
| 3 | * All rights reserved. | ||
| 4 | * | ||
| 5 | * Redistribution and use in source and binary forms, with or without | ||
| 6 | * modification, are permitted provided that the following conditions are met: | ||
| 7 | * | ||
| 8 | * * Redistributions of source code must retain the above copyright notice, | ||
| 9 | * this list of conditions and the following disclaimer. | ||
| 10 | * * Redistributions in binary form must reproduce the above copyright notice, | ||
| 11 | * this list of conditions and the following disclaimer in the documentation | ||
| 12 | * and/or other materials provided with the distribution. | ||
| 13 | * * Neither the name of the ALICE Project-Team nor the names of its | ||
| 14 | * contributors may be used to endorse or promote products derived from this | ||
| 15 | * software without specific prior written permission. | ||
| 16 | * | ||
| 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" | ||
| 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE | ||
| 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | ||
| 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE | ||
| 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR | ||
| 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF | ||
| 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS | ||
| 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN | ||
| 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | ||
| 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| 27 | * POSSIBILITY OF SUCH DAMAGE. | ||
| 28 | * | ||
| 29 | * Contact: Bruno Levy | ||
| 30 | * | ||
| 31 | * https://www.inria.fr/fr/bruno-levy | ||
| 32 | * | ||
| 33 | * Inria, | ||
| 34 | * Domaine de Voluceau, | ||
| 35 | * 78150 Le Chesnay - Rocquencourt | ||
| 36 | * FRANCE | ||
| 37 | * | ||
| 38 | */ | ||
| 39 | |||
| 40 | #include <geogram/numerics/matrix_util.h> | ||
| 41 | #include <geogram/basic/process.h> | ||
| 42 | |||
| 43 | namespace GEO { | ||
| 44 | |||
| 45 | namespace { | ||
| 46 | const double EPS = 0.00001; | ||
| 47 | index_t MAX_ITER = 100; | ||
| 48 | } | ||
| 49 | |||
| 50 | namespace MatrixUtil { | ||
| 51 | |||
| 52 | 158810 | void semi_definite_symmetric_eigen( | |
| 53 | const double* mat, index_t n, double* eigen_vec, double* eigen_val | ||
| 54 | ) { | ||
| 55 | // Number of entries in mat | ||
| 56 | |||
| 57 | 158810 | index_t nn = (n * (n + 1)) / 2; | |
| 58 | |||
| 59 | // ==== Step 1: Copy mat to a | ||
| 60 | |||
| 61 | // Note: a is allocated on the stack, | ||
| 62 | // it is more multithread friendly. | ||
| 63 | 158810 | double* a = (double*) (alloca(sizeof(double) * nn)); | |
| 64 |
2/2✓ Branch 0 taken 952860 times.
✓ Branch 1 taken 158810 times.
|
1111670 | for(index_t ij = 0; ij < nn; ij++) { |
| 65 | 952860 | a[ij] = mat[ij]; | |
| 66 | } | ||
| 67 | |||
| 68 | // Ugly Fortran-porting trick: indices for a are between 1 and n | ||
| 69 | 158810 | a--; | |
| 70 | |||
| 71 | // ==== Step 2 : Init diagonalization matrix as the unit matrix | ||
| 72 | |||
| 73 | // Note: v is allocated on the stack, | ||
| 74 | // it is more multithread friendly. | ||
| 75 | 158810 | double* v = (double*) (alloca(sizeof(double) * n * n)); | |
| 76 | |||
| 77 | 158810 | index_t ij = 0; | |
| 78 |
2/2✓ Branch 0 taken 476430 times.
✓ Branch 1 taken 158810 times.
|
635240 | for(index_t i = 0; i < n; i++) { |
| 79 |
2/2✓ Branch 0 taken 1429290 times.
✓ Branch 1 taken 476430 times.
|
1905720 | for(index_t j = 0; j < n; j++) { |
| 80 |
2/2✓ Branch 0 taken 476430 times.
✓ Branch 1 taken 952860 times.
|
1429290 | v[ij] = (i == j) ? 1.0 : 0.0; |
| 81 | 1429290 | ij++; | |
| 82 | } | ||
| 83 | } | ||
| 84 | |||
| 85 | // Ugly Fortran-porting trick: indices for v are between 1 and n | ||
| 86 | 158810 | v--; | |
| 87 | |||
| 88 | // ==== Step 3 : compute the weight of the non diagonal terms | ||
| 89 | 158810 | ij = 1; | |
| 90 | 158810 | double a_norm = 0.0; | |
| 91 |
2/2✓ Branch 0 taken 476430 times.
✓ Branch 1 taken 158810 times.
|
635240 | for(index_t i = 1; i <= n; i++) { |
| 92 |
2/2✓ Branch 0 taken 952860 times.
✓ Branch 1 taken 476430 times.
|
1429290 | for(index_t j = 1; j <= i; j++) { |
| 93 |
2/2✓ Branch 0 taken 476430 times.
✓ Branch 1 taken 476430 times.
|
952860 | if(i != j) { |
| 94 | 476430 | double a_ij = a[ij]; | |
| 95 | 476430 | a_norm += a_ij * a_ij; | |
| 96 | } | ||
| 97 | 952860 | ij++; | |
| 98 | } | ||
| 99 | } | ||
| 100 | |||
| 101 |
1/2✓ Branch 0 taken 158810 times.
✗ Branch 1 not taken.
|
158810 | if(a_norm != 0.0) { |
| 102 | |||
| 103 | 158810 | double a_normEPS = a_norm * EPS; | |
| 104 | 158810 | double thr = a_norm; | |
| 105 | 158810 | index_t nb_iter = 0; | |
| 106 | |||
| 107 | // Step 4 : rotations | ||
| 108 |
3/4✓ Branch 0 taken 523634 times.
✓ Branch 1 taken 158810 times.
✓ Branch 2 taken 523634 times.
✗ Branch 3 not taken.
|
682444 | while(thr > a_normEPS && nb_iter < MAX_ITER) { |
| 109 | |||
| 110 | 523634 | nb_iter++; | |
| 111 | 523634 | double thr_nn = thr / double(nn); | |
| 112 | |||
| 113 |
2/2✓ Branch 0 taken 1047268 times.
✓ Branch 1 taken 523634 times.
|
1570902 | for(index_t l = 1; l < n; l++) { |
| 114 |
2/2✓ Branch 0 taken 1570902 times.
✓ Branch 1 taken 1047268 times.
|
2618170 | for(index_t m = l + 1; m <= n; m++) { |
| 115 | |||
| 116 | // compute sinx and cosx | ||
| 117 | |||
| 118 | 1570902 | index_t lq = (l * l - l) / 2; | |
| 119 | 1570902 | index_t mq = (m * m - m) / 2; | |
| 120 | |||
| 121 | 1570902 | index_t lm = l + mq; | |
| 122 | 1570902 | double a_lm = a[lm]; | |
| 123 | 1570902 | double a_lm_2 = a_lm * a_lm; | |
| 124 | |||
| 125 |
2/2✓ Branch 0 taken 853907 times.
✓ Branch 1 taken 716995 times.
|
1570902 | if(a_lm_2 < thr_nn) { |
| 126 | 853907 | continue; | |
| 127 | } | ||
| 128 | |||
| 129 | 716995 | index_t ll = l + lq; | |
| 130 | 716995 | index_t mm = m + mq; | |
| 131 | 716995 | double a_ll = a[ll]; | |
| 132 | 716995 | double a_mm = a[mm]; | |
| 133 | |||
| 134 | 716995 | double delta = a_ll - a_mm; | |
| 135 | |||
| 136 | double x; | ||
| 137 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 716995 times.
|
716995 | if(delta == 0.0) { |
| 138 | ✗ | x = -M_PI / 4; | |
| 139 | } else { | ||
| 140 | 716995 | x = -atan((a_lm + a_lm) / delta) / 2.0; | |
| 141 | } | ||
| 142 | |||
| 143 | 716995 | double sinx = sin(x); | |
| 144 | 716995 | double cosx = cos(x); | |
| 145 | 716995 | double sinx_2 = sinx * sinx; | |
| 146 | 716995 | double cosx_2 = cosx * cosx; | |
| 147 | 716995 | double sincos = sinx * cosx; | |
| 148 | |||
| 149 | // rotate L and M columns | ||
| 150 | |||
| 151 | 716995 | index_t ilv = n * (l - 1); | |
| 152 | 716995 | index_t imv = n * (m - 1); | |
| 153 | |||
| 154 |
2/2✓ Branch 0 taken 2150985 times.
✓ Branch 1 taken 716995 times.
|
2867980 | for(index_t i = 1; i <= n; i++) { |
| 155 |
4/4✓ Branch 0 taken 1433990 times.
✓ Branch 1 taken 716995 times.
✓ Branch 2 taken 716995 times.
✓ Branch 3 taken 716995 times.
|
2150985 | if((i != l) && (i != m)) { |
| 156 | 716995 | index_t iq = (i * i - i) / 2; | |
| 157 | index_t im; | ||
| 158 | |||
| 159 |
2/2✓ Branch 0 taken 460707 times.
✓ Branch 1 taken 256288 times.
|
716995 | if(i < m) { |
| 160 | 460707 | im = i + mq; | |
| 161 | } else { | ||
| 162 | 256288 | im = m + iq; | |
| 163 | } | ||
| 164 | 716995 | double a_im = a[im]; | |
| 165 | |||
| 166 | index_t il; | ||
| 167 |
2/2✓ Branch 0 taken 200522 times.
✓ Branch 1 taken 516473 times.
|
716995 | if(i < l) { |
| 168 | 200522 | il = i + lq; | |
| 169 | } else { | ||
| 170 | 516473 | il = l + iq; | |
| 171 | } | ||
| 172 | 716995 | double a_il = a[il]; | |
| 173 | |||
| 174 | 716995 | a[il] = a_il * cosx - a_im * sinx; | |
| 175 | 716995 | a[im] = a_il * sinx + a_im * cosx; | |
| 176 | } | ||
| 177 | |||
| 178 | 2150985 | ilv++; | |
| 179 | 2150985 | imv++; | |
| 180 | |||
| 181 | 2150985 | double v_ilv = v[ilv]; | |
| 182 | 2150985 | double v_imv = v[imv]; | |
| 183 | |||
| 184 | 2150985 | v[ilv] = cosx * v_ilv - sinx * v_imv; | |
| 185 | 2150985 | v[imv] = sinx * v_ilv + cosx * v_imv; | |
| 186 | } | ||
| 187 | |||
| 188 | 716995 | x = a_lm * sincos; | |
| 189 | 716995 | x += x; | |
| 190 | |||
| 191 | 716995 | a[ll] = a_ll * cosx_2 + a_mm * sinx_2 - x; | |
| 192 | 716995 | a[mm] = a_ll * sinx_2 + a_mm * cosx_2 + x; | |
| 193 | 716995 | a[lm] = 0.0; | |
| 194 | |||
| 195 | 716995 | thr = fabs(thr - a_lm_2); | |
| 196 | } | ||
| 197 | } | ||
| 198 | } | ||
| 199 | } | ||
| 200 | |||
| 201 | // ==== Step 5: index conversion and copy eigen values | ||
| 202 | |||
| 203 | // back from Fortran to C++ | ||
| 204 | 158810 | a++; | |
| 205 | |||
| 206 |
2/2✓ Branch 0 taken 476430 times.
✓ Branch 1 taken 158810 times.
|
635240 | for(index_t i = 0; i < n; i++) { |
| 207 | 476430 | index_t k = i + (i * (i + 1)) / 2; | |
| 208 | 476430 | eigen_val[i] = a[k]; | |
| 209 | } | ||
| 210 | |||
| 211 | // ==== Step 6: sort the eigen values and eigen vectors | ||
| 212 | |||
| 213 | // Note: index is allocated on the stack, | ||
| 214 | // it is more multithread friendly. | ||
| 215 | 158810 | index_t* index = (index_t*) (alloca(sizeof(index_t) * n)); | |
| 216 | |||
| 217 |
2/2✓ Branch 0 taken 476430 times.
✓ Branch 1 taken 158810 times.
|
635240 | for(index_t i = 0; i < n; i++) { |
| 218 | 476430 | index[i] = i; | |
| 219 | } | ||
| 220 | |||
| 221 |
2/2✓ Branch 0 taken 317620 times.
✓ Branch 1 taken 158810 times.
|
476430 | for(index_t i = 0; i < (n - 1); i++) { |
| 222 | 317620 | double x = eigen_val[i]; | |
| 223 | 317620 | index_t k = i; | |
| 224 | |||
| 225 |
2/2✓ Branch 0 taken 476430 times.
✓ Branch 1 taken 317620 times.
|
794050 | for(index_t j = i + 1; j < n; j++) { |
| 226 |
2/2✓ Branch 0 taken 200787 times.
✓ Branch 1 taken 275643 times.
|
476430 | if(x < eigen_val[j]) { |
| 227 | 200787 | k = j; | |
| 228 | 200787 | x = eigen_val[j]; | |
| 229 | } | ||
| 230 | } | ||
| 231 | |||
| 232 | 317620 | eigen_val[k] = eigen_val[i]; | |
| 233 | 317620 | eigen_val[i] = x; | |
| 234 | |||
| 235 | 317620 | index_t jj = index[k]; | |
| 236 | 317620 | index[k] = index[i]; | |
| 237 | 317620 | index[i] = jj; | |
| 238 | } | ||
| 239 | |||
| 240 | // ==== Step 7: save the eigen vectors | ||
| 241 | |||
| 242 | 158810 | v++; // back from Fortran to to C++ | |
| 243 | |||
| 244 | 158810 | ij = 0; | |
| 245 |
2/2✓ Branch 0 taken 476430 times.
✓ Branch 1 taken 158810 times.
|
635240 | for(index_t k = 0; k < n; k++) { |
| 246 | 476430 | index_t ik = index[k] * n; | |
| 247 |
2/2✓ Branch 0 taken 1429290 times.
✓ Branch 1 taken 476430 times.
|
1905720 | for(index_t i = 0; i < n; i++) { |
| 248 | 1429290 | eigen_vec[ij++] = v[ik++]; | |
| 249 | } | ||
| 250 | } | ||
| 251 | 158810 | } | |
| 252 | } | ||
| 253 | } | ||
| 254 |