Branch data Line data Source code
1 : : /* *****************************************************************
2 : : MESQUITE -- The Mesh Quality Improvement Toolkit
3 : :
4 : : Copyright 2004 Sandia Corporation and Argonne National
5 : : Laboratory. Under the terms of Contract DE-AC04-94AL85000
6 : : with Sandia Corporation, the U.S. Government retains certain
7 : : 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 : : This library is distributed in the hope that it will be useful,
15 : : but WITHOUT ANY WARRANTY; without even the implied warranty of
16 : : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 : : Lesser General Public License for more details.
18 : :
19 : : You should have received a copy of the GNU Lesser General Public License
20 : : (lgpl.txt) along with this library; if not, write to the Free Software
21 : : Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
22 : :
23 : : [email protected], [email protected], [email protected],
24 : : [email protected], [email protected], [email protected]
25 : :
26 : : ***************************************************************** */
27 : : //
28 : : // AUTHOR: Thomas Leurent <[email protected]>
29 : : // ORG: Argonne National Laboratory
30 : : // E-MAIL: [email protected]
31 : : //
32 : : // ORIG-DATE: 18-Dec-02 at 11:08:22
33 : : // LAST-MOD: 27-May-04 at 14:48:56 by Thomas Leurent
34 : : //
35 : : // DESCRIPTION:
36 : : // ============
37 : : /*! \file Matrix3D.hpp
38 : :
39 : : 3*3 Matric class, row-oriented, 0-based [i][j] indexing.
40 : :
41 : : \author Thomas Leurent
42 : :
43 : : */
44 : : // DESCRIP-END.
45 : : //
46 : :
47 : : #ifndef Matrix3D_hpp
48 : : #define Matrix3D_hpp
49 : :
50 : : #include <iostream>
51 : : #include <sstream>
52 : : #include <cstdlib>
53 : :
54 : : #include "Mesquite.hpp"
55 : : #include "Vector3D.hpp"
56 : : #include "SymMatrix3D.hpp"
57 : :
58 : : namespace MBMesquite
59 : : {
60 : :
61 : : /*! \class Matrix3D
62 : : \brief 3*3 Matric class, row-oriented, 0-based [i][j] indexing.
63 : :
64 : : Since the size of the object is fixed at compile time, the Matrix3D
65 : : object is as fast as a double[9] array.
66 : : */
67 : : class MESQUITE_EXPORT Matrix3D
68 : : {
69 : : protected:
70 : : double v_[9];
71 : :
72 : 6296524 : void copy( const double* v )
73 : : {
74 : 6296524 : v_[0] = v[0];
75 : 6296524 : v_[1] = v[1];
76 : 6296524 : v_[2] = v[2];
77 : 6296524 : v_[3] = v[3];
78 : 6296524 : v_[4] = v[4];
79 : 6296524 : v_[5] = v[5];
80 : 6296524 : v_[6] = v[6];
81 : 6296524 : v_[7] = v[7];
82 : 6296524 : v_[8] = v[8];
83 : 6296524 : }
84 : :
85 : 396452 : void set( double val )
86 : : {
87 : 396452 : v_[0] = val;
88 : 396452 : v_[1] = val;
89 : 396452 : v_[2] = val;
90 : 396452 : v_[3] = val;
91 : 396452 : v_[4] = val;
92 : 396452 : v_[5] = val;
93 : 396452 : v_[6] = val;
94 : 396452 : v_[7] = val;
95 : 396452 : v_[8] = val;
96 : 396452 : }
97 : :
98 : : inline void set_values( const char* s );
99 : :
100 : : public:
101 : : // constructors
102 : : //! Default constructor sets all entries to 0.
103 : 4400119 : Matrix3D()
104 : : {
105 : 4400119 : zero();
106 : 4400119 : }
107 : :
108 : 5317759 : Matrix3D( const Matrix3D& A )
109 : : {
110 : 5317759 : copy( A.v_ );
111 : 5317759 : }
112 : :
113 : : //! sets all entries of the matrix to value.
114 : 244358 : Matrix3D( double value )
115 : : {
116 : 244358 : set( value );
117 : 244358 : }
118 : :
119 : 0 : Matrix3D( double a00, double a01, double a02, double a10, double a11, double a12, double a20, double a21,
120 : : double a22 )
121 : : {
122 : 0 : v_[0] = a00;
123 : 0 : v_[1] = a01;
124 : 0 : v_[2] = a02;
125 : 0 : v_[3] = a10;
126 : 0 : v_[4] = a11;
127 : 0 : v_[5] = a12;
128 : 0 : v_[6] = a20;
129 : 0 : v_[7] = a21;
130 : 0 : v_[8] = a22;
131 : 0 : }
132 : :
133 : : Matrix3D( const Vector3D& col1, const Vector3D& col2, const Vector3D& col3 )
134 : : {
135 : : set_column( 0, col1 );
136 : : set_column( 1, col2 );
137 : : set_column( 2, col3 );
138 : : }
139 : :
140 : : Matrix3D( double radians, const Vector3D& axis )
141 : : {
142 : : Vector3D v( axis );
143 : : v.normalize();
144 : : const double c = std::cos( radians );
145 : : const double s = std::sin( radians );
146 : : v_[0] = c + ( 1.0 - c ) * v[0] * v[0];
147 : : v_[1] = -v[2] * s + ( 1.0 - c ) * v[0] * v[1];
148 : : v_[2] = v[1] * s + ( 1.0 - c ) * v[0] * v[2];
149 : : v_[3] = v[2] * s + ( 1.0 - c ) * v[0] * v[1];
150 : : v_[4] = c + ( 1.0 - c ) * v[1] * v[1];
151 : : v_[5] = -v[0] * s + ( 1.0 - c ) * v[1] * v[2];
152 : : v_[6] = -v[1] * s + ( 1.0 - c ) * v[0] * v[2];
153 : : v_[7] = v[0] * s + ( 1.0 - c ) * v[1] * v[2];
154 : : v_[8] = c + ( 1.0 - c ) * v[2] * v[2];
155 : : }
156 : :
157 : : //! sets matrix entries to values in array.
158 : : //! \param v is an array of 9 doubles.
159 : 1 : Matrix3D( const double* v )
160 : : {
161 : 1 : copy( v );
162 : 1 : }
163 : :
164 : : //! for test purposes, matrices can be instantiated as
165 : : //! \code Matrix3D A("3 2 1 4 5 6 9 8 7"); \endcode
166 : : Matrix3D( const char* s )
167 : : {
168 : : set_values( s );
169 : : }
170 : :
171 : : Matrix3D( const SymMatrix3D& m )
172 : : {
173 : : *this = m;
174 : : }
175 : :
176 : : // destructor
177 : 9953170 : ~Matrix3D() {}
178 : :
179 : : // assignments
180 : 978764 : Matrix3D& operator=( const Matrix3D& A )
181 : : {
182 : 978764 : copy( A.v_ );
183 : 978764 : return *this;
184 : : }
185 : :
186 : : Matrix3D& operator=( const SymMatrix3D& m )
187 : : {
188 : : v_[0] = m[0];
189 : : v_[1] = v_[3] = m[1];
190 : : v_[2] = v_[6] = m[2];
191 : : v_[4] = m[3];
192 : : v_[5] = v_[7] = m[4];
193 : : v_[8] = m[5];
194 : : return *this;
195 : : }
196 : :
197 : 152094 : Matrix3D& operator=( double scalar )
198 : : {
199 : 152094 : set( scalar );
200 : 152094 : return *this;
201 : : }
202 : :
203 : : //! for test purposes, matrices can be assigned as follows
204 : : //! \code A = "3 2 1 4 5 6 9 8 7"; \endcode
205 : : Matrix3D& operator=( const char* s )
206 : : {
207 : : set_values( s );
208 : : return *this;
209 : : }
210 : :
211 : : //! Sets all entries to zero (more efficient than assignement).
212 : 5827731 : void zero()
213 : : {
214 : 5827731 : v_[0] = 0.;
215 : 5827731 : v_[1] = 0.;
216 : 5827731 : v_[2] = 0.;
217 : 5827731 : v_[3] = 0.;
218 : 5827731 : v_[4] = 0.;
219 : 5827731 : v_[5] = 0.;
220 : 5827731 : v_[6] = 0.;
221 : 5827731 : v_[7] = 0.;
222 : 5827731 : v_[8] = 0.;
223 : 5827731 : }
224 : :
225 : : void identity()
226 : : {
227 : : v_[0] = 1.;
228 : : v_[1] = 0.;
229 : : v_[2] = 0.;
230 : : v_[3] = 0.;
231 : : v_[4] = 1.;
232 : : v_[5] = 0.;
233 : : v_[6] = 0.;
234 : : v_[7] = 0.;
235 : : v_[8] = 1.;
236 : : }
237 : :
238 : : //! Sets column j (0, 1 or 2) to Vector3D c.
239 : : void set_column( int j, const Vector3D& c )
240 : : {
241 : : v_[0 + j] = c[0];
242 : : v_[3 + j] = c[1];
243 : : v_[6 + j] = c[2];
244 : : }
245 : :
246 : : //! returns the column length -- i is 0-based.
247 : : double column_length( int i ) const
248 : : {
249 : : return sqrt( v_[0 + i] * v_[0 + i] + v_[3 + i] * v_[3 + i] + v_[6 + i] * v_[6 + i] );
250 : : }
251 : :
252 : : double sub_det( int r, int c ) const
253 : : {
254 : : int r1 = 3 * ( ( r + 1 ) % 3 );
255 : : int r2 = 3 * ( ( r + 2 ) % 3 );
256 : : int c1 = ( ( c + 1 ) % 3 );
257 : : int c2 = ( ( c + 2 ) % 3 );
258 : : return v_[r1 + c1] * v_[r2 + c2] - v_[r2 + c1] * v_[r1 + c2];
259 : : }
260 : :
261 : : // Matrix Operators
262 : : friend bool operator==( const Matrix3D& lhs, const Matrix3D& rhs );
263 : : friend bool operator!=( const Matrix3D& lhs, const Matrix3D& rhs );
264 : : friend Matrix3D operator-( const Matrix3D& A );
265 : : friend double Frobenius_2( const Matrix3D& A );
266 : : friend Matrix3D transpose( const Matrix3D& A );
267 : : inline Matrix3D& transpose();
268 : : friend const Matrix3D operator+( const Matrix3D& A, const Matrix3D& B );
269 : : friend const Matrix3D operator-( const Matrix3D& A, const Matrix3D& B );
270 : : friend const Matrix3D operator*( const Matrix3D& A, const Matrix3D& B );
271 : : inline Matrix3D& equal_mult_elem( const Matrix3D& A );
272 : : friend const Matrix3D mult_element( const Matrix3D& A, const Matrix3D& B );
273 : : inline Matrix3D& assign_product( const Matrix3D& A, const Matrix3D& B );
274 : : friend void matmult( Matrix3D& C, const Matrix3D& A, const Matrix3D& B );
275 : : friend const Vector3D operator*( const Matrix3D& A, const Vector3D& x );
276 : : friend const Vector3D operator*( const Vector3D& x, const Matrix3D& A );
277 : : const Matrix3D operator*( double s ) const;
278 : : friend const Matrix3D operator*( double s, const Matrix3D& A );
279 : : void operator+=( const Matrix3D& rhs );
280 : : void operator+=( const SymMatrix3D& rhs );
281 : : void operator-=( const Matrix3D& rhs );
282 : : void operator-=( const SymMatrix3D& rhs );
283 : : void operator*=( double s );
284 : : friend Matrix3D plus_transpose( const Matrix3D& A, const Matrix3D& B );
285 : : Matrix3D& plus_transpose_equal( const Matrix3D& B );
286 : : Matrix3D& outer_product( const Vector3D& v1, const Vector3D& v2 );
287 : : void fill_lower_triangle();
288 : :
289 : : //! \f$ v = A*x \f$
290 : : friend void eqAx( Vector3D& v, const Matrix3D& A, const Vector3D& x );
291 : : //! \f$ v += A*x \f$
292 : : friend void plusEqAx( Vector3D& v, const Matrix3D& A, const Vector3D& x );
293 : : friend void eqTransAx( Vector3D& v, const Matrix3D& A, const Vector3D& x );
294 : : //! \f$ v += A^T*x \f$
295 : : friend void plusEqTransAx( Vector3D& v, const Matrix3D& A, const Vector3D& x );
296 : :
297 : : //! \f$ B += a*A \f$
298 : : friend void plusEqaA( Matrix3D& B, const double a, const Matrix3D& A );
299 : :
300 : : //! determinant of matrix A, det(A).
301 : : friend double det( const Matrix3D& A );
302 : :
303 : : //! \f$ B = A^{-1} \f$
304 : : friend void inv( Matrix3D& B, const Matrix3D& A );
305 : :
306 : : //! \f$ B *= A^{-1} \f$
307 : : friend void timesInvA( Matrix3D& B, const Matrix3D& A );
308 : :
309 : : //! \f$ Q*R = A \f$
310 : : friend void QR( Matrix3D& Q, Matrix3D& R, const Matrix3D& A );
311 : :
312 : : size_t num_rows() const
313 : : {
314 : : return 3;
315 : : }
316 : : size_t num_cols() const
317 : : {
318 : : return 3;
319 : : }
320 : :
321 : : //! returns a pointer to a row.
322 : 49933996 : inline double* operator[]( unsigned i )
323 : : {
324 : 49933996 : return v_ + 3 * i;
325 : : }
326 : :
327 : : //! returns a pointer to a row.
328 : 454064 : inline const double* operator[]( unsigned i ) const
329 : : {
330 : 454064 : return v_ + 3 * i;
331 : : }
332 : :
333 : 15584670 : inline double& operator()( unsigned short r, unsigned short c )
334 : : {
335 : 15584670 : return v_[3 * r + c];
336 : : }
337 : : inline double operator()( unsigned short r, unsigned short c ) const
338 : : {
339 : : return v_[3 * r + c];
340 : : }
341 : :
342 : : inline Vector3D row( unsigned r ) const
343 : : {
344 : : return Vector3D( v_ + 3 * r );
345 : : }
346 : :
347 : : inline Vector3D column( unsigned c ) const
348 : : {
349 : : return Vector3D( v_[c], v_[c + 3], v_[c + 6] );
350 : : }
351 : :
352 : : inline bool positive_definite() const;
353 : :
354 : 0 : inline SymMatrix3D upper() const
355 : : {
356 : 0 : return SymMatrix3D( v_[0], v_[1], v_[2], v_[4], v_[5], v_[8] );
357 : : }
358 : : inline SymMatrix3D lower() const
359 : : {
360 : : return SymMatrix3D( v_[0], v_[3], v_[6], v_[4], v_[7], v_[8] );
361 : : }
362 : : };
363 : :
364 : : /* *********** I/O **************/
365 : :
366 : 0 : inline std::ostream& operator<<( std::ostream& s, const Matrix3D& A )
367 : : {
368 [ # # ]: 0 : for( size_t i = 0; i < 3; ++i )
369 : : {
370 [ # # ]: 0 : for( size_t j = 0; j < 3; ++j )
371 : 0 : s << A[i][j] << " ";
372 : 0 : s << "\n";
373 : : }
374 : 0 : return s;
375 : : }
376 : :
377 : : inline std::istream& operator>>( std::istream& s, Matrix3D& A )
378 : : {
379 : : for( size_t i = 0; i < 3; i++ )
380 : : for( size_t j = 0; j < 3; j++ )
381 : : {
382 : : s >> A[i][j];
383 : : }
384 : : return s;
385 : : }
386 : :
387 : : void Matrix3D::set_values( const char* s )
388 : : {
389 : : std::istringstream ins( s );
390 : : ins >> *this;
391 : : }
392 : :
393 : : // *********** matrix operators *******************
394 : :
395 : : // comparison functions
396 : : inline bool operator==( const Matrix3D& lhs, const Matrix3D& rhs )
397 : : {
398 : : return lhs.v_[0] == rhs.v_[0] && lhs.v_[1] == rhs.v_[1] && lhs.v_[2] == rhs.v_[2] && lhs.v_[3] == rhs.v_[3] &&
399 : : lhs.v_[4] == rhs.v_[4] && lhs.v_[5] == rhs.v_[5] && lhs.v_[6] == rhs.v_[6] && lhs.v_[7] == rhs.v_[7] &&
400 : : lhs.v_[8] == rhs.v_[8];
401 : : }
402 : : inline bool operator!=( const Matrix3D& lhs, const Matrix3D& rhs )
403 : : {
404 : : return !( lhs == rhs );
405 : : }
406 : :
407 : : inline Matrix3D operator-( const Matrix3D& A )
408 : : {
409 : : return Matrix3D( -A.v_[0], -A.v_[1], -A.v_[2], -A.v_[3], -A.v_[4], -A.v_[5], -A.v_[6], -A.v_[7], -A.v_[8] );
410 : : }
411 : :
412 : : //! \return A+B
413 : 0 : inline const Matrix3D operator+( const Matrix3D& A, const Matrix3D& B )
414 : : {
415 : 0 : Matrix3D tmp( A );
416 [ # # ]: 0 : tmp += B;
417 : 0 : return tmp;
418 : : }
419 : :
420 : : inline Matrix3D operator+( const Matrix3D& A, const SymMatrix3D& B )
421 : : {
422 : : return Matrix3D( A( 0, 0 ) + B[SymMatrix3D::T00], A( 0, 1 ) + B[SymMatrix3D::T01], A( 0, 2 ) + B[SymMatrix3D::T02],
423 : : A( 1, 0 ) + B[SymMatrix3D::T10], A( 1, 1 ) + B[SymMatrix3D::T11], A( 1, 2 ) + B[SymMatrix3D::T12],
424 : : A( 2, 0 ) + B[SymMatrix3D::T20], A( 2, 1 ) + B[SymMatrix3D::T21],
425 : : A( 2, 2 ) + B[SymMatrix3D::T22] );
426 : : }
427 : : inline Matrix3D operator+( const SymMatrix3D& B, const Matrix3D& A )
428 : : {
429 : : return A + B;
430 : : }
431 : :
432 : : //! \return A-B
433 : 0 : inline const Matrix3D operator-( const Matrix3D& A, const Matrix3D& B )
434 : : {
435 : 0 : Matrix3D tmp( A );
436 [ # # ]: 0 : tmp -= B;
437 : 0 : return tmp;
438 : : }
439 : :
440 : : inline Matrix3D operator-( const Matrix3D& A, const SymMatrix3D& B )
441 : : {
442 : : return Matrix3D( A( 0, 0 ) - B[SymMatrix3D::T00], A( 0, 1 ) - B[SymMatrix3D::T01], A( 0, 2 ) - B[SymMatrix3D::T02],
443 : : A( 1, 0 ) - B[SymMatrix3D::T10], A( 1, 1 ) - B[SymMatrix3D::T11], A( 1, 2 ) - B[SymMatrix3D::T12],
444 : : A( 2, 0 ) - B[SymMatrix3D::T20], A( 2, 1 ) - B[SymMatrix3D::T21],
445 : : A( 2, 2 ) - B[SymMatrix3D::T22] );
446 : : }
447 : : inline Matrix3D operator-( const SymMatrix3D& B, const Matrix3D& A )
448 : : {
449 : : return Matrix3D( B[SymMatrix3D::T00] - A( 0, 0 ), B[SymMatrix3D::T01] - A( 0, 1 ), B[SymMatrix3D::T02] - A( 0, 2 ),
450 : : B[SymMatrix3D::T10] - A( 1, 0 ), B[SymMatrix3D::T11] - A( 1, 1 ), B[SymMatrix3D::T12] - A( 1, 2 ),
451 : : B[SymMatrix3D::T20] - A( 2, 0 ), B[SymMatrix3D::T21] - A( 2, 1 ),
452 : : B[SymMatrix3D::T22] - A( 2, 2 ) );
453 : : }
454 : :
455 : : inline Matrix3D& Matrix3D::equal_mult_elem( const Matrix3D& A )
456 : : {
457 : : v_[0] *= A.v_[0];
458 : : v_[1] *= A.v_[1];
459 : : v_[2] *= A.v_[2];
460 : : v_[3] *= A.v_[3];
461 : : v_[4] *= A.v_[4];
462 : : v_[5] *= A.v_[5];
463 : : v_[6] *= A.v_[6];
464 : : v_[7] *= A.v_[7];
465 : : v_[8] *= A.v_[8];
466 : : return *this;
467 : : }
468 : :
469 : : //! Multiplies entry by entry. This is NOT a matrix multiplication.
470 : : inline const Matrix3D mult_element( const Matrix3D& A, const Matrix3D& B )
471 : : {
472 : : Matrix3D tmp( A );
473 : : tmp.equal_mult_elem( B );
474 : : return tmp;
475 : : }
476 : :
477 : : //! Return the square of the Frobenius norm of A, i.e. sum (diag (A' * A))
478 : 0 : inline double Frobenius_2( const Matrix3D& A )
479 : : {
480 : 0 : return A.v_[0] * A.v_[0] + A.v_[1] * A.v_[1] + A.v_[2] * A.v_[2] + A.v_[3] * A.v_[3] + A.v_[4] * A.v_[4] +
481 : 0 : A.v_[5] * A.v_[5] + A.v_[6] * A.v_[6] + A.v_[7] * A.v_[7] + A.v_[8] * A.v_[8];
482 : : }
483 : :
484 : : inline Matrix3D& Matrix3D::transpose()
485 : : {
486 : : double t;
487 : : t = v_[1];
488 : : v_[1] = v_[3];
489 : : v_[3] = t;
490 : : t = v_[2];
491 : : v_[2] = v_[6];
492 : : v_[6] = t;
493 : : t = v_[5];
494 : : v_[5] = v_[7];
495 : : v_[7] = t;
496 : : return *this;
497 : : }
498 : :
499 : 0 : inline Matrix3D transpose( const Matrix3D& A )
500 : : {
501 : 0 : Matrix3D S;
502 : : // size_t i;
503 : : // for (i=0; i<3; ++i) {
504 : : // S[size_t(0)][i] = A[i][0];
505 : : // S[size_t(1)][i] = A[i][1];
506 : : // S[size_t(2)][i] = A[i][2];
507 : : // }
508 : 0 : S.v_[0] = A.v_[0];
509 : 0 : S.v_[1] = A.v_[3];
510 : 0 : S.v_[2] = A.v_[6];
511 : 0 : S.v_[3] = A.v_[1];
512 : 0 : S.v_[4] = A.v_[4];
513 : 0 : S.v_[5] = A.v_[7];
514 : 0 : S.v_[6] = A.v_[2];
515 : 0 : S.v_[7] = A.v_[5];
516 : 0 : S.v_[8] = A.v_[8];
517 : :
518 : 0 : return S;
519 : : }
520 : :
521 : 7798327 : inline void Matrix3D::operator+=( const Matrix3D& rhs )
522 : : {
523 : 7798327 : v_[0] += rhs.v_[0];
524 : 7798327 : v_[1] += rhs.v_[1];
525 : 7798327 : v_[2] += rhs.v_[2];
526 : 7798327 : v_[3] += rhs.v_[3];
527 : 7798327 : v_[4] += rhs.v_[4];
528 : 7798327 : v_[5] += rhs.v_[5];
529 : 7798327 : v_[6] += rhs.v_[6];
530 : 7798327 : v_[7] += rhs.v_[7];
531 : 7798327 : v_[8] += rhs.v_[8];
532 : 7798327 : }
533 : :
534 : : inline void Matrix3D::operator+=( const SymMatrix3D& rhs )
535 : : {
536 : : v_[0] += rhs[0];
537 : : v_[1] += rhs[1];
538 : : v_[2] += rhs[2];
539 : : v_[3] += rhs[1];
540 : : v_[4] += rhs[3];
541 : : v_[5] += rhs[4];
542 : : v_[6] += rhs[2];
543 : : v_[7] += rhs[4];
544 : : v_[8] += rhs[5];
545 : : }
546 : :
547 : 0 : inline void Matrix3D::operator-=( const Matrix3D& rhs )
548 : : {
549 : 0 : v_[0] -= rhs.v_[0];
550 : 0 : v_[1] -= rhs.v_[1];
551 : 0 : v_[2] -= rhs.v_[2];
552 : 0 : v_[3] -= rhs.v_[3];
553 : 0 : v_[4] -= rhs.v_[4];
554 : 0 : v_[5] -= rhs.v_[5];
555 : 0 : v_[6] -= rhs.v_[6];
556 : 0 : v_[7] -= rhs.v_[7];
557 : 0 : v_[8] -= rhs.v_[8];
558 : 0 : }
559 : :
560 : : inline void Matrix3D::operator-=( const SymMatrix3D& rhs )
561 : : {
562 : : v_[0] -= rhs[0];
563 : : v_[1] -= rhs[1];
564 : : v_[2] -= rhs[2];
565 : : v_[3] -= rhs[1];
566 : : v_[4] -= rhs[3];
567 : : v_[5] -= rhs[4];
568 : : v_[6] -= rhs[2];
569 : : v_[7] -= rhs[4];
570 : : v_[8] -= rhs[5];
571 : : }
572 : :
573 : : //! multiplies each entry by the scalar s
574 : 10135988 : inline void Matrix3D::operator*=( double s )
575 : : {
576 : 10135988 : v_[0] *= s;
577 : 10135988 : v_[1] *= s;
578 : 10135988 : v_[2] *= s;
579 : 10135988 : v_[3] *= s;
580 : 10135988 : v_[4] *= s;
581 : 10135988 : v_[5] *= s;
582 : 10135988 : v_[6] *= s;
583 : 10135988 : v_[7] *= s;
584 : 10135988 : v_[8] *= s;
585 : 10135988 : }
586 : :
587 : : //! \f$ += B^T \f$
588 : 1878096 : inline Matrix3D& Matrix3D::plus_transpose_equal( const Matrix3D& b )
589 : : {
590 [ - + ]: 1878096 : if( &b == this )
591 : : {
592 : 0 : v_[0] *= 2.0;
593 : 0 : v_[1] += v_[3];
594 : 0 : v_[2] += v_[6];
595 : 0 : v_[3] = v_[1];
596 : 0 : v_[4] *= 2.0;
597 : 0 : v_[5] += v_[7];
598 : 0 : v_[6] = v_[2];
599 : 0 : v_[7] = v_[5];
600 : 0 : v_[8] *= 2.0;
601 : : }
602 : : else
603 : : {
604 : 1878096 : v_[0] += b.v_[0];
605 : 1878096 : v_[1] += b.v_[3];
606 : 1878096 : v_[2] += b.v_[6];
607 : :
608 : 1878096 : v_[3] += b.v_[1];
609 : 1878096 : v_[4] += b.v_[4];
610 : 1878096 : v_[5] += b.v_[7];
611 : :
612 : 1878096 : v_[6] += b.v_[2];
613 : 1878096 : v_[7] += b.v_[5];
614 : 1878096 : v_[8] += b.v_[8];
615 : : }
616 : 1878096 : return *this;
617 : : }
618 : :
619 : : //! \f$ + B^T \f$
620 : : inline Matrix3D plus_transpose( const Matrix3D& A, const Matrix3D& B )
621 : : {
622 : : Matrix3D tmp( A );
623 : : tmp.plus_transpose_equal( B );
624 : : return tmp;
625 : : }
626 : :
627 : : //! Computes \f$ A = v_1 v_2^T \f$
628 : 622510 : inline Matrix3D& Matrix3D::outer_product( const Vector3D& v1, const Vector3D& v2 )
629 : : {
630 : : // remember, matrix entries are v_[0] to v_[8].
631 : :
632 : : // diagonal
633 : 622510 : v_[0] = v1[0] * v2[0];
634 : 622510 : v_[4] = v1[1] * v2[1];
635 : 622510 : v_[8] = v1[2] * v2[2];
636 : :
637 : : // upper triangular part
638 : 622510 : v_[1] = v1[0] * v2[1];
639 : 622510 : v_[2] = v1[0] * v2[2];
640 : 622510 : v_[5] = v1[1] * v2[2];
641 : :
642 : : // lower triangular part
643 : 622510 : v_[3] = v2[0] * v1[1];
644 : 622510 : v_[6] = v2[0] * v1[2];
645 : 622510 : v_[7] = v2[1] * v1[2];
646 : :
647 : 622510 : return *this;
648 : : }
649 : :
650 : 935484 : inline void Matrix3D::fill_lower_triangle()
651 : : {
652 : 935484 : v_[3] = v_[1];
653 : 935484 : v_[6] = v_[2];
654 : 935484 : v_[7] = v_[5];
655 : 935484 : }
656 : :
657 : : //! \return A*B
658 : 0 : inline const Matrix3D operator*( const Matrix3D& A, const Matrix3D& B )
659 : : {
660 : 0 : Matrix3D tmp;
661 [ # # ]: 0 : tmp.assign_product( A, B );
662 : 0 : return tmp;
663 : : }
664 : :
665 : : inline const Matrix3D operator*( const Matrix3D& A, const SymMatrix3D& B )
666 : : {
667 : : return Matrix3D(
668 : : A( 0, 0 ) * B[0] + A( 0, 1 ) * B[1] + A( 0, 2 ) * B[2], A( 0, 0 ) * B[1] + A( 0, 1 ) * B[3] + A( 0, 2 ) * B[4],
669 : : A( 0, 0 ) * B[2] + A( 0, 1 ) * B[4] + A( 0, 2 ) * B[5],
670 : :
671 : : A( 1, 0 ) * B[0] + A( 1, 1 ) * B[1] + A( 1, 2 ) * B[2], A( 1, 0 ) * B[1] + A( 1, 1 ) * B[3] + A( 1, 2 ) * B[4],
672 : : A( 1, 0 ) * B[2] + A( 1, 1 ) * B[4] + A( 1, 2 ) * B[5],
673 : :
674 : : A( 2, 0 ) * B[0] + A( 2, 1 ) * B[1] + A( 2, 2 ) * B[2], A( 2, 0 ) * B[1] + A( 2, 1 ) * B[3] + A( 2, 2 ) * B[4],
675 : : A( 2, 0 ) * B[2] + A( 2, 1 ) * B[4] + A( 2, 2 ) * B[5] );
676 : : }
677 : :
678 : : inline const Matrix3D operator*( const SymMatrix3D& B, const Matrix3D& A )
679 : : {
680 : : return Matrix3D(
681 : : A( 0, 0 ) * B[0] + A( 1, 0 ) * B[1] + A( 2, 0 ) * B[2], A( 0, 1 ) * B[0] + A( 1, 1 ) * B[1] + A( 2, 1 ) * B[2],
682 : : A( 0, 2 ) * B[0] + A( 1, 2 ) * B[1] + A( 2, 2 ) * B[2],
683 : :
684 : : A( 0, 0 ) * B[1] + A( 1, 0 ) * B[3] + A( 2, 0 ) * B[4], A( 0, 1 ) * B[1] + A( 1, 1 ) * B[3] + A( 2, 1 ) * B[4],
685 : : A( 0, 2 ) * B[1] + A( 1, 2 ) * B[3] + A( 2, 2 ) * B[4],
686 : :
687 : : A( 0, 0 ) * B[2] + A( 1, 0 ) * B[4] + A( 2, 0 ) * B[5], A( 0, 1 ) * B[2] + A( 1, 1 ) * B[4] + A( 2, 1 ) * B[5],
688 : : A( 0, 2 ) * B[2] + A( 1, 2 ) * B[4] + A( 2, 2 ) * B[5] );
689 : : }
690 : :
691 : : inline const Matrix3D operator*( const SymMatrix3D& a, const SymMatrix3D& b )
692 : : {
693 : : return Matrix3D( a[0] * b[0] + a[1] * b[1] + a[2] * b[2], a[0] * b[1] + a[1] * b[3] + a[2] * b[4],
694 : : a[0] * b[2] + a[1] * b[4] + a[2] * b[5],
695 : :
696 : : a[1] * b[0] + a[3] * b[1] + a[4] * b[2], a[1] * b[1] + a[3] * b[3] + a[4] * b[4],
697 : : a[1] * b[2] + a[3] * b[4] + a[4] * b[5],
698 : :
699 : : a[2] * b[0] + a[4] * b[1] + a[5] * b[2], a[2] * b[1] + a[4] * b[3] + a[5] * b[4],
700 : : a[2] * b[2] + a[4] * b[4] + a[5] * b[5] );
701 : : }
702 : :
703 : : //! multiplies each entry by the scalar s
704 : 0 : inline const Matrix3D Matrix3D::operator*( double s ) const
705 : : {
706 : 0 : Matrix3D temp( *this );
707 [ # # ]: 0 : temp *= s;
708 : 0 : return temp;
709 : : }
710 : : //! friend function to allow for commutatative property of
711 : : //! scalar mulitplication.
712 : 0 : inline const Matrix3D operator*( double s, const Matrix3D& A )
713 : : {
714 : 0 : return ( A.operator*( s ) );
715 : : }
716 : :
717 : 0 : inline Matrix3D& Matrix3D::assign_product( const Matrix3D& A, const Matrix3D& B )
718 : : {
719 : 0 : v_[0] = A.v_[0] * B.v_[0] + A.v_[1] * B.v_[3] + A.v_[2] * B.v_[6];
720 : 0 : v_[1] = A.v_[0] * B.v_[1] + A.v_[1] * B.v_[4] + A.v_[2] * B.v_[7];
721 : 0 : v_[2] = A.v_[0] * B.v_[2] + A.v_[1] * B.v_[5] + A.v_[2] * B.v_[8];
722 : 0 : v_[3] = A.v_[3] * B.v_[0] + A.v_[4] * B.v_[3] + A.v_[5] * B.v_[6];
723 : 0 : v_[4] = A.v_[3] * B.v_[1] + A.v_[4] * B.v_[4] + A.v_[5] * B.v_[7];
724 : 0 : v_[5] = A.v_[3] * B.v_[2] + A.v_[4] * B.v_[5] + A.v_[5] * B.v_[8];
725 : 0 : v_[6] = A.v_[6] * B.v_[0] + A.v_[7] * B.v_[3] + A.v_[8] * B.v_[6];
726 : 0 : v_[7] = A.v_[6] * B.v_[1] + A.v_[7] * B.v_[4] + A.v_[8] * B.v_[7];
727 : 0 : v_[8] = A.v_[6] * B.v_[2] + A.v_[7] * B.v_[5] + A.v_[8] * B.v_[8];
728 : 0 : return *this;
729 : : }
730 : :
731 : : //! \f$ C = A \times B \f$
732 : : inline void matmult( Matrix3D& C, const Matrix3D& A, const Matrix3D& B )
733 : : {
734 : : C.assign_product( A, B );
735 : : }
736 : :
737 : : /*! \brief Computes \f$ A v \f$ . */
738 : 110 : inline const Vector3D operator*( const Matrix3D& A, const Vector3D& x )
739 : : {
740 : 110 : Vector3D tmp;
741 : 110 : eqAx( tmp, A, x );
742 : 110 : return tmp;
743 : : }
744 : :
745 : : /*! \brief Computes \f$ v^T A \f$ .
746 : :
747 : : This function implicitly considers the transpose of vector x times
748 : : the matrix A and it is implicit that the returned vector must be
749 : : transposed. */
750 : : inline const Vector3D operator*( const Vector3D& x, const Matrix3D& A )
751 : : {
752 : : Vector3D tmp;
753 : : eqTransAx( tmp, A, x );
754 : : return tmp;
755 : : }
756 : :
757 : 242313 : inline void eqAx( Vector3D& v, const Matrix3D& A, const Vector3D& x )
758 : : {
759 : 242313 : v.mCoords[0] = A.v_[0] * x[0] + A.v_[1] * x.mCoords[1] + A.v_[2] * x.mCoords[2];
760 : 242313 : v.mCoords[1] = A.v_[3] * x[0] + A.v_[4] * x.mCoords[1] + A.v_[5] * x.mCoords[2];
761 : 242313 : v.mCoords[2] = A.v_[6] * x[0] + A.v_[7] * x.mCoords[1] + A.v_[8] * x.mCoords[2];
762 : 242313 : }
763 : :
764 : 12865993 : inline void plusEqAx( Vector3D& v, const Matrix3D& A, const Vector3D& x )
765 : : {
766 : 12865993 : v.mCoords[0] += A.v_[0] * x[0] + A.v_[1] * x.mCoords[1] + A.v_[2] * x.mCoords[2];
767 : 12865993 : v.mCoords[1] += A.v_[3] * x[0] + A.v_[4] * x.mCoords[1] + A.v_[5] * x.mCoords[2];
768 : 12865993 : v.mCoords[2] += A.v_[6] * x[0] + A.v_[7] * x.mCoords[1] + A.v_[8] * x.mCoords[2];
769 : 12865993 : }
770 : :
771 : : inline void eqTransAx( Vector3D& v, const Matrix3D& A, const Vector3D& x )
772 : : {
773 : : v.mCoords[0] = A.v_[0] * x.mCoords[0] + A.v_[3] * x.mCoords[1] + A.v_[6] * x.mCoords[2];
774 : : v.mCoords[1] = A.v_[1] * x.mCoords[0] + A.v_[4] * x.mCoords[1] + A.v_[7] * x.mCoords[2];
775 : : v.mCoords[2] = A.v_[2] * x.mCoords[0] + A.v_[5] * x.mCoords[1] + A.v_[8] * x.mCoords[2];
776 : : }
777 : :
778 : 10757727 : inline void plusEqTransAx( Vector3D& v, const Matrix3D& A, const Vector3D& x )
779 : : {
780 : 10757727 : v.mCoords[0] += A.v_[0] * x.mCoords[0] + A.v_[3] * x.mCoords[1] + A.v_[6] * x.mCoords[2];
781 : 10757727 : v.mCoords[1] += A.v_[1] * x.mCoords[0] + A.v_[4] * x.mCoords[1] + A.v_[7] * x.mCoords[2];
782 : 10757727 : v.mCoords[2] += A.v_[2] * x.mCoords[0] + A.v_[5] * x.mCoords[1] + A.v_[8] * x.mCoords[2];
783 : 10757727 : }
784 : :
785 : : inline void plusEqaA( Matrix3D& B, const double a, const Matrix3D& A )
786 : : {
787 : : B.v_[0] += a * A.v_[0];
788 : : B.v_[1] += a * A.v_[1];
789 : : B.v_[2] += a * A.v_[2];
790 : : B.v_[3] += a * A.v_[3];
791 : : B.v_[4] += a * A.v_[4];
792 : : B.v_[5] += a * A.v_[5];
793 : : B.v_[6] += a * A.v_[6];
794 : : B.v_[7] += a * A.v_[7];
795 : : B.v_[8] += a * A.v_[8];
796 : : }
797 : :
798 : : inline double det( const Matrix3D& A )
799 : : {
800 : : return ( A.v_[0] * ( A.v_[4] * A.v_[8] - A.v_[7] * A.v_[5] ) - A.v_[1] * ( A.v_[3] * A.v_[8] - A.v_[6] * A.v_[5] ) +
801 : : A.v_[2] * ( A.v_[3] * A.v_[7] - A.v_[6] * A.v_[4] ) );
802 : : }
803 : :
804 : : inline void inv( Matrix3D& Ainv, const Matrix3D& A )
805 : : {
806 : : double inv_detA = 1.0 / ( det( A ) );
807 : : // First row of Ainv
808 : : Ainv.v_[0] = inv_detA * ( A.v_[4] * A.v_[8] - A.v_[5] * A.v_[7] );
809 : : Ainv.v_[1] = inv_detA * ( A.v_[2] * A.v_[7] - A.v_[8] * A.v_[1] );
810 : : Ainv.v_[2] = inv_detA * ( A.v_[1] * A.v_[5] - A.v_[4] * A.v_[2] );
811 : : // Second row of Ainv
812 : : Ainv.v_[3] = inv_detA * ( A.v_[5] * A.v_[6] - A.v_[8] * A.v_[3] );
813 : : Ainv.v_[4] = inv_detA * ( A.v_[0] * A.v_[8] - A.v_[6] * A.v_[2] );
814 : : Ainv.v_[5] = inv_detA * ( A.v_[2] * A.v_[3] - A.v_[5] * A.v_[0] );
815 : : // Third row of Ainv
816 : : Ainv.v_[6] = inv_detA * ( A.v_[3] * A.v_[7] - A.v_[6] * A.v_[4] );
817 : : Ainv.v_[7] = inv_detA * ( A.v_[1] * A.v_[6] - A.v_[7] * A.v_[0] );
818 : : Ainv.v_[8] = inv_detA * ( A.v_[0] * A.v_[4] - A.v_[3] * A.v_[1] );
819 : : }
820 : :
821 : : inline void timesInvA( Matrix3D& B, const Matrix3D& A )
822 : : {
823 : :
824 : : Matrix3D Ainv;
825 : : inv( Ainv, A );
826 : : B = B * Ainv;
827 : : }
828 : :
829 : : inline void QR( Matrix3D& Q, Matrix3D& R, const Matrix3D& A )
830 : : {
831 : : // Compute the QR factorization of A. This code uses the
832 : : // Modified Gram-Schmidt method for computing the factorization.
833 : : // The Householder version is more stable, but costs twice as many
834 : : // floating point operations.
835 : :
836 : : Q = A;
837 : :
838 : : R[0][0] = sqrt( Q[0][0] * Q[0][0] + Q[1][0] * Q[1][0] + Q[2][0] * Q[2][0] );
839 : : double temp_dbl = 1.0 / R[0][0];
840 : : R[1][0] = 0.0L;
841 : : R[2][0] = 0.0L;
842 : : // Q[0][0] /= R[0][0];
843 : : // Q[1][0] /= R[0][0];
844 : : // Q[2][0] /= R[0][0];
845 : : Q[0][0] *= temp_dbl;
846 : : Q[1][0] *= temp_dbl;
847 : : Q[2][0] *= temp_dbl;
848 : :
849 : : R[0][1] = Q[0][0] * Q[0][1] + Q[1][0] * Q[1][1] + Q[2][0] * Q[2][1];
850 : : Q[0][1] -= Q[0][0] * R[0][1];
851 : : Q[1][1] -= Q[1][0] * R[0][1];
852 : : Q[2][1] -= Q[2][0] * R[0][1];
853 : :
854 : : R[0][2] = Q[0][0] * Q[0][2] + Q[1][0] * Q[1][2] + Q[2][0] * Q[2][2];
855 : : Q[0][2] -= Q[0][0] * R[0][2];
856 : : Q[1][2] -= Q[1][0] * R[0][2];
857 : : Q[2][2] -= Q[2][0] * R[0][2];
858 : :
859 : : R[1][1] = sqrt( Q[0][1] * Q[0][1] + Q[1][1] * Q[1][1] + Q[2][1] * Q[2][1] );
860 : : temp_dbl = 1.0 / R[1][1];
861 : : R[2][1] = 0.0L;
862 : : // Q[0][1] /= R[1][1];
863 : : // Q[1][1] /= R[1][1];
864 : : // Q[2][1] /= R[1][1];
865 : : Q[0][1] *= temp_dbl;
866 : : Q[1][1] *= temp_dbl;
867 : : Q[2][1] *= temp_dbl;
868 : :
869 : : R[1][2] = Q[0][1] * Q[0][2] + Q[1][1] * Q[1][2] + Q[2][1] * Q[2][2];
870 : : Q[0][2] -= Q[0][1] * R[1][2];
871 : : Q[1][2] -= Q[1][1] * R[1][2];
872 : : Q[2][2] -= Q[2][1] * R[1][2];
873 : :
874 : : R[2][2] = sqrt( Q[0][2] * Q[0][2] + Q[1][2] * Q[1][2] + Q[2][2] * Q[2][2] );
875 : : temp_dbl = 1.0 / R[2][2];
876 : :
877 : : // Q[0][2] /= R[2][2];
878 : : // Q[1][2] /= R[2][2];
879 : : // Q[2][2] /= R[2][2];
880 : : Q[0][2] *= temp_dbl;
881 : : Q[1][2] *= temp_dbl;
882 : : Q[2][2] *= temp_dbl;
883 : :
884 : : return;
885 : : }
886 : :
887 : : inline bool Matrix3D::positive_definite() const
888 : : {
889 : : // A = B + C
890 : : // where
891 : : // B = (A + transpose(A))/2
892 : : // C = (A - transpose(A))/2
893 : : // B is always a symmetric matrix and
894 : : // A is positive definite iff B is positive definite.
895 : : Matrix3D B( *this );
896 : : B.plus_transpose_equal( *this );
897 : : B *= 0.5;
898 : :
899 : : // Sylvester's Criterion for positive definite symmetric matrix
900 : : return ( B[0][0] > 0.0 ) && ( B.sub_det( 2, 2 ) > 0.0 ) && ( det( B ) > 0.0 );
901 : : }
902 : :
903 : : } // namespace MBMesquite
904 : :
905 : : #endif // Matrix3D_hpp
|
