Branch data Line data Source code
1 : : /* *****************************************************************
2 : : MESQUITE -- The Mesh Quality Improvement Toolkit
3 : :
4 : : Copyright 2006 Sandia National Laboratories. Developed at the
5 : : University of Wisconsin--Madison under SNL contract number
6 : : 624796. The U.S. Government and the University of Wisconsin
7 : : retain certain rights to 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 : : (2006) [email protected]
24 : :
25 : : ***************************************************************** */
26 : :
27 : : /** \file MsqMatrix.hpp
28 : : * \brief
29 : : * \author Jason Kraftcheck
30 : : */
31 : :
32 : : #ifndef MSQ_MSQ_MATRIX_HPP
33 : : #define MSQ_MSQ_MATRIX_HPP
34 : :
35 : : #include "Mesquite.hpp"
36 : :
37 : : #include <string>
38 : : #include <istream>
39 : : #include <ostream>
40 : : #include <sstream>
41 : :
42 : : namespace MBMesquite
43 : : {
44 : :
45 : : /**\brief Fixed-size matrix class
46 : : *
47 : : * This class implements a fixed-size 2-dimensional matrix.
48 : : * The actual size is specified with template parameters.
49 : : */
50 : : template < unsigned R, unsigned C >
51 : : class MsqMatrix
52 : : {
53 : : protected:
54 : : double m[R * C];
55 : :
56 : : public:
57 : : typedef MsqMatrix< R, C > my_type;
58 : :
59 : : enum
60 : : {
61 : : ROWS = R,
62 : : COLS = C
63 : : };
64 : :
65 : : /** Constructor for uninitialized matrix */
66 : 692296584 : MsqMatrix() {}
67 : : /** Initialize diagonal values, zero others */
68 : 16140865 : MsqMatrix( double v )
69 : : {
70 : 16140865 : diag( v );
71 : 16140865 : }
72 : : /** Initialize to an array of values */
73 : 80196195 : MsqMatrix( const double* v )
74 : : {
75 : 80196195 : set( v );
76 : 80196195 : }
77 : : /** Initialize from 2D array */
78 : : MsqMatrix( const double v[R][C] )
79 : : {
80 : : set( v );
81 : : }
82 : : /** Initialize with column vectors */
83 : 3148348 : MsqMatrix( const MsqMatrix< R, 1 >* c )
84 : : {
85 : 3148348 : set_columns( c );
86 : 3148348 : }
87 : : /** Initialize with row vectors */
88 : : MsqMatrix( const MsqMatrix< 1, C >* r )
89 : : {
90 : : set_rows( r );
91 : : }
92 : : /** Parse initial values from string */
93 : : MsqMatrix( const char* s )
94 : : {
95 : : set( s );
96 : : }
97 : : /** Parse initial values from string */
98 : : MsqMatrix( const std::string& s )
99 : : {
100 : : set( s );
101 : : }
102 : : /** Initialize to the minor of a larger matrix
103 : : * This matrix is the passed matrix with the
104 : : * specified row and column removed.
105 : : */
106 : 0 : MsqMatrix( const MsqMatrix< R + 1, C + 1 >& p_m, unsigned p_r, unsigned p_c )
107 : : {
108 : 0 : make_minor( p_m, p_r, p_c );
109 : 0 : }
110 : :
111 : : MsqMatrix< R, C >& operator=( double v )
112 : : {
113 : : set( v );
114 : : return *this;
115 : : }
116 : : MsqMatrix< R, C >& operator=( const double* v )
117 : : {
118 : : set( v );
119 : : return *this;
120 : : }
121 : : MsqMatrix< R, C >& operator=( const char* s )
122 : : {
123 : : set( s );
124 : : return *this;
125 : : }
126 : : MsqMatrix< R, C >& operator=( const std::string& s )
127 : : {
128 : : set( s );
129 : : return *this;
130 : : }
131 : :
132 : 2517166705 : double& operator()( unsigned r, unsigned c )
133 : : {
134 : 2517166705 : return m[r * C + c];
135 : : }
136 : 5273149495 : double operator()( unsigned r, unsigned c ) const
137 : : {
138 : 5273149495 : return m[r * C + c];
139 : : }
140 : 33502856 : double* data()
141 : : {
142 : 33502856 : return m;
143 : : }
144 : 1102324094 : const double* data() const
145 : : {
146 : 1102324094 : return m;
147 : : }
148 : :
149 : 20018727 : void zero()
150 : : {
151 : 20018727 : set( 0.0 );
152 : 20018727 : }
153 : : void identity()
154 : : {
155 : : diag( 1.0 );
156 : : }
157 : 20018727 : void set( double v )
158 : : {
159 [ + + ][ + + ]: 157654338 : for( unsigned i = 0; i < R * C; ++i )
160 : 137635611 : m[i] = v;
161 : 20018727 : }
162 : 82312497 : void set( const double* v )
163 : : {
164 [ + + ][ + + ]: 328830898 : for( unsigned i = 0; i < R * C; ++i )
[ + + ]
165 : 246518401 : m[i] = v[i];
166 : 82312497 : }
167 : : void set( const double v[R][C] );
168 : : void set( const char* s )
169 : : {
170 : : std::istringstream i( s );
171 : : i >> *this;
172 : : }
173 : : void set( const std::string& s )
174 : : {
175 : : set( s.c_str() );
176 : : }
177 : : /** Set diagonal value to passed values, others to zero. */
178 : : inline void diag( double v );
179 : : /** Set diagonal values to passed values, others to zero. */
180 : : inline void diag( const double* v );
181 : : /** Set this matrix to the minor of a larger matrix */
182 : : inline void make_minor( const MsqMatrix< R + 1, C + 1 >& m, unsigned r, unsigned c );
183 : :
184 : : /** *this += transpose(other) */
185 : : inline MsqMatrix< R, C >& assign_add_transpose( const MsqMatrix< C, R >& other );
186 : : /** *this = s*m */
187 : : inline MsqMatrix< R, C >& assign_product( double s, const MsqMatrix< R, C >& m );
188 : : /** *this += s*m */
189 : : inline MsqMatrix< R, C >& assign_add_product( double s, const MsqMatrix< R, C >& m );
190 : : /** multiply each element by the cooresponding element in m */
191 : : inline MsqMatrix< R, C >& assign_multiply_elements( const MsqMatrix< R, C >& m );
192 : :
193 : : inline MsqMatrix< R, C >& operator+=( const MsqMatrix< R, C >& other );
194 : : inline MsqMatrix< R, C >& operator-=( const MsqMatrix< R, C >& other );
195 : : inline MsqMatrix< R, C >& operator+=( double scalar );
196 : : inline MsqMatrix< R, C >& operator-=( double scalar );
197 : : inline MsqMatrix< R, C >& operator*=( double scalar );
198 : : inline MsqMatrix< R, C >& operator/=( double scalar );
199 : :
200 : : // MsqMatrix<1,C>& row( unsigned r ) { return *(MsqMatrix<1,C>*)(m+C*r); }
201 : : // const MsqMatrix<1,C>& row( unsigned r ) const { return *(MsqMatrix<1,C>*)(m+C*r); }
202 : 8928057 : MsqMatrix< 1, C > row( unsigned r ) const
203 : : {
204 : 8928057 : return MsqMatrix< 1, C >( m + C * r );
205 : : }
206 : : MsqMatrix< R, 1 > column( unsigned c ) const;
207 : : MsqMatrix< 1, R > column_transpose( unsigned c ) const;
208 : :
209 : : void set_row( unsigned r, const MsqMatrix< 1, C >& v );
210 : : void add_row( unsigned r, const MsqMatrix< 1, C >& v );
211 : : void set_row_transpose( unsigned r, const MsqMatrix< C, 1 >& v );
212 : : void set_rows( const MsqMatrix< 1, C >* v );
213 : : void set_column( unsigned c, const MsqMatrix< R, 1 >& v );
214 : : void add_column( unsigned c, const MsqMatrix< R, 1 >& v );
215 : : void set_column_transpose( unsigned c, const MsqMatrix< 1, R >& v );
216 : : void set_columns( const MsqMatrix< R, 1 >* v );
217 : : };
218 : :
219 : : template <>
220 : : class MsqMatrix< 1, 1 >
221 : : {
222 : : protected:
223 : : double m;
224 : :
225 : : public:
226 : : typedef MsqMatrix< 1, 1 > my_type;
227 : :
228 : : enum
229 : : {
230 : : ROWS = 1,
231 : : COLS = 1
232 : : };
233 : :
234 : : /** Constructor for uninitialized matrix */
235 : 13315530 : MsqMatrix() {}
236 : : /** Initialize diagonal values, zero others */
237 : : MsqMatrix( double v ) : m( v ) {}
238 : : /** Initialize to an array of values */
239 : : MsqMatrix( const double* v ) : m( *v ) {}
240 : : /** Parse initial values from string */
241 : : MsqMatrix( const char* s )
242 : : {
243 : : set( s );
244 : : }
245 : : /** Parse initial values from string */
246 : : MsqMatrix( const std::string& s )
247 : : {
248 : : set( s );
249 : : }
250 : : /** Initialize to the minor of a larger matrix
251 : : * This matrix is the passed matrix with the
252 : : * specified row and column removed.
253 : : */
254 : : MsqMatrix( const MsqMatrix< 2, 2 >& M, unsigned r, unsigned c ) : m( M( r, c ) ) {}
255 : :
256 : : MsqMatrix< 1, 1 >& operator=( double v )
257 : : {
258 : : m = v;
259 : : return *this;
260 : : }
261 : : MsqMatrix< 1, 1 >& operator=( const double* v )
262 : : {
263 : : m = *v;
264 : : return *this;
265 : : }
266 : : MsqMatrix< 1, 1 >& operator=( const char* s )
267 : : {
268 : : set( s );
269 : : return *this;
270 : : }
271 : : MsqMatrix< 1, 1 >& operator=( const std::string& s )
272 : : {
273 : : set( s );
274 : : return *this;
275 : : }
276 : :
277 : 13315530 : double& operator()( unsigned, unsigned )
278 : : {
279 : 13315530 : return m;
280 : : }
281 : : double operator()( unsigned, unsigned ) const
282 : : {
283 : : return m;
284 : : }
285 : : double* data()
286 : : {
287 : : return &m;
288 : : }
289 : : const double* data() const
290 : : {
291 : : return &m;
292 : : }
293 : :
294 : : void zero()
295 : : {
296 : : m = 0.0;
297 : : }
298 : : void identity()
299 : : {
300 : : m = 1.0;
301 : : }
302 : : void set( double v )
303 : : {
304 : : m = v;
305 : : }
306 : : void set( const double* v )
307 : : {
308 : : m = *v;
309 : : }
310 : : void set( const char* s )
311 : : {
312 : : std::istringstream i( s );
313 : : i >> m;
314 : : }
315 : : void set( const std::string& s )
316 : : {
317 : : set( s.c_str() );
318 : : }
319 : : /** Set diagonal value to passed values, others to zero. */
320 : : inline void diag( double v )
321 : : {
322 : : m = v;
323 : : }
324 : : /** Set diagonal values to passed values, others to zero. */
325 : : inline void diag( const double* v )
326 : : {
327 : : m = *v;
328 : : }
329 : : /** Set this matrix to the minor of a larger matrix */
330 : : inline void make_minor( const MsqMatrix< 2, 2 >& M, unsigned r, unsigned c )
331 : : {
332 : : m = M( r, c );
333 : : }
334 : :
335 : : /** *this += transpose(other) */
336 : : inline MsqMatrix< 1, 1 >& assign_add_transpose( const MsqMatrix< 1, 1 >& other )
337 : : {
338 : : m += other.m;
339 : : return *this;
340 : : }
341 : : /** *this = s*m */
342 : : inline MsqMatrix< 1, 1 >& assign_product( double s, const MsqMatrix< 1, 1 >& other )
343 : : {
344 : : m = s * other.m;
345 : : return *this;
346 : : }
347 : : /** *this += s*m */
348 : : inline MsqMatrix< 1, 1 >& assign_add_product( double s, const MsqMatrix< 1, 1 >& other )
349 : : {
350 : : m += s * other.m;
351 : : return *this;
352 : : }
353 : : /** multiply each element by the cooresponding element in m */
354 : : inline MsqMatrix< 1, 1 >& assign_multiply_elements( const MsqMatrix< 1, 1 >& other )
355 : : {
356 : : m *= other.m;
357 : : return *this;
358 : : }
359 : :
360 : 13315530 : operator double() const
361 : : {
362 : 13315530 : return m;
363 : : }
364 : : };
365 : :
366 : : /** \brief Vector is a 1xL Matrix
367 : : *
368 : : * Define a Vector as a 1xL Matrix
369 : : * Add single-index access operators
370 : : */
371 : : template < unsigned L >
372 : : class MsqVector : public MsqMatrix< L, 1 >
373 : : {
374 : : public:
375 : 221918268 : MsqVector() {}
376 : : MsqVector( double v )
377 : : {
378 : : MsqMatrix< L, 1 >::set( v );
379 : : }
380 : 2116302 : MsqVector( const double* v )
381 : 2116302 : {
382 : 2116302 : MsqMatrix< L, 1 >::set( v );
383 : 2116302 : }
384 : : MsqVector( const char* s )
385 : : {
386 : : MsqMatrix< L, 1 >::set( s );
387 : : }
388 : : MsqVector( const std::string& s )
389 : : {
390 : : MsqMatrix< L, 1 >::set( s );
391 : : }
392 : 13365864 : MsqVector( const MsqMatrix< L, 1 >& pm ) : MsqMatrix< L, 1 >( pm ) {}
393 : :
394 : 192739880 : double& operator[]( unsigned idx )
395 : : {
396 : 192739880 : return MsqMatrix< L, 1 >::operator()( idx, 0 );
397 : : }
398 : 660 : double operator[]( unsigned idx ) const
399 : : {
400 : 660 : return MsqMatrix< L, 1 >::operator()( idx, 0 );
401 : : }
402 : : double& operator()( unsigned idx )
403 : : {
404 : : return MsqMatrix< L, 1 >::operator()( idx, 0 );
405 : : }
406 : : double operator()( unsigned idx ) const
407 : : {
408 : : return MsqMatrix< L, 1 >::operator()( idx, 0 );
409 : : }
410 : :
411 : 24394229 : MsqVector< L >& operator=( const MsqMatrix< L, 1 >& p_m )
412 : : {
413 : 24394229 : MsqMatrix< L, 1 >::operator=( p_m );
414 : 24394229 : return *this;
415 : : }
416 : : };
417 : :
418 : : /**\brief A subclass of MsqMatrix that behaves more like the old Matrix3D class
419 : : *
420 : : * A MsqMartix that behaves more like the old Matrx3D class.
421 : : * * Constructors are different--be careful.
422 : : * * Adds operator[] for c-array style access.
423 : : */
424 : : template < unsigned R, unsigned C >
425 : : class MsqMatrixA : public MsqMatrix< R, C >
426 : : {
427 : : public:
428 : : /** Initialize all elements to zero */
429 : : MsqMatrixA()
430 : : {
431 : : MsqMatrix< R, C >::set( 0 );
432 : : }
433 : : /** Initialize all elements to passed value */
434 : : MsqMatrixA( double v )
435 : : {
436 : : MsqMatrix< R, C >::set( v );
437 : : }
438 : : MsqMatrixA( const double* v )
439 : : {
440 : : MsqMatrix< R, C >::set( v );
441 : : }
442 : : MsqMatrixA( const char* s )
443 : : {
444 : : MsqMatrix< R, C >::set( s );
445 : : }
446 : : MsqMatrixA( const std::string& s )
447 : : {
448 : : MsqMatrix< R, C >::set( s );
449 : : }
450 : : MsqMatrixA( const MsqMatrix< R, C >& m ) : MsqMatrix< R, C >( m ) {}
451 : : MsqMatrixA( const MsqMatrix< R + 1, C + 1 >& m, unsigned r, unsigned c )
452 : : {
453 : : MsqMatrix< R, C >::make_minor( m, r, c );
454 : : }
455 : :
456 : : double* operator[]( unsigned idx )
457 : : {
458 : : return MsqMatrix< R, C >::m + C * idx;
459 : : }
460 : : const double* operator[]( unsigned idx ) const
461 : : {
462 : : return MsqMatrix< R, C >::m + C * idx;
463 : : }
464 : :
465 : : MsqMatrixA< R, C >& operator=( const MsqMatrixA< R, C >& m )
466 : : {
467 : : MsqMatrixA< R, C >::operator=( m );
468 : : return *this;
469 : : }
470 : : };
471 : :
472 : : template < unsigned R, unsigned C >
473 : : inline void MsqMatrix< R, C >::set( const double v[R][C] )
474 : : {
475 : : for( unsigned r = 0; r < R; ++r )
476 : : for( unsigned c = 0; c < C; ++c )
477 : : operator()( r, c ) = v[r][c];
478 : : }
479 : :
480 : : template < unsigned R, unsigned C >
481 : 16140865 : inline void MsqMatrix< R, C >::diag( double v )
482 : : {
483 : : // for (unsigned r = 0; r < R; ++r)
484 : : // for (unsigned c = 0; c < C; ++c)
485 : : // operator()(r,c) = (r == c) ? v : 0.0;
486 : :
487 : : switch( R )
488 : : {
489 : : default:
490 : : for( unsigned r = 4; r < R; ++r )
491 : : switch( C )
492 : : {
493 : : default:
494 : : for( unsigned k = 4; k < C; ++k )
495 : : operator()( r, k ) = r == k ? v : 0.0;
496 : : case 4:
497 : : operator()( r, 3 ) = 0.0;
498 : : case 3:
499 : : operator()( r, 2 ) = 0.0;
500 : : case 2:
501 : : operator()( r, 1 ) = 0.0;
502 : : case 1:
503 : : operator()( r, 0 ) = 0.0;
504 : : }
505 : : case 4:
506 : : switch( C )
507 : : {
508 : : default:
509 : : for( unsigned k = 4; k < C; ++k )
510 : : operator()( 3, k ) = 0.0;
511 : : case 4:
512 : : operator()( 3, 3 ) = v;
513 : : case 3:
514 : : operator()( 3, 2 ) = 0.0;
515 : : case 2:
516 : : operator()( 3, 1 ) = 0.0;
517 : : case 1:
518 : : operator()( 3, 0 ) = 0.0;
519 : : }
520 : : case 3:
521 : : switch( C )
522 : : {
523 : : default:
524 : : for( unsigned k = 4; k < C; ++k )
525 : : operator()( 2, k ) = 0.0;
526 : : case 4:
527 : : operator()( 2, 3 ) = 0.0;
528 : : case 3:
529 : 1437586 : operator()( 2, 2 ) = v;
530 : : case 2:
531 : 4033910 : operator()( 2, 1 ) = 0.0;
532 : : case 1:
533 : 4033910 : operator()( 2, 0 ) = 0.0;
534 : : }
535 : : case 2:
536 : : switch( C )
537 : : {
538 : : default:
539 : : for( unsigned k = 4; k < C; ++k )
540 : : operator()( 1, k ) = 0.0;
541 : : case 4:
542 : : operator()( 1, 3 ) = 0.0;
543 : : case 3:
544 : 1437586 : operator()( 1, 2 ) = 0.0;
545 : : case 2:
546 : 16140865 : operator()( 1, 1 ) = v;
547 : : case 1:
548 : 16140865 : operator()( 1, 0 ) = 0.0;
549 : : }
550 : : case 1:
551 : : switch( C )
552 : : {
553 : : default:
554 : : for( unsigned k = 4; k < C; ++k )
555 : : operator()( 0, k ) = 0.0;
556 : : case 4:
557 : : operator()( 0, 3 ) = 0.0;
558 : : case 3:
559 : 1437586 : operator()( 0, 2 ) = 0.0;
560 : : case 2:
561 : 16140865 : operator()( 0, 1 ) = 0.0;
562 : : case 1:
563 : 16140865 : operator()( 0, 0 ) = v;
564 : : }
565 : : }
566 : 16140865 : }
567 : :
568 : : template < unsigned R, unsigned C >
569 : : inline void MsqMatrix< R, C >::diag( const double* v )
570 : : {
571 : : // for (unsigned r = 0; r < R; ++r)
572 : : // for (unsigned c = 0; c < C; ++c)
573 : : // operator()(r,c) = (r == c) ? v[r] : 0.0;
574 : :
575 : : switch( R )
576 : : {
577 : : default:
578 : : for( unsigned r = 4; r < R; ++r )
579 : : switch( C )
580 : : {
581 : : default:
582 : : for( unsigned k = 4; k < C; ++k )
583 : : operator()( r, k ) = r == k ? v[r] : 0.0;
584 : : case 4:
585 : : operator()( r, 3 ) = 0.0;
586 : : case 3:
587 : : operator()( r, 2 ) = 0.0;
588 : : case 2:
589 : : operator()( r, 1 ) = 0.0;
590 : : case 1:
591 : : operator()( r, 0 ) = 0.0;
592 : : }
593 : : case 4:
594 : : switch( C )
595 : : {
596 : : default:
597 : : for( unsigned k = 4; k < C; ++k )
598 : : operator()( 3, k ) = 0.0;
599 : : case 4:
600 : : operator()( 3, 3 ) = v[3];
601 : : case 3:
602 : : operator()( 3, 2 ) = 0.0;
603 : : case 2:
604 : : operator()( 3, 1 ) = 0.0;
605 : : case 1:
606 : : operator()( 3, 0 ) = 0.0;
607 : : }
608 : : case 3:
609 : : switch( C )
610 : : {
611 : : default:
612 : : for( unsigned k = 4; k < C; ++k )
613 : : operator()( 2, k ) = 0.0;
614 : : case 4:
615 : : operator()( 2, 3 ) = 0.0;
616 : : case 3:
617 : : operator()( 2, 2 ) = v[2];
618 : : case 2:
619 : : operator()( 2, 1 ) = 0.0;
620 : : case 1:
621 : : operator()( 2, 0 ) = 0.0;
622 : : }
623 : : case 2:
624 : : switch( C )
625 : : {
626 : : default:
627 : : for( unsigned k = 4; k < C; ++k )
628 : : operator()( 1, k ) = 0.0;
629 : : case 4:
630 : : operator()( 1, 3 ) = 0.0;
631 : : case 3:
632 : : operator()( 1, 2 ) = 0.0;
633 : : case 2:
634 : : operator()( 1, 1 ) = v[1];
635 : : case 1:
636 : : operator()( 1, 0 ) = 0.0;
637 : : }
638 : : case 1:
639 : : switch( C )
640 : : {
641 : : default:
642 : : for( unsigned k = 4; k < C; ++k )
643 : : operator()( 0, k ) = 0.0;
644 : : case 4:
645 : : operator()( 0, 3 ) = 0.0;
646 : : case 3:
647 : : operator()( 0, 2 ) = 0.0;
648 : : case 2:
649 : : operator()( 0, 1 ) = 0.0;
650 : : case 1:
651 : : operator()( 0, 0 ) = v[0];
652 : : }
653 : : }
654 : : }
655 : :
656 : : /**\brief Extract minor of a matrix and assign to *this
657 : : *
658 : : * Given a matrix m, a row r and an column c, set *this to
659 : : * the matrix that is m with row r and column c deleted.
660 : : */
661 : : template < unsigned R, unsigned C >
662 : 0 : inline void MsqMatrix< R, C >::make_minor( const MsqMatrix< R + 1, C + 1 >& M, unsigned r, unsigned c )
663 : : {
664 [ # # ][ # # ]: 0 : for( unsigned i = 0; i < r; ++i )
665 : : {
666 [ # # ][ # # ]: 0 : for( unsigned j = 0; j < c; ++j )
667 : 0 : operator()( i, j ) = M( i, j );
668 [ # # ][ # # ]: 0 : for( unsigned j = c; j < C; ++j )
669 : 0 : operator()( i, j ) = M( i, j + 1 );
670 : : }
671 [ # # ][ # # ]: 0 : for( unsigned i = r; i < R; ++i )
672 : : {
673 [ # # ][ # # ]: 0 : for( unsigned j = 0; j < c; ++j )
674 : 0 : operator()( i, j ) = M( i + 1, j );
675 [ # # ][ # # ]: 0 : for( unsigned j = c; j < C; ++j )
676 : 0 : operator()( i, j ) = M( i + 1, j + 1 );
677 : : }
678 : 0 : }
679 : :
680 : : template < unsigned R, unsigned C >
681 : 419097 : inline void MsqMatrix< R, C >::set_row( unsigned r, const MsqMatrix< 1, C >& v )
682 : : {
683 [ + + ]: 1257291 : for( unsigned i = 0; i < C; ++i )
684 : 838194 : operator()( r, i ) = v( 0, i );
685 : 419097 : }
686 : : template < unsigned R, unsigned C >
687 : 0 : inline void MsqMatrix< R, C >::add_row( unsigned r, const MsqMatrix< 1, C >& v )
688 : : {
689 [ # # ][ # # ]: 0 : for( unsigned i = 0; i < C; ++i )
690 : 0 : operator()( r, i ) += v( 0, i );
691 : 0 : }
692 : : template < unsigned R, unsigned C >
693 : : inline void MsqMatrix< R, C >::set_row_transpose( unsigned r, const MsqMatrix< C, 1 >& v )
694 : : {
695 : : for( unsigned i = 0; i < C; ++i )
696 : : operator()( r, i ) = v( i, 0 );
697 : : }
698 : : template < unsigned R, unsigned C >
699 : : inline void MsqMatrix< R, C >::set_rows( const MsqMatrix< 1, C >* v )
700 : : {
701 : : for( unsigned r = 0; r < R; ++r )
702 : : set_row( r, v[r] );
703 : : }
704 : :
705 : : template < unsigned R, unsigned C >
706 : 47433841 : inline void MsqMatrix< R, C >::set_column( unsigned c, const MsqMatrix< R, 1 >& v )
707 : : {
708 [ + + ][ + + ]: 189735364 : for( unsigned i = 0; i < R; ++i )
[ # # ]
709 : 142301523 : operator()( i, c ) = v( i, 0 );
710 : 47433841 : }
711 : : template < unsigned R, unsigned C >
712 : 0 : inline void MsqMatrix< R, C >::add_column( unsigned c, const MsqMatrix< R, 1 >& v )
713 : : {
714 [ # # ][ # # ]: 0 : for( unsigned i = 0; i < R; ++i )
715 : 0 : operator()( i, c ) += v( i, 0 );
716 : 0 : }
717 : : template < unsigned R, unsigned C >
718 : : inline void MsqMatrix< R, C >::set_column_transpose( unsigned c, const MsqMatrix< 1, R >& v )
719 : : {
720 : : for( unsigned i = 0; i < R; ++i )
721 : : operator()( i, c ) = v( 0, i );
722 : : }
723 : : template < unsigned R, unsigned C >
724 : 3148348 : inline void MsqMatrix< R, C >::set_columns( const MsqMatrix< R, 1 >* v )
725 : : {
726 [ + + ][ + + ]: 9997068 : for( unsigned c = 0; c < C; ++c )
727 : 6848720 : set_column( c, v[c] );
728 : 3148348 : }
729 : :
730 : : template < unsigned R, unsigned C >
731 : 132788025 : inline MsqMatrix< R, 1 > MsqMatrix< R, C >::column( unsigned c ) const
732 : : {
733 [ # # ]: 132788025 : MsqMatrix< R, 1 > result;
734 [ + + ][ # # ]: 531152100 : for( unsigned i = 0; i < R; ++i )
[ + + ][ # # ]
[ + + ]
735 [ # # ][ # # ]: 398364075 : result( i, 0 ) = operator()( i, c );
736 : 132788025 : return result;
737 : : }
738 : :
739 : : template < unsigned R, unsigned C >
740 : : inline MsqMatrix< 1, R > MsqMatrix< R, C >::column_transpose( unsigned c ) const
741 : : {
742 : : MsqMatrix< 1, R > result;
743 : : for( unsigned i = 0; i < R; ++i )
744 : : result( 0, i ) = operator()( i, c );
745 : : return result;
746 : : }
747 : :
748 : : /**\brief Set a subset of this matrix to some other matrix */
749 : : template < unsigned R1, unsigned C1, unsigned R2, unsigned C2 >
750 : : inline void set_region( MsqMatrix< R1, C1 >& d, unsigned r, unsigned c, MsqMatrix< R2, C2 >& s )
751 : : {
752 : : const unsigned rmax = r + R2 > R1 ? R1 : r + R2;
753 : : const unsigned cmax = c + C2 > C1 ? C1 : c + C2;
754 : : for( unsigned i = r; i < rmax; ++i )
755 : : for( unsigned j = c; j < cmax; ++j )
756 : : d( i, j ) = s( i - r, j - c );
757 : : }
758 : :
759 : : template < unsigned R, unsigned C >
760 : : inline MsqMatrix< R, C >& MsqMatrix< R, C >::assign_add_transpose( const MsqMatrix< C, R >& other )
761 : : {
762 : : for( unsigned r = 0; r < R; ++r )
763 : : for( unsigned c = 0; c < C; ++c )
764 : : operator()( r, c ) += other( c, r );
765 : : return *this;
766 : : }
767 : :
768 : : template < unsigned R, unsigned C >
769 : : inline MsqMatrix< R, C >& MsqMatrix< R, C >::assign_multiply_elements( const MsqMatrix< R, C >& other )
770 : : {
771 : : for( unsigned i = 0; i < R * C; ++i )
772 : : m[i] *= other.data()[i];
773 : : return *this;
774 : : }
775 : :
776 : : template < unsigned R, unsigned C >
777 : : inline MsqMatrix< R, C >& MsqMatrix< R, C >::assign_product( double s, const MsqMatrix< R, C >& other )
778 : : {
779 : : for( unsigned i = 0; i < R * C; ++i )
780 : : m[i] = s * other.data()[i];
781 : : return *this;
782 : : }
783 : :
784 : : template < unsigned R, unsigned C >
785 : : inline MsqMatrix< R, C >& MsqMatrix< R, C >::assign_add_product( double s, const MsqMatrix< R, C >& other )
786 : : {
787 : : for( unsigned i = 0; i < R * C; ++i )
788 : : m[i] += s * other.data()[i];
789 : : return *this;
790 : : }
791 : :
792 : : template < unsigned R, unsigned C >
793 : 81665204 : inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator+=( const MsqMatrix< R, C >& other )
794 : : {
795 [ + + ][ + + ]: 655348765 : for( unsigned i = 0; i < R * C; ++i )
796 : 573683561 : m[i] += other.data()[i];
797 : 81665204 : return *this;
798 : : }
799 : :
800 : : template < unsigned R, unsigned C >
801 : 11846514 : inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator-=( const MsqMatrix< R, C >& other )
802 : : {
803 [ + + ][ + + ]: 57513248 : for( unsigned i = 0; i < R * C; ++i )
804 : 45666734 : m[i] -= other.data()[i];
805 : 11846514 : return *this;
806 : : }
807 : :
808 : : template < unsigned R, unsigned C >
809 : : inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator+=( double s )
810 : : {
811 : : for( unsigned i = 0; i < R * C; ++i )
812 : : m[i] += s;
813 : : return *this;
814 : : }
815 : :
816 : : template < unsigned R, unsigned C >
817 : 0 : inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator-=( double s )
818 : : {
819 [ # # ]: 0 : for( unsigned i = 0; i < R * C; ++i )
820 : 0 : m[i] -= s;
821 : 0 : return *this;
822 : : }
823 : :
824 : : template < unsigned R, unsigned C >
825 : 101123443 : inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator*=( double s )
826 : : {
827 [ + + ][ + + ]: 496251895 : for( unsigned i = 0; i < R * C; ++i )
[ # # ]
828 : 395128452 : m[i] *= s;
829 : 101123443 : return *this;
830 : : }
831 : :
832 : : template < unsigned R, unsigned C >
833 : 5499122 : inline MsqMatrix< R, C >& MsqMatrix< R, C >::operator/=( double s )
834 : : {
835 [ # # ][ + + ]: 21996488 : for( unsigned i = 0; i < R * C; ++i )
836 : 16497366 : m[i] /= s;
837 : 5499122 : return *this;
838 : : }
839 : :
840 : : template < unsigned R, unsigned C >
841 : 1257308 : inline MsqMatrix< R, C > operator-( const MsqMatrix< R, C >& m )
842 : : {
843 [ + - ]: 1257308 : MsqMatrix< R, C > result;
844 [ + + ][ + + ]: 4610135 : for( unsigned i = 0; i < R * C; ++i )
[ + + ]
845 [ + - ][ + - ]: 3352827 : result.data()[i] = -m.data()[i];
846 : 1257308 : return result;
847 : : }
848 : :
849 : : template < unsigned R, unsigned C >
850 : : inline MsqMatrix< R, C > operator+( const MsqMatrix< R, C >& m, double s )
851 : : {
852 : : MsqMatrix< R, C > tmp( m );
853 : : tmp += s;
854 : : return tmp;
855 : : }
856 : :
857 : : template < unsigned R, unsigned C >
858 : : inline MsqMatrix< R, C > operator+( double s, const MsqMatrix< R, C >& m )
859 : : {
860 : : MsqMatrix< R, C > tmp( m );
861 : : tmp += s;
862 : : return tmp;
863 : : }
864 : :
865 : : template < unsigned R, unsigned C >
866 : 709080 : inline MsqMatrix< R, C > operator+( const MsqMatrix< R, C >& A, const MsqMatrix< R, C >& B )
867 : : {
868 : 709080 : MsqMatrix< R, C > tmp( A );
869 : 709080 : tmp += B;
870 : 709080 : return tmp;
871 : : }
872 : :
873 : : template < unsigned R, unsigned C >
874 : 0 : inline MsqMatrix< R, C > operator-( const MsqMatrix< R, C >& m, double s )
875 : : {
876 : 0 : MsqMatrix< R, C > tmp( m );
877 : 0 : tmp -= s;
878 : 0 : return tmp;
879 : : }
880 : :
881 : : template < unsigned R, unsigned C >
882 : : inline MsqMatrix< R, C > operator-( double s, const MsqMatrix< R, C >& m )
883 : : {
884 : : MsqMatrix< R, C > tmp( m );
885 : : tmp -= s;
886 : : return tmp;
887 : : }
888 : :
889 : : template < unsigned R, unsigned C >
890 : 5499122 : inline MsqMatrix< R, C > operator-( const MsqMatrix< R, C >& A, const MsqMatrix< R, C >& B )
891 : : {
892 : 5499122 : MsqMatrix< R, C > tmp( A );
893 [ # # ]: 5499122 : tmp -= B;
894 : 5499122 : return tmp;
895 : : }
896 : :
897 : : template < unsigned R, unsigned C >
898 : 28174550 : inline MsqMatrix< R, C > operator*( const MsqMatrix< R, C >& m, double s )
899 : : {
900 : 28174550 : MsqMatrix< R, C > tmp( m );
901 [ # # ]: 28174550 : tmp *= s;
902 : 28174550 : return tmp;
903 : : }
904 : :
905 : : template < unsigned R, unsigned C >
906 : 23847242 : inline MsqMatrix< R, C > operator*( double s, const MsqMatrix< R, C >& m )
907 : : {
908 : 23847242 : MsqMatrix< R, C > tmp( m );
909 [ # # ]: 23847242 : tmp *= s;
910 : 23847242 : return tmp;
911 : : }
912 : :
913 : : template < unsigned R, unsigned RC, unsigned C >
914 : 874181025 : inline double multiply_helper_result_val( unsigned r, unsigned c, const MsqMatrix< R, RC >& A,
915 : : const MsqMatrix< RC, C >& B )
916 : : {
917 : 874181025 : double tmp = A( r, 0 ) * B( 0, c );
918 : : switch( RC )
919 : : {
920 : : default:
921 : : for( unsigned k = 6; k < RC; ++k )
922 : : tmp += A( r, k ) * B( k, c );
923 : : case 6:
924 : : tmp += A( r, 5 ) * B( 5, c );
925 : : case 5:
926 : : tmp += A( r, 4 ) * B( 4, c );
927 : : case 4:
928 : : tmp += A( r, 3 ) * B( 3, c );
929 : : case 3:
930 : 147044264 : tmp += A( r, 2 ) * B( 2, c );
931 : : case 2:
932 : 369344781 : tmp += A( r, 1 ) * B( 1, c );
933 : : case 1:;
934 : : }
935 : 874181025 : return tmp;
936 : : }
937 : :
938 : : template < unsigned R, unsigned RC, unsigned C >
939 : 160396158 : inline MsqMatrix< R, C > operator*( const MsqMatrix< R, RC >& A, const MsqMatrix< RC, C >& B )
940 : : {
941 : : // MsqMatrix<R,C> result(0.0);
942 : : // for (unsigned i = 0; i < R; ++i)
943 : : // for (unsigned j = 0; j < C; ++j)
944 : : // for (unsigned k = 0; k < RC; ++k)
945 : : // result(i,j) += A(i,k) * B(k,j);
946 : :
947 [ + - ]: 160396158 : MsqMatrix< R, C > result;
948 [ + + ][ + + ]: 574327149 : for( unsigned r = 0; r < R; ++r )
[ + + ][ + + ]
[ + + ][ + + ]
[ + + ][ + + ]
[ + + ][ + + ]
949 [ + + ][ + + ]: 1288112016 : for( unsigned c = 0; c < C; ++c )
[ + + ][ + + ]
[ + + ][ + + ]
[ + + ][ + + ]
[ + + ][ + + ]
950 [ + - ][ + - ]: 874181025 : result( r, c ) = multiply_helper_result_val( r, c, A, B );
951 : :
952 : 160396158 : return result;
953 : : }
954 : :
955 : : template < unsigned R, unsigned C >
956 : 5499122 : inline MsqMatrix< R, C > operator/( const MsqMatrix< R, C >& m, double s )
957 : : {
958 : 5499122 : MsqMatrix< R, C > tmp( m );
959 [ # # ]: 5499122 : tmp /= s;
960 : 5499122 : return tmp;
961 : : }
962 : :
963 : : template < unsigned RC >
964 : 0 : inline double cofactor( const MsqMatrix< RC, RC >& m, unsigned r, unsigned c )
965 : : {
966 : 0 : const double sign[] = { 1.0, -1.0 };
967 [ # # ][ # # ]: 0 : return sign[( r + c ) % 2] * det( MsqMatrix< RC - 1, RC - 1 >( m, r, c ) );
968 : : }
969 : :
970 : : template < unsigned RC >
971 : 0 : inline double det( const MsqMatrix< RC, RC >& m )
972 : : {
973 : 0 : double result = 0.0;
974 [ # # ]: 0 : for( unsigned i = 0; i < RC; ++i )
975 : 0 : result += m( 0, i ) * cofactor< RC >( m, 0, i );
976 : 0 : return result;
977 : : }
978 : :
979 : : // inline
980 : : // double det( const MsqMatrix<1,1>& m )
981 : : // { return m(0,0); }
982 : :
983 : 42476549 : inline double det( const MsqMatrix< 2, 2 >& m )
984 : : {
985 : 42476549 : return m( 0, 0 ) * m( 1, 1 ) - m( 0, 1 ) * m( 1, 0 );
986 : : }
987 : :
988 : 11336297 : inline double det( const MsqMatrix< 3, 3 >& m )
989 : : {
990 : 11336297 : return m( 0, 0 ) * ( m( 1, 1 ) * m( 2, 2 ) - m( 2, 1 ) * m( 1, 2 ) ) +
991 : 11336297 : m( 0, 1 ) * ( m( 2, 0 ) * m( 1, 2 ) - m( 1, 0 ) * m( 2, 2 ) ) +
992 : 11336297 : m( 0, 2 ) * ( m( 1, 0 ) * m( 2, 1 ) - m( 2, 0 ) * m( 1, 1 ) );
993 : : }
994 : :
995 : 14608270 : inline MsqMatrix< 2, 2 > adj( const MsqMatrix< 2, 2 >& m )
996 : : {
997 : 14608270 : MsqMatrix< 2, 2 > result;
998 : 14608270 : result( 0, 0 ) = m( 1, 1 );
999 : 14608270 : result( 0, 1 ) = -m( 0, 1 );
1000 : 14608270 : result( 1, 0 ) = -m( 1, 0 );
1001 : 14608270 : result( 1, 1 ) = m( 0, 0 );
1002 : 14608270 : return result;
1003 : : }
1004 : :
1005 : 7189684 : inline MsqMatrix< 2, 2 > transpose_adj( const MsqMatrix< 2, 2 >& m )
1006 : : {
1007 : 7189684 : MsqMatrix< 2, 2 > result;
1008 : 7189684 : result( 0, 0 ) = m( 1, 1 );
1009 : 7189684 : result( 1, 0 ) = -m( 0, 1 );
1010 : 7189684 : result( 0, 1 ) = -m( 1, 0 );
1011 : 7189684 : result( 1, 1 ) = m( 0, 0 );
1012 : 7189684 : return result;
1013 : : }
1014 : :
1015 : 5495214 : inline MsqMatrix< 3, 3 > adj( const MsqMatrix< 3, 3 >& m )
1016 : : {
1017 : 5495214 : MsqMatrix< 3, 3 > result;
1018 : :
1019 : 5495214 : result( 0, 0 ) = m( 1, 1 ) * m( 2, 2 ) - m( 1, 2 ) * m( 2, 1 );
1020 : 5495214 : result( 0, 1 ) = m( 0, 2 ) * m( 2, 1 ) - m( 0, 1 ) * m( 2, 2 );
1021 : 5495214 : result( 0, 2 ) = m( 0, 1 ) * m( 1, 2 ) - m( 0, 2 ) * m( 1, 1 );
1022 : :
1023 : 5495214 : result( 1, 0 ) = m( 1, 2 ) * m( 2, 0 ) - m( 1, 0 ) * m( 2, 2 );
1024 : 5495214 : result( 1, 1 ) = m( 0, 0 ) * m( 2, 2 ) - m( 0, 2 ) * m( 2, 0 );
1025 : 5495214 : result( 1, 2 ) = m( 0, 2 ) * m( 1, 0 ) - m( 0, 0 ) * m( 1, 2 );
1026 : :
1027 : 5495214 : result( 2, 0 ) = m( 1, 0 ) * m( 2, 1 ) - m( 1, 1 ) * m( 2, 0 );
1028 : 5495214 : result( 2, 1 ) = m( 0, 1 ) * m( 2, 0 ) - m( 0, 0 ) * m( 2, 1 );
1029 : 5495214 : result( 2, 2 ) = m( 0, 0 ) * m( 1, 1 ) - m( 0, 1 ) * m( 1, 0 );
1030 : :
1031 : 5495214 : return result;
1032 : : }
1033 : :
1034 : 874140 : inline MsqMatrix< 3, 3 > transpose_adj( const MsqMatrix< 3, 3 >& m )
1035 : : {
1036 : 874140 : MsqMatrix< 3, 3 > result;
1037 : :
1038 : 874140 : result( 0, 0 ) = m( 1, 1 ) * m( 2, 2 ) - m( 1, 2 ) * m( 2, 1 );
1039 : 874140 : result( 0, 1 ) = m( 1, 2 ) * m( 2, 0 ) - m( 1, 0 ) * m( 2, 2 );
1040 : 874140 : result( 0, 2 ) = m( 1, 0 ) * m( 2, 1 ) - m( 1, 1 ) * m( 2, 0 );
1041 : :
1042 : 874140 : result( 1, 0 ) = m( 0, 2 ) * m( 2, 1 ) - m( 0, 1 ) * m( 2, 2 );
1043 : 874140 : result( 1, 1 ) = m( 0, 0 ) * m( 2, 2 ) - m( 0, 2 ) * m( 2, 0 );
1044 : 874140 : result( 1, 2 ) = m( 0, 1 ) * m( 2, 0 ) - m( 0, 0 ) * m( 2, 1 );
1045 : :
1046 : 874140 : result( 2, 0 ) = m( 0, 1 ) * m( 1, 2 ) - m( 0, 2 ) * m( 1, 1 );
1047 : 874140 : result( 2, 1 ) = m( 0, 2 ) * m( 1, 0 ) - m( 0, 0 ) * m( 1, 2 );
1048 : 874140 : result( 2, 2 ) = m( 0, 0 ) * m( 1, 1 ) - m( 0, 1 ) * m( 1, 0 );
1049 : :
1050 : 874140 : return result;
1051 : : }
1052 : :
1053 : : template < unsigned R, unsigned C >
1054 : 20079104 : inline MsqMatrix< R, C > inverse( const MsqMatrix< R, C >& m )
1055 : : {
1056 [ + - ][ + - ]: 20079104 : return adj( m ) * ( 1.0 / det( m ) );
1057 : : }
1058 : :
1059 : : template < unsigned R, unsigned C >
1060 : 92023264 : inline MsqMatrix< R, C > transpose( const MsqMatrix< C, R >& m )
1061 : : {
1062 [ # # ][ # # ]: 92023264 : MsqMatrix< R, C > result;
[ + - ]
1063 [ + + ][ # # ]: 222277236 : for( unsigned r = 0; r < R; ++r )
[ + + ][ # # ]
[ # # ][ + + ]
[ + + ][ + + ]
1064 [ + + ][ # # ]: 411629300 : for( unsigned c = 0; c < C; ++c )
[ + + ][ # # ]
[ # # ][ + + ]
[ + + ][ + + ]
1065 [ # # ][ - # ]: 281375328 : result( r, c ) = m( c, r );
[ # # ][ # ]
[ + ][ + - ]
1066 : 92023264 : return result;
1067 : : }
1068 : : /*
1069 : : template <unsigned R> inline
1070 : : const MsqMatrix<R,1>& transpose( const MsqMatrix<1,R>& m )
1071 : : { return *reinterpret_cast<const MsqMatrix<R,1>*>(&m); }
1072 : :
1073 : : template <unsigned C> inline
1074 : : const MsqMatrix<1,C>& transpose( const MsqMatrix<C,1>& m )
1075 : : { return *reinterpret_cast<const MsqMatrix<1,C>*>(&m); }
1076 : : */
1077 : : template < unsigned RC >
1078 : 291580 : inline double trace( const MsqMatrix< RC, RC >& m )
1079 : : {
1080 : 291580 : double result = m( 0, 0 );
1081 [ # # ][ + + ]: 583160 : for( unsigned i = 1; i < RC; ++i )
1082 : 291580 : result += m( i, i );
1083 : 291580 : return result;
1084 : : }
1085 : :
1086 : : template < unsigned R, unsigned C >
1087 : 65160533 : inline double sqr_Frobenius( const MsqMatrix< R, C >& m )
1088 : : {
1089 : 65160533 : double result = *m.data() * *m.data();
1090 [ + + ][ + + ]: 239810486 : for( unsigned i = 1; i < R * C; ++i )
1091 : 174649953 : result += m.data()[i] * m.data()[i];
1092 : 65160533 : return result;
1093 : : }
1094 : :
1095 : : template < unsigned R, unsigned C >
1096 : 53700934 : inline double Frobenius( const MsqMatrix< R, C >& m )
1097 : : {
1098 : 53700934 : return std::sqrt( sqr_Frobenius< R, C >( m ) );
1099 : : }
1100 : :
1101 : : template < unsigned R, unsigned C >
1102 : : inline bool operator==( const MsqMatrix< R, C >& A, const MsqMatrix< R, C >& B )
1103 : : {
1104 : : for( unsigned i = 0; i < R * C; ++i )
1105 : : if( A.data()[i] != B.data()[i] ) return false;
1106 : : return true;
1107 : : }
1108 : :
1109 : : template < unsigned R, unsigned C >
1110 : : inline bool operator!=( const MsqMatrix< R, C >& A, const MsqMatrix< R, C >& B )
1111 : : {
1112 : : return !( A == B );
1113 : : }
1114 : :
1115 : : template < unsigned R, unsigned C >
1116 : : inline std::ostream& operator<<( std::ostream& str, const MsqMatrix< R, C >& m )
1117 : : {
1118 : : str << m.data()[0];
1119 : : for( unsigned i = 1; i < R * C; ++i )
1120 : : str << ' ' << m.data()[i];
1121 : : return str;
1122 : : }
1123 : :
1124 : : template < unsigned R, unsigned C >
1125 : : inline std::istream& operator>>( std::istream& str, MsqMatrix< R, C >& m )
1126 : : {
1127 : : for( unsigned i = 0; i < R * C; ++i )
1128 : : str >> m.data()[i];
1129 : : return str;
1130 : : }
1131 : :
1132 : : template < unsigned R >
1133 : : inline double sqr_length( const MsqMatrix< R, 1 >& v )
1134 : : {
1135 : : return sqr_Frobenius( v );
1136 : : }
1137 : :
1138 : : template < unsigned C >
1139 : : inline double sqr_length( const MsqMatrix< 1, C >& v )
1140 : : {
1141 : : return sqr_Frobenius( v );
1142 : : }
1143 : :
1144 : : template < unsigned R >
1145 : 48307716 : inline double length( const MsqMatrix< R, 1 >& v )
1146 : : {
1147 : 48307716 : return Frobenius( v );
1148 : : }
1149 : :
1150 : : template < unsigned C >
1151 : : inline double length( const MsqMatrix< 1, C >& v )
1152 : : {
1153 : : return Frobenius( v );
1154 : : }
1155 : :
1156 : : template < unsigned R, unsigned C >
1157 : 40691734 : inline double inner_product( const MsqMatrix< R, C >& m1, const MsqMatrix< R, C >& m2 )
1158 : : {
1159 : 40691734 : double result = 0.0;
1160 [ + + ][ # # ]: 162766936 : for( unsigned r = 0; r < R; ++r )
1161 [ + + ][ # # ]: 244150404 : for( unsigned c = 0; c < C; ++c )
1162 : 122075202 : result += m1( r, c ) * m2( r, c );
1163 : 40691734 : return result;
1164 : : }
1165 : :
1166 : : template < unsigned R, unsigned C >
1167 : 17845442 : inline MsqMatrix< R, C > outer( const MsqMatrix< R, 1 >& v1, const MsqMatrix< C, 1 >& v2 )
1168 : : {
1169 : 17845442 : MsqMatrix< R, C > result;
1170 [ + + ][ + + ]: 71381768 : for( unsigned r = 0; r < R; ++r )
1171 [ + + ][ + + ]: 164653206 : for( unsigned c = 0; c < C; ++c )
1172 : 111116880 : result( r, c ) = v1( r, 0 ) * v2( c, 0 );
1173 : 17845442 : return result;
1174 : : }
1175 : :
1176 : : template < unsigned R, unsigned C >
1177 : 0 : inline MsqMatrix< R, C > outer( const MsqMatrix< 1, R >& v1, const MsqMatrix< 1, C >& v2 )
1178 : : {
1179 : 0 : MsqMatrix< R, C > result;
1180 [ # # ][ # # ]: 0 : for( unsigned r = 0; r < R; ++r )
1181 [ # # ][ # # ]: 0 : for( unsigned c = 0; c < C; ++c )
1182 : 0 : result( r, c ) = v1( 0, r ) * v2( 0, c );
1183 : 0 : return result;
1184 : : }
1185 : :
1186 : : inline double vector_product( const MsqMatrix< 2, 1 >& v1, const MsqMatrix< 2, 1 >& v2 )
1187 : : {
1188 : : return v1( 0, 0 ) * v2( 1, 0 ) - v1( 1, 0 ) * v2( 0, 0 );
1189 : : }
1190 : :
1191 : : inline double vector_product( const MsqMatrix< 1, 2 >& v1, const MsqMatrix< 1, 2 >& v2 )
1192 : : {
1193 : : return v1( 0, 0 ) * v2( 0, 1 ) - v1( 0, 1 ) * v2( 0, 0 );
1194 : : }
1195 : :
1196 : 45476920 : inline MsqMatrix< 3, 1 > vector_product( const MsqMatrix< 3, 1 >& a, const MsqMatrix< 3, 1 >& b )
1197 : : {
1198 : 45476920 : MsqMatrix< 3, 1 > result;
1199 : 45476920 : result( 0, 0 ) = a( 1, 0 ) * b( 2, 0 ) - a( 2, 0 ) * b( 1, 0 );
1200 : 45476920 : result( 1, 0 ) = a( 2, 0 ) * b( 0, 0 ) - a( 0, 0 ) * b( 2, 0 );
1201 : 45476920 : result( 2, 0 ) = a( 0, 0 ) * b( 1, 0 ) - a( 1, 0 ) * b( 0, 0 );
1202 : 45476920 : return result;
1203 : : }
1204 : :
1205 : : inline MsqMatrix< 1, 3 > vector_product( const MsqMatrix< 1, 3 >& a, const MsqMatrix< 1, 3 >& b )
1206 : : {
1207 : : MsqMatrix< 1, 3 > result;
1208 : : result( 0, 0 ) = a( 0, 1 ) * b( 0, 2 ) - a( 0, 2 ) * b( 0, 1 );
1209 : : result( 0, 1 ) = a( 0, 2 ) * b( 0, 0 ) - a( 0, 0 ) * b( 0, 2 );
1210 : : result( 0, 2 ) = a( 0, 0 ) * b( 0, 1 ) - a( 0, 1 ) * b( 0, 0 );
1211 : : return result;
1212 : : }
1213 : :
1214 : : template < unsigned R, unsigned C >
1215 : 40691734 : inline double operator%( const MsqMatrix< R, C >& v1, const MsqMatrix< R, C >& v2 )
1216 : : {
1217 : 40691734 : return inner_product( v1, v2 );
1218 : : }
1219 : :
1220 : : inline double operator*( const MsqMatrix< 2, 1 >& v1, const MsqMatrix< 2, 1 >& v2 )
1221 : : {
1222 : : return vector_product( v1, v2 );
1223 : : }
1224 : :
1225 : : inline double operator*( const MsqMatrix< 1, 2 >& v1, const MsqMatrix< 1, 2 >& v2 )
1226 : : {
1227 : : return vector_product( v1, v2 );
1228 : : }
1229 : :
1230 : 45476920 : inline MsqMatrix< 3, 1 > operator*( const MsqMatrix< 3, 1 >& v1, const MsqMatrix< 3, 1 >& v2 )
1231 : : {
1232 : 45476920 : return vector_product( v1, v2 );
1233 : : }
1234 : :
1235 : : inline MsqMatrix< 1, 3 > operator*( const MsqMatrix< 1, 3 >& v1, const MsqMatrix< 1, 3 >& v2 )
1236 : : {
1237 : : return vector_product( v1, v2 );
1238 : : }
1239 : :
1240 : : /** Compute QR factorization of A */
1241 : : inline void QR( const MsqMatrix< 3, 3 >& A, MsqMatrix< 3, 3 >& Q, MsqMatrix< 3, 3 >& R )
1242 : : {
1243 : : Q = A;
1244 : :
1245 : : R( 0, 0 ) = sqrt( Q( 0, 0 ) * Q( 0, 0 ) + Q( 1, 0 ) * Q( 1, 0 ) + Q( 2, 0 ) * Q( 2, 0 ) );
1246 : : double temp_dbl = 1.0 / R( 0, 0 );
1247 : : R( 1, 0 ) = 0.0;
1248 : : R( 2, 0 ) = 0.0;
1249 : : Q( 0, 0 ) *= temp_dbl;
1250 : : Q( 1, 0 ) *= temp_dbl;
1251 : : Q( 2, 0 ) *= temp_dbl;
1252 : :
1253 : : R( 0, 1 ) = Q( 0, 0 ) * Q( 0, 1 ) + Q( 1, 0 ) * Q( 1, 1 ) + Q( 2, 0 ) * Q( 2, 1 );
1254 : : Q( 0, 1 ) -= Q( 0, 0 ) * R( 0, 1 );
1255 : : Q( 1, 1 ) -= Q( 1, 0 ) * R( 0, 1 );
1256 : : Q( 2, 1 ) -= Q( 2, 0 ) * R( 0, 1 );
1257 : :
1258 : : R( 0, 2 ) = Q( 0, 0 ) * Q( 0, 2 ) + Q( 1, 0 ) * Q( 1, 2 ) + Q( 2, 0 ) * Q( 2, 2 );
1259 : : Q( 0, 2 ) -= Q( 0, 0 ) * R( 0, 2 );
1260 : : Q( 1, 2 ) -= Q( 1, 0 ) * R( 0, 2 );
1261 : : Q( 2, 2 ) -= Q( 2, 0 ) * R( 0, 2 );
1262 : :
1263 : : R( 1, 1 ) = sqrt( Q( 0, 1 ) * Q( 0, 1 ) + Q( 1, 1 ) * Q( 1, 1 ) + Q( 2, 1 ) * Q( 2, 1 ) );
1264 : : temp_dbl = 1.0 / R( 1, 1 );
1265 : : R( 2, 1 ) = 0.0;
1266 : : Q( 0, 1 ) *= temp_dbl;
1267 : : Q( 1, 1 ) *= temp_dbl;
1268 : : Q( 2, 1 ) *= temp_dbl;
1269 : :
1270 : : R( 1, 2 ) = Q( 0, 1 ) * Q( 0, 2 ) + Q( 1, 1 ) * Q( 1, 2 ) + Q( 2, 1 ) * Q( 2, 2 );
1271 : : Q( 0, 2 ) -= Q( 0, 1 ) * R( 1, 2 );
1272 : : Q( 1, 2 ) -= Q( 1, 1 ) * R( 1, 2 );
1273 : : Q( 2, 2 ) -= Q( 2, 1 ) * R( 1, 2 );
1274 : :
1275 : : R( 2, 2 ) = sqrt( Q( 0, 2 ) * Q( 0, 2 ) + Q( 1, 2 ) * Q( 1, 2 ) + Q( 2, 2 ) * Q( 2, 2 ) );
1276 : : temp_dbl = 1.0 / R( 2, 2 );
1277 : : Q( 0, 2 ) *= temp_dbl;
1278 : : Q( 1, 2 ) *= temp_dbl;
1279 : : Q( 2, 2 ) *= temp_dbl;
1280 : : }
1281 : :
1282 : : /** Compute QR factorization of A */
1283 : : inline void QR( const MsqMatrix< 2, 2 >& A, MsqMatrix< 2, 2 >& Q, MsqMatrix< 2, 2 >& R )
1284 : : {
1285 : : R( 0, 0 ) = std::sqrt( A( 0, 0 ) * A( 0, 0 ) + A( 1, 0 ) * A( 1, 0 ) );
1286 : : const double r0inv = 1.0 / R( 0, 0 );
1287 : : Q( 0, 0 ) = Q( 1, 1 ) = A( 0, 0 ) * r0inv;
1288 : : Q( 1, 0 ) = A( 1, 0 ) * r0inv;
1289 : : Q( 0, 1 ) = -Q( 1, 0 );
1290 : : R( 0, 1 ) = r0inv * ( A( 0, 0 ) * A( 0, 1 ) + A( 1, 0 ) * A( 1, 1 ) );
1291 : : R( 1, 1 ) = r0inv * ( A( 0, 0 ) * A( 1, 1 ) - A( 0, 1 ) * A( 1, 0 ) );
1292 : : R( 1, 0 ) = 0.0;
1293 : : }
1294 : :
1295 : : } // namespace MBMesquite
1296 : :
1297 : : #endif
|
