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 : :
28 : : /** \file Matrix.hpp
29 : : * \brief
30 : : * \author Jason Kraftcheck
31 : : */
32 : :
33 : : #ifndef MESHKIT_MATRIX_HPP
34 : : #define MESHKIT_MATRIX_HPP
35 : :
36 : : #include <string>
37 : : #include <istream>
38 : : #include <ostream>
39 : : #include <sstream>
40 : : #include <cmath>
41 : :
42 : : namespace MeshKit {
43 : :
44 : : /**\brief Fixed-size matrix class
45 : : *
46 : : * This class implements a fixed-size 2-dimensional matrix.
47 : : * The actual size is specified with template parameters.
48 : : */
49 : : template <unsigned R, unsigned C>
50 : : class Matrix {
51 : : protected:
52 : : double m[R*C];
53 : :
54 : : public:
55 : : typedef Matrix<R,C> my_type;
56 : :
57 : : enum { ROWS = R, COLS = C };
58 : :
59 : : /** Constructor for uninitialized matrix */
60 : 74184 : Matrix() { }
61 : : /** Initialize diagonal values, zero others */
62 : 128 : Matrix( double v ) { diag(v); }
63 : : /** Initialize to an array of values */
64 : : Matrix( const double* v ) { set(v); }
65 : : /** Initialize from 2D array */
66 : : Matrix( const double v[R][C] ) { set(v); }
67 : : /** Initialize with column vectors */
68 : : Matrix( const Matrix<R,1>* c ) { set_columns(c); }
69 : : /** Initialize with row vectors */
70 : : Matrix( const Matrix<1,C>* r ) { set_rows(r); }
71 : : /** Parse initial values from string */
72 : : Matrix( const char* s ) { set(s); }
73 : : /** Parse initial values from string */
74 : : Matrix( const std::string& s ) { set(s); }
75 : : /** Initialize to the minor of a larger matrix
76 : : * This matrix is the passed matrix with the
77 : : * specified row and column removed.
78 : : */
79 : : Matrix( const Matrix<R+1,C+1>& m, unsigned r, unsigned c )
80 : : { make_minor(m,r,c); }
81 : :
82 : : Matrix<R,C>& operator=( double v ) { set(v); return *this; }
83 : : Matrix<R,C>& operator=( const double* v ) { set(v); return *this; }
84 : : Matrix<R,C>& operator=( const char* s ) { set(s); return *this; }
85 : : Matrix<R,C>& operator=( const std::string& s ) { set(s); return *this; }
86 : :
87 : 913942 : double& operator()( unsigned r, unsigned c ) { return m[r*C+c]; }
88 : 10998612 : double operator()( unsigned r, unsigned c ) const { return m[r*C+c]; }
89 : 120484 : double* data() { return m; }
90 : 424524 : const double* data() const { return m; }
91 : :
92 : : void zero() { set(0.0); }
93 : : void identity() { diag(1.0); }
94 : 37 : void set( double v ) {
95 [ + + ]: 148 : for (unsigned i = 0; i < R*C; ++i) m[i] = v;
96 : 37 : }
97 : 511 : void set( const double* v ) {
98 [ + + ]: 2044 : for (unsigned i = 0; i < R*C; ++i) m[i] = v[i];
99 : 511 : }
100 : : void set( const double v[R][C] );
101 : : void set( const char* s ) { std::istringstream i(s); i >> *this; }
102 : : void set( const std::string& s ) { set( s.c_str() ); }
103 : : /** Set diagonal value to passed values, others to zero. */
104 : : inline void diag( double v );
105 : : /** Set diagonal values to passed values, others to zero. */
106 : : inline void diag( const double* v );
107 : : /** Set this matrix to the minor of a larger matrix */
108 : : inline void make_minor( const Matrix<R+1,C+1>& m, unsigned r, unsigned c );
109 : :
110 : : /** *this += transpose(other) */
111 : : inline Matrix<R,C>& assign_add_transpose( const Matrix<C,R>& other );
112 : : /** *this = s*m */
113 : : inline Matrix<R,C>& assign_product( double s, const Matrix<R,C>& m );
114 : : /** *this += s*m */
115 : : inline Matrix<R,C>& assign_add_product( double s, const Matrix<R,C>& m );
116 : : /** multiply each element by the cooresponding element in m */
117 : : inline Matrix<R,C>& assign_multiply_elements( const Matrix<R,C>& m );
118 : :
119 : : inline Matrix<R,C>& operator+=( const Matrix<R,C>& other );
120 : : inline Matrix<R,C>& operator-=( const Matrix<R,C>& other );
121 : : inline Matrix<R,C>& operator+=( double scalar );
122 : : inline Matrix<R,C>& operator-=( double scalar );
123 : : inline Matrix<R,C>& operator*=( double scalar );
124 : : inline Matrix<R,C>& operator/=( double scalar );
125 : :
126 : : // Matrix<1,C>& row( unsigned r ) { return *(Matrix<1,C>*)(m+C*r); }
127 : : //const Matrix<1,C>& row( unsigned r ) const { return *(Matrix<1,C>*)(m+C*r); }
128 : : Matrix<1,C> row( unsigned r ) const { return Matrix<1,C>(m+C*r); }
129 : : Matrix<R,1> column( unsigned c ) const;
130 : : Matrix<1,R> column_transpose( unsigned c ) const;
131 : :
132 : : void set_row( unsigned r, const Matrix<1,C>& v );
133 : : void add_row( unsigned r, const Matrix<1,C>& v );
134 : : void set_row_transpose( unsigned r, const Matrix<C,1>& v );
135 : : void set_rows( const Matrix<1,C>* v );
136 : : void set_column( unsigned c, const Matrix<R,1>& v );
137 : : void add_column( unsigned c, const Matrix<R,1>& v );
138 : : void set_column_transpose( unsigned c, const Matrix<1,R>& v );
139 : : void set_columns( const Matrix<R,1>* v );
140 : : };
141 : :
142 : : template <>
143 : : class Matrix<1,1> {
144 : : protected:
145 : : double m;
146 : :
147 : : public:
148 : : typedef Matrix<1,1> my_type;
149 : :
150 : : enum { ROWS = 1, COLS = 1 };
151 : :
152 : : /** Constructor for uninitialized matrix */
153 : : Matrix() { }
154 : : /** Initialize diagonal values, zero others */
155 : : Matrix( double v ) : m(v) {}
156 : : /** Initialize to an array of values */
157 : : Matrix( const double* v ) : m(*v) {}
158 : : /** Parse initial values from string */
159 : : Matrix( const char* s ) { set(s); }
160 : : /** Parse initial values from string */
161 : : Matrix( const std::string& s ) { set(s); }
162 : : /** Initialize to the minor of a larger matrix
163 : : * This matrix is the passed matrix with the
164 : : * specified row and column removed.
165 : : */
166 : : Matrix( const Matrix<2,2>& M, unsigned r, unsigned c ) : m(M(r,c)) {}
167 : :
168 : : Matrix<1,1>& operator=( double v ) { m = v; return *this; }
169 : : Matrix<1,1>& operator=( const double* v ) { m = *v; return *this; }
170 : : Matrix<1,1>& operator=( const char* s ) { set(s); return *this; }
171 : : Matrix<1,1>& operator=( const std::string& s ) { set(s); return *this; }
172 : :
173 : : double& operator()( unsigned, unsigned ) { return m; }
174 : : double operator()( unsigned, unsigned ) const { return m; }
175 : : double* data() { return &m; }
176 : : const double* data() const { return &m; }
177 : :
178 : : void zero() { m = 0.0; }
179 : : void identity() { m = 1.0; }
180 : : void set( double v ) { m = v; }
181 : : void set( const double* v ) { m= *v; }
182 : : void set( const char* s ) { std::istringstream i(s); i >> m; }
183 : : void set( const std::string& s ) { set( s.c_str() ); }
184 : : /** Set diagonal value to passed values, others to zero. */
185 : : inline void diag( double v ) { m = v; }
186 : : /** Set diagonal values to passed values, others to zero. */
187 : : inline void diag( const double* v ) { m = *v; }
188 : : /** Set this matrix to the minor of a larger matrix */
189 : : inline void make_minor( const Matrix<2,2>& M, unsigned r, unsigned c )
190 : : { m = M(r,c); }
191 : :
192 : : /** *this += transpose(other) */
193 : : inline Matrix<1,1>& assign_add_transpose( const Matrix<1,1>& other ) {
194 : : m += other.m; return *this;
195 : : }
196 : : /** *this = s*m */
197 : : inline Matrix<1,1>& assign_product( double s, const Matrix<1,1>& other ) {
198 : : m = s*other.m; return *this;
199 : : }
200 : : /** *this += s*m */
201 : : inline Matrix<1,1>& assign_add_product( double s, const Matrix<1,1>& other ) {
202 : : m += s*other.m; return *this;
203 : : }
204 : : /** multiply each element by the cooresponding element in m */
205 : : inline Matrix<1,1>& assign_multiply_elements( const Matrix<1,1>& other ) {
206 : : m *= other.m; return *this;
207 : : }
208 : :
209 : : operator double () const {
210 : : return m;
211 : : }
212 : : };
213 : :
214 : : /** \brief Vector is a 1xL Matrix
215 : : *
216 : : * Define a Vector as a 1xL Matrix
217 : : * Add single-index access operators
218 : : */
219 : : template <unsigned L>
220 : : class Vector : public Matrix<L,1>
221 : : {
222 : : public:
223 : 117054 : Vector() { }
224 : 48 : Vector( double v ) { Matrix<L,1>::set(v); }
225 : 1022 : Vector( const double* v ) { Matrix<L,1>::set(v); }
226 : : Vector( const char* s ) { Matrix<L,1>::set(s); }
227 : : Vector( const std::string& s ) { Matrix<L,1>::set(s); }
228 : 14117 : Vector( const Matrix<L,1>& m) : Matrix<L,1>(m) {}
229 : :
230 : 759670 : double& operator[](unsigned idx) { return Matrix<L,1>::operator()(idx,0); }
231 : 10426572 : double operator[](unsigned idx) const { return Matrix<L,1>::operator()(idx,0); }
232 : : double& operator()(unsigned idx) { return Matrix<L,1>::operator()(idx,0); }
233 : : double operator()(unsigned idx) const { return Matrix<L,1>::operator()(idx,0); }
234 : :
235 : 9211 : Vector<L>& operator=( const Matrix<L,1>& m ) {
236 : 9211 : Matrix<L,1>::operator=(m); return *this;
237 : : }
238 : : };
239 : :
240 : : template <unsigned R, unsigned C> inline
241 : : void Matrix<R,C>::set( const double v[R][C] ) {
242 : : for (unsigned r = 0; r < R; ++r)
243 : : for (unsigned c = 0; c < C; ++c)
244 : : operator()(r,c) = v[r][c];
245 : : }
246 : :
247 : : template <unsigned R, unsigned C> inline
248 : 64 : void Matrix<R,C>::diag( double v ) {
249 : : //for (unsigned r = 0; r < R; ++r)
250 : : // for (unsigned c = 0; c < C; ++c)
251 : : // operator()(r,c) = (r == c) ? v : 0.0;
252 : :
253 : : switch (R) {
254 : : default: for (unsigned r = 4; r < R; ++r)
255 : : switch (C) {
256 : : default: for (unsigned k = 4; k < C; ++k)
257 : : operator()(r,k) = r == k ? v : 0.0;
258 : : case 4: operator()(r,3) = 0.0;
259 : : case 3: operator()(r,2) = 0.0;
260 : : case 2: operator()(r,1) = 0.0;
261 : : case 1: operator()(r,0) = 0.0;
262 : : }
263 : : case 4:
264 : : switch (C) {
265 : : default: for (unsigned k = 4; k < C; ++k)
266 : : operator()(3,k) = 0.0;
267 : : case 4: operator()(3,3) = v;
268 : : case 3: operator()(3,2) = 0.0;
269 : : case 2: operator()(3,1) = 0.0;
270 : : case 1: operator()(3,0) = 0.0;
271 : : }
272 : : case 3:
273 : : switch (C) {
274 : : default: for (unsigned k = 4; k < C; ++k)
275 : : operator()(2,k) = 0.0;
276 : : case 4: operator()(2,3) = 0.0;
277 : 64 : case 3: operator()(2,2) = v;
278 : 64 : case 2: operator()(2,1) = 0.0;
279 : 64 : case 1: operator()(2,0) = 0.0;
280 : : }
281 : : case 2:
282 : : switch (C) {
283 : : default: for (unsigned k = 4; k < C; ++k)
284 : : operator()(1,k) = 0.0;
285 : : case 4: operator()(1,3) = 0.0;
286 : 64 : case 3: operator()(1,2) = 0.0;
287 : 64 : case 2: operator()(1,1) = v;
288 : 64 : case 1: operator()(1,0) = 0.0;
289 : : }
290 : : case 1:
291 : : switch (C) {
292 : : default: for (unsigned k = 4; k < C; ++k)
293 : : operator()(0,k) = 0.0;
294 : : case 4: operator()(0,3) = 0.0;
295 : 64 : case 3: operator()(0,2) = 0.0;
296 : 64 : case 2: operator()(0,1) = 0.0;
297 : 64 : case 1: operator()(0,0) = v;
298 : : }
299 : : }
300 : 64 : }
301 : :
302 : : template <unsigned R, unsigned C> inline
303 : : void Matrix<R,C>::diag( const double* v ) {
304 : : //for (unsigned r = 0; r < R; ++r)
305 : : // for (unsigned c = 0; c < C; ++c)
306 : : // operator()(r,c) = (r == c) ? v[r] : 0.0;
307 : :
308 : : switch (R) {
309 : : default: for (unsigned r = 4; r < R; ++r)
310 : : switch (C) {
311 : : default: for (unsigned k = 4; k < C; ++k)
312 : : operator()(r,k) = r == k ? v[r] : 0.0;
313 : : case 4: operator()(r,3) = 0.0;
314 : : case 3: operator()(r,2) = 0.0;
315 : : case 2: operator()(r,1) = 0.0;
316 : : case 1: operator()(r,0) = 0.0;
317 : : }
318 : : case 4:
319 : : switch (C) {
320 : : default: for (unsigned k = 4; k < C; ++k)
321 : : operator()(3,k) = 0.0;
322 : : case 4: operator()(3,3) = v[3];
323 : : case 3: operator()(3,2) = 0.0;
324 : : case 2: operator()(3,1) = 0.0;
325 : : case 1: operator()(3,0) = 0.0;
326 : : }
327 : : case 3:
328 : : switch (C) {
329 : : default: for (unsigned k = 4; k < C; ++k)
330 : : operator()(2,k) = 0.0;
331 : : case 4: operator()(2,3) = 0.0;
332 : : case 3: operator()(2,2) = v[2];
333 : : case 2: operator()(2,1) = 0.0;
334 : : case 1: operator()(2,0) = 0.0;
335 : : }
336 : : case 2:
337 : : switch (C) {
338 : : default: for (unsigned k = 4; k < C; ++k)
339 : : operator()(1,k) = 0.0;
340 : : case 4: operator()(1,3) = 0.0;
341 : : case 3: operator()(1,2) = 0.0;
342 : : case 2: operator()(1,1) = v[1];
343 : : case 1: operator()(1,0) = 0.0;
344 : : }
345 : : case 1:
346 : : switch (C) {
347 : : default: for (unsigned k = 4; k < C; ++k)
348 : : operator()(0,k) = 0.0;
349 : : case 4: operator()(0,3) = 0.0;
350 : : case 3: operator()(0,2) = 0.0;
351 : : case 2: operator()(0,1) = 0.0;
352 : : case 1: operator()(0,0) = v[0];
353 : : }
354 : : }
355 : : }
356 : :
357 : : /**\brief Extract minor of a matrix and assign to *this
358 : : *
359 : : * Given a matrix m, a row r and an column c, set *this to
360 : : * the matrix that is m with row r and column c deleted.
361 : : */
362 : : template <unsigned R, unsigned C> inline
363 : : void Matrix<R,C>::make_minor( const Matrix<R+1,C+1>& M, unsigned r, unsigned c ) {
364 : : for (unsigned i = 0; i < r; ++i) {
365 : : for (unsigned j = 0; j < c; ++j)
366 : : operator()(i,j) = M(i,j);
367 : : for (unsigned j = c; j < C; ++j)
368 : : operator()(i,j) = M(i,j+1);
369 : : }
370 : : for (unsigned i = r; i < R; ++i) {
371 : : for (unsigned j = 0; j < c; ++j)
372 : : operator()(i,j) = M(i+1,j);
373 : : for (unsigned j = c; j < C; ++j)
374 : : operator()(i,j) = M(i+1,j+1);
375 : : }
376 : : }
377 : :
378 : : template <unsigned R, unsigned C> inline
379 : : void Matrix<R,C>::set_row( unsigned r, const Matrix<1,C>& v ) {
380 : : for (unsigned i = 0; i < C; ++i) operator()(r,i) = v(0,i);
381 : : }
382 : : template <unsigned R, unsigned C> inline
383 : : void Matrix<R,C>::add_row( unsigned r, const Matrix<1,C>& v ) {
384 : : for (unsigned i = 0; i < C; ++i) operator()(r,i) += v(0,i);
385 : : }
386 : : template <unsigned R, unsigned C> inline
387 : : void Matrix<R,C>::set_row_transpose( unsigned r, const Matrix<C,1>& v ) {
388 : : for (unsigned i = 0; i < C; ++i) operator()(r,i) = v(i,0);
389 : : }
390 : : template <unsigned R, unsigned C> inline
391 : : void Matrix<R,C>::set_rows( const Matrix<1,C>* v ) {
392 : : for( unsigned r = 0; r < R; ++r) set_row( r, v[r] );
393 : : }
394 : :
395 : : template <unsigned R, unsigned C> inline
396 : 1788 : void Matrix<R,C>::set_column( unsigned c, const Matrix<R,1>& v ) {
397 [ + + ]: 7152 : for (unsigned i = 0; i < R; ++i) operator()(i,c) = v(i,0);
398 : 1788 : }
399 : : template <unsigned R, unsigned C> inline
400 : : void Matrix<R,C>::add_column( unsigned c, const Matrix<R,1>& v ) {
401 : : for (unsigned i = 0; i < R; ++i) operator()(i,c) += v(i,0);
402 : : }
403 : : template <unsigned R, unsigned C> inline
404 : : void Matrix<R,C>::set_column_transpose( unsigned c, const Matrix<1,R>& v ) {
405 : : for (unsigned i = 0; i < R; ++i) operator()(i,c) = v(0,i);
406 : : }
407 : : template <unsigned R, unsigned C> inline
408 : : void Matrix<R,C>::set_columns( const Matrix<R,1>* v ) {
409 : : for( unsigned c = 0; c < C; ++c) set_column( c, v[c] );
410 : : }
411 : :
412 : :
413 : : template <unsigned R, unsigned C> inline
414 : : Matrix<R,1> Matrix<R,C>::column( unsigned c ) const {
415 : : Matrix<R,1> result;
416 : : for (unsigned i = 0; i < R; ++i)
417 : : result(i,0) = operator()(i,c);
418 : : return result;
419 : : }
420 : :
421 : : template <unsigned R, unsigned C> inline
422 : : Matrix<1,R> Matrix<R,C>::column_transpose( unsigned c ) const {
423 : : Matrix<1,R> result;
424 : : for (unsigned i = 0; i < R; ++i)
425 : : result(0,i) = operator()(i,c);
426 : : return result;
427 : : }
428 : :
429 : : /**\brief Set a subset of this matrix to some other matrix */
430 : : template <unsigned R1, unsigned C1, unsigned R2, unsigned C2> inline
431 : : void set_region( Matrix<R1,C1>& d, unsigned r, unsigned c, Matrix<R2,C2>& s ) {
432 : : const unsigned rmax = r+R2 > R1 ? R1 : r+R2;
433 : : const unsigned cmax = c+C2 > C1 ? C1 : c+C2;
434 : : for (unsigned i = r; i < rmax; ++i)
435 : : for (unsigned j = c; j < cmax; ++j)
436 : : d(i,j) = s(i-r,j-c);
437 : : }
438 : :
439 : :
440 : : template <unsigned R, unsigned C> inline
441 : : Matrix<R,C>& Matrix<R,C>::assign_add_transpose( const Matrix<C,R>& other ) {
442 : : for (unsigned r = 0; r < R; ++r)
443 : : for (unsigned c = 0; c < C; ++c)
444 : : operator()(r,c) += other(c,r);
445 : : return *this;
446 : : }
447 : :
448 : : template <unsigned R, unsigned C> inline
449 : : Matrix<R,C>& Matrix<R,C>::assign_multiply_elements( const Matrix<R,C>& other ) {
450 : : for (unsigned i = 0; i < R*C; ++i) m[i] *= other.data()[i]; return *this;
451 : : }
452 : :
453 : : template <unsigned R, unsigned C> inline
454 : : Matrix<R,C>& Matrix<R,C>::assign_product( double s, const Matrix<R,C>& other ) {
455 : : for (unsigned i = 0; i < R*C; ++i) m[i] = s * other.data()[i]; return *this;
456 : : }
457 : :
458 : : template <unsigned R, unsigned C> inline
459 : : Matrix<R,C>& Matrix<R,C>::assign_add_product( double s, const Matrix<R,C>& other ) {
460 : : for (unsigned i = 0; i < R*C; ++i) m[i] += s * other.data()[i]; return *this;
461 : : }
462 : :
463 : : template <unsigned R, unsigned C> inline
464 : 40330 : Matrix<R,C>& Matrix<R,C>::operator+=( const Matrix<R,C>& other ) {
465 [ + + ][ + + ]: 185968 : for (unsigned i = 0; i < R*C; ++i) m[i] += other.data()[i]; return *this;
466 : : }
467 : :
468 : : template <unsigned R, unsigned C> inline
469 : 21456 : Matrix<R,C>& Matrix<R,C>::operator-=( const Matrix<R,C>& other ) {
470 [ + + ]: 85824 : for (unsigned i = 0; i < R*C; ++i) m[i] -= other.data()[i]; return *this;
471 : : }
472 : :
473 : : template <unsigned R, unsigned C> inline
474 : : Matrix<R,C>& Matrix<R,C>::operator+=( double s ) {
475 : : for (unsigned i = 0; i < R*C; ++i) m[i] += s; return *this;
476 : : }
477 : :
478 : : template <unsigned R, unsigned C> inline
479 : : Matrix<R,C>& Matrix<R,C>::operator-=( double s ) {
480 : : for (unsigned i = 0; i < R*C; ++i) m[i] -= s; return *this;
481 : : }
482 : :
483 : : template <unsigned R, unsigned C> inline
484 : 41877 : Matrix<R,C>& Matrix<R,C>::operator*=( double s ) {
485 [ + + ][ + + ]: 169488 : for (unsigned i = 0; i < R*C; ++i) m[i] *= s; return *this;
486 : : }
487 : :
488 : : template <unsigned R, unsigned C> inline
489 : 20016 : Matrix<R,C>& Matrix<R,C>::operator/=( double s ) {
490 [ + + ]: 80064 : for (unsigned i = 0; i < R*C; ++i) m[i] /= s; return *this;
491 : : }
492 : :
493 : :
494 : : template <unsigned R, unsigned C> inline
495 : : Matrix<R,C> operator-( const Matrix<R,C>& m ) {
496 : : Matrix<R,C> result;
497 : : for (unsigned i = 0; i < R*C; ++i)
498 : : result.data()[i] = -m.data()[i];
499 : : return result;
500 : : }
501 : :
502 : : template <unsigned R, unsigned C> inline
503 : : Matrix<R,C> operator+( const Matrix<R,C>& m, double s ) {
504 : : Matrix<R,C> tmp(m); tmp += s; return tmp;
505 : : }
506 : :
507 : : template <unsigned R, unsigned C> inline
508 : : Matrix<R,C> operator+( double s, const Matrix<R,C>& m ) {
509 : : Matrix<R,C> tmp(m); tmp += s; return tmp;
510 : : }
511 : :
512 : : template <unsigned R, unsigned C> inline
513 : 33553 : Matrix<R,C> operator+( const Matrix<R,C>& A, const Matrix<R,C>& B ) {
514 : 33553 : Matrix<R,C> tmp(A); tmp += B; return tmp;
515 : : }
516 : :
517 : :
518 : : template <unsigned R, unsigned C> inline
519 : : Matrix<R,C> operator-( const Matrix<R,C>& m, double s ) {
520 : : Matrix<R,C> tmp(m); tmp -= s; return tmp;
521 : : }
522 : :
523 : : template <unsigned R, unsigned C> inline
524 : : Matrix<R,C> operator-( double s, const Matrix<R,C>& m ) {
525 : : Matrix<R,C> tmp(m); tmp -= s; return tmp;
526 : : }
527 : :
528 : : template <unsigned R, unsigned C> inline
529 : 11454 : Matrix<R,C> operator-( const Matrix<R,C>& A, const Matrix<R,C>& B ) {
530 : 11454 : Matrix<R,C> tmp(A); tmp -= B; return tmp;
531 : : }
532 : :
533 : :
534 : : template <unsigned R, unsigned C> inline
535 : 1872 : Matrix<R,C> operator*( const Matrix<R,C>& m, double s ) {
536 : 1872 : Matrix<R,C> tmp(m); tmp *= s; return tmp;
537 : : }
538 : :
539 : : template <unsigned R, unsigned C> inline
540 : 40003 : Matrix<R,C> operator*( double s, const Matrix<R,C>& m ) {
541 : 40003 : Matrix<R,C> tmp(m); tmp *= s; return tmp;
542 : : }
543 : :
544 : : template <unsigned R, unsigned RC, unsigned C> inline
545 : 54957 : double multiply_helper_result_val( unsigned r, unsigned c,
546 : : const Matrix<R,RC>& A,
547 : : const Matrix<RC,C>& B ) {
548 : 54957 : double tmp = A(r,0)*B(0,c);
549 : : switch (RC) {
550 : : default: for (unsigned k = 6; k < RC; ++k)
551 : : tmp += A(r,k)*B(k,c);
552 : : case 6: tmp += A(r,5)*B(5,c);
553 : : case 5: tmp += A(r,4)*B(4,c);
554 : : case 4: tmp += A(r,3)*B(3,c);
555 : 17985 : case 3: tmp += A(r,2)*B(2,c);
556 : 17985 : case 2: tmp += A(r,1)*B(1,c);
557 : : case 1: ;
558 : : }
559 : 54957 : return tmp;
560 : : }
561 : :
562 : : template <unsigned R, unsigned RC, unsigned C> inline
563 : 9443 : Matrix<R,C> operator*( const Matrix<R,RC>& A, const Matrix<RC,C>& B ) {
564 : : //Matrix<R,C> result(0.0);
565 : : //for (unsigned i = 0; i < R; ++i)
566 : : // for (unsigned j = 0; j < C; ++j)
567 : : // for (unsigned k = 0; k < RC; ++k)
568 : : // result(i,j) += A(i,k) * B(k,j);
569 : :
570 : 9443 : Matrix<R,C> result;
571 [ + + ][ + + ]: 37772 : for (unsigned r = 0; r < R; ++r)
[ + + ]
572 [ + + ][ + + ]: 83286 : for (unsigned c = 0; c < C; ++c)
[ + + ]
573 : 54957 : result(r,c) = multiply_helper_result_val( r, c, A, B );
574 : :
575 : 9443 : return result;
576 : : }
577 : :
578 : :
579 : : template <unsigned R, unsigned C> inline
580 : 40 : Matrix<R,C> operator/( const Matrix<R,C>& m, double s ) {
581 : 40 : Matrix<R,C> tmp(m); tmp /= s; return tmp;
582 : : }
583 : :
584 : : template <unsigned RC> inline
585 : : double cofactor( const Matrix<RC,RC>& m, unsigned r, unsigned c ) {
586 : : const double sign[] = { 1.0, -1.0 };
587 : : return sign[(r+c)%2] * det( Matrix<RC-1,RC-1>( m, r, c ) );
588 : : }
589 : :
590 : : template <unsigned RC> inline
591 : : double det( const Matrix<RC,RC>& m ) {
592 : : double result = 0.0;
593 : : for (unsigned i = 0; i < RC; ++i)
594 : : result += m(0,i) * cofactor<RC>( m, 0, i );
595 : : return result;
596 : : }
597 : :
598 : : //inline
599 : : //double det( const Matrix<1,1>& m ) {
600 : : // return m(0,0);
601 : : //}
602 : :
603 : : inline
604 : : double det( const Matrix<2,2>& m ) {
605 : : return m(0,0)*m(1,1) - m(0,1)*m(1,0);
606 : : }
607 : :
608 : : inline
609 : 660 : double det( const Matrix<3,3>& m ) {
610 : 660 : return m(0,0)*(m(1,1)*m(2,2) - m(2,1)*m(1,2))
611 : 660 : + m(0,1)*(m(2,0)*m(1,2) - m(1,0)*m(2,2))
612 : 660 : + m(0,2)*(m(1,0)*m(2,1) - m(2,0)*m(1,1));
613 : : }
614 : :
615 : : inline
616 : : Matrix<2,2> adj( const Matrix<2,2>& m ) {
617 : : Matrix<2,2> result;
618 : : result(0,0) = m(1,1);
619 : : result(0,1) = -m(0,1);
620 : : result(1,0) = -m(1,0);
621 : : result(1,1) = m(0,0);
622 : : return result;
623 : : }
624 : :
625 : : inline
626 : : Matrix<2,2> transpose_adj( const Matrix<2,2>& m ) {
627 : : Matrix<2,2> result;
628 : : result(0,0) = m(1,1);
629 : : result(1,0) = -m(0,1);
630 : : result(0,1) = -m(1,0);
631 : : result(1,1) = m(0,0);
632 : : return result;
633 : : }
634 : :
635 : : inline
636 : 330 : Matrix<3,3> adj( const Matrix<3,3>& m ) {
637 : 330 : Matrix<3,3> result;
638 : :
639 : 330 : result(0,0) = m(1,1)*m(2,2) - m(1,2)*m(2,1);
640 : 330 : result(0,1) = m(0,2)*m(2,1) - m(0,1)*m(2,2);
641 : 330 : result(0,2) = m(0,1)*m(1,2) - m(0,2)*m(1,1);
642 : :
643 : 330 : result(1,0) = m(1,2)*m(2,0) - m(1,0)*m(2,2);
644 : 330 : result(1,1) = m(0,0)*m(2,2) - m(0,2)*m(2,0);
645 : 330 : result(1,2) = m(0,2)*m(1,0) - m(0,0)*m(1,2);
646 : :
647 : 330 : result(2,0) = m(1,0)*m(2,1) - m(1,1)*m(2,0);
648 : 330 : result(2,1) = m(0,1)*m(2,0) - m(0,0)*m(2,1);
649 : 330 : result(2,2) = m(0,0)*m(1,1) - m(0,1)*m(1,0);
650 : :
651 : 330 : return result;
652 : : }
653 : :
654 : : inline
655 : : Matrix<3,3> transpose_adj( const Matrix<3,3>& m ) {
656 : : Matrix<3,3> result;
657 : :
658 : : result(0,0) = m(1,1)*m(2,2) - m(1,2)*m(2,1);
659 : : result(0,1) = m(1,2)*m(2,0) - m(1,0)*m(2,2);
660 : : result(0,2) = m(1,0)*m(2,1) - m(1,1)*m(2,0);
661 : :
662 : : result(1,0) = m(0,2)*m(2,1) - m(0,1)*m(2,2);
663 : : result(1,1) = m(0,0)*m(2,2) - m(0,2)*m(2,0);
664 : : result(1,2) = m(0,1)*m(2,0) - m(0,0)*m(2,1);
665 : :
666 : : result(2,0) = m(0,1)*m(1,2) - m(0,2)*m(1,1);
667 : : result(2,1) = m(0,2)*m(1,0) - m(0,0)*m(1,2);
668 : : result(2,2) = m(0,0)*m(1,1) - m(0,1)*m(1,0);
669 : :
670 : : return result;
671 : : }
672 : :
673 : : template <unsigned R, unsigned C> inline
674 : 330 : Matrix<R,C> inverse( const Matrix<R,C>& m ) {
675 [ + - ]: 330 : return adj(m) * (1.0 / det(m));
676 : : }
677 : :
678 : : template <unsigned R, unsigned C> inline
679 : 4108 : Matrix<R,C> transpose( const Matrix<C,R>& m ) {
680 : 4108 : Matrix<R,C> result;
681 [ + + ]: 8216 : for (unsigned r = 0; r < R; ++r)
682 [ + + ]: 16432 : for (unsigned c = 0; c < C; ++c)
683 : 12324 : result(r,c) = m(c,r);
684 : 4108 : return result;
685 : : }
686 : : /*
687 : : template <unsigned R> inline
688 : : const Matrix<R,1>& transpose( const Matrix<1,R>& m ) {
689 : : return *reinterpret_cast<const Matrix<R,1>*>(&m);
690 : : }
691 : :
692 : : template <unsigned C> inline
693 : : const Matrix<1,C>& transpose( const Matrix<C,1>& m ) {
694 : : return *reinterpret_cast<const Matrix<1,C>*>(&m);
695 : : }
696 : : */
697 : : template <unsigned RC> inline
698 : : double trace( const Matrix<RC,RC>& m ) {
699 : : double result = m(0,0);
700 : : for (unsigned i = 1; i < RC; ++i)
701 : : result += m(i,i);
702 : : return result;
703 : : }
704 : :
705 : : template <unsigned R, unsigned C> inline
706 : 376 : double sqr_Frobenius( const Matrix<R,C>& m ) {
707 : 376 : double result = *m.data() * *m.data();
708 [ + + ]: 1128 : for (unsigned i = 1; i < R*C; ++i)
709 : 752 : result += m.data()[i] * m.data()[i];
710 : 376 : return result;
711 : : }
712 : :
713 : : template <unsigned R, unsigned C> inline
714 : 376 : double Frobenius( const Matrix<R,C>& m ) {
715 : 376 : return std::sqrt( sqr_Frobenius<R,C>( m ) );
716 : : }
717 : :
718 : : template <unsigned R, unsigned C> inline
719 : : bool operator==( const Matrix<R,C>& A, const Matrix<R,C>& B ) {
720 : : for (unsigned i = 0; i < R*C; ++i)
721 : : if (A.data()[i] != B.data()[i])
722 : : return false;
723 : : return true;
724 : : }
725 : :
726 : : template <unsigned R, unsigned C> inline
727 : : bool operator!=( const Matrix<R,C>& A, const Matrix<R,C>& B ) {
728 : : return !(A == B);
729 : : }
730 : :
731 : : template <unsigned R, unsigned C> inline
732 : : std::ostream& operator<<( std::ostream& str, const Matrix<R,C>& m ) {
733 : : str << m.data()[0];
734 : : for (unsigned i = 1; i < R*C; ++i)
735 : : str << ' ' << m.data()[i];
736 : : return str;
737 : : }
738 : :
739 : : template <unsigned R, unsigned C> inline
740 : : std::istream& operator>>( std::istream& str, Matrix<R,C>& m ) {
741 : : for (unsigned i = 0; i < R*C; ++i)
742 : : str >> m.data()[i];
743 : : return str;
744 : : }
745 : :
746 : : template <unsigned R> inline
747 : : double sqr_length( const Matrix<R,1>& v ) {
748 : : return sqr_Frobenius(v);
749 : : }
750 : :
751 : : template <unsigned C> inline
752 : : double sqr_length( const Matrix<1,C>& v ) {
753 : : return sqr_Frobenius(v);
754 : : }
755 : :
756 : : template <unsigned R> inline
757 : 376 : double length( const Matrix<R,1>& v ) {
758 : 376 : return Frobenius(v);
759 : : }
760 : :
761 : : template <unsigned C> inline
762 : : double length( const Matrix<1,C>& v ) {
763 : : return Frobenius(v);
764 : : }
765 : :
766 : : template <unsigned R, unsigned C> inline
767 : 10153 : double inner_product( const Matrix<R,C>& m1, const Matrix<R,C>& m2 ) {
768 : 10153 : double result = 0.0;
769 [ + + ]: 40612 : for (unsigned r = 0; r < R; ++r)
770 [ + + ]: 60918 : for (unsigned c = 0; c < C; ++c)
771 : 30459 : result += m1(r,c) * m2(r,c);
772 : 10153 : return result;
773 : : }
774 : :
775 : : template <unsigned R, unsigned C> inline
776 : : Matrix<R,C> outer( const Matrix<R,1>& v1, const Matrix<C,1>& v2 ) {
777 : : Matrix<R,C> result;
778 : : for (unsigned r = 0; r < R; ++r)
779 : : for (unsigned c = 0; c < C; ++c)
780 : : result(r,c) = v1(r,0) * v2(c,0);
781 : : return result;
782 : : }
783 : :
784 : : template <unsigned R, unsigned C> inline
785 : : Matrix<R,C> outer( const Matrix<1,R>& v1, const Matrix<1,C>& v2 ) {
786 : : Matrix<R,C> result;
787 : : for (unsigned r = 0; r < R; ++r)
788 : : for (unsigned c = 0; c < C; ++c)
789 : : result(r,c) = v1(0,r) * v2(0,c);
790 : : return result;
791 : : }
792 : :
793 : : inline
794 : : double vector_product( const Matrix<2,1>& v1, const Matrix<2,1>& v2 ) {
795 : : return v1(0,0) * v2(1,0) - v1(1,0) * v2(0,0);
796 : : }
797 : :
798 : : inline
799 : : double vector_product( const Matrix<1,2>& v1, const Matrix<1,2>& v2 ) {
800 : : return v1(0,0) * v2(0,1) - v1(0,1) * v2(0,0);
801 : : }
802 : :
803 : : inline
804 : 315 : Matrix<3,1> vector_product( const Matrix<3,1>& a, const Matrix<3,1>& b ) {
805 : 315 : Matrix<3,1> result;
806 : 315 : result(0,0) = a(1,0)*b(2,0) - a(2,0)*b(1,0);
807 : 315 : result(1,0) = a(2,0)*b(0,0) - a(0,0)*b(2,0);
808 : 315 : result(2,0) = a(0,0)*b(1,0) - a(1,0)*b(0,0);
809 : 315 : return result;
810 : : }
811 : :
812 : : inline
813 : : Matrix<1,3> vector_product( const Matrix<1,3>& a, const Matrix<1,3>& b ) {
814 : : Matrix<1,3> result;
815 : : result(0,0) = a(0,1)*b(0,2) - a(0,2)*b(0,1);
816 : : result(0,1) = a(0,2)*b(0,0) - a(0,0)*b(0,2);
817 : : result(0,2) = a(0,0)*b(0,1) - a(0,1)*b(0,0);
818 : : return result;
819 : : }
820 : :
821 : : template <unsigned R, unsigned C> inline
822 : 158 : double operator%( const Matrix<R,C>& v1, const Matrix<R,C>& v2 ) {
823 : 158 : return inner_product( v1, v2 );
824 : : }
825 : :
826 : : inline
827 : : double operator*( const Matrix<2,1>& v1, const Matrix<2,1>& v2 ) {
828 : : return vector_product( v1, v2 );
829 : : }
830 : :
831 : : inline
832 : : double operator*( const Matrix<1,2>& v1, const Matrix<1,2>& v2 ) {
833 : : return vector_product( v1, v2 );
834 : : }
835 : :
836 : : inline
837 : 304 : Matrix<3,1> operator*( const Matrix<3,1>& v1, const Matrix<3,1>& v2 ) {
838 : 304 : return vector_product( v1, v2 );
839 : : }
840 : :
841 : : inline
842 : : Matrix<1,3> operator*( const Matrix<1,3>& v1, const Matrix<1,3>& v2 ) {
843 : : return vector_product( v1, v2 );
844 : : }
845 : :
846 : : /** Compute QR factorization of A */
847 : : inline
848 : : void QR( const Matrix<3,3>& A, Matrix<3,3>& Q, Matrix<3,3>& R ) {
849 : : Q = A;
850 : :
851 : : R(0,0) = sqrt(Q(0,0)*Q(0,0) + Q(1,0)*Q(1,0) + Q(2,0)*Q(2,0));
852 : : double temp_dbl = 1.0/R(0,0);
853 : : R(1,0) = 0.0;
854 : : R(2,0) = 0.0;
855 : : Q(0,0) *= temp_dbl;
856 : : Q(1,0) *= temp_dbl;
857 : : Q(2,0) *= temp_dbl;
858 : :
859 : :
860 : : R(0,1) = Q(0,0)*Q(0,1) + Q(1,0)*Q(1,1) + Q(2,0)*Q(2,1);
861 : : Q(0,1) -= Q(0,0)*R(0,1);
862 : : Q(1,1) -= Q(1,0)*R(0,1);
863 : : Q(2,1) -= Q(2,0)*R(0,1);
864 : :
865 : : R(0,2) = Q(0,0)*Q(0,2) + Q(1,0)*Q(1,2) + Q(2,0)*Q(2,2);
866 : : Q(0,2) -= Q(0,0)*R(0,2);
867 : : Q(1,2) -= Q(1,0)*R(0,2);
868 : : Q(2,2) -= Q(2,0)*R(0,2);
869 : :
870 : : R(1,1) = sqrt(Q(0,1)*Q(0,1) + Q(1,1)*Q(1,1) + Q(2,1)*Q(2,1));
871 : : temp_dbl = 1.0 / R(1,1);
872 : : R(2,1) = 0.0;
873 : : Q(0,1) *= temp_dbl;
874 : : Q(1,1) *= temp_dbl;
875 : : Q(2,1) *= temp_dbl;
876 : :
877 : :
878 : : R(1,2) = Q(0,1)*Q(0,2) + Q(1,1)*Q(1,2) + Q(2,1)*Q(2,2);
879 : : Q(0,2) -= Q(0,1)*R(1,2);
880 : : Q(1,2) -= Q(1,1)*R(1,2);
881 : : Q(2,2) -= Q(2,1)*R(1,2);
882 : :
883 : : R(2,2) = sqrt(Q(0,2)*Q(0,2) + Q(1,2)*Q(1,2) + Q(2,2)*Q(2,2));
884 : : temp_dbl = 1.0 / R(2,2);
885 : : Q(0,2) *= temp_dbl;
886 : : Q(1,2) *= temp_dbl;
887 : : Q(2,2) *= temp_dbl;
888 : : }
889 : :
890 : :
891 : : /** Compute QR factorization of A */
892 : : inline
893 : : void QR( const Matrix<2,2>& A, Matrix<2,2>& Q, Matrix<2,2>& R ) {
894 : : R(0,0) = std::sqrt( A(0,0)*A(0,0) + A(1,0)*A(1,0) );
895 : : const double r0inv = 1.0 / R(0,0);
896 : : Q(0,0) = Q(1,1) = A(0,0) * r0inv;
897 : : Q(1,0) = A(1,0) * r0inv;
898 : : Q(0,1) = -Q(1,0);
899 : : R(0,1) = r0inv * (A(0,0)*A(0,1) + A(1,0)*A(1,1));
900 : : R(1,1) = r0inv * (A(0,0)*A(1,1) - A(0,1)*A(1,0));
901 : : R(1,0) = 0.0;
902 : : }
903 : :
904 : : } // namespace MeshKit
905 : :
906 : : #endif
|
