Jpp
JMatrix4S.hh
Go to the documentation of this file.
1 #ifndef __JMATRIX4S__
2 #define __JMATRIX4S__
3 
4 #include <cmath>
5 #include <algorithm>
6 
7 #include "JMath/JMatrix4D.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  * 4 x 4 symmetric matrix
25  */
26  class JMatrix4S :
27  public JMatrix4D
28  {
29  public:
30  /**
31  * Default constructor.
32  */
34  JMatrix4D()
35  {}
36 
37 
38  /**
39  * Contructor.
40  *
41  * \param A matrix
42  */
43  JMatrix4S(const JMatrix4D& A) :
44  JMatrix4D(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 __a11 (1,1)
56  * \param __a12 (1,2)
57  * \param __a13 (1,3)
58  * \param __a22 (2,2)
59  * \param __a23 (2,3)
60  * \param __a33 (3,3)
61  */
62  JMatrix4S(const double __a00, const double __a01, const double __a02, const double __a03,
63  /* */ const double __a11, const double __a12, const double __a13,
64  /* */ const double __a22, const double __a23,
65  /* */ const double __a33) :
66  JMatrix4D(__a00, __a01, __a02, __a03,
67  __a01, __a11, __a12, __a13,
68  __a02, __a12, __a22, __a23,
69  __a03, __a13, __a23, __a33)
70  {}
71 
72 
73  /**
74  * Invert matrix.
75  */
76  void invert()
77  {
78  using std::swap;
79 
80  // LDU decomposition
81 
82  int p0 = 0; // permute row 0
83  int p1 = 0; // permute row 1
84  int p2 = 0; // permute row 2
85 
86  double val;
87 
88  val = a00;
89 
90  if (fabs(a10) > fabs(val)) {
91  p0 = 1;
92  val = a10;
93  }
94 
95  if (fabs(a20) > fabs(val)) {
96  p0 = 2;
97  val = a20;
98  }
99 
100  if (fabs(a30) > fabs(val)) {
101  p0 = 3;
102  val = a30;
103  }
104 
105  switch (p0) {
106 
107  case 1:
108  swap(a00,a10);
109  swap(a01,a11);
110  swap(a02,a12);
111  swap(a03,a13);
112  break;
113 
114  case 2:
115  swap(a00,a20);
116  swap(a01,a21);
117  swap(a02,a22);
118  swap(a03,a23);
119  break;
120 
121  case 3:
122  swap(a00,a30);
123  swap(a01,a31);
124  swap(a02,a32);
125  swap(a03,a33);
126  break;
127  }
128 
129  if (val == 0) {
130  throw JDivisionByZero("LDU decomposition zero pivot");
131  }
132 
133  a10 /= val;
134  a11 -= a10 * a01;
135  a12 -= a10 * a02;
136  a13 -= a10 * a03;
137 
138  a20 /= val;
139  a21 -= a20 * a01;
140  a22 -= a20 * a02;
141  a23 -= a20 * a03;
142 
143  a30 /= val;
144  a31 -= a30 * a01;
145  a32 -= a30 * a02;
146  a33 -= a30 * a03;
147 
148  val = a11;
149 
150  if (fabs(a21) > fabs(val)) {
151  p1 = 2;
152  val = a21;
153  }
154 
155  if (fabs(a31) > fabs(val)) {
156  p1 = 3;
157  val = a31;
158  }
159 
160  switch (p1) {
161 
162  case 2:
163  swap(a10,a20);
164  swap(a11,a21);
165  swap(a12,a22);
166  swap(a13,a23);
167  break;
168 
169  case 3:
170  swap(a10,a30);
171  swap(a11,a31);
172  swap(a12,a32);
173  swap(a13,a33);
174  break;
175  }
176 
177  if (val == 0) {
178  throw JDivisionByZero("LDU decomposition zero pivot");
179  }
180 
181  a21 /= val;
182  a22 -= a21 * a12;
183  a23 -= a21 * a13;
184 
185  a31 /= val;
186  a32 -= a31 * a12;
187  a33 -= a31 * a13;
188 
189  val = a22;
190 
191  if (fabs(a32) > fabs(val)) {
192  p2 = 3;
193  val = a32;
194  }
195 
196  switch (p2) {
197 
198  case 3:
199  swap(a20,a30);
200  swap(a21,a31);
201  swap(a22,a32);
202  swap(a23,a33);
203  break;
204  }
205 
206  if (val == 0) {
207  throw JDivisionByZero("LDU decomposition zero pivot");
208  }
209 
210  a32 /= val;
211  a33 -= a32 * a23;
212 
213  // invert D
214 
215  if (a33 == 0) {
216  throw JDivisionByZero("D matrix not invertable");
217  }
218 
219  a00 = 1.0 / a00;
220  a11 = 1.0 / a11;
221  a22 = 1.0 / a22;
222  a33 = 1.0 / a33;
223 
224  // invert U
225 
226  a01 *= -a00;
227  a01 *= a11;
228 
229  a02 *= -a00;
230  a02 -= a01 * a12;
231  a02 *= a22;
232 
233  a03 *= -a00;
234  a03 -= a01 * a13;
235  a03 -= a02 * a23;
236  a03 *= a33;
237 
238  a12 *= -a11;
239  a12 *= a22;
240 
241  a13 *= -a11;
242  a13 -= a12 * a23;
243  a13 *= a33;
244 
245  a23 *= -a22;
246  a23 *= a33;
247 
248  // invert L
249 
250  a32 = -a32;
251 
252  a31 = -a31;
253  a31 -= a32 * a21;
254 
255  a30 = -a30;
256  a30 -= a31 * a10;
257  a30 -= a32 * a20;
258 
259  a21 = -a21;
260  a20 = -a20;
261  a20 -= a21 * a10;
262  a10 = -a10;
263 
264  // U^-1 x L^-1
265 
266  a00 += a01 * a10 + a02 * a20 + a03 * a30;
267  a01 += a02 * a21 + a03 * a31;
268  a02 += a03 * a32;
269 
270  a10 *= a11;
271  a10 += a12 * a20 + a13 * a30;
272  a11 += a12 * a21 + a13 * a31;
273  a12 += a13 * a32;
274 
275  a20 *= a22;
276  a20 += a23 * a30;
277  a21 *= a22;
278  a21 += a23 * a31;
279  a22 += a23 * a32;
280 
281  a30 *= a33;
282  a31 *= a33;
283  a32 *= a33;
284 
285  switch (p2) {
286 
287  case 3:
288  swap(a02,a03);
289  swap(a12,a13);
290  swap(a22,a23);
291  swap(a32,a33);
292  break;
293  }
294 
295  switch (p1) {
296 
297  case 2:
298  swap(a01,a02);
299  swap(a11,a12);
300  swap(a21,a22);
301  swap(a31,a32);
302  break;
303 
304  case 3:
305  swap(a01,a03);
306  swap(a11,a13);
307  swap(a21,a23);
308  swap(a31,a33);
309  break;
310  }
311 
312  switch (p0) {
313 
314  case 1:
315  swap(a00,a01);
316  swap(a10,a11);
317  swap(a20,a21);
318  swap(a30,a31);
319  break;
320 
321  case 2:
322  swap(a00,a02);
323  swap(a10,a12);
324  swap(a20,a22);
325  swap(a30,a32);
326  break;
327 
328  case 3:
329  swap(a00,a03);
330  swap(a10,a13);
331  swap(a20,a23);
332  swap(a30,a33);
333  break;
334  }
335  }
336  };
337 }
338 
339 #endif
JException.hh
JMATH::JMatrix4D::a11
double a11
Definition: JMatrix4D.hh:439
JMATH::JMatrix4S::invert
void invert()
Invert matrix.
Definition: JMatrix4S.hh:76
JMATH::JMatrix4D::a02
double a02
Definition: JMatrix4D.hh:438
JMATH::JMatrix4D::a23
double a23
Definition: JMatrix4D.hh:440
JMATH::JMatrix4S::JMatrix4S
JMatrix4S()
Default constructor.
Definition: JMatrix4S.hh:33
JMATH::JMatrix4D::a30
double a30
Definition: JMatrix4D.hh:441
JMATH::JMatrix4D
4 x 4 matrix
Definition: JMatrix4D.hh:33
JMATH::JMatrix4S::JMatrix4S
JMatrix4S(const JMatrix4D &A)
Contructor.
Definition: JMatrix4S.hh:43
JPP
This name space includes all other name spaces (except KM3NETDAQ, KM3NET and ANTARES).
Definition: JAAnetToolkit.hh:37
JMATH::JMatrix4D::a20
double a20
Definition: JMatrix4D.hh:440
JMATH::JMatrix4S
4 x 4 symmetric matrix
Definition: JMatrix4S.hh:26
JMATH
Auxiliary classes and methods for mathematical operations.
Definition: JCalculator.hh:9
JMATH::JMatrix4D::a03
double a03
Definition: JMatrix4D.hh:438
JMATH::JMatrix4D::a10
double a10
Definition: JMatrix4D.hh:439
JMATH::JMatrix4D::a31
double a31
Definition: JMatrix4D.hh:441
JMATH::JMatrix4D::a33
double a33
Definition: JMatrix4D.hh:441
JMATH::JMatrix4D::a32
double a32
Definition: JMatrix4D.hh:441
JMATH::JMatrix4D::a00
double a00
Definition: JMatrix4D.hh:438
JMATH::JMatrix4D::a13
double a13
Definition: JMatrix4D.hh:439
JLANG::JDivisionByZero
Exception for division by zero.
Definition: JException.hh:270
JMATH::JMatrix4D::a21
double a21
Definition: JMatrix4D.hh:440
JMATH::JMatrix4D::a12
double a12
Definition: JMatrix4D.hh:439
JMATH::JMatrix4S::JMatrix4S
JMatrix4S(const double __a00, const double __a01, const double __a02, const double __a03, const double __a11, const double __a12, const double __a13, const double __a22, const double __a23, const double __a33)
Contructor.
Definition: JMatrix4S.hh:62
JMATH::JMatrix4D::a01
double a01
Definition: JMatrix4D.hh:438
p1
TPaveText * p1
Definition: JDrawModule3D.cc:35
JMATH::JMatrix4D::a22
double a22
Definition: JMatrix4D.hh:440
JMatrix4D.hh