Jpp
JMatrix5S.hh
Go to the documentation of this file.
1 #ifndef __JMATRIX5S__
2 #define __JMATRIX5S__
3 
4 #include <cmath>
5 #include <algorithm>
6 
7 #include "JMath/JMatrix5D.hh"
8 #include "JLang/JException.hh"
9 
10 
11 /**
12  * \author mdejong
13  */
14 
15 namespace JMATH {}
16 namespace JPP { using namespace JMATH; }
17 
18 namespace JMATH {
19 
20  using JLANG::JDivisionByZero;
21 
22 
23  /**
24  * 5 x 5 symmetric matrix
25  */
26  class JMatrix5S :
27  public JMatrix5D
28  {
29  public:
30  /**
31  * Default constructor.
32  */
33  JMatrix5S() :
34  JMatrix5D()
35  {}
36 
37 
38  /**
39  * Contructor.
40  *
41  * \param A matrix
42  */
43  JMatrix5S(const JMatrix5D& A) :
44  JMatrix5D(A)
45  {}
46 
47 
48  /**
49  * Contructor.
50  *
51  * \param __a00 (0,0)
52  * \param __a01 (0,1)
53  * \param __a02 (0,2)
54  * \param __a03 (0,3)
55  * \param __a04 (0,4)
56  * \param __a11 (1,1)
57  * \param __a12 (1,2)
58  * \param __a13 (1,3)
59  * \param __a14 (1,4)
60  * \param __a22 (2,2)
61  * \param __a23 (2,3)
62  * \param __a24 (2,4)
63  * \param __a33 (3,3)
64  * \param __a34 (3,4)
65  * \param __a44 (4,4)
66  */
67  JMatrix5S(const double __a00, const double __a01, const double __a02, const double __a03, const double __a04,
68  /* */ const double __a11, const double __a12, const double __a13, const double __a14,
69  /* */ const double __a22, const double __a23, const double __a24,
70  /* */ const double __a33, const double __a34,
71  /* */ const double __a44) :
72  JMatrix5D(__a00, __a01, __a02, __a03, __a04,
73  __a01, __a11, __a12, __a13, __a14,
74  __a02, __a12, __a22, __a23, __a24,
75  __a03, __a13, __a23, __a33, __a34,
76  __a04, __a14, __a24, __a34, __a44)
77  {}
78 
79 
80  /**
81  * Invert matrix.
82  */
83  void invert()
84  {
85  using std::swap;
86 
87  if (isIdentity()) {
88  return;
89  }
90 
91  // LDU decomposition
92 
93  int p0 = 0; // permute row 0
94  int p1 = 0; // permute row 1
95  int p2 = 0; // permute row 2
96  int p3 = 0; // permute row 3
97 
98  double val;
99 
100  val = a00;
101 
102  if (fabs(a10) > fabs(val)) {
103  p0 = 1;
104  val = a10;
105  }
106 
107  if (fabs(a20) > fabs(val)) {
108  p0 = 2;
109  val = a20;
110  }
111 
112  if (fabs(a30) > fabs(val)) {
113  p0 = 3;
114  val = a30;
115  }
116 
117  if (fabs(a40) > fabs(val)) {
118  p0 = 4;
119  val = a40;
120  }
121 
122  switch (p0) {
123 
124  case 1:
125  swap(a00,a10);
126  swap(a01,a11);
127  swap(a02,a12);
128  swap(a03,a13);
129  swap(a04,a14);
130  break;
131 
132  case 2:
133  swap(a00,a20);
134  swap(a01,a21);
135  swap(a02,a22);
136  swap(a03,a23);
137  swap(a04,a24);
138  break;
139 
140  case 3:
141  swap(a00,a30);
142  swap(a01,a31);
143  swap(a02,a32);
144  swap(a03,a33);
145  swap(a04,a34);
146  break;
147 
148  case 4:
149  swap(a00,a40);
150  swap(a01,a41);
151  swap(a02,a42);
152  swap(a03,a43);
153  swap(a04,a44);
154  break;
155  }
156 
157  if (val == 0) {
158  throw JDivisionByZero("LDU decomposition zero pivot");
159  }
160 
161  a10 /= val;
162  a11 -= a10 * a01;
163  a12 -= a10 * a02;
164  a13 -= a10 * a03;
165  a14 -= a10 * a04;
166 
167  a20 /= val;
168  a21 -= a20 * a01;
169  a22 -= a20 * a02;
170  a23 -= a20 * a03;
171  a24 -= a20 * a04;
172 
173  a30 /= val;
174  a31 -= a30 * a01;
175  a32 -= a30 * a02;
176  a33 -= a30 * a03;
177  a34 -= a30 * a04;
178 
179  a40 /= val;
180  a41 -= a40 * a01;
181  a42 -= a40 * a02;
182  a43 -= a40 * a03;
183  a44 -= a40 * a04;
184 
185  val = a11;
186 
187  if (fabs(a21) > fabs(val)) {
188  p1 = 2;
189  val = a21;
190  }
191 
192  if (fabs(a31) > fabs(val)) {
193  p1 = 3;
194  val = a31;
195  }
196 
197  if (fabs(a41) > fabs(val)) {
198  p1 = 4;
199  val = a41;
200  }
201 
202  switch (p1) {
203 
204  case 2:
205  swap(a10,a20);
206  swap(a11,a21);
207  swap(a12,a22);
208  swap(a13,a23);
209  swap(a14,a24);
210  break;
211 
212  case 3:
213  swap(a10,a30);
214  swap(a11,a31);
215  swap(a12,a32);
216  swap(a13,a33);
217  swap(a14,a34);
218  break;
219 
220  case 4:
221  swap(a10,a40);
222  swap(a11,a41);
223  swap(a12,a42);
224  swap(a13,a43);
225  swap(a14,a44);
226  break;
227  }
228 
229  if (val == 0) {
230  throw JDivisionByZero("LDU decomposition zero pivot");
231  }
232 
233  a21 /= val;
234  a22 -= a21 * a12;
235  a23 -= a21 * a13;
236  a24 -= a21 * a14;
237 
238  a31 /= val;
239  a32 -= a31 * a12;
240  a33 -= a31 * a13;
241  a34 -= a31 * a14;
242 
243  a41 /= val;
244  a42 -= a41 * a12;
245  a43 -= a41 * a13;
246  a44 -= a41 * a14;
247 
248  val = a22;
249 
250  if (fabs(a32) > fabs(val)) {
251  p2 = 3;
252  val = a32;
253  }
254 
255  if (fabs(a42) > fabs(val)) {
256  p2 = 4;
257  val = a42;
258  }
259 
260  switch (p2) {
261 
262  case 3:
263  swap(a20,a30);
264  swap(a21,a31);
265  swap(a22,a32);
266  swap(a23,a33);
267  swap(a24,a34);
268  break;
269 
270  case 4:
271  swap(a20,a40);
272  swap(a21,a41);
273  swap(a22,a42);
274  swap(a23,a43);
275  swap(a24,a44);
276  break;
277  }
278 
279  if (val == 0) {
280  throw JDivisionByZero("LDU decomposition zero pivot");
281  }
282 
283  a32 /= val;
284  a33 -= a32 * a23;
285  a34 -= a32 * a24;
286 
287  a42 /= val;
288  a43 -= a42 * a23;
289  a44 -= a42 * a24;
290 
291  val = a33;
292 
293  if (fabs(a43) > fabs(val)) {
294  p3 = 4;
295  val = a43;
296  }
297 
298  switch (p3) {
299 
300  case 4:
301  swap(a30,a40);
302  swap(a31,a41);
303  swap(a32,a42);
304  swap(a33,a43);
305  swap(a34,a44);
306  break;
307  }
308 
309  if (val == 0) {
310  throw JDivisionByZero("LDU decomposition zero pivot");
311  }
312 
313  a43 /= val;
314  a44 -= a43 * a34;
315 
316  // invert D
317 
318  if (a44 == 0) {
319  throw JDivisionByZero("D matrix not invertable");
320  }
321 
322  a00 = 1.0 / a00;
323  a11 = 1.0 / a11;
324  a22 = 1.0 / a22;
325  a33 = 1.0 / a33;
326  a44 = 1.0 / a44;
327 
328  // invert U
329 
330  a01 *= -a00;
331  a01 *= a11;
332 
333  a02 *= -a00;
334  a02 -= a01 * a12;
335  a02 *= a22;
336 
337  a03 *= -a00;
338  a03 -= a01 * a13;
339  a03 -= a02 * a23;
340  a03 *= a33;
341 
342  a04 *= -a00;
343  a04 -= a01 * a14;
344  a04 -= a02 * a24;
345  a04 -= a03 * a34;
346  a04 *= a44;
347 
348  a12 *= -a11;
349  a12 *= a22;
350 
351  a13 *= -a11;
352  a13 -= a12 * a23;
353  a13 *= a33;
354 
355  a14 *= -a11;
356  a14 -= a12 * a24;
357  a14 -= a13 * a34;
358  a14 *= a44;
359 
360  a23 *= -a22;
361  a23 *= a33;
362 
363  a24 *= -a22;
364  a24 -= a23 * a34;
365  a24 *= a44;
366 
367  a34 *= -a33;
368  a34 *= a44;
369 
370  // invert L
371 
372  a43 = -a43;
373 
374  a42 = -a42;
375  a42 -= a43 * a32;
376 
377  a41 = -a41;
378  a41 -= a42 * a21;
379  a41 -= a43 * a31;
380 
381  a40 = -a40;
382  a40 -= a41 * a10;
383  a40 -= a42 * a20;
384  a40 -= a43 * a30;
385 
386  a32 = -a32;
387 
388  a31 = -a31;
389  a31 -= a32 * a21;
390 
391  a30 = -a30;
392  a30 -= a31 * a10;
393  a30 -= a32 * a20;
394 
395  a21 = -a21;
396  a20 = -a20;
397  a20 -= a21 * a10;
398  a10 = -a10;
399 
400  // U^-1 x L^-1
401 
402  a00 += a01 * a10 + a02 * a20 + a03 * a30 + a04 * a40;
403  a01 += a02 * a21 + a03 * a31 + a04 * a41;
404  a02 += a03 * a32 + a04 * a42;
405  a03 += a04 * a43;
406 
407  a10 *= a11;
408  a10 += a12 * a20 + a13 * a30 + a14 * a40;
409  a11 += a12 * a21 + a13 * a31 + a14 * a41;
410  a12 += a13 * a32 + a14 * a42;
411  a13 += a14 * a43;
412 
413  a20 *= a22;
414  a20 += a23 * a30 + a24 * a40;
415  a21 *= a22;
416  a21 += a23 * a31 + a24 * a41;
417  a22 += a23 * a32 + a24 * a42;
418  a23 += a24 * a43;
419 
420  a30 *= a33;
421  a30 += a34 * a40;
422  a31 *= a33;
423  a31 += a34 * a41;
424  a32 *= a33;
425  a32 += a34 * a42;
426  a33 += a34 * a43;
427 
428  a40 *= a44;
429  a41 *= a44;
430  a42 *= a44;
431  a43 *= a44;
432 
433  switch (p3) {
434 
435  case 4:
436  swap(a03,a04);
437  swap(a13,a14);
438  swap(a23,a24);
439  swap(a33,a34);
440  swap(a43,a44);
441  break;
442  }
443 
444  switch (p2) {
445 
446  case 3:
447  swap(a02,a03);
448  swap(a12,a13);
449  swap(a22,a23);
450  swap(a32,a33);
451  swap(a42,a43);
452  break;
453 
454  case 4:
455  swap(a02,a04);
456  swap(a12,a14);
457  swap(a22,a24);
458  swap(a32,a34);
459  swap(a42,a44);
460  break;
461  }
462 
463  switch (p1) {
464 
465  case 2:
466  swap(a01,a02);
467  swap(a11,a12);
468  swap(a21,a22);
469  swap(a31,a32);
470  swap(a41,a42);
471  break;
472 
473  case 3:
474  swap(a01,a03);
475  swap(a11,a13);
476  swap(a21,a23);
477  swap(a31,a33);
478  swap(a41,a43);
479  break;
480 
481  case 4:
482  swap(a01,a04);
483  swap(a11,a14);
484  swap(a21,a24);
485  swap(a31,a34);
486  swap(a41,a44);
487  break;
488  }
489 
490  switch (p0) {
491 
492  case 1:
493  swap(a00,a01);
494  swap(a10,a11);
495  swap(a20,a21);
496  swap(a30,a31);
497  swap(a40,a41);
498  break;
499 
500  case 2:
501  swap(a00,a02);
502  swap(a10,a12);
503  swap(a20,a22);
504  swap(a30,a32);
505  swap(a40,a42);
506  break;
507 
508  case 3:
509  swap(a00,a03);
510  swap(a10,a13);
511  swap(a20,a23);
512  swap(a30,a33);
513  swap(a40,a43);
514  break;
515 
516  case 4:
517  swap(a00,a04);
518  swap(a10,a14);
519  swap(a20,a24);
520  swap(a30,a34);
521  swap(a40,a44);
522  break;
523  }
524  }
525  };
526 }
527 
528 #endif
JException.hh
JMATH::JMatrix5D::a31
double a31
Definition: JMatrix5D.hh:525
JMATH::JMatrix5D::a04
double a04
Definition: JMatrix5D.hh:522
JMATH::JMatrix5S::invert
void invert()
Invert matrix.
Definition: JMatrix5S.hh:83
JMATH::JMatrix5D::a21
double a21
Definition: JMatrix5D.hh:524
JMATH::JMatrix5D::a32
double a32
Definition: JMatrix5D.hh:525
JMATH::JMatrix5D::a20
double a20
Definition: JMatrix5D.hh:524
JMATH::JMatrix5D::a30
double a30
Definition: JMatrix5D.hh:525
JMATH::JMatrix5D::a01
double a01
Definition: JMatrix5D.hh:522
JMATH::JMatrix5D::a34
double a34
Definition: JMatrix5D.hh:525
JMATH::JMatrix5D::a11
double a11
Definition: JMatrix5D.hh:523
JMATH::JMatrix5D::a13
double a13
Definition: JMatrix5D.hh:523
JPP
This name space includes all other name spaces (except KM3NETDAQ, KM3NET and ANTARES).
Definition: JAAnetToolkit.hh:37
JMATH::JMatrix5D::a12
double a12
Definition: JMatrix5D.hh:523
JMATH::JMatrix5D::a00
double a00
Definition: JMatrix5D.hh:522
JMATH::JMatrix5D::a41
double a41
Definition: JMatrix5D.hh:526
JMATH::JMatrix5S::JMatrix5S
JMatrix5S()
Default constructor.
Definition: JMatrix5S.hh:33
JMATH::JMatrix5D::a02
double a02
Definition: JMatrix5D.hh:522
JMATH::JMatrix5D::a33
double a33
Definition: JMatrix5D.hh:525
JMATH::JMatrix5S::JMatrix5S
JMatrix5S(const JMatrix5D &A)
Contructor.
Definition: JMatrix5S.hh:43
JMatrix5D.hh
JMATH::JMatrix5D::a10
double a10
Definition: JMatrix5D.hh:523
JMATH::JMatrix5D::a40
double a40
Definition: JMatrix5D.hh:526
JMATH
Auxiliary classes and methods for mathematical operations.
Definition: JCalculator.hh:9
JMATH::JMatrix5D::a03
double a03
Definition: JMatrix5D.hh:522
JMATH::JMatrix5S::JMatrix5S
JMatrix5S(const double __a00, const double __a01, const double __a02, const double __a03, const double __a04, const double __a11, const double __a12, const double __a13, const double __a14, const double __a22, const double __a23, const double __a24, const double __a33, const double __a34, const double __a44)
Contructor.
Definition: JMatrix5S.hh:67
JMATH::JMatrix5D::a42
double a42
Definition: JMatrix5D.hh:526
JMATH::JMatrix5D
5 x 5 matrix
Definition: JMatrix5D.hh:33
JMATH::JMatrix5D::a22
double a22
Definition: JMatrix5D.hh:524
JLANG::JDivisionByZero
Exception for division by zero.
Definition: JException.hh:270
p1
TPaveText * p1
Definition: JDrawModule3D.cc:35
JMATH::JMatrix5D::a23
double a23
Definition: JMatrix5D.hh:524
JMATH::JMatrix5D::a14
double a14
Definition: JMatrix5D.hh:523
JMATH::JMatrix5D::a43
double a43
Definition: JMatrix5D.hh:526
JMATH::JMatrix5D::isIdentity
bool isIdentity(const double eps=std::numeric_limits< double >::min()) const
Test identity.
Definition: JMatrix5D.hh:364
JMATH::JMatrix5D::a24
double a24
Definition: JMatrix5D.hh:524
JMATH::JMatrix5S
5 x 5 symmetric matrix
Definition: JMatrix5S.hh:26
JMATH::JMatrix5D::a44
double a44
Definition: JMatrix5D.hh:526
Generated by   doxygen 1.8.16