Line data Source code
1 : /*
2 : * Copyright (c) 2009-2015: G-CSC, Goethe University Frankfurt
3 : * Author: Andreas Vogel
4 : *
5 : * This file is part of UG4.
6 : *
7 : * UG4 is free software: you can redistribute it and/or modify it under the
8 : * terms of the GNU Lesser General Public License version 3 (as published by the
9 : * Free Software Foundation) with the following additional attribution
10 : * requirements (according to LGPL/GPL v3 ��7):
11 : *
12 : * (1) The following notice must be displayed in the Appropriate Legal Notices
13 : * of covered and combined works: "Based on UG4 (www.ug4.org/license)".
14 : *
15 : * (2) The following notice must be displayed at a prominent place in the
16 : * terminal output of covered works: "Based on UG4 (www.ug4.org/license)".
17 : *
18 : * (3) The following bibliography is recommended for citation and must be
19 : * preserved in all covered files:
20 : * "Reiter, S., Vogel, A., Heppner, I., Rupp, M., and Wittum, G. A massively
21 : * parallel geometric multigrid solver on hierarchically distributed grids.
22 : * Computing and visualization in science 16, 4 (2013), 151-164"
23 : * "Vogel, A., Reiter, S., Rupp, M., N��gel, A., and Wittum, G. UG4 -- a novel
24 : * flexible software system for simulating pde based models on high performance
25 : * computers. Computing and visualization in science 16, 4 (2013), 165-179"
26 : *
27 : * This program is distributed in the hope that it will be useful,
28 : * but WITHOUT ANY WARRANTY; without even the implied warranty of
29 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
30 : * GNU Lesser General Public License for more details.
31 : */
32 :
33 : #ifndef __H__UG__COMMON__MATH_MATRIX_FUNCTIONS_COMMON_IMPL__
34 : #define __H__UG__COMMON__MATH_MATRIX_FUNCTIONS_COMMON_IMPL__
35 :
36 : #include <cmath>
37 : #include <iostream>
38 : #include <iomanip>
39 : #include <cassert>
40 : #include "math_matrix.h"
41 : #include "common/assert.h"
42 : #include "common/static_assert.h"
43 :
44 : namespace ug{
45 :
46 : ////////////////////////////////////////////////////////////////////////////////
47 : // Addition of Matrices
48 : ////////////////////////////////////////////////////////////////////////////////
49 :
50 : template <typename matrix_t>
51 : inline void
52 : MatAdd(matrix_t& mOut, const matrix_t& m1, const matrix_t& m2)
53 : {
54 : typedef typename matrix_t::size_type size_type;
55 : for(size_type i = 0; i < mOut.num_rows(); ++i)
56 : for(size_type j = 0; j < mOut.num_cols(); ++j)
57 : {
58 : mOut(i,j) = m1(i,j) + m2(i,j);
59 : }
60 : }
61 :
62 : ////////////////////////////////////////////////////////////////////////////////
63 : // Subtraction of Matrices
64 : ////////////////////////////////////////////////////////////////////////////////
65 :
66 : template <typename matrix_t>
67 : inline void
68 : MatSubtract(matrix_t& mOut, const matrix_t& m1, const matrix_t& m2)
69 : {
70 : typedef typename matrix_t::size_type size_type;
71 : for(size_type i = 0; i < mOut.num_rows(); ++i)
72 : for(size_type j = 0; j < mOut.num_cols(); ++j)
73 : {
74 : mOut(i,j) = m1(i,j) - m2(i,j);
75 : }
76 : }
77 :
78 :
79 : ////////////////////////////////////////////////////////////////////////////////
80 : // Multiplication of Matrices
81 : ////////////////////////////////////////////////////////////////////////////////
82 :
83 : template <size_t N, size_t M, size_t L, typename T>
84 : inline void
85 : MatMultiply(MathMatrix<N, M, T>& mOut, const MathMatrix<N, L, T>& m1,
86 : const MathMatrix<L, M, T>& m2)
87 : {
88 : for(size_t i = 0; i < N; ++i)
89 : for(size_t j = 0; j < M; ++j)
90 : {
91 : mOut(i,j) = 0;
92 : for(size_t k = 0; k < L; ++k)
93 : {
94 : mOut(i,j) += m1(i,k) * m2(k,j);
95 : }
96 : }
97 : }
98 :
99 : template <size_t N, size_t M, size_t L, size_t P, typename T>
100 : inline void
101 : MatMultiply(MathMatrix<N, M, T>& mOut, const MathMatrix<N, L, T>& m1,
102 : const MathMatrix<L, P, T>& m2, const MathMatrix<P, M, T>& m3)
103 : {
104 : MathMatrix<L, M, T> help;
105 :
106 : for(size_t i = 0; i < N; ++i)
107 : for(size_t j = 0; j < M; ++j)
108 : {
109 : mOut(i,j) = 0;
110 : for(size_t k = 0; k < L; ++k)
111 : {
112 : help(k,j) = 0;
113 : for(size_t l = 0; l < P; ++l)
114 : {
115 : help(k,j) += m2(k,l) * m3(l,j);
116 : }
117 :
118 : mOut(i,j) += m1(i,k) * help(k,j);
119 : }
120 : }
121 : }
122 :
123 : template <size_t N, size_t M, size_t L, typename T>
124 : inline void
125 0 : MatMultiplyTransposed(MathMatrix<N, M, T>& mOut, const MathMatrix<L, N, T>& m1,
126 : const MathMatrix<M, L, T>& m2)
127 : {
128 0 : for(size_t i = 0; i < N; ++i)
129 0 : for(size_t j = 0; j < M; ++j)
130 : {
131 0 : mOut(i,j) = 0;
132 0 : for(size_t k = 0; k < L; ++k)
133 : {
134 0 : mOut(i,j) += m1(k,i) * m2(j,k);
135 : }
136 : }
137 0 : }
138 :
139 : template <size_t N, size_t M, typename T>
140 : inline void
141 0 : MatMultiplyMTM(MathMatrix<N, N, T>& mOut, const MathMatrix<M, N, T>& m)
142 : {
143 0 : for(size_t i = 0; i < N; ++i)
144 : {
145 0 : for(size_t j = 0; j < i; ++j)
146 : {
147 0 : mOut(i,j) = 0;
148 0 : for(size_t k = 0; k < M; ++k)
149 : {
150 0 : mOut(i,j) += m(k,i) * m(k,j);
151 : }
152 0 : mOut(j,i) = mOut(i,j);
153 : }
154 0 : mOut(i,i) = 0;
155 0 : for(size_t k = 0; k < M; ++k)
156 : {
157 0 : mOut(i,i) += m(k,i) * m(k,i);
158 : }
159 : }
160 0 : }
161 :
162 : template <size_t N, size_t M, typename T>
163 : inline void
164 0 : MatMultiplyMMT(MathMatrix<M, M, T>& mOut, const MathMatrix<M, N, T>& m)
165 : {
166 0 : for(size_t i = 0; i < M; ++i)
167 : {
168 0 : for(size_t j = 0; j < i; ++j)
169 : {
170 0 : mOut(i,j) = 0;
171 0 : for(size_t k = 0; k < N; ++k)
172 : {
173 0 : mOut(i,j) += m(i,k) * m(j,k);
174 : }
175 0 : mOut(j,i) = mOut(i,j);
176 : }
177 0 : mOut(i,i) = 0;
178 0 : for(size_t k = 0; k < N; ++k)
179 : {
180 0 : mOut(i,i) += m(i,k) * m(i,k);
181 : }
182 : }
183 0 : }
184 :
185 : template <size_t N, size_t M, size_t L, typename T>
186 : inline void
187 : MatMultiplyMBT(MathMatrix<N, M, T>& mOut, const MathMatrix<N, L, T>& m1,
188 : const MathMatrix<M, L, T>& m2)
189 : {
190 : for(size_t i = 0; i < N; ++i)
191 : {
192 : for(size_t j = 0; j < M; ++j)
193 : {
194 : mOut(i,j) = 0;
195 : for(size_t k = 0; k < L; ++k)
196 : {
197 : mOut(i,j) += m1(i,k) * m2(j,k);
198 : }
199 : }
200 : }
201 : }
202 :
203 : template <size_t N, size_t M, size_t L, typename T>
204 : inline void
205 : MatMultiplyMTB(MathMatrix<N, M, T>& mOut, const MathMatrix<L, N, T>& m1,
206 : const MathMatrix<L, M, T>& m2)
207 : {
208 : for(size_t i = 0; i < N; ++i)
209 : {
210 : for(size_t j = 0; j < M; ++j)
211 : {
212 : mOut(i,j) = 0;
213 : for(size_t k = 0; k < L; ++k)
214 : {
215 : mOut(i,j) += m1(k,i) * m2(k,j);
216 : }
217 : }
218 : }
219 : }
220 :
221 : template <size_t N, size_t M, typename T>
222 : inline void
223 : MatMultiplyMBMT(MathMatrix<N, N, T>& mOut, const MathMatrix<N, M, T>& m1,
224 : const MathMatrix<M, M, T>& m2)
225 : {
226 : MathMatrix<M, N, T> help;
227 :
228 : for(size_t i = 0; i < N; ++i)
229 : for(size_t j = 0; j < N; ++j)
230 : {
231 : mOut(i,j) = 0;
232 : for(size_t k = 0; k < M; ++k)
233 : {
234 : help(k,j) = 0;
235 : for(size_t l = 0; l < M; ++l)
236 : {
237 : help(k,j) += m2(k,l) * m1(j,l);
238 : }
239 :
240 : mOut(i,j) += m1(i,k) * help(k,j);
241 : }
242 : }
243 : }
244 :
245 : template <size_t N, size_t M, typename T>
246 : inline void
247 : MatMultiplyMTBM(MathMatrix<N, N, T>& mOut, const MathMatrix<M, N, T>& m1,
248 : const MathMatrix<M, M, T>& m2)
249 : {
250 : MathMatrix<M, N, T> help;
251 :
252 : for(size_t i = 0; i < N; ++i)
253 : for(size_t j = 0; j < N; ++j)
254 : {
255 : mOut(i,j) = 0;
256 : for(size_t k = 0; k < M; ++k)
257 : {
258 : help(k,j) = 0;
259 : for(size_t l = 0; l < M; ++l)
260 : {
261 : help(k,j) += m2(k,l) * m1(l,j);
262 : }
263 :
264 : mOut(i,j) += m1(k,i) * help(k,j);
265 : }
266 : }
267 : }
268 :
269 : ////////////////////////////////////////////////////////////////////////////////
270 : // "Contraction" for Matrices (note: contraction is usually known regarding tensors!)
271 : ////////////////////////////////////////////////////////////////////////////////
272 :
273 : template <typename matrix_t>
274 : inline typename matrix_t::value_type
275 : MatContraction(const matrix_t& m1, const matrix_t& m2)
276 : {
277 : typename matrix_t::value_type norm = 0;
278 : typedef typename matrix_t::size_type size_type;
279 : for(size_type i = 0; i < m1.num_rows(); ++i)
280 : for(size_type j = 0; j < m1.num_cols(); ++j)
281 : {
282 : norm += m1(i,j) * m2(i,j);
283 : }
284 :
285 : return norm;
286 : }
287 :
288 :
289 : ////////////////////////////////////////////////////////////////////////////////
290 : // "Deviator" and trace for Matrices
291 : ////////////////////////////////////////////////////////////////////////////////
292 :
293 : template <typename matrix_t>
294 : inline typename matrix_t::value_type
295 : MatDeviatorTrace(const matrix_t& m, matrix_t& dev)
296 : {
297 : typename matrix_t::value_type trace = Trace(m);
298 :
299 : typedef typename matrix_t::size_type size_type;
300 : for(size_type i = 0; i < m.num_rows(); ++i)
301 : {
302 : for(size_type j = 0; j < m.num_cols(); ++j)
303 : {
304 : dev(i,j) = m(i,j);
305 : }
306 : dev(i,i) -= 1.0 / 3.0 * trace;
307 : }
308 : return trace;
309 : }
310 :
311 : ////////////////////////////////////////////////////////////////////////////////
312 : // Scaling of Matrices
313 : ////////////////////////////////////////////////////////////////////////////////
314 :
315 : template <typename matrix_t>
316 : inline void
317 : MatScale(matrix_t& mOut, typename matrix_t::value_type s, const matrix_t& m)
318 : {
319 : typedef typename matrix_t::size_type size_type;
320 0 : for(size_type i = 0; i < mOut.num_rows(); ++i)
321 0 : for(size_type j = 0; j < mOut.num_cols(); ++j)
322 : {
323 0 : mOut(i, j) = m(i, j) * s;
324 : }
325 : }
326 :
327 : template <typename matrix_t>
328 : inline void
329 0 : MatScaleAppend(matrix_t& mOut, typename matrix_t::value_type s, const matrix_t& m)
330 : {
331 : typedef typename matrix_t::size_type size_type;
332 0 : for(size_type i = 0; i < mOut.num_rows(); ++i)
333 0 : for(size_type j = 0; j < mOut.num_cols(); ++j)
334 : {
335 0 : mOut(i, j) += m(i, j) * s;
336 : }
337 0 : }
338 :
339 : ////////////////////////////////////////////////////////////////////////////////
340 : // Transposed of Matrix
341 : ////////////////////////////////////////////////////////////////////////////////
342 :
343 : template <size_t N, size_t M, typename T>
344 : inline void
345 : Transpose(MathMatrix<N,M,T>& mOut, const MathMatrix<M,N,T>& m)
346 : {
347 : typedef typename MathMatrix<N,M,T>::size_type size_type;
348 0 : for(size_type i = 0; i < mOut.num_rows(); ++i)
349 0 : for(size_type j = 0; j < mOut.num_cols(); ++j)
350 : {
351 0 : mOut(i, j) = m(j, i);
352 : }
353 : }
354 :
355 : template <typename matrix_t>
356 : inline void
357 : Transpose(matrix_t& m)
358 : {
359 : UG_ASSERT(m.num_rows()==m.num_cols(), "Transpose: Square Matrix needed");
360 :
361 : typedef typename matrix_t::size_type size_type;
362 : matrix_t _temp;
363 : for(size_type i = 1; i < m.num_rows(); ++i)
364 : for(size_type j = 0; j < i; ++j)
365 : _temp(i, j) = m(i, j);
366 :
367 : for(size_type i = 1; i < m.num_rows(); ++i)
368 : for(size_type j = 0; j < i; ++j)
369 : m(i, j) = m(j, i);
370 :
371 : for(size_type i = 0; i < m.num_rows()-1; ++i)
372 : for(size_type j = i+1; j < m.num_cols(); ++j)
373 : m(i, j) = _temp(j, i);
374 : }
375 :
376 : ////////////////////////////////////////////////////////////////////////////////
377 : // Determinant of Matrix
378 : ////////////////////////////////////////////////////////////////////////////////
379 :
380 : template <size_t N, typename T>
381 : inline typename MathMatrix<N,N,T>::value_type
382 : Determinant(const MathMatrix<N,N,T>& m)
383 : {
384 : UG_THROW("Determinant for matrix of size "<<N<<"x"<<N<<" not implemented.");
385 : }
386 :
387 : template <typename T>
388 : inline typename MathMatrix<0,0,T>::value_type
389 : Determinant(const MathMatrix<0,0,T>& m)
390 : {
391 : return 0;
392 : }
393 :
394 : template <typename T>
395 : inline typename MathMatrix<1,1,T>::value_type
396 : Determinant(const MathMatrix<1,1,T>& m)
397 : {
398 0 : return m(0,0);
399 : }
400 :
401 : template <typename T>
402 : inline typename MathMatrix<2,2,T>::value_type
403 : Determinant(const MathMatrix<2,2,T>& m)
404 : {
405 137376 : return (m(0,0)*m(1,1) - m(1,0)*m(0,1));
406 : }
407 :
408 : template <typename T>
409 : inline typename MathMatrix<3,3,T>::value_type
410 0 : Determinant(const MathMatrix<3,3,T>& m)
411 : {
412 0 : return m(0,0)*m(1,1)*m(2,2)
413 0 : + m(0,1)*m(1,2)*m(2,0)
414 0 : + m(0,2)*m(1,0)*m(2,1)
415 0 : - m(0,0)*m(1,2)*m(2,1)
416 0 : - m(0,1)*m(1,0)*m(2,2)
417 0 : - m(0,2)*m(1,1)*m(2,0);
418 : }
419 :
420 : template <typename T>
421 : inline typename MathMatrix<4,4,T>::value_type
422 : Determinant(const MathMatrix<4,4,T>& m)
423 : {
424 : return m(0,3)*m(1,2)*m(2,1)*m(3,0)-m(0,2)*m(1,3)*m(2,1)*m(3,0)
425 : - m(0,3)*m(1,1)*m(2,2)*m(3,0)+m(0,1)*m(1,3)*m(2,2)*m(3,0)
426 : + m(0,2)*m(1,1)*m(2,3)*m(3,0)-m(0,1)*m(1,2)*m(2,3)*m(3,0)
427 : - m(0,3)*m(1,2)*m(2,0)*m(3,1)+m(0,2)*m(1,3)*m(2,0)*m(3,1)
428 : + m(0,3)*m(1,0)*m(2,2)*m(3,1)-m(0,0)*m(1,3)*m(2,2)*m(3,1)
429 : - m(0,2)*m(1,0)*m(2,3)*m(3,1)+m(0,0)*m(1,2)*m(2,3)*m(3,1)
430 : + m(0,3)*m(1,1)*m(2,0)*m(3,2)-m(0,1)*m(1,3)*m(2,0)*m(3,2)
431 : - m(0,3)*m(1,0)*m(2,1)*m(3,2)+m(0,0)*m(1,3)*m(2,1)*m(3,2)
432 : + m(0,1)*m(1,0)*m(2,3)*m(3,2)-m(0,0)*m(1,1)*m(2,3)*m(3,2)
433 : - m(0,2)*m(1,1)*m(2,0)*m(3,3)+m(0,1)*m(1,2)*m(2,0)*m(3,3)
434 : + m(0,2)*m(1,0)*m(2,1)*m(3,3)-m(0,0)*m(1,2)*m(2,1)*m(3,3)
435 : - m(0,1)*m(1,0)*m(2,2)*m(3,3)+m(0,0)*m(1,1)*m(2,2)*m(3,3);
436 : }
437 :
438 : ////////////////////////////////////////////////////////////////////////////////
439 : // Gram Determinant of Matrix
440 : ////////////////////////////////////////////////////////////////////////////////
441 :
442 : template <size_t N, size_t M, typename T>
443 : inline typename MathMatrix<N,M,T>::value_type
444 : GramDeterminant(const MathMatrix<N,M,T>& m)
445 : {
446 : if(N <= M)
447 : {
448 : MathMatrix<N,N,T> mmT;
449 0 : MatMultiplyMMT(mmT, m);
450 : return Determinant(mmT);
451 : }
452 : else
453 : {
454 : MathMatrix<M,M,T> mTm;
455 0 : MatMultiplyMTM(mTm, m);
456 : return Determinant(mTm);
457 : }
458 : }
459 :
460 : template <typename T>
461 : inline typename MathMatrix<0,0,T>::value_type
462 : GramDeterminant(const MathMatrix<0,0,T>& m)
463 : {
464 : return 0;
465 : }
466 :
467 : template <size_t N, typename T>
468 : inline typename MathMatrix<N,0,T>::value_type
469 : GramDeterminant(const MathMatrix<N,0,T>& m)
470 : {
471 : return 0;
472 : }
473 :
474 : template <size_t M, typename T>
475 : inline typename MathMatrix<0,M,T>::value_type
476 : GramDeterminant(const MathMatrix<0,M,T>& m)
477 : {
478 : return 0;
479 : }
480 :
481 : template <typename T>
482 : inline typename MathMatrix<1,1,T>::value_type
483 : GramDeterminant(const MathMatrix<1,1,T>& m)
484 : {
485 : return pow(Determinant(m), 2);
486 : }
487 :
488 : template <typename T>
489 : inline typename MathMatrix<2,2,T>::value_type
490 : GramDeterminant(const MathMatrix<2,2,T>& m)
491 : {
492 : return pow(Determinant(m), 2);
493 : }
494 :
495 : template <typename T>
496 : inline typename MathMatrix<3,3,T>::value_type
497 : GramDeterminant(const MathMatrix<3,3,T>& m)
498 : {
499 : return pow(Determinant(m), 2);
500 : }
501 :
502 : ////////////////////////////////////////////////////////////////////////////////
503 : // Sqrt Gram Determinant of Matrix
504 : ////////////////////////////////////////////////////////////////////////////////
505 :
506 : template <size_t N, size_t M, typename T>
507 : inline typename MathMatrix<N,M,T>::value_type
508 0 : SqrtGramDeterminant(const MathMatrix<N,M,T>& m)
509 : {
510 0 : return sqrt(GramDeterminant(m));
511 : }
512 :
513 : template <typename T>
514 : inline typename MathMatrix<0,0,T>::value_type
515 : SqrtGramDeterminant(const MathMatrix<0,0,T>& m)
516 : {
517 : return 0;
518 : }
519 :
520 : template <size_t N, typename T>
521 : inline typename MathMatrix<N,0,T>::value_type
522 : SqrtGramDeterminant(const MathMatrix<N,0,T>& m)
523 : {
524 : return 0;
525 : }
526 :
527 : template <size_t M, typename T>
528 : inline typename MathMatrix<0,M,T>::value_type
529 : SqrtGramDeterminant(const MathMatrix<0,M,T>& m)
530 : {
531 : return 0;
532 : }
533 :
534 : template <typename T>
535 : inline typename MathMatrix<1,1,T>::value_type
536 : SqrtGramDeterminant(const MathMatrix<1,1,T>& m)
537 : {
538 0 : return fabs(Determinant(m));
539 : }
540 :
541 : template <typename T>
542 : inline typename MathMatrix<2,2,T>::value_type
543 : SqrtGramDeterminant(const MathMatrix<2,2,T>& m)
544 : {
545 0 : return fabs(Determinant(m));
546 : }
547 :
548 : template <typename T>
549 : inline typename MathMatrix<3,3,T>::value_type
550 : SqrtGramDeterminant(const MathMatrix<3,3,T>& m)
551 : {
552 0 : return fabs(Determinant(m));
553 : }
554 :
555 : ////////////////////////////////////////////////////////////////////////////////
556 : // Inverse of Matrix
557 : ////////////////////////////////////////////////////////////////////////////////
558 : template <size_t N, size_t M, typename T>
559 : inline typename MathMatrix<N,M,T>::value_type
560 0 : Inverse(MathMatrix<N,M,T>& mOut, const MathMatrix<M,N,T>& m)
561 : {
562 0 : UG_THROW("Inverse for matrix of size "<<M<<"x"<<N<<" not implemented. You could use GeneralizedInverse for pseudo-Inverse.");
563 : }
564 :
565 : template <typename T>
566 : inline typename MathMatrix<1,1,T>::value_type
567 : Inverse(MathMatrix<1,1,T>& mOut, const MathMatrix<1,1,T>& m)
568 : {
569 0 : const typename MathMatrix<1,1,T>::value_type det = m(0,0);
570 : UG_ASSERT(&mOut != &m, "Inverse: mOut and m have to be different");
571 : UG_ASSERT(det != 0, "Inverse: determinate is zero, can not Invert Matrix");
572 0 : mOut(0,0) = 1./m(0,0);
573 : return det;
574 : }
575 :
576 :
577 : template <typename T>
578 : inline typename MathMatrix<2,2,T>::value_type
579 : Inverse(MathMatrix<2,2,T>& mOut, const MathMatrix<2,2,T>& m)
580 : {
581 : const typename MathMatrix<2,2,T>::value_type det = Determinant(m);
582 : UG_ASSERT(&mOut != &m, "Inverse: mOut and m have to be different");
583 : UG_ASSERT(det != 0, "Inverse: determinate is zero, can not Invert Matrix");
584 137376 : const typename MathMatrix<2,2,T>::value_type invdet = 1./det;
585 :
586 137376 : mOut(0,0) = m(1,1)*invdet;
587 137376 : mOut(1,0) = -m(1,0)*invdet;
588 137376 : mOut(0,1) = -m(0,1)*invdet;
589 137376 : mOut(1,1) = m(0,0)*invdet;
590 :
591 137376 : return det;
592 : }
593 :
594 : template <typename T>
595 : inline typename MathMatrix<3,3,T>::value_type
596 0 : Inverse(MathMatrix<3,3,T>& mOut, const MathMatrix<3,3,T>& m)
597 : {
598 0 : const typename MathMatrix<3,3,T>::value_type det = Determinant(m);
599 : UG_ASSERT(&mOut != &m, "Inverse: mOut and m have to be different");
600 : UG_ASSERT(det != 0, "Inverse: determinate is zero, can not Invert Matrix");
601 0 : const typename MathMatrix<3,3,T>::value_type invdet = 1./det;
602 :
603 0 : mOut(0,0) = ( m(1,1)*m(2,2) - m(1,2)*m(2,1)) * invdet;
604 0 : mOut(0,1) = (-m(0,1)*m(2,2) + m(0,2)*m(2,1)) * invdet;
605 0 : mOut(0,2) = ( m(0,1)*m(1,2) - m(0,2)*m(1,1)) * invdet;
606 0 : mOut(1,0) = (-m(1,0)*m(2,2) + m(1,2)*m(2,0)) * invdet;
607 0 : mOut(1,1) = ( m(0,0)*m(2,2) - m(0,2)*m(2,0)) * invdet;
608 0 : mOut(1,2) = (-m(0,0)*m(1,2) + m(0,2)*m(1,0)) * invdet;
609 0 : mOut(2,0) = ( m(1,0)*m(2,1) - m(1,1)*m(2,0)) * invdet;
610 0 : mOut(2,1) = (-m(0,0)*m(2,1) + m(0,1)*m(2,0)) * invdet;
611 0 : mOut(2,2) = ( m(0,0)*m(1,1) - m(0,1)*m(1,0)) * invdet;
612 :
613 0 : return det;
614 : }
615 :
616 : ////////////////////////////////////////////////////////////////////////////////
617 : // Inverse Transposed of Matrix
618 : ////////////////////////////////////////////////////////////////////////////////
619 :
620 : template <size_t N, size_t M, typename T>
621 : inline typename MathMatrix<N,M,T>::value_type
622 : InverseTransposed(MathMatrix<N,M,T>& mOut, const MathMatrix<M,N,T>& m)
623 : {
624 : UG_THROW("InverseTransposed for matrix of size "<<M<<"x"<<N<<" not implemented.");
625 : }
626 :
627 : template <typename T>
628 : inline typename MathMatrix<1,1,T>::value_type
629 : InverseTransposed(MathMatrix<1,1,T>& mOut, const MathMatrix<1,1,T>& m)
630 : {
631 : return Inverse(mOut, m);
632 : }
633 :
634 : template <typename T>
635 : inline typename MathMatrix<2,2,T>::value_type
636 : InverseTransposed(MathMatrix<2,2,T>& mOut, const MathMatrix<2,2,T>& m)
637 : {
638 : const typename MathMatrix<2,2,T>::value_type det = Determinant(m);
639 : UG_ASSERT(&mOut != &m, "InverseTransposed: mOut and m have to be different");
640 : UG_ASSERT(det != 0, "InverseTransposed: determinate is zero, can not Invert Matrix");
641 : const typename MathMatrix<2,2,T>::value_type invdet = 1./det;
642 :
643 : mOut(0,0) = m(1,1)*invdet;
644 : mOut(1,0) = -m(0,1)*invdet;
645 : mOut(0,1) = -m(1,0)*invdet;
646 : mOut(1,1) = m(0,0)*invdet;
647 :
648 : return det;
649 : }
650 :
651 : template <typename T>
652 : inline typename MathMatrix<3,3,T>::value_type
653 : InverseTransposed(MathMatrix<3,3,T>& mOut, const MathMatrix<3,3,T>& m)
654 : {
655 : const typename MathMatrix<3,3,T>::value_type det = Determinant(m);
656 : UG_ASSERT(&mOut != &m, "InverseTransposed: mOut and m have to be different");
657 : UG_ASSERT(det != 0, "InverseTransposed: determinate is zero, can not Invert Matrix");
658 : const typename MathMatrix<3,3,T>::value_type invdet = 1./det;
659 :
660 : mOut(0,0) = ( m(1,1)*m(2,2) - m(2,1)*m(1,2)) * invdet;
661 : mOut(0,1) = (-m(1,0)*m(2,2) + m(2,0)*m(1,2)) * invdet;
662 : mOut(0,2) = ( m(1,0)*m(2,1) - m(2,0)*m(1,1)) * invdet;
663 : mOut(1,0) = (-m(0,1)*m(2,2) + m(2,1)*m(0,2)) * invdet;
664 : mOut(1,1) = ( m(0,0)*m(2,2) - m(2,0)*m(0,2)) * invdet;
665 : mOut(1,2) = (-m(0,0)*m(2,1) + m(2,0)*m(0,1)) * invdet;
666 : mOut(2,0) = ( m(0,1)*m(1,2) - m(1,1)*m(0,2)) * invdet;
667 : mOut(2,1) = (-m(0,0)*m(1,2) + m(1,0)*m(0,2)) * invdet;
668 : mOut(2,2) = ( m(0,0)*m(1,1) - m(1,0)*m(0,1)) * invdet;
669 :
670 : return det;
671 : }
672 :
673 : ////////////////////////////////////////////////////////////////////////////////
674 : // Right-Inverse of Matrix
675 : ////////////////////////////////////////////////////////////////////////////////
676 :
677 : template <size_t N, size_t M, typename T>
678 : inline typename MathMatrix<N,M,T>::value_type
679 0 : RightInverse(MathMatrix<N,M,T>& mOut, const MathMatrix<M,N,T>& m)
680 : {
681 : //UG_STATIC_ASSERT(M <= N, pseudo_inverse_does_not_exist);
682 : if (M > N) // note: this is a 'static condition', it should be eliminated by the optimizer
683 0 : UG_THROW ("RightInverse: Type mismatch, cannot right-invert a MxN-matrix with M > N!");
684 :
685 : // H = m*mT (H is symmetric)
686 : // TODO: Since H is symmetric, we could store only lower or upper elements
687 : MathMatrix<M,M,T> H, HInv;
688 0 : MatMultiplyMMT(H, m);
689 : // Invert H
690 0 : const number det = Inverse(HInv, H);
691 :
692 0 : MatMultiplyTransposed(mOut, m, HInv);
693 :
694 0 : return sqrt(det);
695 : }
696 :
697 : template <typename T>
698 : inline typename MathMatrix<1,1,T>::value_type
699 : RightInverse(MathMatrix<1,1>& mOut, const MathMatrix<1,1>& m){return fabs(Inverse(mOut, m));}
700 :
701 : template <typename T>
702 : inline typename MathMatrix<2,2,T>::value_type
703 : RightInverse(MathMatrix<2,2>& mOut, const MathMatrix<2,2>& m){return fabs(Inverse(mOut, m));}
704 :
705 : template <typename T>
706 : inline typename MathMatrix<3,3,T>::value_type
707 : RightInverse(MathMatrix<3,3>& mOut, const MathMatrix<3,3>& m){return fabs(Inverse(mOut, m));}
708 :
709 : ////////////////////////////////////////////////////////////////////////////////
710 : // Left-Inverse of Matrix
711 : ////////////////////////////////////////////////////////////////////////////////
712 :
713 : template <size_t N, size_t M, typename T>
714 : inline typename MathMatrix<N,M,T>::value_type
715 0 : LeftInverse(MathMatrix<N,M,T>& mOut, const MathMatrix<M,N,T>& m)
716 : {
717 : //UG_STATIC_ASSERT(N <= M, pseudo_inverse_does_not_exist);
718 : if (N > M) // note: this is a 'static condition', it should be eliminated by the optimizer
719 : UG_THROW ("LeftInverse: Type mismatch, cannot right-invert a MxN-matrix with M < N!");
720 :
721 : // H = mT*m (H is symmetric)
722 : // TODO: Since H is symmetric, we could store only lower or upper elements
723 : MathMatrix<N,N,T> H, HInv;
724 0 : MatMultiplyMTM(H, m);
725 :
726 : // Invert H
727 0 : const number det = Inverse(HInv, H);
728 :
729 0 : MatMultiplyTransposed(mOut, HInv, m);
730 :
731 0 : return sqrt(det);
732 : }
733 :
734 : template <typename T>
735 : inline typename MathMatrix<1,1,T>::value_type
736 : LeftInverse(MathMatrix<1,1>& mOut, const MathMatrix<1,1>& m){return fabs(Inverse(mOut, m));}
737 :
738 : template <typename T>
739 : inline typename MathMatrix<2,2,T>::value_type
740 : LeftInverse(MathMatrix<2,2>& mOut, const MathMatrix<2,2>& m){return fabs(Inverse(mOut, m));}
741 :
742 : template <typename T>
743 : inline typename MathMatrix<3,3,T>::value_type
744 : LeftInverse(MathMatrix<3,3>& mOut, const MathMatrix<3,3>& m){return fabs(Inverse(mOut, m));}
745 :
746 : ////////////////////////////////////////////////////////////////////////////////
747 : // Generalized-Inverse of Matrix
748 : ////////////////////////////////////////////////////////////////////////////////
749 : template<size_t N, size_t M, typename T>
750 : inline typename MathMatrix<N,M,T>::value_type
751 : GeneralizedInverse(MathMatrix<N,M,T>& mOut, const MathMatrix<M,N,T>& m)
752 : {
753 : if(M<N){//UG_LOG("Right Inverse for matrix of size "<<M<<"x"<<N<<".");
754 : return RightInverse(mOut,m);
755 : }
756 :
757 : if(M>N){//UG_LOG("Left Inverse for matrix of size "<<M<<"x"<<N<<".");
758 : return LeftInverse(mOut,m);
759 : }
760 : return Inverse(mOut,m);
761 : }
762 :
763 : ////////////////////////////////////////////////////////////////////////////////
764 : // Trace of Matrix
765 : ////////////////////////////////////////////////////////////////////////////////
766 :
767 : template <typename T>
768 : inline typename MathMatrix<1,1,T>::value_type
769 : Trace(const MathMatrix<1,1,T>& m)
770 : {
771 : return m(0,0);
772 : }
773 :
774 : template <typename T>
775 : inline typename MathMatrix<2,2,T>::value_type
776 : Trace(const MathMatrix<2,2,T>& m)
777 : {
778 : return (m(0,0)+m(1,1));
779 : }
780 :
781 : template <typename T>
782 : inline typename MathMatrix<3,3,T>::value_type
783 : Trace(const MathMatrix<3,3,T>& m)
784 : {
785 : return (m(0,0)+m(1,1)+m(2,2));
786 : }
787 :
788 : ////////////////////////////////////////////////////////////////////////////////
789 : // Scalar operations for Matrices
790 : ////////////////////////////////////////////////////////////////////////////////
791 :
792 : template <typename matrix_t>
793 : inline void
794 : MatSet(matrix_t& mInOut, typename matrix_t::value_type s)
795 : {
796 : typedef typename matrix_t::size_type size_type;
797 0 : for(size_type i = 0; i < mInOut.num_rows(); ++i)
798 0 : for(size_type j = 0; j < mInOut.num_cols(); ++j)
799 : {
800 0 : mInOut(i, j) = s;
801 : }
802 : }
803 :
804 : template <typename matrix_t>
805 : inline void
806 : MatDiagSet(matrix_t& mInOut, typename matrix_t::value_type s)
807 : {
808 : typedef typename matrix_t::size_type size_type;
809 : for(size_type i = 0; i < mInOut.num_rows(); ++i)
810 : {
811 : mInOut(i, i) = s;
812 : }
813 : }
814 :
815 : template <typename matrix_t>
816 : inline void
817 : MatAdd(matrix_t& mOut, const matrix_t& m, typename matrix_t::value_type s)
818 : {
819 : typedef typename matrix_t::size_type size_type;
820 0 : for(size_type i = 0; i < mOut.num_rows(); ++i)
821 0 : for(size_type j = 0; j < mOut.num_cols(); ++j)
822 : {
823 0 : mOut(i, j) = m(i,j) + s;
824 : }
825 : }
826 :
827 : template <typename matrix_t>
828 : inline void
829 : MatSubtract(matrix_t& mOut, const matrix_t& m, typename matrix_t::value_type s)
830 : {
831 : typedef typename matrix_t::size_type size_type;
832 : for(size_type i = 0; i < mOut.num_rows(); ++i)
833 : for(size_type j = 0; j < mOut.num_cols(); ++j)
834 : {
835 : mOut(i, j) = m(i,j) - s;
836 : }
837 : }
838 :
839 : template <typename matrix_t>
840 : inline void
841 : MatDivide(matrix_t& mOut, const matrix_t& m, typename matrix_t::value_type s)
842 : {
843 : typedef typename matrix_t::size_type size_type;
844 : for(size_type i = 0; i < mOut.num_rows(); ++i)
845 : for(size_type j = 0; j < mOut.num_cols(); ++j)
846 : {
847 : mOut(i, j) = m(i,j) /s;
848 : }
849 : }
850 :
851 : template <typename matrix_t>
852 : inline void
853 : MatMultiply(matrix_t& mOut, const matrix_t& m, typename matrix_t::value_type s)
854 : {
855 : typedef typename matrix_t::size_type size_type;
856 : for(size_type i = 0; i < mOut.num_rows(); ++i)
857 : for(size_type j = 0; j < mOut.num_cols(); ++j)
858 : {
859 : mOut(i, j) = m(i,j) * s;
860 : }
861 : }
862 :
863 : template <typename matrix_t>
864 : inline void
865 : MatIdentity(matrix_t& mOut)
866 : {
867 : MatSet(mOut, 0);
868 : MatDiagSet(mOut, 1);
869 : }
870 :
871 : template <typename matrix_t>
872 : inline void
873 : MatRotationX(matrix_t& mOut, typename matrix_t::value_type rads)
874 : {
875 : //UG_STATIC_ASSERT(matrix_t::RowSize > 2, AT_LEAST_3_ROWS_REQUIRED);
876 : //UG_STATIC_ASSERT(matrix_t::ColSize > 2, AT_LEAST_3_COLS_REQUIRED);
877 :
878 : MatIdentity(mOut);
879 : typename matrix_t::value_type s = sin(rads);
880 : typename matrix_t::value_type c = cos(rads);
881 :
882 : mOut(1, 1) = c; mOut(1, 2) = -s;
883 : mOut(2, 1) = s; mOut(2, 2) = c;
884 : }
885 :
886 : template <typename matrix_t>
887 : inline void
888 : MatRotationY(matrix_t& mOut, typename matrix_t::value_type rads)
889 : {
890 : //UG_STATIC_ASSERT(matrix_t::RowSize > 2, AT_LEAST_3_ROWS_REQUIRED);
891 : //UG_STATIC_ASSERT(matrix_t::ColSize > 2, AT_LEAST_3_COLS_REQUIRED);
892 :
893 : MatIdentity(mOut);
894 : typename matrix_t::value_type s = sin(rads);
895 : typename matrix_t::value_type c = cos(rads);
896 :
897 : mOut(0, 0) = c; mOut(0, 2) = s;
898 : mOut(2, 0) = -s; mOut(2, 2) = c;
899 : }
900 :
901 : template <typename matrix_t>
902 : inline void
903 : MatRotationZ(matrix_t& mOut, typename matrix_t::value_type rads)
904 : {
905 : MatIdentity(mOut);
906 : typename matrix_t::value_type s = sin(rads);
907 : typename matrix_t::value_type c = cos(rads);
908 :
909 : mOut(0, 0) = c; mOut(0, 1) = -s;
910 : mOut(1, 0) = s; mOut(1, 1) = c;
911 : }
912 :
913 : ////////////////////////////////////////////////////////////////////////////////
914 : /// Creates a rotation matrix given yaw, pitch and roll in radiants.
915 : ////////////////////////////////////////////////////////////////////////////////
916 :
917 : template <typename matrix_t>
918 : inline void
919 : MatRotationYawPitchRoll(matrix_t& mOut,
920 : typename matrix_t::value_type yaw,
921 : typename matrix_t::value_type pitch,
922 : typename matrix_t::value_type roll)
923 : {
924 : //UG_STATIC_ASSERT(matrix_t::RowSize > 2, AT_LEAST_3_ROWS_REQUIRED);
925 : //UG_STATIC_ASSERT(matrix_t::ColSize > 2, AT_LEAST_3_COLS_REQUIRED);
926 :
927 : matrix_t tMat1, tMat2, tMat3;
928 : MatRotationX(tMat1, yaw);
929 : MatRotationY(tMat2, pitch);
930 : MatMultiply(tMat3, tMat1, tMat2);
931 : MatRotationZ(tMat1, roll);
932 : MatMultiply(mOut, tMat3, tMat1);
933 : }
934 :
935 : ////////////////////////////////////////////////////////////////////////////////
936 : /// Creates a householder matrix given the orthogonal vector to the
937 : /// householder-hypersphere through the origin.
938 : ////////////////////////////////////////////////////////////////////////////////
939 :
940 : template <typename matrix_t, typename vector_t>
941 : inline void
942 0 : MatHouseholder(matrix_t& mOut, const vector_t& orthoVec)
943 : {
944 : assert(vector_t::Size == matrix_t::RowSize);
945 : assert(vector_t::Size == matrix_t::ColSize);
946 :
947 : typename vector_t::value_type scalarProd = VecDot(orthoVec, orthoVec);
948 :
949 : typedef typename matrix_t::size_type size_type_mat;
950 0 : for(size_type_mat i = 0; i < mOut.num_rows(); ++i)
951 : {
952 0 : for(size_type_mat j = 0; j < mOut.num_cols(); ++j){
953 0 : mOut(i,j) = - 2.0/scalarProd * orthoVec[i] * orthoVec[j];
954 : }
955 0 : mOut(i,i) += 1.0;
956 : }
957 :
958 0 : }
959 :
960 : ////////////////////////////////////////////////////////////////////////////////
961 : // Norms for Matrices
962 : ////////////////////////////////////////////////////////////////////////////////
963 :
964 : template <typename matrix_t>
965 : inline typename matrix_t::value_type
966 : MatFrobeniusNormSq(matrix_t& m)
967 : {
968 : typename matrix_t::value_type norm = 0;
969 : typedef typename matrix_t::size_type size_type;
970 : for(size_type i = 0; i < m.num_rows(); ++i)
971 : for(size_type j = 0; j < m.num_cols(); ++j)
972 : {
973 : norm += m(i,j)*m(i,j);
974 : }
975 :
976 : return norm;
977 : }
978 :
979 : template <typename matrix_t>
980 : inline typename matrix_t::value_type
981 : MatFrobeniusNorm(matrix_t& m)
982 : {
983 : return static_cast<typename matrix_t::value_type>(sqrt(MatFrobeniusNormSq(m)));
984 : }
985 :
986 : template <typename matrix_t>
987 : inline typename matrix_t::value_type
988 : MatOneNorm(matrix_t& m)
989 : {
990 : typename matrix_t::value_type sum, max = 0;
991 : typedef typename matrix_t::size_type size_type;
992 : for(size_type j = 0; j < m.num_cols(); ++j)
993 : {
994 : sum = 0;
995 : for(size_type i = 0; i < m.num_rows(); ++i)
996 : {
997 : sum += fabs(m(i,j));
998 : }
999 : max = (sum > max) ? sum : max;
1000 : }
1001 : return max;
1002 : }
1003 :
1004 : template <typename matrix_t>
1005 : inline typename matrix_t::value_type
1006 : MatInftyNorm(matrix_t& m)
1007 : {
1008 : typename matrix_t::value_type sum, max = 0;
1009 : typedef typename matrix_t::size_type size_type;
1010 : for(size_type i = 0; i < m.num_rows(); ++i)
1011 : {
1012 : sum = 0;
1013 : for(size_type j = 0; j < m.num_cols(); ++j)
1014 : {
1015 : sum += fabs(m(i,j));
1016 : }
1017 : max = (sum > max) ? sum : max;
1018 : }
1019 : return max;
1020 : }
1021 :
1022 : template <typename matrix_t>
1023 : inline typename matrix_t::value_type
1024 : MatMaxNorm(matrix_t& m)
1025 : {
1026 : typename matrix_t::value_type max = 0;
1027 : typedef typename matrix_t::size_type size_type;
1028 : for(size_type i = 0; i < m.num_rows(); ++i)
1029 : for(size_type j = 0; j < m.num_cols(); ++j)
1030 : {
1031 : max = (m(i,j) > max) ? m(i,j) : max;
1032 : }
1033 :
1034 : return max;
1035 : }
1036 :
1037 :
1038 :
1039 : template <size_t N, size_t M, typename T>
1040 : inline typename MathMatrix<N,M,T>::value_type
1041 : MaxAbsEigenvalue(const MathMatrix<M,N,T>& m)
1042 : {
1043 : UG_THROW("MaxAbsEigenvalue for matrix of size "<<N<<"x"<<M<<" not implemented.");
1044 : }
1045 :
1046 : template <typename T>
1047 : inline typename MathMatrix<1,1,T>::value_type
1048 : MaxAbsEigenvalue(const MathMatrix<1,1,T>& m)
1049 : {
1050 : const typename MathMatrix<1,1,T>::value_type val = m(0,0);
1051 : return fabs(val);
1052 : }
1053 :
1054 : template <typename T>
1055 : inline typename MathMatrix<2,2,T>::value_type
1056 : MaxAbsEigenvalue(const MathMatrix<2,2,T>& m)
1057 : {
1058 : typename MathMatrix<2,2,T>::value_type minus_p_half, val;
1059 : minus_p_half = m(0,0)+m(1,1);
1060 : val = minus_p_half*minus_p_half - (m(0,0)*m(1,1) - m(0,1)*m(1,0));
1061 : UG_ASSERT(val >= 0.0, "MaxAbsEigenvalues: Complex Eigenvalues???");
1062 :
1063 : if (minus_p_half >=0.0) {return (minus_p_half + sqrt(val));}
1064 : else {return fabs(minus_p_half-sqrt(val));}
1065 : }
1066 :
1067 :
1068 : template <typename matrix_t>
1069 : inline typename matrix_t::value_type
1070 : MinAbsEigenvalue(matrix_t& m)
1071 : {
1072 : matrix_t inv;
1073 : Inverse(inv, m);
1074 : typename matrix_t::value_type min=1.0/MaxAbsEigenvalue(inv);
1075 : return min;
1076 : }
1077 :
1078 : } // end of namespace
1079 :
1080 : #endif /* __H__UG__COMMON__MATH_MATRIX_FUNCTIONS_COMMON_IMPL__ */
|
