Jpp 19.3.0-rc.2
the software that should make you happy
Loading...
Searching...
No Matches
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
15namespace JMATH {}
16namespace JPP { using namespace JMATH; }
17
18namespace 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
TPaveText * p1
Exceptions.
Exception for division by zero.
4 x 4 matrix
Definition JMatrix4D.hh:36
4 x 4 symmetric matrix
Definition JMatrix4S.hh:28
JMatrix4S(const JMatrix4D &A)
Contructor.
Definition JMatrix4S.hh:43
void invert()
Invert matrix.
Definition JMatrix4S.hh:77
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
JMatrix4S()
Default constructor.
Definition JMatrix4S.hh:33
Auxiliary classes and methods for mathematical operations.
Definition JEigen3D.hh:88
This name space includes all other name spaces (except KM3NETDAQ, KM3NET and ANTARES).