Jpp  18.0.0
the software that should make you happy
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
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.\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  */
63  JMatrix4S(const double __a00,
64  const double __a10, const double __a11,
65  const double __a20, const double __a21, const double __a22,
66  const double __a30, const double __a31, const double __a32, const double __a33) :
67  JMatrix4D(__a00, __a10, __a20, __a30,
68  __a10, __a11, __a21, __a31,
69  __a20, __a21, __a22, __a32,
70  __a30, __a31, __a32, __a33)
71  {}
72 
73 
74  /**
75  * Invert matrix.
76  */
77  void invert()
78  {
79  using std::swap;
80 
81  // LDU decomposition
82 
83  int p0 = 0; // permute row 0
84  int p1 = 0; // permute row 1
85  int p2 = 0; // permute row 2
86 
87  double val;
88 
89  val = a00;
90 
91  if (fabs(a10) > fabs(val)) {
92  p0 = 1;
93  val = a10;
94  }
95 
96  if (fabs(a20) > fabs(val)) {
97  p0 = 2;
98  val = a20;
99  }
100 
101  if (fabs(a30) > fabs(val)) {
102  p0 = 3;
103  val = a30;
104  }
105 
106  switch (p0) {
107 
108  case 1:
109  swap(a00,a10);
110  swap(a01,a11);
111  swap(a02,a12);
112  swap(a03,a13);
113  break;
114 
115  case 2:
116  swap(a00,a20);
117  swap(a01,a21);
118  swap(a02,a22);
119  swap(a03,a23);
120  break;
121 
122  case 3:
123  swap(a00,a30);
124  swap(a01,a31);
125  swap(a02,a32);
126  swap(a03,a33);
127  break;
128  }
129 
130  if (val == 0) {
131  throw JDivisionByZero("LDU decomposition zero pivot");
132  }
133 
134  a10 /= val;
135  a11 -= a10 * a01;
136  a12 -= a10 * a02;
137  a13 -= a10 * a03;
138 
139  a20 /= val;
140  a21 -= a20 * a01;
141  a22 -= a20 * a02;
142  a23 -= a20 * a03;
143 
144  a30 /= val;
145  a31 -= a30 * a01;
146  a32 -= a30 * a02;
147  a33 -= a30 * a03;
148 
149  val = a11;
150 
151  if (fabs(a21) > fabs(val)) {
152  p1 = 2;
153  val = a21;
154  }
155 
156  if (fabs(a31) > fabs(val)) {
157  p1 = 3;
158  val = a31;
159  }
160 
161  switch (p1) {
162 
163  case 2:
164  swap(a10,a20);
165  swap(a11,a21);
166  swap(a12,a22);
167  swap(a13,a23);
168  break;
169 
170  case 3:
171  swap(a10,a30);
172  swap(a11,a31);
173  swap(a12,a32);
174  swap(a13,a33);
175  break;
176  }
177 
178  if (val == 0) {
179  throw JDivisionByZero("LDU decomposition zero pivot");
180  }
181 
182  a21 /= val;
183  a22 -= a21 * a12;
184  a23 -= a21 * a13;
185 
186  a31 /= val;
187  a32 -= a31 * a12;
188  a33 -= a31 * a13;
189 
190  val = a22;
191 
192  if (fabs(a32) > fabs(val)) {
193  p2 = 3;
194  val = a32;
195  }
196 
197  switch (p2) {
198 
199  case 3:
200  swap(a20,a30);
201  swap(a21,a31);
202  swap(a22,a32);
203  swap(a23,a33);
204  break;
205  }
206 
207  if (val == 0) {
208  throw JDivisionByZero("LDU decomposition zero pivot");
209  }
210 
211  a32 /= val;
212  a33 -= a32 * a23;
213 
214  // invert D
215 
216  if (a33 == 0) {
217  throw JDivisionByZero("D matrix not invertable");
218  }
219 
220  a00 = 1.0 / a00;
221  a11 = 1.0 / a11;
222  a22 = 1.0 / a22;
223  a33 = 1.0 / a33;
224 
225  // invert U
226 
227  a01 *= -a00;
228  a01 *= a11;
229 
230  a02 *= -a00;
231  a02 -= a01 * a12;
232  a02 *= a22;
233 
234  a03 *= -a00;
235  a03 -= a01 * a13;
236  a03 -= a02 * a23;
237  a03 *= a33;
238 
239  a12 *= -a11;
240  a12 *= a22;
241 
242  a13 *= -a11;
243  a13 -= a12 * a23;
244  a13 *= a33;
245 
246  a23 *= -a22;
247  a23 *= a33;
248 
249  // invert L
250 
251  a32 = -a32;
252 
253  a31 = -a31;
254  a31 -= a32 * a21;
255 
256  a30 = -a30;
257  a30 -= a31 * a10;
258  a30 -= a32 * a20;
259 
260  a21 = -a21;
261  a20 = -a20;
262  a20 -= a21 * a10;
263  a10 = -a10;
264 
265  // U^-1 x L^-1
266 
267  a00 += a01 * a10 + a02 * a20 + a03 * a30;
268  a01 += a02 * a21 + a03 * a31;
269  a02 += a03 * a32;
270 
271  a10 *= a11;
272  a10 += a12 * a20 + a13 * a30;
273  a11 += a12 * a21 + a13 * a31;
274  a12 += a13 * a32;
275 
276  a20 *= a22;
277  a20 += a23 * a30;
278  a21 *= a22;
279  a21 += a23 * a31;
280  a22 += a23 * a32;
281 
282  a30 *= a33;
283  a31 *= a33;
284  a32 *= a33;
285 
286  switch (p2) {
287 
288  case 3:
289  swap(a02,a03);
290  swap(a12,a13);
291  swap(a22,a23);
292  swap(a32,a33);
293  break;
294  }
295 
296  switch (p1) {
297 
298  case 2:
299  swap(a01,a02);
300  swap(a11,a12);
301  swap(a21,a22);
302  swap(a31,a32);
303  break;
304 
305  case 3:
306  swap(a01,a03);
307  swap(a11,a13);
308  swap(a21,a23);
309  swap(a31,a33);
310  break;
311  }
312 
313  switch (p0) {
314 
315  case 1:
316  swap(a00,a01);
317  swap(a10,a11);
318  swap(a20,a21);
319  swap(a30,a31);
320  break;
321 
322  case 2:
323  swap(a00,a02);
324  swap(a10,a12);
325  swap(a20,a22);
326  swap(a30,a32);
327  break;
328 
329  case 3:
330  swap(a00,a03);
331  swap(a10,a13);
332  swap(a20,a23);
333  swap(a30,a33);
334  break;
335  }
336  }
337  };
338 }
339 
340 #endif
JMatrix4S()
Default constructor.
Definition: JMatrix4S.hh:33
Exceptions.
TPaveText * p1
void invert()
Invert matrix.
Definition: JMatrix4S.hh:77
4 x 4 matrix
Definition: JMatrix4D.hh:33
JMatrix4S(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)
Contructor.
Definition: JMatrix4S.hh:63
p2
Definition: module-Z:fit.sh:74
Exception for division by zero.
Definition: JException.hh:270
JMatrix4S(const JMatrix4D &A)
Contructor.
Definition: JMatrix4S.hh:43
source $JPP_DIR setenv csh $JPP_DIR &dev null eval JShellParser o a A
4 x 4 symmetric matrix
Definition: JMatrix4S.hh:26