Mesh Oriented datABase  (version 5.5.1)
An array-based unstructured mesh library
Matrix3.hpp
Go to the documentation of this file.
1 /*
2  * MOAB, a Mesh-Oriented datABase, is a software component for creating,
3  * storing and accessing finite element mesh data.
4  *
5  * Copyright 2004 Sandia Corporation. Under the terms of Contract
6  * DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government
7  * retains certain rights in this software.
8  *
9  * This library is free software; you can redistribute it and/or
10  * modify it under the terms of the GNU Lesser General Public
11  * License as published by the Free Software Foundation; either
12  * version 2.1 of the License, or (at your option) any later version.
13  */
14 
15 /**\file Matrix3.hpp
16  *\author Jason Kraftcheck ([email protected])
17  *\date 2006-07-18
18  *\date 2012-08-2 Updated by rhl to be more generic. less code that does more!
19  * TODO: Remove all 'inline' keywords as it is only a suggestion to the compiler
20  * anyways, and it will ignore it or add it when it thinks its necessary.
21  *\date 2016-08-03 Updated to use Eigen3 support underneath to improve performance
22  */
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 // save diagnostic state
45 #pragma GCC diagnostic push
46 // turn off the specific warning. Can also use "-Wshadow"
47 #pragma GCC diagnostic ignored "-Wshadow"
48 #endif
49 
50 #define EIGEN_DEFAULT_TO_ROW_MAJOR
51 #define EIGEN_INITIALIZE_MATRICES_BY_ZERO
52 // #define EIGEN_NO_STATIC_ASSERT
53 #include "Eigen/Dense"
54 
55 #ifdef __GNUC__
56 // turn the warnings back on
57 #pragma GCC diagnostic pop
58 #endif
59 
60 #endif // ifdef MOAB_HAVE_EIGEN3
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 // We will rely on LAPACK directly
73 #ifdef WIN32
74 
75 // Should use second form below for windows but
76 // needed to do this to make it work.
77 // TODO: Need to clean this up
78 #define MOAB_dsyevd MOAB_FC_FUNC( dsyevd, DSYEVD )
79 #define MOAB_dgeev MOAB_FC_FUNC( dgeev, DGEEV )
80 
81 #else // ifndef WIN32
82 
83 #define MOAB_dsyevd MOAB_FC_WRAPPER( dsyevd, DSYEVD )
84 #define MOAB_dgeev MOAB_FC_WRAPPER( dgeev, DGEEV )
85 
86 #endif // ifdef WIN32
87 
88 extern "C" {
89 
90 // Computes all eigenvalues and, optionally, eigenvectors of a
91 // real symmetric matrix A. If eigenvectors are desired, it uses a
92 // divide and conquer algorithm.
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 // Computes for an N-by-N real nonsymmetric matrix A, the
106 // eigenvalues and, optionally, the left and/or right eigenvectors.
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 // ifdef MOAB_HAVE_LAPACK
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 } // namespace Matrix
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  // Default Constructor
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  // TODO: Deprecate this.
201  // Then we can go from three Constructors to one.
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  // TODO: not strictly correct as the Matrix3 object
225  // is a double d[ 9] so the only valid model of T is
226  // double, or any refinement (int, float)
227  //*but* it doesn't really matter anything else
228  // will fail to compile.
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  // Copy constructor
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  // Weird constructors
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  /*
320  * \deprecated { Use instead the constructor with explicit fourth argument, bool isRow, above }
321  *
322  */
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]; // Row Major
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  // get pointer to array of nine doubles
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  // Uncomment below if you want point-wise multiplication instead (.*)
484  // for (int i=0; i < Matrix3::size; ++i) _mat[i] *= m._mat[i];
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  // Determinant:= d(6)*subdet6 + d(7)*subdet7 + d(8)*subdet8;
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  // Determinant:= d(6)*subdet6 + d(7)*subdet7 + d(8)*subdet8;
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  /* Solve eigenproblem */
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  // devmat.assign( _mat, _mat + size );
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  // The result eigenvalues are ordered as high-->low
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  // Make an empty call to find the optimal work vector size
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  // The result eigenvalues are ordered as low-->high, but vectors are in rows of A.
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(); //.col(1)
650  return MB_SUCCESS;
651 
652 #elif defined( MOAB_HAVE_LAPACK )
653  return eigen_decomposition_lapack( bisSymmetric, evals, evecs );
654 
655  // Vector evals_native;
656  // Matrix3 evecs_native;
657  // eigen_decomposition_native( evals_native, evecs_native );
658  // // std::cout << "LAPACK: " << evals << " NATIVE: " << evals_native << " Error: " << evals-evals_native << std::endl;
659  // return MB_SUCCESS;
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; // Tolerance for stopping the iteration
670  constexpr int maxiters = 500;
671  Matrix3 matrix = *this;
672 
673  // Initialize the eigenvector matrix as the identity matrix
674  eigenvectors = moab::Matrix3::identity();
675 
676  // Function to perform a Jacobi rotation
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; // Zero the off-diagonal element
689 
690  // Update other elements
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  // Update the eigenvector matrix
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  // Iterate to apply Jacobi rotations to find
711  // eigenvalues and eigenvectors of a symmetric 3x3 matrix
712  bool converged = false;
713  for( int iter = 0; iter < maxiters; ++iter )
714  {
715  // Find the largest off-diagonal element
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  // If the largest off-diagonal element is smaller than tolerance, stop
732  if( maxOffDiag < EPSILON )
733  {
734  converged = true;
735  break;
736  }
737 
738  // Apply Jacobi rotation to zero out matrix[p][q]
739  if( fabs( matrix( p, q ) ) > EPSILON ) jacobiRotate( matrix, eigenvectors, p, q );
740  }
741 
742  // The diagonal elements of A are the eigenvalues
743  for( int i = 0; i < n; ++i )
744  {
745  // Extract eigenvalues (diagonal elements of the matrix)
746  eigenvalues[i] = matrix( i, i );
747  eigenvectors.col( i ).normalize();
748  }
749 
750  if( !converged )
751  {
752  // if iterative scheme did not converge, show warning and return with failure
753  std::cerr << "Jacobi method did not converge within " << maxiters << " iterations\n";
754  }
755  else
756  {
757  // iterative method converged; let us sort the eigenvalues and eigenvectors in increasing order
758  auto sortIndices = []( const CartVect& vec ) -> std::array< int, 3 > {
759  // Initialize the index array with values 0, 1, 2
760  std::array< int, 3 > indices = { 0, 1, 2 };
761 
762  // Sort the indices based on the values in the original vector
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  // eigenvectors.transpose_inplace();
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  // Identity matrix
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  // return Matrix::compute_inverse( *this, this->determinant() );
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; // invert the 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  // Calculate determinant of 2x2 submatrix composed of the
1094  // elements not in the passed row or column.
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 }; // class Matrix3
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 } // namespace moab
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 // MOAB_MATRIX3_OPERATORLESS
1199 
1200 #endif // MOAB_MATRIX3_HPP