Easy3D 2.6.1
Loading...
Searching...
No Matches
eigen_solver.h
1/********************************************************************
2 * Copyright (C) 2015-2021 by Liangliang Nan <liangliang.nan@gmail.com>
3 * Copyright (C) Copyright(c) 2000, 2001. Magic Software, Inc.
4 *
5 * The code in this file is partly from Magic Software with
6 * modifications and enhancement:
7 * http://www.magic-software.com
8 * The original code was under the "FREE SOURCE CODE" licence
9 * http://www.magic-software.com/License/free.pdf
10 ********************************************************************/
11
12#ifndef EASY3D_CORE_EIGEN_SOLVER_H
13#define EASY3D_CORE_EIGEN_SOLVER_H
14
15#include <cmath>
16#include <cassert>
17
18
19namespace easy3d
20{
28 template <class MAT, typename T = double>
29 class EigenSolver
30 {
31 public:
41
42 public:
47 explicit EigenSolver(int n);
51 ~EigenSolver();
52
58 void solve(const MAT& mat, SortingMethod sm = NO_SORTING);
59
65 T eigen_value(int i) const { return diag_[i]; }
72 T eigen_vector(int comp, int i) const { return matrix_(comp, i); }
73
78 T* eigen_values() { return diag_; }
79
84 const MAT& eigen_vectors() { return matrix_; }
85
86 protected:
87 int size_;
88 MAT matrix_;
89 T* diag_;
90 T* subd_;
91
92 private:
93 // Householder reduction to tridiagonal form
94 static void tridiagonal_2(MAT& mat, T* diag, T* subd);
95 static void tridiagonal_3(MAT& mat, T* diag, T* subd);
96 static void tridiagonal_4(MAT& mat, T* diag, T* subd);
97 static void tridiagonal_n(int n, MAT& mat, T* diag, T* subd);
98
99 // QL algorithm with implicit shifting, applies to tridiagonal matrices
100 static bool ql_algorithm(int n, T* diag, T* subd, MAT& mat);
101
102 // sort eigenvalues and eigenvectors in the descending order
103 static void decreasing_sort(int n, T* eigval, MAT& eigvec);
104
105 // sort eigenvalues and eigenvectors in the ascending order
106 static void increasing_sort(int n, T* eigval, MAT& eigvec);
107 };
108
109
110 //----------------------------------------------------------------------------
111
112
113 template <class MAT, typename T>
114 EigenSolver<MAT, T>::EigenSolver(int n)
115 {
116 assert(n >= 2);
117 size_ = n;
118 diag_ = new T[size_];
119 subd_ = new T[size_];
120 }
121
122 template <class MAT, typename T>
123 EigenSolver<MAT, T>::~EigenSolver()
124 {
125 delete[] subd_;
126 delete[] diag_;
127 }
128
129
130 template <class MAT, typename T>
131 void EigenSolver<MAT, T>::tridiagonal_2(MAT& matrix, T* diag, T* subd)
132 {
133 // matrix is already tridiagonal
134 diag[0] = matrix(0, 0);
135 diag[1] = matrix(1, 1);
136 subd[0] = matrix(0, 1);
137 subd[1] = 0.0f;
138 matrix(0, 0) = 1.0f;
139 matrix(0, 1) = 0.0f;
140 matrix(1, 0) = 0.0f;
141 matrix(1, 1) = 1.0f;
142 }
143
144
145 template <class MAT, typename T>
146 void EigenSolver<MAT, T>::tridiagonal_3(MAT& matrix, T* diag, T* subd)
147 {
148 T fM00 = matrix(0, 0);
149 T fM01 = matrix(0, 1);
150 T fM02 = matrix(0, 2);
151 T fM11 = matrix(1, 1);
152 T fM12 = matrix(1, 2);
153 T fM22 = matrix(2, 2);
154
155 diag[0] = fM00;
156 subd[2] = 0.0f;
157 if (fM02 != 0.0f)
158 {
159 T fLength = std::sqrt(fM01 * fM01 + fM02 * fM02);
160 T fInvLength = 1.0f / fLength;
161 fM01 *= fInvLength;
162 fM02 *= fInvLength;
163 T fQ = 2.0f * fM01 * fM12 + fM02 * (fM22 - fM11);
164 diag[1] = fM11 + fM02 * fQ;
165 diag[2] = fM22 - fM02 * fQ;
166 subd[0] = fLength;
167 subd[1] = fM12 - fM01 * fQ;
168 matrix(0, 0) = 1.0f;
169 matrix(0, 1) = 0.0f;
170 matrix(0, 2) = 0.0f;
171 matrix(1, 0) = 0.0f;
172 matrix(1, 1) = fM01;
173 matrix(1, 2) = fM02;
174 matrix(2, 0) = 0.0f;
175 matrix(2, 1) = fM02;
176 matrix(2, 2) = -fM01;
177 }
178 else
179 {
180 diag[1] = fM11;
181 diag[2] = fM22;
182 subd[0] = fM01;
183 subd[1] = fM12;
184 matrix(0, 0) = 1.0f;
185 matrix(0, 1) = 0.0f;
186 matrix(0, 2) = 0.0f;
187 matrix(1, 0) = 0.0f;
188 matrix(1, 1) = 1.0f;
189 matrix(1, 2) = 0.0f;
190 matrix(2, 0) = 0.0f;
191 matrix(2, 1) = 0.0f;
192 matrix(2, 2) = 1.0f;
193 }
194 }
195
196
197 template <class MAT, typename T>
198 void EigenSolver<MAT, T>::tridiagonal_4(MAT& matrix, T* diag, T* subd)
199 {
200 // save matrix M
201 T fM00 = matrix(0, 0);
202 T fM01 = matrix(0, 1);
203 T fM02 = matrix(0, 2);
204 T fM03 = matrix(0, 3);
205 T fM11 = matrix(1, 1);
206 T fM12 = matrix(1, 2);
207 T fM13 = matrix(1, 3);
208 T fM22 = matrix(2, 2);
209 T fM23 = matrix(2, 3);
210 T fM33 = matrix(3, 3);
211
212 diag[0] = fM00;
213 subd[3] = 0.0f;
214
215 matrix(0, 0) = 1.0f;
216 matrix(0, 1) = 0.0f;
217 matrix(0, 2) = 0.0f;
218 matrix(0, 3) = 0.0f;
219 matrix(1, 0) = 0.0f;
220 matrix(2, 0) = 0.0f;
221 matrix(3, 0) = 0.0f;
222
223 T fLength, fInvLength;
224
225 if (fM02 != 0.0f || fM03 != 0.0f)
226 {
227 T fQ11, fQ12, fQ13;
228 T fQ21, fQ22, fQ23;
229 T fQ31, fQ32, fQ33;
230
231 // build column Q1
232 fLength = std::sqrt(fM01 * fM01 + fM02 * fM02 + fM03 * fM03);
233 fInvLength = 1.0f / fLength;
234 fQ11 = fM01 * fInvLength;
235 fQ21 = fM02 * fInvLength;
236 fQ31 = fM03 * fInvLength;
237
238 subd[0] = fLength;
239
240 // compute S*Q1
241 T fV0 = fM11 * fQ11 + fM12 * fQ21 + fM13 * fQ31;
242 T fV1 = fM12 * fQ11 + fM22 * fQ21 + fM23 * fQ31;
243 T fV2 = fM13 * fQ11 + fM23 * fQ21 + fM33 * fQ31;
244
245 diag[1] = fQ11 * fV0 + fQ21 * fV1 + fQ31 * fV2;
246
247 // build column Q3 = Q1x(S*Q1)
248 fQ13 = fQ21 * fV2 - fQ31 * fV1;
249 fQ23 = fQ31 * fV0 - fQ11 * fV2;
250 fQ33 = fQ11 * fV1 - fQ21 * fV0;
251 fLength = std::sqrt(fQ13 * fQ13 + fQ23 * fQ23 + fQ33 * fQ33);
252 if (fLength > 0.0f)
253 {
254 fInvLength = 1.0f / fLength;
255 fQ13 *= fInvLength;
256 fQ23 *= fInvLength;
257 fQ33 *= fInvLength;
258
259 // build column Q2 = Q3xQ1
260 fQ12 = fQ23 * fQ31 - fQ33 * fQ21;
261 fQ22 = fQ33 * fQ11 - fQ13 * fQ31;
262 fQ32 = fQ13 * fQ21 - fQ23 * fQ11;
263
264 fV0 = fQ12 * fM11 + fQ22 * fM12 + fQ32 * fM13;
265 fV1 = fQ12 * fM12 + fQ22 * fM22 + fQ32 * fM23;
266 fV2 = fQ12 * fM13 + fQ22 * fM23 + fQ32 * fM33;
267 subd[1] = fQ11 * fV0 + fQ21 * fV1 + fQ31 * fV2;
268 diag[2] = fQ12 * fV0 + fQ22 * fV1 + fQ32 * fV2;
269 subd[2] = fQ13 * fV0 + fQ23 * fV1 + fQ33 * fV2;
270
271 fV0 = fQ13 * fM11 + fQ23 * fM12 + fQ33 * fM13;
272 fV1 = fQ13 * fM12 + fQ23 * fM22 + fQ33 * fM23;
273 fV2 = fQ13 * fM13 + fQ23 * fM23 + fQ33 * fM33;
274 diag[3] = fQ13 * fV0 + fQ23 * fV1 + fQ33 * fV2;
275 }
276 else
277 {
278 // S*Q1 parallel to Q1, choose any valid Q2 and Q3
279 subd[1] = 0.0f;
280
281 fLength = fQ21 * fQ21 + fQ31 * fQ31;
282 if (fLength > 0.0f)
283 {
284 fInvLength = 1.0f / fLength;
285 T fTmp = fQ11 - 1.0f;
286 fQ12 = -fQ21;
287 fQ22 = 1.0f + fTmp * fQ21 * fQ21 * fInvLength;
288 fQ32 = fTmp * fQ21 * fQ31 * fInvLength;
289
290 fQ13 = -fQ31;
291 fQ23 = fQ32;
292 fQ33 = 1.0f + fTmp * fQ31 * fQ31 * fInvLength;
293
294 fV0 = fQ12 * fM11 + fQ22 * fM12 + fQ32 * fM13;
295 fV1 = fQ12 * fM12 + fQ22 * fM22 + fQ32 * fM23;
296 fV2 = fQ12 * fM13 + fQ22 * fM23 + fQ32 * fM33;
297 diag[2] = fQ12 * fV0 + fQ22 * fV1 + fQ32 * fV2;
298 subd[2] = fQ13 * fV0 + fQ23 * fV1 + fQ33 * fV2;
299
300 fV0 = fQ13 * fM11 + fQ23 * fM12 + fQ33 * fM13;
301 fV1 = fQ13 * fM12 + fQ23 * fM22 + fQ33 * fM23;
302 fV2 = fQ13 * fM13 + fQ23 * fM23 + fQ33 * fM33;
303 diag[3] = fQ13 * fV0 + fQ23 * fV1 + fQ33 * fV2;
304 }
305 else
306 {
307 // Q1 =(+-1,0,0)
308 fQ12 = 0.0f;
309 fQ22 = 1.0f;
310 fQ32 = 0.0f;
311 fQ13 = 0.0f;
312 fQ23 = 0.0f;
313 fQ33 = 1.0f;
314
315 diag[2] = fM22;
316 diag[3] = fM33;
317 subd[2] = fM23;
318 }
319 }
320
321 matrix(1, 1) = fQ11;
322 matrix(1, 2) = fQ12;
323 matrix(1, 3) = fQ13;
324 matrix(2, 1) = fQ21;
325 matrix(2, 2) = fQ22;
326 matrix(2, 3) = fQ23;
327 matrix(3, 1) = fQ31;
328 matrix(3, 2) = fQ32;
329 matrix(3, 3) = fQ33;
330 }
331 else
332 {
333 diag[1] = fM11;
334 subd[0] = fM01;
335 matrix(1, 1) = 1.0f;
336 matrix(2, 1) = 0.0f;
337 matrix(3, 1) = 0.0f;
338
339 if (fM13 != 0.0f)
340 {
341 fLength = std::sqrt(fM12 * fM12 + fM13 * fM13);
342 fInvLength = 1.0f / fLength;
343 fM12 *= fInvLength;
344 fM13 *= fInvLength;
345 T fQ = 2.0f * fM12 * fM23 + fM13 * (fM33 - fM22);
346
347 diag[2] = fM22 + fM13 * fQ;
348 diag[3] = fM33 - fM13 * fQ;
349 subd[1] = fLength;
350 subd[2] = fM23 - fM12 * fQ;
351 matrix(1, 2) = 0.0f;
352 matrix(1, 3) = 0.0f;
353 matrix(2, 2) = fM12;
354 matrix(2, 3) = fM13;
355 matrix(3, 2) = fM13;
356 matrix(3, 3) = -fM12;
357 }
358 else
359 {
360 diag[2] = fM22;
361 diag[3] = fM33;
362 subd[1] = fM12;
363 subd[2] = fM23;
364 matrix(1, 2) = 0.0f;
365 matrix(1, 3) = 0.0f;
366 matrix(2, 2) = 1.0f;
367 matrix(2, 3) = 0.0f;
368 matrix(3, 2) = 0.0f;
369 matrix(3, 3) = 1.0f;
370 }
371 }
372 }
373
374
375 template <class MAT, typename T>
376 void EigenSolver<MAT, T>::tridiagonal_n(int n, MAT& matrix, T* diag, T* subd)
377 {
378 int i0, i1, i2, i3;
379
380 for (i0 = n - 1, i3 = n - 2; i0 >= 1; i0--, i3--)
381 {
382 T fH = 0.0f, fScale = 0.0f;
383
384 if (i3 > 0)
385 {
386 for (i2 = 0; i2 <= i3; i2++)
387 fScale += std::abs(matrix(i0, i2));
388 if (fScale == 0)
389 {
390 subd[i0] = matrix(i0, i3);
391 }
392 else
393 {
394 T fInvScale = 1.0f / fScale;
395 for (i2 = 0; i2 <= i3; i2++)
396 {
397 matrix(i0, i2) *= fInvScale;
398 fH += matrix(i0, i2) * matrix(i0, i2);
399 }
400 T fF = matrix(i0, i3);
401 T fG = std::sqrt(fH);
402 if (fF > 0.0f)
403 fG = -fG;
404 subd[i0] = fScale * fG;
405 fH -= fF * fG;
406 matrix(i0, i3) = fF - fG;
407 fF = 0.0f;
408 T fInvH = 1.0f / fH;
409 for (i1 = 0; i1 <= i3; i1++)
410 {
411 matrix(i1, i0) = matrix(i0, i1) * fInvH;
412 fG = 0.0f;
413 for (i2 = 0; i2 <= i1; i2++)
414 fG += matrix(i1, i2) * matrix(i0, i2);
415 for (i2 = i1 + 1; i2 <= i3; i2++)
416 fG += matrix(i2, i1) * matrix(i0, i2);
417 subd[i1] = fG * fInvH;
418 fF += subd[i1] * matrix(i0, i1);
419 }
420 T fHalfFdivH = 0.5f * fF * fInvH;
421 for (i1 = 0; i1 <= i3; i1++)
422 {
423 fF = matrix(i0, i1);
424 fG = subd[i1] - fHalfFdivH * fF;
425 subd[i1] = fG;
426 for (i2 = 0; i2 <= i1; i2++)
427 {
428 matrix(i1, i2) -= fF * subd[i2] + fG * matrix(i0, i2);
429 }
430 }
431 }
432 }
433 else
434 {
435 subd[i0] = matrix(i0, i3);
436 }
437
438 diag[i0] = fH;
439 }
440
441 diag[0] = 0.0f;
442 subd[0] = 0.0f;
443 for (i0 = 0, i3 = -1; i0 <= n - 1; i0++, i3++)
444 {
445 if (diag[i0])
446 {
447 for (i1 = 0; i1 <= i3; i1++)
448 {
449 T fSum = 0.0f;
450 for (i2 = 0; i2 <= i3; i2++)
451 fSum += matrix(i0, i2) * matrix(i2, i1);
452 for (i2 = 0; i2 <= i3; i2++)
453 matrix(i2, i1) -= fSum * matrix(i2, i0);
454 }
455 }
456 diag[i0] = matrix(i0, i0);
457 matrix(i0, i0) = 1.0f;
458 for (i1 = 0; i1 <= i3; i1++)
459 {
460 matrix(i1, i0) = 0.0f;
461 matrix(i0, i1) = 0.0f;
462 }
463 }
464
465 // re-ordering if Eigen<T>::ql_algorithm is used subsequently
466 for (i0 = 1, i3 = 0; i0 < n; i0++, i3++)
467 subd[i3] = subd[i0];
468 subd[n - 1] = 0.0f;
469 }
470
471
472 template <class MAT, typename T>
473 bool EigenSolver<MAT, T>::ql_algorithm(int n, T* diag, T* subd, MAT& matrix)
474 {
475 const int iMaxIter = 32;
476
477 for (int i0 = 0; i0 < n; i0++)
478 {
479 int i1;
480 for (i1 = 0; i1 < iMaxIter; i1++)
481 {
482 int i2;
483 for (i2 = i0; i2 <= n - 2; i2++)
484 {
485 T fTmp = std::abs(diag[i2]) +
486 std::abs(diag[i2 + 1]);
487 if (std::abs(subd[i2]) + fTmp == fTmp)
488 break;
489 }
490 if (i2 == i0)
491 break;
492
493 T fG = (diag[i0 + 1] - diag[i0]) / (2.0f * subd[i0]);
494 T fR = std::sqrt(fG * fG + 1.0f);
495 if (fG < 0.0f)
496 fG = diag[i2] - diag[i0] + subd[i0] / (fG - fR);
497 else
498 fG = diag[i2] - diag[i0] + subd[i0] / (fG + fR);
499 T fSin = 1.0f, fCos = 1.0, fP = 0.0f;
500 for (int i3 = i2 - 1; i3 >= i0; i3--)
501 {
502 T fF = fSin * subd[i3];
503 T fB = fCos * subd[i3];
504 if (std::abs(fF) >= std::abs(fG))
505 {
506 fCos = fG / fF;
507 fR = std::sqrt(fCos * fCos + 1.0f);
508 subd[i3 + 1] = fF * fR;
509 fSin = 1.0f / fR;
510 fCos *= fSin;
511 }
512 else
513 {
514 fSin = fF / fG;
515 fR = std::sqrt(fSin * fSin + 1.0f);
516 subd[i3 + 1] = fG * fR;
517 fCos = 1.0f / fR;
518 fSin *= fCos;
519 }
520 fG = diag[i3 + 1] - fP;
521 fR = (diag[i3] - fG) * fSin + 2.0f * fB * fCos;
522 fP = fSin * fR;
523 diag[i3 + 1] = fG + fP;
524 fG = fCos * fR - fB;
525
526 for (int i4 = 0; i4 < n; i4++)
527 {
528 fF = matrix(i4, i3 + 1);
529 matrix(i4, i3 + 1) = fSin * matrix(i4, i3) + fCos * fF;
530 matrix(i4, i3) = fCos * matrix(i4, i3) - fSin * fF;
531 }
532 }
533 diag[i0] -= fP;
534 subd[i0] = fG;
535 subd[i2] = 0.0f;
536 }
537 if (i1 == iMaxIter)
538 return false;
539 }
540
541 return true;
542 }
543
544
545 template <class MAT, typename T>
546 void EigenSolver<MAT, T>::decreasing_sort(int n, T* eigval, MAT& eigvec)
547 {
548 // sort eigenvalues in decreasing order, e[0] >= ... >= e[n-1]
549 for (int i0 = 0, i1; i0 <= n - 2; i0++)
550 {
551 // locate maximum eigenvalue
552 i1 = i0;
553 T fMax = eigval[i1];
554 int i2;
555 for (i2 = i0 + 1; i2 < n; i2++)
556 {
557 if (eigval[i2] > fMax)
558 {
559 i1 = i2;
560 fMax = eigval[i1];
561 }
562 }
563
564 if (i1 != i0)
565 {
566 // swap eigenvalues
567 eigval[i1] = eigval[i0];
568 eigval[i0] = fMax;
569
570 // swap eigenvectors
571 for (i2 = 0; i2 < n; i2++)
572 {
573 T fTmp = eigvec(i2, i0);
574 eigvec(i2, i0) = eigvec(i2, i1);
575 eigvec(i2, i1) = fTmp;
576 }
577 }
578 }
579 }
580
581
582 template <class MAT, typename T>
583 void EigenSolver<MAT, T>::increasing_sort(int n, T* eigval, MAT& eigvec)
584 {
585 // sort eigenvalues in increasing order, e[0] <= ... <= e[n-1]
586 for (int i0 = 0, i1; i0 <= n - 2; i0++)
587 {
588 // locate minimum eigenvalue
589 i1 = i0;
590 T fMin = eigval[i1];
591 int i2;
592 for (i2 = i0 + 1; i2 < n; i2++)
593 {
594 if (eigval[i2] < fMin)
595 {
596 i1 = i2;
597 fMin = eigval[i1];
598 }
599 }
600
601 if (i1 != i0)
602 {
603 // swap eigenvalues
604 eigval[i1] = eigval[i0];
605 eigval[i0] = fMin;
606
607 // swap eigenvectors
608 for (i2 = 0; i2 < n; i2++)
609 {
610 T fTmp = eigvec(i2, i0);
611 eigvec(i2, i0) = eigvec(i2, i1);
612 eigvec(i2, i1) = fTmp;
613 }
614 }
615 }
616 }
617
618
619 template <class MAT, typename T>
620 void EigenSolver<MAT, T>::solve(const MAT& mat, SortingMethod sm /* = NO_SORTING*/)
621 {
622 matrix_ = mat;
623
624 switch (size_)
625 {
626 case 2:
627 tridiagonal_2(matrix_, diag_, subd_);
628 break;
629 case 3:
630 tridiagonal_3(matrix_, diag_, subd_);
631 break;
632 case 4:
633 tridiagonal_4(matrix_, diag_, subd_);
634 break;
635 default:
636 tridiagonal_n(size_, matrix_, diag_, subd_);
637 break;
638 }
639
640 ql_algorithm(size_, diag_, subd_, matrix_);
641
642 switch (sm)
643 {
644 case INCREASING:
645 increasing_sort(size_, diag_, matrix_);
646 break;
647 case DECREASING:
648 decreasing_sort(size_, diag_, matrix_);
649 break;
650 default:
651 break;
652 }
653 }
654}
655
656#endif // EASY3D_CORE_EIGEN_SOLVER_H
easy3d::EigenSolver::eigen_value
T eigen_value(int i) const
Retrieves the eigenvalue at the specified index.
Definition eigen_solver.h:65
easy3d::EigenSolver::solve
void solve(const MAT &mat, SortingMethod sm=NO_SORTING)
Computes the eigenvalues and eigenvectors of the specified input matrix.
Definition eigen_solver.h:620
easy3d::EigenSolver::eigen_vectors
const MAT & eigen_vectors()
Retrieves the matrix of eigenvectors, stored as columns.
Definition eigen_solver.h:84
easy3d::EigenSolver::eigen_vector
T eigen_vector(int comp, int i) const
Retrieves the specified component of the eigenvector at the given index.
Definition eigen_solver.h:72
easy3d::EigenSolver::eigen_values
T * eigen_values()
Retrieves the array of eigenvalues.
Definition eigen_solver.h:78
easy3d::EigenSolver::SortingMethod
SortingMethod
Enumeration of sorting methods for eigenvalues and their corresponding eigenvectors.
Definition eigen_solver.h:36
easy3d::EigenSolver::INCREASING
@ INCREASING
Sort in increasing order.
Definition eigen_solver.h:38
easy3d::EigenSolver::DECREASING
@ DECREASING
Sort in decreasing order.
Definition eigen_solver.h:39
easy3d::EigenSolver::NO_SORTING
@ NO_SORTING
No sorting.
Definition eigen_solver.h:37
easy3d::EigenSolver::~EigenSolver
~EigenSolver()
Destructor.
Definition eigen_solver.h:123
easy3d::EigenSolver::EigenSolver
EigenSolver(int n)
Constructs an EigenSolver with a specified matrix size.
Definition eigen_solver.h:114
easy3d
Definition collider.cpp:182