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