1
14
15
23
24 #ifndef MOAB_MATRIX3_HPP
25 #define MOAB_MATRIX3_HPP
26
27 #include <iostream>
28 #include <iosfwd>
29 #include <limits>
30 #include <cmath>
31 #include <array>
32 #include <algorithm>
33 #include <cassert>
34
35 #include "moab/MOABConfig.h"
36 #include "moab/ErrorHandler.hpp"
37 #include "moab/Util.hpp"
38 #include "moab/Types.hpp"
39 #include "moab/CartVect.hpp"
40
41 #ifdef MOAB_HAVE_EIGEN3
42
43 #ifdef __GNUC__
44
45 #pragma GCC diagnostic push
46
47 #pragma GCC diagnostic ignored "-Wshadow"
48 #endif
49
50 #define EIGEN_DEFAULT_TO_ROW_MAJOR
51 #define EIGEN_INITIALIZE_MATRICES_BY_ZERO
52
53 #include "Eigen/Dense"
54
55 #ifdef __GNUC__
56
57 #pragma GCC diagnostic pop
58 #endif
59
60 #endif
61
62 #ifdef MOAB_HAVE_LAPACK
63
64 #if defined( MOAB_FC_FUNC_ )
65 #define MOAB_FC_WRAPPER MOAB_FC_FUNC_
66 #elif defined( MOAB_FC_FUNC )
67 #define MOAB_FC_WRAPPER MOAB_FC_FUNC
68 #else
69 #define MOAB_FC_WRAPPER( name, NAME ) name##_
70 #endif
71
72
73 #ifdef WIN32
74
75
76
77
78 #define MOAB_dsyevd MOAB_FC_FUNC( dsyevd, DSYEVD )
79 #define MOAB_dgeev MOAB_FC_FUNC( dgeev, DGEEV )
80
81 #else
82
83 #define MOAB_dsyevd MOAB_FC_WRAPPER( dsyevd, DSYEVD )
84 #define MOAB_dgeev MOAB_FC_WRAPPER( dgeev, DGEEV )
85
86 #endif
87
88 extern "C" {
89
90
91
92
93 void MOAB_dsyevd( char* jobz,
94 char* uplo,
95 int* n,
96 double a[],
97 int* lda,
98 double w[],
99 double work[],
100 int* lwork,
101 int iwork[],
102 int* liwork,
103 int* info );
104
105
106
107 void MOAB_dgeev( char* jobvl,
108 char* jobvr,
109 int* n,
110 double* a,
111 int* lda,
112 double* wr,
113 double* wi,
114 double* vl,
115 int* ldvl,
116 double* vr,
117 int* ldvr,
118 double* work,
119 int* lwork,
120 int* info );
121 }
122
123 #endif
124
125 #define MOAB_DMEMZERO( a, b ) memset( a, 0, ( b ) * sizeof( double ) )
126
127 namespace moab
128 {
129
130 namespace Matrix
131 {
132 template < typename Matrix >
133 inline Matrix mmult3( const Matrix& a, const Matrix& b )
134 {
135 return Matrix( a( 0, 0 ) * b( 0, 0 ) + a( 0, 1 ) * b( 1, 0 ) + a( 0, 2 ) * b( 2, 0 ),
136 a( 0, 0 ) * b( 0, 1 ) + a( 0, 1 ) * b( 1, 1 ) + a( 0, 2 ) * b( 2, 1 ),
137 a( 0, 0 ) * b( 0, 2 ) + a( 0, 1 ) * b( 1, 2 ) + a( 0, 2 ) * b( 2, 2 ),
138 a( 1, 0 ) * b( 0, 0 ) + a( 1, 1 ) * b( 1, 0 ) + a( 1, 2 ) * b( 2, 0 ),
139 a( 1, 0 ) * b( 0, 1 ) + a( 1, 1 ) * b( 1, 1 ) + a( 1, 2 ) * b( 2, 1 ),
140 a( 1, 0 ) * b( 0, 2 ) + a( 1, 1 ) * b( 1, 2 ) + a( 1, 2 ) * b( 2, 2 ),
141 a( 2, 0 ) * b( 0, 0 ) + a( 2, 1 ) * b( 1, 0 ) + a( 2, 2 ) * b( 2, 0 ),
142 a( 2, 0 ) * b( 0, 1 ) + a( 2, 1 ) * b( 1, 1 ) + a( 2, 2 ) * b( 2, 1 ),
143 a( 2, 0 ) * b( 0, 2 ) + a( 2, 1 ) * b( 1, 2 ) + a( 2, 2 ) * b( 2, 2 ) );
144 }
145
146 template < typename Matrix >
147 inline const Matrix inverse( const Matrix& d )
148 {
149 const double det = 1.0 / determinant3( d );
150 return inverse( d, det );
151 }
152
153 template < typename Vector, typename Matrix >
154 inline Vector vector_matrix( const Vector& v, const Matrix& m )
155 {
156 return Vector( v[0] * m( 0, 0 ) + v[1] * m( 1, 0 ) + v[2] * m( 2, 0 ),
157 v[0] * m( 0, 1 ) + v[1] * m( 1, 1 ) + v[2] * m( 2, 1 ),
158 v[0] * m( 0, 2 ) + v[1] * m( 1, 2 ) + v[2] * m( 2, 2 ) );
159 }
160
161 template < typename Vector, typename Matrix >
162 inline Vector matrix_vector( const Matrix& m, const Vector& v )
163 {
164 Vector res;
165 res[0] = v[0] * m( 0, 0 ) + v[1] * m( 0, 1 ) + v[2] * m( 0, 2 );
166 res[1] = v[0] * m( 1, 0 ) + v[1] * m( 1, 1 ) + v[2] * m( 1, 2 );
167 res[2] = v[0] * m( 2, 0 ) + v[1] * m( 2, 1 ) + v[2] * m( 2, 2 );
168 return res;
169 }
170
171 }
172
173 class Matrix3
174 {
175 public:
176 const static int size = 9;
177
178 private:
179 #ifdef MOAB_HAVE_EIGEN3
180 Eigen::Matrix3d _mat;
181 #else
182 double _mat[size];
183 #endif
184
185 public:
186
187 inline Matrix3()
188 {
189 #ifdef MOAB_HAVE_EIGEN3
190 _mat.fill( 0.0 );
191 #else
192 MOAB_DMEMZERO( _mat, Matrix3::size );
193 #endif
194 }
195
196 #ifdef MOAB_HAVE_EIGEN3
197 inline Matrix3( Eigen::Matrix3d mat ) : _mat( mat ) {}
198 #endif
199
200
201
202 inline Matrix3( double diagonal )
203 {
204 #ifdef MOAB_HAVE_EIGEN3
205 _mat << diagonal, 0.0, 0.0, 0.0, diagonal, 0.0, 0.0, 0.0, diagonal;
206 #else
207 MOAB_DMEMZERO( _mat, Matrix3::size );
208 _mat[0] = _mat[4] = _mat[8] = diagonal;
209 #endif
210 }
211
212 inline Matrix3( const CartVect& diagonal )
213 {
214 #ifdef MOAB_HAVE_EIGEN3
215 _mat << diagonal[0], 0.0, 0.0, 0.0, diagonal[1], 0.0, 0.0, 0.0, diagonal[2];
216 #else
217 MOAB_DMEMZERO( _mat, Matrix3::size );
218 _mat[0] = diagonal[0];
219 _mat[4] = diagonal[1];
220 _mat[8] = diagonal[2];
221 #endif
222 }
223
224
225
226
227
228
229 inline Matrix3( const std::vector< double >& diagonal )
230 {
231 #ifdef MOAB_HAVE_EIGEN3
232 _mat << diagonal[0], 0.0, 0.0, 0.0, diagonal[1], 0.0, 0.0, 0.0, diagonal[2];
233 #else
234 MOAB_DMEMZERO( _mat, Matrix3::size );
235 _mat[0] = diagonal[0];
236 _mat[4] = diagonal[1];
237 _mat[8] = diagonal[2];
238 #endif
239 }
240
241 inline Matrix3( double v00,
242 double v01,
243 double v02,
244 double v10,
245 double v11,
246 double v12,
247 double v20,
248 double v21,
249 double v22 )
250 {
251 #ifdef MOAB_HAVE_EIGEN3
252 _mat << v00, v01, v02, v10, v11, v12, v20, v21, v22;
253 #else
254 MOAB_DMEMZERO( _mat, Matrix3::size );
255 _mat[0] = v00;
256 _mat[1] = v01;
257 _mat[2] = v02;
258 _mat[3] = v10;
259 _mat[4] = v11;
260 _mat[5] = v12;
261 _mat[6] = v20;
262 _mat[7] = v21;
263 _mat[8] = v22;
264 #endif
265 }
266
267
268 Matrix3( const Matrix3& f )
269 {
270 #ifdef MOAB_HAVE_EIGEN3
271 _mat = f._mat;
272 #else
273 memcpy( _mat, f._mat, size * sizeof( double ) );
274 #endif
275 }
276
277
278 template < typename Vector >
279 inline Matrix3( const Vector& row0, const Vector& row1, const Vector& row2, const bool isRow )
280 {
281 #ifdef MOAB_HAVE_EIGEN3
282 if( isRow )
283 {
284 _mat << row0[0], row0[1], row0[2], row1[0], row1[1], row1[2], row2[0], row2[1], row2[2];
285 }
286 else
287 {
288 _mat << row0[0], row1[0], row2[0], row0[1], row1[1], row2[1], row0[2], row1[2], row2[2];
289 }
290 #else
291 MOAB_DMEMZERO( _mat, Matrix3::size );
292 if( isRow )
293 {
294 _mat[0] = row0[0];
295 _mat[1] = row0[1];
296 _mat[2] = row0[2];
297 _mat[3] = row1[0];
298 _mat[4] = row1[1];
299 _mat[5] = row1[2];
300 _mat[6] = row2[0];
301 _mat[7] = row2[1];
302 _mat[8] = row2[2];
303 }
304 else
305 {
306 _mat[0] = row0[0];
307 _mat[1] = row1[0];
308 _mat[2] = row2[0];
309 _mat[3] = row0[1];
310 _mat[4] = row1[1];
311 _mat[5] = row2[1];
312 _mat[6] = row0[2];
313 _mat[7] = row1[2];
314 _mat[8] = row2[2];
315 }
316 #endif
317 }
318
319
323 inline Matrix3( const double v[size] )
324 {
325 #ifdef MOAB_HAVE_EIGEN3
326 _mat << v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8];
327 #else
328 memcpy( _mat, v, size * sizeof( double ) );
329 #endif
330 }
331
332 inline void copyto( double v[Matrix3::size] )
333 {
334 #ifdef MOAB_HAVE_EIGEN3
335 std::copy( _mat.data(), _mat.data() + size, v );
336 #else
337 memcpy( v, _mat, size * sizeof( double ) );
338 #endif
339 }
340
341 inline Matrix3& operator=( const Matrix3& m )
342 {
343 #ifdef MOAB_HAVE_EIGEN3
344 _mat = m._mat;
345 #else
346 memcpy( _mat, m._mat, size * sizeof( double ) );
347 #endif
348 return *this;
349 }
350
351 inline Matrix3& operator=( const double v[size] )
352 {
353 #ifdef MOAB_HAVE_EIGEN3
354 _mat << v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8];
355 #else
356 memcpy( _mat, v, size * sizeof( double ) );
357 #endif
358 return *this;
359 }
360
361 inline double* operator[]( unsigned i )
362 {
363 #ifdef MOAB_HAVE_EIGEN3
364 return _mat.row( i ).data();
365 #else
366 return &_mat[i * 3];
367 #endif
368 }
369
370 inline const double* operator[]( unsigned i ) const
371 {
372 #ifdef MOAB_HAVE_EIGEN3
373 return _mat.row( i ).data();
374 #else
375 return &_mat[i * 3];
376 #endif
377 }
378
379 inline double& operator()( unsigned r, unsigned c )
380 {
381 #ifdef MOAB_HAVE_EIGEN3
382 return _mat( r, c );
383 #else
384 return _mat[r * 3 + c];
385 #endif
386 }
387
388 inline double operator()( unsigned r, unsigned c ) const
389 {
390 #ifdef MOAB_HAVE_EIGEN3
391 return _mat( r, c );
392 #else
393 return _mat[r * 3 + c];
394 #endif
395 }
396
397 inline double& operator()( unsigned i )
398 {
399 #ifdef MOAB_HAVE_EIGEN3
400 return _mat( i );
401 #else
402 return _mat[i];
403 #endif
404 }
405
406 inline double operator()( unsigned i ) const
407 {
408 #ifdef MOAB_HAVE_EIGEN3
409 return _mat( i );
410 #else
411 return _mat[i];
412 #endif
413 }
414
415
416 inline double* array()
417 {
418 #ifdef MOAB_HAVE_EIGEN3
419 return _mat.data();
420 #else
421 return _mat;
422 #endif
423 }
424
425 inline const double* array() const
426 {
427 #ifdef MOAB_HAVE_EIGEN3
428 return _mat.data();
429 #else
430 return _mat;
431 #endif
432 }
433
434 inline Matrix3& operator+=( const Matrix3& m )
435 {
436 #ifdef MOAB_HAVE_EIGEN3
437 _mat += m._mat;
438 #else
439 for( int i = 0; i < Matrix3::size; ++i )
440 _mat[i] += m._mat[i];
441 #endif
442 return *this;
443 }
444
445 inline Matrix3& operator-=( const Matrix3& m )
446 {
447 #ifdef MOAB_HAVE_EIGEN3
448 _mat -= m._mat;
449 #else
450 for( int i = 0; i < Matrix3::size; ++i )
451 _mat[i] -= m._mat[i];
452 #endif
453 return *this;
454 }
455
456 inline Matrix3& operator*=( double s )
457 {
458 #ifdef MOAB_HAVE_EIGEN3
459 _mat *= s;
460 #else
461 for( int i = 0; i < Matrix3::size; ++i )
462 _mat[i] *= s;
463 #endif
464 return *this;
465 }
466
467 inline Matrix3& operator/=( double s )
468 {
469 #ifdef MOAB_HAVE_EIGEN3
470 _mat /= s;
471 #else
472 for( int i = 0; i < Matrix3::size; ++i )
473 _mat[i] /= s;
474 #endif
475 return *this;
476 }
477
478 inline Matrix3& operator*=( const Matrix3& m )
479 {
480 #ifdef MOAB_HAVE_EIGEN3
481 _mat *= m._mat;
482 #else
483
484
485 std::vector< double > dmat;
486 dmat.assign( _mat, _mat + size );
487 _mat[0] = dmat[0] * m._mat[0] + dmat[1] * m._mat[3] + dmat[2] * m._mat[6];
488 _mat[1] = dmat[0] * m._mat[1] + dmat[1] * m._mat[4] + dmat[2] * m._mat[7];
489 _mat[2] = dmat[0] * m._mat[2] + dmat[1] * m._mat[5] + dmat[2] * m._mat[8];
490 _mat[3] = dmat[3] * m._mat[0] + dmat[4] * m._mat[3] + dmat[5] * m._mat[6];
491 _mat[4] = dmat[3] * m._mat[1] + dmat[4] * m._mat[4] + dmat[5] * m._mat[7];
492 _mat[5] = dmat[3] * m._mat[2] + dmat[4] * m._mat[5] + dmat[5] * m._mat[8];
493 _mat[6] = dmat[6] * m._mat[0] + dmat[7] * m._mat[3] + dmat[8] * m._mat[6];
494 _mat[7] = dmat[6] * m._mat[1] + dmat[7] * m._mat[4] + dmat[8] * m._mat[7];
495 _mat[8] = dmat[6] * m._mat[2] + dmat[7] * m._mat[5] + dmat[8] * m._mat[8];
496 #endif
497 return *this;
498 }
499
500 inline bool is_symmetric()
501 {
502 const double EPS = 1e-14;
503 #ifdef MOAB_HAVE_EIGEN3
504 if( ( fabs( _mat( 1 ) - _mat( 3 ) ) < EPS ) && ( fabs( _mat( 2 ) - _mat( 6 ) ) < EPS ) &&
505 ( fabs( _mat( 5 ) - _mat( 7 ) ) < EPS ) )
506 return true;
507 #else
508 if( ( fabs( _mat[1] - _mat[3] ) < EPS ) && ( fabs( _mat[2] - _mat[6] ) < EPS ) &&
509 ( fabs( _mat[5] - _mat[7] ) < EPS ) )
510 return true;
511 #endif
512 else
513 return false;
514 }
515
516 inline bool is_positive_definite()
517 {
518 #ifdef MOAB_HAVE_EIGEN3
519 double subdet6 = _mat( 1 ) * _mat( 5 ) - _mat( 2 ) * _mat( 4 );
520 double subdet7 = _mat( 2 ) * _mat( 3 ) - _mat( 0 ) * _mat( 5 );
521 double subdet8 = _mat( 0 ) * _mat( 4 ) - _mat( 1 ) * _mat( 3 );
522
523 const double det = _mat( 6 ) * subdet6 + _mat( 7 ) * subdet7 + _mat( 8 ) * subdet8;
524 return _mat( 0 ) > 0 && subdet8 > 0 && det > 0;
525 #else
526 double subdet6 = _mat[1] * _mat[5] - _mat[2] * _mat[4];
527 double subdet7 = _mat[2] * _mat[3] - _mat[0] * _mat[5];
528 double subdet8 = _mat[0] * _mat[4] - _mat[1] * _mat[3];
529
530 const double det = _mat[6] * subdet6 + _mat[7] * subdet7 + _mat[8] * subdet8;
531 return _mat[0] > 0 && subdet8 > 0 && det > 0;
532 #endif
533 }
534
535 #ifdef MOAB_HAVE_LAPACK
536 template < typename Vector >
537 inline ErrorCode eigen_decomposition_lapack( bool bisSymmetric, Vector& evals, Matrix3& evecs )
538 {
539 int info;
540
541 double devreal[3], drevecs[9];
542 if( !bisSymmetric )
543 {
544 double devimag[3], dlevecs[9], dwork[102];
545 char dgeev_opts[2] = { 'N', 'V' };
546 int N = 3, LWORK = 102, NL = 1, NR = N;
547 std::vector< double > devmat( 9 );
548 #ifdef MOAB_HAVE_EIGEN3
549
550 std::copy( _mat.data(), _mat.data() + size, devmat.data() );
551 #else
552 memcpy( devmat.data(), _mat, size * sizeof( double ) );
553 #endif
554 MOAB_dgeev( &dgeev_opts[0], &dgeev_opts[1], &N, &devmat[0], &N, devreal, devimag, dlevecs, &NL, drevecs,
555 &NR, dwork, &LWORK, &info );
556
557 evals[0] = devreal[2];
558 evals[1] = devreal[1];
559 evals[2] = devreal[0];
560 evecs._mat[0] = drevecs[6];
561 evecs._mat[1] = drevecs[3];
562 evecs._mat[2] = drevecs[0];
563 evecs._mat[3] = drevecs[7];
564 evecs._mat[4] = drevecs[4];
565 evecs._mat[5] = drevecs[1];
566 evecs._mat[6] = drevecs[8];
567 evecs._mat[7] = drevecs[5];
568 evecs._mat[8] = drevecs[2];
569 std::cout << "DGEEV: Optimal work vector: dsize = " << dwork[0] << ".\n";
570 }
571 else
572 {
573 char dgeev_opts[2] = { 'V', 'L' };
574 const bool find_optimal = false;
575 std::vector< int > iwork( 18 );
576 std::vector< double > devmat( 9, 0.0 );
577 std::vector< double > dwork( 38 );
578 int N = 3, lwork = 38, liwork = 18;
579 devmat[0] = _mat[0];
580 devmat[1] = _mat[1];
581 devmat[2] = _mat[2];
582 devmat[4] = _mat[4];
583 devmat[5] = _mat[5];
584 devmat[8] = _mat[8];
585 if( find_optimal )
586 {
587 int _lwork = -1;
588 int _liwork = -1;
589 double query_work_size = 0;
590 int query_iwork_size = 0;
591
592 MOAB_dsyevd( &dgeev_opts[0], &dgeev_opts[1], &N, NULL, &N, NULL, &query_work_size, &_lwork,
593 &query_iwork_size, &_liwork, &info );
594 lwork = (int)query_work_size;
595 dwork.resize( lwork );
596 liwork = query_iwork_size;
597 iwork.resize( liwork );
598 std::cout << "DSYEVD: Optimal work vector: dsize = " << lwork << ", and isize = " << liwork << ".\n";
599 }
600
601 MOAB_dsyevd( &dgeev_opts[0], &dgeev_opts[1], &N, &devmat[0], &N, devreal, &dwork[0], &lwork, &iwork[0],
602 &liwork, &info );
603 for( int i = 0; i < 9; ++i )
604 drevecs[i] = devmat[i];
605
606 evals[0] = devreal[0];
607 evals[1] = devreal[1];
608 evals[2] = devreal[2];
609 evecs._mat[0] = drevecs[0];
610 evecs._mat[3] = drevecs[1];
611 evecs._mat[6] = drevecs[2];
612 evecs._mat[1] = drevecs[3];
613 evecs._mat[4] = drevecs[4];
614 evecs._mat[7] = drevecs[5];
615 evecs._mat[2] = drevecs[6];
616 evecs._mat[5] = drevecs[7];
617 evecs._mat[8] = drevecs[8];
618 }
619
620 if( !info )
621 {
622 return MB_SUCCESS;
623 }
624 else
625 {
626 std::cout << "Failure in LAPACK_" << ( bisSymmetric ? "DSYEVD" : "DGEEV" )
627 << " call for eigen decomposition.\n";
628 std::cout << "Failed with error = " << info << ".\n";
629 return MB_FAILURE;
630 }
631 }
632 #endif
633
634 template < typename Vector >
635 inline ErrorCode eigen_decomposition( Vector& evals, Matrix3& evecs )
636 {
637 const bool bisSymmetric = this->is_symmetric();
638 if( !bisSymmetric )
639 {
640 MB_CHK_SET_ERR( MB_FAILURE, "Unsymmetric matrix implementation with Matrix3 are currently not supported." );
641 }
642 #if defined( MOAB_HAVE_EIGEN3 )
643 Eigen::SelfAdjointEigenSolver< Eigen::Matrix3d > eigensolver( this->_mat );
644 if( eigensolver.info() != Eigen::Success ) return MB_FAILURE;
645 const Eigen::SelfAdjointEigenSolver< Eigen::Matrix3d >::RealVectorType& e3evals = eigensolver.eigenvalues();
646 evals[0] = e3evals( 0 );
647 evals[1] = e3evals( 1 );
648 evals[2] = e3evals( 2 );
649 evecs._mat = eigensolver.eigenvectors();
650 return MB_SUCCESS;
651
652 #elif defined( MOAB_HAVE_LAPACK )
653 return eigen_decomposition_lapack( bisSymmetric, evals, evecs );
654
655
656
657
658
659
660 #else
661 return eigen_decomposition_native( evals, evecs );
662 #endif
663 }
664
665 template < typename Vector >
666 inline ErrorCode eigen_decomposition_native( Vector& eigenvalues, Matrix3& eigenvectors )
667 {
668 constexpr int n = 3;
669 constexpr double EPSILON = 1e-14;
670 constexpr int maxiters = 500;
671 Matrix3 matrix = *this;
672
673
674 eigenvectors = moab::Matrix3::identity();
675
676
677 auto jacobiRotate = []( moab::Matrix3& A, moab::Matrix3& V, int p, int q ) {
678 double theta, t, c, s;
679 theta = ( A( q, q ) - A( p, p ) ) / ( 2.0 * A( p, q ) );
680 t = ( theta >= 0 ) ? 1.0 / ( theta + std::sqrt( 1.0 + theta * theta ) )
681 : 1.0 / ( theta - std::sqrt( 1.0 + theta * theta ) );
682 c = 1.0 / std::sqrt( 1.0 + t * t );
683 s = t * c;
684
685 double app = A( p, p ), aqq = A( q, q ), apq = A( p, q );
686 A( p, p ) = c * c * app - 2.0 * c * s * apq + s * s * aqq;
687 A( q, q ) = s * s * app + 2.0 * c * s * apq + c * c * aqq;
688 A( p, q ) = A( q, p ) = 0.0;
689
690
691 for( int i = 0; i < 3; i++ )
692 {
693 if( i != p && i != q )
694 {
695 double aip = A( i, p ), aiq = A( i, q );
696 A( i, p ) = A( p, i ) = c * aip - s * aiq;
697 A( i, q ) = A( q, i ) = s * aip + c * aiq;
698 }
699 }
700
701
702 for( int i = 0; i < 3; i++ )
703 {
704 double vip = V( i, p ), viq = V( i, q );
705 V( i, p ) = c * vip - s * viq;
706 V( i, q ) = s * vip + c * viq;
707 }
708 };
709
710
711
712 bool converged = false;
713 for( int iter = 0; iter < maxiters; ++iter )
714 {
715
716 int p = 0, q = 1;
717 double maxOffDiag = std::abs( matrix( p, q ) );
718 for( int i = 0; i < n; ++i )
719 {
720 for( int j = i + 1; j < n; ++j )
721 {
722 if( std::abs( matrix( i, j ) ) > maxOffDiag )
723 {
724 maxOffDiag = std::abs( matrix( i, j ) );
725 p = i;
726 q = j;
727 }
728 }
729 }
730
731
732 if( maxOffDiag < EPSILON )
733 {
734 converged = true;
735 break;
736 }
737
738
739 if( fabs( matrix( p, q ) ) > EPSILON ) jacobiRotate( matrix, eigenvectors, p, q );
740 }
741
742
743 for( int i = 0; i < n; ++i )
744 {
745
746 eigenvalues[i] = matrix( i, i );
747 eigenvectors.col( i ).normalize();
748 }
749
750 if( !converged )
751 {
752
753 std::cerr << "Jacobi method did not converge within " << maxiters << " iterations\n";
754 }
755 else
756 {
757
758 auto sortIndices = []( const CartVect& vec ) -> std::array< int, 3 > {
759
760 std::array< int, 3 > indices = { 0, 1, 2 };
761
762
763 std::sort( indices.begin(), indices.end(), [&]( int i, int j ) { return vec[i] < vec[j]; } );
764
765 return indices;
766 };
767
768 auto newIndices = sortIndices( eigenvalues );
769
770
771 CartVect teigenvalues = eigenvalues;
772 Matrix3 teigenvectors = eigenvectors;
773 for( int i = 0; i < 3; i++ )
774 {
775 eigenvalues[i] = teigenvalues[newIndices[i]];
776 eigenvectors( i, 0 ) = teigenvectors( i, newIndices[0] );
777 eigenvectors( i, 1 ) = teigenvectors( i, newIndices[1] );
778 eigenvectors( i, 2 ) = teigenvectors( i, newIndices[2] );
779 }
780 }
781
782 return ( converged ? MB_SUCCESS : MB_FAILURE );
783 }
784
785 inline static Matrix3 identity()
786 {
787
788 return Matrix3( 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 );
789 }
790
791 inline void transpose_inplace()
792 {
793 #ifdef MOAB_HAVE_EIGEN3
794 _mat.transposeInPlace();
795 #else
796 Matrix3 mtmp( *this );
797 _mat[1] = mtmp._mat[3];
798 _mat[3] = mtmp._mat[1];
799 _mat[2] = mtmp._mat[6];
800 _mat[6] = mtmp._mat[2];
801 _mat[5] = mtmp._mat[7];
802 _mat[7] = mtmp._mat[5];
803 #endif
804 }
805
806 inline Matrix3 transpose() const
807 {
808 #ifdef MOAB_HAVE_EIGEN3
809 return Matrix3( _mat.transpose() );
810 #else
811 Matrix3 mtmp( *this );
812 mtmp._mat[1] = _mat[3];
813 mtmp._mat[3] = _mat[1];
814 mtmp._mat[2] = _mat[6];
815 mtmp._mat[6] = _mat[2];
816 mtmp._mat[5] = _mat[7];
817 mtmp._mat[7] = _mat[5];
818 return mtmp;
819 #endif
820 }
821
822 template < typename Vector >
823 inline void copycol( int index, Vector& vol )
824 {
825 #ifdef MOAB_HAVE_EIGEN3
826 _mat.col( index ).swap( vol );
827 #else
828 switch( index )
829 {
830 case 0:
831 _mat[0] = vol[0];
832 _mat[3] = vol[1];
833 _mat[6] = vol[2];
834 break;
835 case 1:
836 _mat[1] = vol[0];
837 _mat[4] = vol[1];
838 _mat[7] = vol[2];
839 break;
840 case 2:
841 _mat[2] = vol[0];
842 _mat[5] = vol[1];
843 _mat[8] = vol[2];
844 break;
845 }
846 #endif
847 }
848
849 inline void swapcol( int srcindex, int destindex )
850 {
851 assert( srcindex < Matrix3::size );
852 assert( destindex < Matrix3::size );
853 #ifdef MOAB_HAVE_EIGEN3
854 _mat.col( srcindex ).swap( _mat.col( destindex ) );
855 #else
856 CartVect svol = this->vcol< CartVect >( srcindex );
857 CartVect dvol = this->vcol< CartVect >( destindex );
858 switch( srcindex )
859 {
860 case 0:
861 _mat[0] = dvol[0];
862 _mat[3] = dvol[1];
863 _mat[6] = dvol[2];
864 break;
865 case 1:
866 _mat[1] = dvol[0];
867 _mat[4] = dvol[1];
868 _mat[7] = dvol[2];
869 break;
870 case 2:
871 _mat[2] = dvol[0];
872 _mat[5] = dvol[1];
873 _mat[8] = dvol[2];
874 break;
875 }
876 switch( destindex )
877 {
878 case 0:
879 _mat[0] = svol[0];
880 _mat[3] = svol[1];
881 _mat[6] = svol[2];
882 break;
883 case 1:
884 _mat[1] = svol[0];
885 _mat[4] = svol[1];
886 _mat[7] = svol[2];
887 break;
888 case 2:
889 _mat[2] = svol[0];
890 _mat[5] = svol[1];
891 _mat[8] = svol[2];
892 break;
893 }
894 #endif
895 }
896
897 template < typename Vector >
898 inline Vector vcol( int index ) const
899 {
900 assert( index < Matrix3::size );
901 #ifdef MOAB_HAVE_EIGEN3
902 return _mat.col( index );
903 #else
904 switch( index )
905 {
906 case 0:
907 return Vector( _mat[0], _mat[3], _mat[6] );
908 case 1:
909 return Vector( _mat[1], _mat[4], _mat[7] );
910 case 2:
911 return Vector( _mat[2], _mat[5], _mat[8] );
912 }
913 return Vector( 0.0 );
914 #endif
915 }
916
917 inline void colscale( int index, double scale )
918 {
919 assert( index < Matrix3::size );
920 #ifdef MOAB_HAVE_EIGEN3
921 _mat.col( index ) *= scale;
922 #else
923 switch( index )
924 {
925 case 0:
926 _mat[0] *= scale;
927 _mat[3] *= scale;
928 _mat[6] *= scale;
929 break;
930 case 1:
931 _mat[1] *= scale;
932 _mat[4] *= scale;
933 _mat[7] *= scale;
934 break;
935 case 2:
936 _mat[2] *= scale;
937 _mat[5] *= scale;
938 _mat[8] *= scale;
939 break;
940 }
941 #endif
942 }
943
944 inline void rowscale( int index, double scale )
945 {
946 assert( index < Matrix3::size );
947 #ifdef MOAB_HAVE_EIGEN3
948 _mat.row( index ) *= scale;
949 #else
950 switch( index )
951 {
952 case 0:
953 _mat[0] *= scale;
954 _mat[1] *= scale;
955 _mat[2] *= scale;
956 break;
957 case 1:
958 _mat[3] *= scale;
959 _mat[4] *= scale;
960 _mat[5] *= scale;
961 break;
962 case 2:
963 _mat[6] *= scale;
964 _mat[7] *= scale;
965 _mat[8] *= scale;
966 break;
967 }
968 #endif
969 }
970
971 inline CartVect col( int index ) const
972 {
973 assert( index < Matrix3::size );
974 #ifdef MOAB_HAVE_EIGEN3
975 Eigen::Vector3d mvec = _mat.col( index );
976 return CartVect( mvec[0], mvec[1], mvec[2] );
977 #else
978 switch( index )
979 {
980 case 0:
981 return CartVect( _mat[0], _mat[3], _mat[6] );
982 case 1:
983 return CartVect( _mat[1], _mat[4], _mat[7] );
984 case 2:
985 return CartVect( _mat[2], _mat[5], _mat[8] );
986 }
987 return CartVect( 0.0 );
988 #endif
989 }
990
991 inline CartVect row( int index ) const
992 {
993 assert( index < Matrix3::size );
994 #ifdef MOAB_HAVE_EIGEN3
995 Eigen::Vector3d mvec = _mat.row( index );
996 return CartVect( mvec[0], mvec[1], mvec[2] );
997 #else
998 switch( index )
999 {
1000 case 0:
1001 return CartVect( _mat[0], _mat[1], _mat[2] );
1002 case 1:
1003 return CartVect( _mat[3], _mat[4], _mat[5] );
1004 case 2:
1005 return CartVect( _mat[6], _mat[7], _mat[8] );
1006 }
1007 return CartVect( 0.0 );
1008 #endif
1009 }
1010
1011 inline CartVect diagonal() const
1012 {
1013 #ifdef MOAB_HAVE_EIGEN3
1014 return CartVect( _mat( 0, 0 ), _mat( 1, 1 ), _mat( 2, 2 ) );
1015 #else
1016 return CartVect( _mat[0], _mat[4], _mat[8] );
1017 #endif
1018 }
1019
1020 inline double trace() const
1021 {
1022 #ifdef MOAB_HAVE_EIGEN3
1023 return _mat.trace();
1024 #else
1025 return _mat[0] + _mat[4] + _mat[8];
1026 #endif
1027 }
1028
1029 friend Matrix3 operator+( const Matrix3& a, const Matrix3& b );
1030 friend Matrix3 operator-( const Matrix3& a, const Matrix3& b );
1031 friend Matrix3 operator*( const Matrix3& a, const Matrix3& b );
1032
1033 inline double determinant() const
1034 {
1035 #ifdef MOAB_HAVE_EIGEN3
1036 return _mat.determinant();
1037 #else
1038 return ( _mat[0] * _mat[4] * _mat[8] + _mat[1] * _mat[5] * _mat[6] + _mat[2] * _mat[3] * _mat[7] -
1039 _mat[0] * _mat[5] * _mat[7] - _mat[1] * _mat[3] * _mat[8] - _mat[2] * _mat[4] * _mat[6] );
1040 #endif
1041 }
1042
1043 inline Matrix3 inverse() const
1044 {
1045 #ifdef MOAB_HAVE_EIGEN3
1046 return Matrix3( _mat.inverse() );
1047 #else
1048
1049 Matrix3 m( 0.0 );
1050 const double d_determinant = 1.0 / this->determinant();
1051 m._mat[0] = d_determinant * ( _mat[4] * _mat[8] - _mat[5] * _mat[7] );
1052 m._mat[1] = d_determinant * ( _mat[2] * _mat[7] - _mat[8] * _mat[1] );
1053 m._mat[2] = d_determinant * ( _mat[1] * _mat[5] - _mat[4] * _mat[2] );
1054 m._mat[3] = d_determinant * ( _mat[5] * _mat[6] - _mat[8] * _mat[3] );
1055 m._mat[4] = d_determinant * ( _mat[0] * _mat[8] - _mat[6] * _mat[2] );
1056 m._mat[5] = d_determinant * ( _mat[2] * _mat[3] - _mat[5] * _mat[0] );
1057 m._mat[6] = d_determinant * ( _mat[3] * _mat[7] - _mat[6] * _mat[4] );
1058 m._mat[7] = d_determinant * ( _mat[1] * _mat[6] - _mat[7] * _mat[0] );
1059 m._mat[8] = d_determinant * ( _mat[0] * _mat[4] - _mat[3] * _mat[1] );
1060 return m;
1061 #endif
1062 }
1063
1064 inline bool invert()
1065 {
1066 bool invertible = false;
1067 double d_determinant;
1068 #ifdef MOAB_HAVE_EIGEN3
1069 Eigen::Matrix3d invMat;
1070 _mat.computeInverseAndDetWithCheck( invMat, d_determinant, invertible );
1071 if( !Util::is_finite( d_determinant ) ) return false;
1072 _mat = invMat;
1073 return invertible;
1074 #else
1075 d_determinant = this->determinant();
1076 if( d_determinant > 1e-13 ) invertible = true;
1077 d_determinant = 1.0 / d_determinant;
1078 std::vector< double > _m;
1079 _m.assign( _mat, _mat + size );
1080 _mat[0] = d_determinant * ( _m[4] * _m[8] - _m[5] * _m[7] );
1081 _mat[1] = d_determinant * ( _m[2] * _m[7] - _m[8] * _m[1] );
1082 _mat[2] = d_determinant * ( _m[1] * _m[5] - _m[4] * _m[2] );
1083 _mat[3] = d_determinant * ( _m[5] * _m[6] - _m[8] * _m[3] );
1084 _mat[4] = d_determinant * ( _m[0] * _m[8] - _m[6] * _m[2] );
1085 _mat[5] = d_determinant * ( _m[2] * _m[3] - _m[5] * _m[0] );
1086 _mat[6] = d_determinant * ( _m[3] * _m[7] - _m[6] * _m[4] );
1087 _mat[7] = d_determinant * ( _m[1] * _m[6] - _m[7] * _m[0] );
1088 _mat[8] = d_determinant * ( _m[0] * _m[4] - _m[3] * _m[1] );
1089 #endif
1090 return invertible;
1091 }
1092
1093
1094
1095 inline double subdet( int r, int c ) const
1096 {
1097 assert( r >= 0 && c >= 0 );
1098 if( r < 0 || c < 0 ) return DBL_MAX;
1099 #ifdef MOAB_HAVE_EIGEN3
1100 const int r1 = ( r + 1 ) % 3, r2 = ( r + 2 ) % 3;
1101 const int c1 = ( c + 1 ) % 3, c2 = ( c + 2 ) % 3;
1102 return _mat( r1, c1 ) * _mat( r2, c2 ) - _mat( r1, c2 ) * _mat( r2, c1 );
1103 #else
1104 const int r1 = Matrix3::size * ( ( r + 1 ) % 3 ), r2 = Matrix3::size * ( ( r + 2 ) % 3 );
1105 const int c1 = ( c + 1 ) % 3, c2 = ( c + 2 ) % 3;
1106 return _mat[r1 + c1] * _mat[r2 + c2] - _mat[r1 + c2] * _mat[r2 + c1];
1107 #endif
1108 }
1109
1110 inline void print( std::ostream& s ) const
1111 {
1112 #ifdef MOAB_HAVE_EIGEN3
1113 s << "| " << _mat( 0 ) << " " << _mat( 1 ) << " " << _mat( 2 ) << " | " << _mat( 3 ) << " " << _mat( 4 ) << " "
1114 << _mat( 5 ) << " | " << _mat( 6 ) << " " << _mat( 7 ) << " " << _mat( 8 ) << " |";
1115 #else
1116 s << "| " << _mat[0] << " " << _mat[1] << " " << _mat[2] << " | " << _mat[3] << " " << _mat[4] << " " << _mat[5]
1117 << " | " << _mat[6] << " " << _mat[7] << " " << _mat[8] << " |";
1118 #endif
1119 }
1120
1121 };
1122
1123 template < typename Vector >
1124 inline Matrix3 outer_product( const Vector& u, const Vector& v )
1125 {
1126 return Matrix3( u[0] * v[0], u[0] * v[1], u[0] * v[2], u[1] * v[0], u[1] * v[1], u[1] * v[2], u[2] * v[0],
1127 u[2] * v[1], u[2] * v[2] );
1128 }
1129
1130 inline Matrix3 operator+( const Matrix3& a, const Matrix3& b )
1131 {
1132 #ifdef MOAB_HAVE_EIGEN3
1133 return Matrix3( a._mat + b._mat );
1134 #else
1135 Matrix3 s( a );
1136 for( int i = 0; i < Matrix3::size; ++i )
1137 s( i ) += b._mat[i];
1138 return s;
1139 #endif
1140 }
1141
1142 inline Matrix3 operator-( const Matrix3& a, const Matrix3& b )
1143 {
1144 #ifdef MOAB_HAVE_EIGEN3
1145 return Matrix3( a._mat - b._mat );
1146 #else
1147 Matrix3 s( a );
1148 for( int i = 0; i < Matrix3::size; ++i )
1149 s( i ) -= b._mat[i];
1150 return s;
1151 #endif
1152 }
1153
1154 inline Matrix3 operator*( const Matrix3& a, const Matrix3& b )
1155 {
1156 #ifdef MOAB_HAVE_EIGEN3
1157 return Matrix3( a._mat * b._mat );
1158 #else
1159 return Matrix::mmult3( a, b );
1160 #endif
1161 }
1162
1163 template < typename T >
1164 inline std::vector< T > operator*( const Matrix3& m, const std::vector< T >& v )
1165 {
1166 return moab::Matrix::matrix_vector( m, v );
1167 }
1168
1169 template < typename T >
1170 inline std::vector< T > operator*( const std::vector< T >& v, const Matrix3& m )
1171 {
1172 return moab::Matrix::vector_matrix( v, m );
1173 }
1174
1175 inline CartVect operator*( const Matrix3& m, const CartVect& v )
1176 {
1177 return moab::Matrix::matrix_vector( m, v );
1178 }
1179
1180 inline CartVect operator*( const CartVect& v, const Matrix3& m )
1181 {
1182 return moab::Matrix::vector_matrix( v, m );
1183 }
1184
1185 }
1186
1187 #ifdef MOAB_HAVE_LAPACK
1188 #undef MOAB_DMEMZERO
1189 #endif
1190
1191 #ifndef MOAB_MATRIX3_OPERATORLESS
1192 #define MOAB_MATRIX3_OPERATORLESS
1193 inline std::ostream& operator<<( std::ostream& s, const moab::Matrix3& m )
1194 {
1195 return s << "| " << m( 0, 0 ) << " " << m( 0, 1 ) << " " << m( 0, 2 ) << " | " << m( 1, 0 ) << " " << m( 1, 1 )
1196 << " " << m( 1, 2 ) << " | " << m( 2, 0 ) << " " << m( 2, 1 ) << " " << m( 2, 2 ) << " |";
1197 }
1198 #endif
1199
1200 #endif