Easy3D 2.5.3
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
23 template <typename FT>
24 class EigenSolver
25 {
26 public:
28 enum SortingMethod { NO_SORTING, INCREASING, DECREASING };
29
30 public:
33 explicit EigenSolver(int n);
34 ~EigenSolver();
35
38 void solve(FT** mat, SortingMethod sm = NO_SORTING);
39
41 FT eigen_value(int i) const { return diag_[i]; }
43 FT eigen_vector(int comp, int i) const { return matrix_[comp][i]; }
44
46 FT* eigen_values() { return diag_; }
48 FT** eigen_vectors() { return matrix_; }
49
50 protected:
51 int size_;
52 FT** matrix_;
53 FT* diag_;
54 FT* subd_;
55
56 private:
57 // Householder reduction to tridiagonal form
58 static void tridiagonal_2(FT** mat, FT* diag, FT* subd);
59 static void tridiagonal_3(FT** mat, FT* diag, FT* subd);
60 static void tridiagonal_4(FT** mat, FT* diag, FT* subd);
61 static void tridiagonal_n(int n, FT** mat, FT* diag, FT* subd);
62
63 // QL algorithm with implicit shifting, applies to tridiagonal matrices
64 static bool ql_algorithm(int n, FT* diag, FT* subd, FT** mat);
65
66 // sort eigenvalues and eigenvectors in the descending order
67 static void decreasing_sort(int n, FT* eigval, FT** eigvec);
68
69 // sort eigenvalues and eigenvectors in the ascending order
70 static void increasing_sort(int n, FT* eigval, FT** eigvec);
71 };
72
73
74 //----------------------------------------------------------------------------
75
76
77 template <typename FT>
78 EigenSolver<FT>::EigenSolver(int n)
79 {
80 assert( n >= 2 );
81 size_ = n;
82
83 matrix_ = nullptr;
84
85 diag_ = new FT[size_];
86 subd_ = new FT[size_];
87 }
88
89 template <typename FT>
90 EigenSolver<FT>::~EigenSolver()
91 {
92 delete[] subd_;
93 delete[] diag_;
94 }
95
96
97 template <typename FT>
98 inline void EigenSolver<FT>::tridiagonal_2(FT** matrix_, FT* diag_, FT* subd_)
99 {
100 // matrix is already tridiagonal
101 diag_[0] = matrix_[0][0];
102 diag_[1] = matrix_[1][1];
103 subd_[0] = matrix_[0][1];
104 subd_[1] = 0.0f;
105 matrix_[0][0] = 1.0f;
106 matrix_[0][1] = 0.0f;
107 matrix_[1][0] = 0.0f;
108 matrix_[1][1] = 1.0f;
109 }
110
111
112 template <typename FT>
113 inline void EigenSolver<FT>::tridiagonal_3(FT** matrix_, FT* diag_, FT* subd_)
114 {
115 FT fM00 = matrix_[0][0];
116 FT fM01 = matrix_[0][1];
117 FT fM02 = matrix_[0][2];
118 FT fM11 = matrix_[1][1];
119 FT fM12 = matrix_[1][2];
120 FT fM22 = matrix_[2][2];
121
122 diag_[0] = fM00;
123 subd_[2] = 0.0f;
124 if( fM02 != 0.0f )
125 {
126 FT fLength = std::sqrt(fM01*fM01+fM02*fM02);
127 FT fInvLength = 1.0f/fLength;
128 fM01 *= fInvLength;
129 fM02 *= fInvLength;
130 FT fQ = 2.0f*fM01*fM12+fM02*(fM22-fM11);
131 diag_[1] = fM11+fM02*fQ;
132 diag_[2] = fM22-fM02*fQ;
133 subd_[0] = fLength;
134 subd_[1] = fM12-fM01*fQ;
135 matrix_[0][0] = 1.0f; matrix_[0][1] = 0.0f; matrix_[0][2] = 0.0f;
136 matrix_[1][0] = 0.0f; matrix_[1][1] = fM01; matrix_[1][2] = fM02;
137 matrix_[2][0] = 0.0f; matrix_[2][1] = fM02; matrix_[2][2] = -fM01;
138 }
139 else
140 {
141 diag_[1] = fM11;
142 diag_[2] = fM22;
143 subd_[0] = fM01;
144 subd_[1] = fM12;
145 matrix_[0][0] = 1.0f; matrix_[0][1] = 0.0f; matrix_[0][2] = 0.0f;
146 matrix_[1][0] = 0.0f; matrix_[1][1] = 1.0f; matrix_[1][2] = 0.0f;
147 matrix_[2][0] = 0.0f; matrix_[2][1] = 0.0f; matrix_[2][2] = 1.0f;
148 }
149 }
150
151
152 template <typename FT>
153 inline void EigenSolver<FT>::tridiagonal_4(FT** matrix_, FT* diag_, FT* subd_)
154 {
155 // save matrix M
156 FT fM00 = matrix_[0][0];
157 FT fM01 = matrix_[0][1];
158 FT fM02 = matrix_[0][2];
159 FT fM03 = matrix_[0][3];
160 FT fM11 = matrix_[1][1];
161 FT fM12 = matrix_[1][2];
162 FT fM13 = matrix_[1][3];
163 FT fM22 = matrix_[2][2];
164 FT fM23 = matrix_[2][3];
165 FT fM33 = matrix_[3][3];
166
167 diag_[0] = fM00;
168 subd_[3] = 0.0f;
169
170 matrix_[0][0] = 1.0f;
171 matrix_[0][1] = 0.0f;
172 matrix_[0][2] = 0.0f;
173 matrix_[0][3] = 0.0f;
174 matrix_[1][0] = 0.0f;
175 matrix_[2][0] = 0.0f;
176 matrix_[3][0] = 0.0f;
177
178 FT fLength, fInvLength;
179
180 if( fM02 != 0.0f || fM03 != 0.0f )
181 {
182 FT fQ11, fQ12, fQ13;
183 FT fQ21, fQ22, fQ23;
184 FT fQ31, fQ32, fQ33;
185
186 // build column Q1
187 fLength = std::sqrt(fM01*fM01+fM02*fM02+fM03*fM03);
188 fInvLength = 1.0f/fLength;
189 fQ11 = fM01*fInvLength;
190 fQ21 = fM02*fInvLength;
191 fQ31 = fM03*fInvLength;
192
193 subd_[0] = fLength;
194
195 // compute S*Q1
196 FT fV0 = fM11*fQ11+fM12*fQ21+fM13*fQ31;
197 FT fV1 = fM12*fQ11+fM22*fQ21+fM23*fQ31;
198 FT fV2 = fM13*fQ11+fM23*fQ21+fM33*fQ31;
199
200 diag_[1] = fQ11*fV0+fQ21*fV1+fQ31*fV2;
201
202 // build column Q3 = Q1x(S*Q1)
203 fQ13 = fQ21*fV2-fQ31*fV1;
204 fQ23 = fQ31*fV0-fQ11*fV2;
205 fQ33 = fQ11*fV1-fQ21*fV0;
206 fLength = std::sqrt(fQ13*fQ13+fQ23*fQ23+fQ33*fQ33);
207 if( fLength > 0.0f )
208 {
209 fInvLength = 1.0f/fLength;
210 fQ13 *= fInvLength;
211 fQ23 *= fInvLength;
212 fQ33 *= fInvLength;
213
214 // build column Q2 = Q3xQ1
215 fQ12 = fQ23*fQ31-fQ33*fQ21;
216 fQ22 = fQ33*fQ11-fQ13*fQ31;
217 fQ32 = fQ13*fQ21-fQ23*fQ11;
218
219 fV0 = fQ12*fM11+fQ22*fM12+fQ32*fM13;
220 fV1 = fQ12*fM12+fQ22*fM22+fQ32*fM23;
221 fV2 = fQ12*fM13+fQ22*fM23+fQ32*fM33;
222 subd_[1] = fQ11*fV0+fQ21*fV1+fQ31*fV2;
223 diag_[2] = fQ12*fV0+fQ22*fV1+fQ32*fV2;
224 subd_[2] = fQ13*fV0+fQ23*fV1+fQ33*fV2;
225
226 fV0 = fQ13*fM11+fQ23*fM12+fQ33*fM13;
227 fV1 = fQ13*fM12+fQ23*fM22+fQ33*fM23;
228 fV2 = fQ13*fM13+fQ23*fM23+fQ33*fM33;
229 diag_[3] = fQ13*fV0+fQ23*fV1+fQ33*fV2;
230 }
231 else
232 {
233 // S*Q1 parallel to Q1, choose any valid Q2 and Q3
234 subd_[1] = 0.0f;
235
236 fLength = fQ21*fQ21+fQ31*fQ31;
237 if( fLength > 0.0f )
238 {
239 fInvLength = 1.0f/fLength;
240 FT fTmp = fQ11-1.0f;
241 fQ12 = -fQ21;
242 fQ22 = 1.0f+fTmp*fQ21*fQ21*fInvLength;
243 fQ32 = fTmp*fQ21*fQ31*fInvLength;
244
245 fQ13 = -fQ31;
246 fQ23 = fQ32;
247 fQ33 = 1.0f+fTmp*fQ31*fQ31*fInvLength;
248
249 fV0 = fQ12*fM11+fQ22*fM12+fQ32*fM13;
250 fV1 = fQ12*fM12+fQ22*fM22+fQ32*fM23;
251 fV2 = fQ12*fM13+fQ22*fM23+fQ32*fM33;
252 diag_[2] = fQ12*fV0+fQ22*fV1+fQ32*fV2;
253 subd_[2] = fQ13*fV0+fQ23*fV1+fQ33*fV2;
254
255 fV0 = fQ13*fM11+fQ23*fM12+fQ33*fM13;
256 fV1 = fQ13*fM12+fQ23*fM22+fQ33*fM23;
257 fV2 = fQ13*fM13+fQ23*fM23+fQ33*fM33;
258 diag_[3] = fQ13*fV0+fQ23*fV1+fQ33*fV2;
259 }
260 else
261 {
262 // Q1 =(+-1,0,0)
263 fQ12 = 0.0f; fQ22 = 1.0f; fQ32 = 0.0f;
264 fQ13 = 0.0f; fQ23 = 0.0f; fQ33 = 1.0f;
265
266 diag_[2] = fM22;
267 diag_[3] = fM33;
268 subd_[2] = fM23;
269 }
270 }
271
272 matrix_[1][1] = fQ11; matrix_[1][2] = fQ12; matrix_[1][3] = fQ13;
273 matrix_[2][1] = fQ21; matrix_[2][2] = fQ22; matrix_[2][3] = fQ23;
274 matrix_[3][1] = fQ31; matrix_[3][2] = fQ32; matrix_[3][3] = fQ33;
275 }
276 else
277 {
278 diag_[1] = fM11;
279 subd_[0] = fM01;
280 matrix_[1][1] = 1.0f;
281 matrix_[2][1] = 0.0f;
282 matrix_[3][1] = 0.0f;
283
284 if( fM13 != 0.0f )
285 {
286 fLength = std::sqrt(fM12*fM12+fM13*fM13);
287 fInvLength = 1.0f/fLength;
288 fM12 *= fInvLength;
289 fM13 *= fInvLength;
290 FT fQ = 2.0f*fM12*fM23+fM13*(fM33-fM22);
291
292 diag_[2] = fM22+fM13*fQ;
293 diag_[3] = fM33-fM13*fQ;
294 subd_[1] = fLength;
295 subd_[2] = fM23-fM12*fQ;
296 matrix_[1][2] = 0.0f;
297 matrix_[1][3] = 0.0f;
298 matrix_[2][2] = fM12;
299 matrix_[2][3] = fM13;
300 matrix_[3][2] = fM13;
301 matrix_[3][3] = -fM12;
302 }
303 else
304 {
305 diag_[2] = fM22;
306 diag_[3] = fM33;
307 subd_[1] = fM12;
308 subd_[2] = fM23;
309 matrix_[1][2] = 0.0f;
310 matrix_[1][3] = 0.0f;
311 matrix_[2][2] = 1.0f;
312 matrix_[2][3] = 0.0f;
313 matrix_[3][2] = 0.0f;
314 matrix_[3][3] = 1.0f;
315 }
316 }
317 }
318
319
320 template <typename FT>
321 inline void EigenSolver<FT>::tridiagonal_n(int n, FT** matrix_, FT* diag_, FT* subd_)
322 {
323 int i0, i1, i2, i3;
324
325 for(i0 = n-1, i3 = n-2; i0 >= 1; i0--, i3--)
326 {
327 FT fH = 0.0f, fScale = 0.0f;
328
329 if( i3 > 0 )
330 {
331 for(i2 = 0; i2 <= i3; i2++)
332 fScale += std::abs(matrix_[i0][i2]);
333 if( fScale == 0 )
334 {
335 subd_[i0] = matrix_[i0][i3];
336 }
337 else
338 {
339 FT fInvScale = 1.0f/fScale;
340 for(i2 = 0; i2 <= i3; i2++)
341 {
342 matrix_[i0][i2] *= fInvScale;
343 fH += matrix_[i0][i2]*matrix_[i0][i2];
344 }
345 FT fF = matrix_[i0][i3];
346 FT fG = std::sqrt(fH);
347 if( fF > 0.0f )
348 fG = -fG;
349 subd_[i0] = fScale*fG;
350 fH -= fF*fG;
351 matrix_[i0][i3] = fF-fG;
352 fF = 0.0f;
353 FT fInvH = 1.0f/fH;
354 for(i1 = 0; i1 <= i3; i1++)
355 {
356 matrix_[i1][i0] = matrix_[i0][i1]*fInvH;
357 fG = 0.0f;
358 for(i2 = 0; i2 <= i1; i2++)
359 fG += matrix_[i1][i2]*matrix_[i0][i2];
360 for(i2 = i1+1; i2 <= i3; i2++)
361 fG += matrix_[i2][i1]*matrix_[i0][i2];
362 subd_[i1] = fG*fInvH;
363 fF += subd_[i1]*matrix_[i0][i1];
364 }
365 FT fHalfFdivH = 0.5f*fF*fInvH;
366 for(i1 = 0; i1 <= i3; i1++)
367 {
368 fF = matrix_[i0][i1];
369 fG = subd_[i1] - fHalfFdivH*fF;
370 subd_[i1] = fG;
371 for(i2 = 0; i2 <= i1; i2++)
372 {
373 matrix_[i1][i2] -= fF*subd_[i2] +
374 fG*matrix_[i0][i2];
375 }
376 }
377 }
378 }
379 else
380 {
381 subd_[i0] = matrix_[i0][i3];
382 }
383
384 diag_[i0] = fH;
385 }
386
387 diag_[0] = 0.0f;
388 subd_[0] = 0.0f;
389 for(i0 = 0, i3 = -1; i0 <= n-1; i0++, i3++)
390 {
391 if( diag_[i0] )
392 {
393 for(i1 = 0; i1 <= i3; i1++)
394 {
395 FT fSum = 0.0f;
396 for(i2 = 0; i2 <= i3; i2++)
397 fSum += matrix_[i0][i2]*matrix_[i2][i1];
398 for(i2 = 0; i2 <= i3; i2++)
399 matrix_[i2][i1] -= fSum*matrix_[i2][i0];
400 }
401 }
402 diag_[i0] = matrix_[i0][i0];
403 matrix_[i0][i0] = 1.0f;
404 for(i1 = 0; i1 <= i3; i1++)
405 {
406 matrix_[i1][i0] = 0.0f;
407 matrix_[i0][i1] = 0.0f;
408 }
409 }
410
411 // re-ordering if Eigen<FT>::ql_algorithm is used subsequently
412 for(i0 = 1, i3 = 0; i0 < n; i0++, i3++)
413 subd_[i3] = subd_[i0];
414 subd_[n-1] = 0.0f;
415 }
416
417
418 template <typename FT>
419 inline bool EigenSolver<FT>::ql_algorithm(int n, FT* diag_, FT* subd_, FT** matrix_)
420 {
421 const int iMaxIter = 32;
422
423 for(int i0 = 0; i0 < n; i0++)
424 {
425 int i1;
426 for(i1 = 0; i1 < iMaxIter; i1++)
427 {
428 int i2;
429 for(i2 = i0; i2 <= n-2; i2++)
430 {
431 FT fTmp = std::abs(diag_[i2]) +
432 std::abs(diag_[i2+1]);
433 if( std::abs(subd_[i2]) + fTmp == fTmp )
434 break;
435 }
436 if( i2 == i0 )
437 break;
438
439 FT fG =(diag_[i0+1]-diag_[i0])/(2.0f*subd_[i0]);
440 FT fR = std::sqrt(fG*fG+1.0f);
441 if( fG < 0.0f )
442 fG = diag_[i2]-diag_[i0]+subd_[i0]/(fG-fR);
443 else
444 fG = diag_[i2]-diag_[i0]+subd_[i0]/(fG+fR);
445 FT fSin = 1.0f, fCos = 1.0, fP = 0.0f;
446 for(int i3 = i2-1; i3 >= i0; i3--)
447 {
448 FT fF = fSin*subd_[i3];
449 FT fB = fCos*subd_[i3];
450 if( std::abs(fF) >= std::abs(fG) )
451 {
452 fCos = fG/fF;
453 fR = std::sqrt(fCos*fCos+1.0f);
454 subd_[i3+1] = fF*fR;
455 fSin = 1.0f/fR;
456 fCos *= fSin;
457 }
458 else
459 {
460 fSin = fF/fG;
461 fR = std::sqrt(fSin*fSin+1.0f);
462 subd_[i3+1] = fG*fR;
463 fCos = 1.0f/fR;
464 fSin *= fCos;
465 }
466 fG = diag_[i3+1]-fP;
467 fR =(diag_[i3]-fG)*fSin+2.0f*fB*fCos;
468 fP = fSin*fR;
469 diag_[i3+1] = fG+fP;
470 fG = fCos*fR-fB;
471
472 for(int i4 = 0; i4 < n; i4++)
473 {
474 fF = matrix_[i4][i3+1];
475 matrix_[i4][i3+1] = fSin*matrix_[i4][i3]+fCos*fF;
476 matrix_[i4][i3] = fCos*matrix_[i4][i3]-fSin*fF;
477 }
478 }
479 diag_[i0] -= fP;
480 subd_[i0] = fG;
481 subd_[i2] = 0.0f;
482 }
483 if( i1 == iMaxIter )
484 return false;
485 }
486
487 return true;
488 }
489
490
491 template <typename FT>
492 inline void EigenSolver<FT>::decreasing_sort(int n, FT* eigval, FT** eigvec)
493 {
494 // sort eigenvalues in decreasing order, e[0] >= ... >= e[n-1]
495 for(int i0 = 0, i1; i0 <= n-2; i0++)
496 {
497 // locate maximum eigenvalue
498 i1 = i0;
499 FT fMax = eigval[i1];
500 int i2;
501 for(i2 = i0+1; i2 < n; i2++)
502 {
503 if( eigval[i2] > fMax )
504 {
505 i1 = i2;
506 fMax = eigval[i1];
507 }
508 }
509
510 if( i1 != i0 )
511 {
512 // swap eigenvalues
513 eigval[i1] = eigval[i0];
514 eigval[i0] = fMax;
515
516 // swap eigenvectors
517 for(i2 = 0; i2 < n; i2++)
518 {
519 FT fTmp = eigvec[i2][i0];
520 eigvec[i2][i0] = eigvec[i2][i1];
521 eigvec[i2][i1] = fTmp;
522 }
523 }
524 }
525 }
526
527
528 template <typename FT>
529 inline void EigenSolver<FT>::increasing_sort(int n, FT* eigval, FT** eigvec)
530 {
531 // sort eigenvalues in increasing order, e[0] <= ... <= e[n-1]
532 for(int i0 = 0, i1; i0 <= n-2; i0++)
533 {
534 // locate minimum eigenvalue
535 i1 = i0;
536 FT fMin = eigval[i1];
537 int i2;
538 for(i2 = i0+1; i2 < n; i2++)
539 {
540 if( eigval[i2] < fMin )
541 {
542 i1 = i2;
543 fMin = eigval[i1];
544 }
545 }
546
547 if( i1 != i0 )
548 {
549 // swap eigenvalues
550 eigval[i1] = eigval[i0];
551 eigval[i0] = fMin;
552
553 // swap eigenvectors
554 for(i2 = 0; i2 < n; i2++)
555 {
556 FT fTmp = eigvec[i2][i0];
557 eigvec[i2][i0] = eigvec[i2][i1];
558 eigvec[i2][i1] = fTmp;
559 }
560 }
561 }
562 }
563
564
565 template <typename FT>
566 inline void EigenSolver<FT>::solve(FT** mat, SortingMethod sm /* = NO_SORTING*/)
567 {
568 matrix_ = mat;
569
570 switch( size_ )
571 {
572 case 2:
573 tridiagonal_2(matrix_,diag_,subd_);
574 break;
575 case 3:
576 tridiagonal_3(matrix_,diag_,subd_);
577 break;
578 case 4:
579 tridiagonal_4(matrix_,diag_,subd_);
580 break;
581 default:
582 tridiagonal_n(size_,matrix_,diag_,subd_);
583 break;
584 }
585
586 ql_algorithm(size_,diag_,subd_,matrix_);
587
588 switch( sm )
589 {
590 case INCREASING:
591 increasing_sort(size_,diag_,matrix_);
592 break;
593 case DECREASING:
594 decreasing_sort(size_,diag_,matrix_);
595 break;
596 default:
597 break;
598 }
599 }
600
601}
602
603#endif // EASY3D_CORE_EIGEN_SOLVER_H
easy3d::EigenSolver
An easy-to-use eigen solver.
Definition: eigen_solver.h:25
easy3d::EigenSolver::eigen_value
FT eigen_value(int i) const
Returns the i_th eigenvalue.
Definition: eigen_solver.h:41
easy3d::EigenSolver::eigen_values
FT * eigen_values()
Returns the eigenvalues.
Definition: eigen_solver.h:46
easy3d::EigenSolver::solve
void solve(FT **mat, SortingMethod sm=NO_SORTING)
Computes the eigenvalues and eigenvectors of the input matrix mat.
Definition: eigen_solver.h:566
easy3d::EigenSolver::eigen_vector
FT eigen_vector(int comp, int i) const
Returns the comp_th component of the i_th eigenvector.
Definition: eigen_solver.h:43
easy3d::EigenSolver::eigen_vectors
FT ** eigen_vectors()
Returns the eigenvectors (stored as the columns of the returned matrix).
Definition: eigen_solver.h:48
easy3d::EigenSolver::SortingMethod
SortingMethod
The sorting method for the eigenvalues and their corresponding eigen vectors.
Definition: eigen_solver.h:28
easy3d::EigenSolver::EigenSolver
EigenSolver(int n)
Default constructor.
Definition: eigen_solver.h:78
easy3d
Definition: collider.cpp:182