GENFIT Rev: NoNumberAvailable
Loading...
Searching...
No Matches
RKTools.cc
Go to the documentation of this file.
1/* Copyright 2008-2010, Technische Universitaet Muenchen,
2 Authors: Christian Hoeppner & Sebastian Neubert & Johannes Rauch
3
4 This file is part of GENFIT.
5
6 GENFIT is free software: you can redistribute it and/or modify
7 it under the terms of the GNU Lesser General Public License as published
8 by the Free Software Foundation, either version 3 of the License, or
9 (at your option) any later version.
10
11 GENFIT is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 GNU Lesser General Public License for more details.
15
16 You should have received a copy of the GNU Lesser General Public License
17 along with GENFIT. If not, see <http://www.gnu.org/licenses/>.
18*/
19
20#include <RKTools.h>
21#include "IO.h"
22
23#include <TMatrixD.h>
24#include <TMatrixDSym.h>
25
26#include <iostream>
27
28namespace genfit {
29
30
31void RKTools::J_pMTxcov5xJ_pM(const M5x7& J_pM, const M5x5& cov5, M7x7& out7){
32 // 5D -> 7D
33
34 // J_pM
35 // 0 0 0 0 0 0 1
36 // 0 0 0 x x x 0
37 // 0 0 0 x x x 0
38 // x x x 0 0 0 0
39 // x x x 0 0 0 0
40
41 // it is assumed that J_pM is only non-zero here:
42 // [6] = 1
43
44 // [1*7+3]
45 // [1*7+4]
46 // [1*7+5]
47
48 // [2*7+3]
49 // [2*7+4]
50 // [2*7+5]
51
52 // [3*7+0]
53 // [3*7+1]
54 // [3*7+2]
55
56 // [4*7+0]
57 // [4*7+1]
58 // [4*7+2]
59
60 double JTC0 = J_pM[21] * cov5[18] + J_pM[28] * cov5[23];
61 double JTC1 = J_pM[21] * cov5[23] + J_pM[28] * cov5[24];
62 double JTC2 = J_pM[21] * cov5[16] + J_pM[28] * cov5[21];
63 double JTC3 = J_pM[21] * cov5[17] + J_pM[28] * cov5[22];
64 double JTC4 = J_pM[22] * cov5[18] + J_pM[29] * cov5[23];
65 double JTC5 = J_pM[22] * cov5[23] + J_pM[29] * cov5[24];
66 double JTC6 = J_pM[22] * cov5[16] + J_pM[29] * cov5[21];
67 double JTC7 = J_pM[22] * cov5[17] + J_pM[29] * cov5[22];
68 double JTC8 = J_pM[23] * cov5[18] + J_pM[30] * cov5[23];
69 double JTC9 = J_pM[23] * cov5[23] + J_pM[30] * cov5[24];
70 double JTC10 = J_pM[23] * cov5[16] + J_pM[30] * cov5[21];
71 double JTC11 = J_pM[23] * cov5[17] + J_pM[30] * cov5[22];
72 double JTC12 = J_pM[10] * cov5[6] + J_pM[17] * cov5[11];
73 double JTC13 = J_pM[10] * cov5[11] + J_pM[17] * cov5[12];
74 double JTC14 = J_pM[11] * cov5[6] + J_pM[18] * cov5[11];
75 double JTC15 = J_pM[11] * cov5[11] + J_pM[18] * cov5[12];
76
77 // loops are vectorizable by the compiler!
78 for (int i=0; i<3; ++i) out7[i] = JTC0 * J_pM[21+i] + JTC1 * J_pM[28+i];
79 for (int i=0; i<3; ++i) out7[3+i] = JTC2 * J_pM[10+i] + JTC3 * J_pM[17+i];
80 out7[6] = J_pM[21] * cov5[15] + J_pM[28] * cov5[20];
81
82 for (int i=0; i<2; ++i) out7[8+i] = JTC4 * J_pM[22+i] + JTC5 * J_pM[29+i];
83 for (int i=0; i<3; ++i) out7[10+i] = JTC6 * J_pM[10+i] + JTC7 * J_pM[17+i];
84 out7[13] = J_pM[22] * cov5[15] + J_pM[29] * cov5[20];
85
86 out7[16] = JTC8 * J_pM[23] + JTC9 * J_pM[30];
87 for (int i=0; i<3; ++i) out7[17+i] = JTC10 * J_pM[10+i] + JTC11 * J_pM[17+i];
88 out7[20] = J_pM[23] * cov5[15] + J_pM[30] * cov5[20];
89
90 for (int i=0; i<3; ++i) out7[24+i] = JTC12 * J_pM[10+i] + JTC13 * J_pM[17+i];
91 out7[27] = J_pM[10] * cov5[5] + J_pM[17] * cov5[10];
92
93 for (int i=0; i<2; ++i) out7[32+i] = JTC14 * J_pM[11+i] + JTC15 * J_pM[18+i];
94 out7[34] = J_pM[11] * cov5[5] + J_pM[18] * cov5[10];
95
96 out7[40] = (J_pM[12] * cov5[6] + J_pM[19] * cov5[11]) * J_pM[12] + (J_pM[12] * cov5[11] + J_pM[19] * cov5[12]) * J_pM[19];
97 out7[41] = J_pM[12] * cov5[5] + J_pM[19] * cov5[10];
98
99 out7[48] = cov5[0];
100
101 // symmetric part
102 out7[7] = out7[1];
103 out7[14] = out7[2]; out7[15] = out7[9];
104 out7[21] = out7[3]; out7[22] = out7[10]; out7[23] = out7[17];
105 out7[28] = out7[4]; out7[29] = out7[11]; out7[30] = out7[18]; out7[31] = out7[25];
106 out7[35] = out7[5]; out7[36] = out7[12]; out7[37] = out7[19]; out7[38] = out7[26]; out7[39] = out7[33];
107 out7[42] = out7[6]; out7[43] = out7[13]; out7[44] = out7[20]; out7[45] = out7[27]; out7[46] = out7[34]; out7[47] = out7[41];
108
109}
110
111
112void RKTools::J_pMTxcov5xJ_pM(const M5x6& J_pM, const M5x5& cov5, M6x6& out6){
113 // 5D -> 6D
114
115 // J_pM
116 // 0 0 0 x x x
117 // 0 0 0 x x x
118 // 0 0 0 x x x
119 // x x x 0 0 0
120 // x x x 0 0 0
121
122 // it is assumed that J_pM is only non-zero here:
123 // [3]
124 // [4]
125 // [5]
126
127 // [1*6+3]
128 // [1*6+4]
129 // [1*6+5]
130
131 // [2*6+3]
132 // [2*6+4]
133 // [2*6+5]
134
135 // [3*6+0]
136 // [3*6+1]
137 // [3*6+2]
138
139 // [4*6+0]
140 // [4*6+1]
141 // [4*6+2]
142
143 double JTC0 = J_pM[18] * cov5[15+3] + J_pM[24] * cov5[20+3];
144 double JTC1 = J_pM[18] * cov5[20+3] + J_pM[24] * cov5[20+4];
145 double JTC2 = J_pM[18] * cov5[15] + J_pM[24] * cov5[20];
146 double JTC3 = J_pM[18] * cov5[15+1] + J_pM[24] * cov5[20+1];
147 double JTC4 = J_pM[18] * cov5[15+2] + J_pM[24] * cov5[20+2];
148 double JTC5 = J_pM[18+1] * cov5[15+3] + J_pM[24+1] * cov5[20+3];
149 double JTC6 = J_pM[18+1] * cov5[20+3] + J_pM[24+1] * cov5[20+4];
150 double JTC7 = J_pM[18+1] * cov5[15] + J_pM[24+1] * cov5[20];
151 double JTC8 = J_pM[18+1] * cov5[15+1] + J_pM[24+1] * cov5[20+1];
152 double JTC9 = J_pM[18+1] * cov5[15+2] + J_pM[24+1] * cov5[20+2];
153 double JTC10 = J_pM[18+2] * cov5[15] + J_pM[24+2] * cov5[20];
154 double JTC11 = J_pM[18+2] * cov5[15+1] + J_pM[24+2] * cov5[20+1];
155 double JTC12 = J_pM[18+2] * cov5[15+2] + J_pM[24+2] * cov5[20+2];
156 double JTC13 = J_pM[3] * cov5[0*5] + J_pM[6+3] * cov5[5] + J_pM[12+3] * cov5[10];
157 double JTC14 = J_pM[3] * cov5[5] + J_pM[6+3] * cov5[5+1] + J_pM[12+3] * cov5[10+1];
158 double JTC15 = J_pM[3] * cov5[10] + J_pM[6+3] * cov5[10+1] + J_pM[12+3] * cov5[10+2];
159 double JTC16 = J_pM[4] * cov5[0*5] + J_pM[6+4] * cov5[5] + J_pM[12+4] * cov5[10];
160 double JTC17 = J_pM[4] * cov5[5] + J_pM[6+4] * cov5[5+1] + J_pM[12+4] * cov5[10+1];
161 double JTC18 = J_pM[4] * cov5[10] + J_pM[6+4] * cov5[10+1] + J_pM[12+4] * cov5[10+2];
162
163 // loops are vectorizable by the compiler!
164 for (int i=0; i<3; ++i) out6[i] = JTC0 * J_pM[18+i] + JTC1 * J_pM[24+i];
165 for (int i=0; i<3; ++i) out6[3+i] = JTC2 * J_pM[3+i] + JTC3 * J_pM[9+i] + JTC4 * J_pM[15+i];
166
167 for (int i=0; i<2; ++i) out6[7+i] = JTC5 * J_pM[19+i] + JTC6 * J_pM[25+i];
168 for (int i=0; i<3; ++i) out6[9+i] = JTC7 * J_pM[3+i] + JTC8 * J_pM[9+i] + JTC9 * J_pM[15+i];
169
170 out6[12+2] = (J_pM[18+2] * cov5[15+3] + J_pM[24+2] * cov5[20+3]) * J_pM[18+2] + (J_pM[18+2] * cov5[20+3] + J_pM[24+2] * cov5[20+4]) * J_pM[24+2];
171 for (int i=0; i<3; ++i) out6[15+i] = JTC10 * J_pM[3+i] + JTC11 * J_pM[9+i] + JTC12 * J_pM[15+i];
172
173 for (int i=0; i<3; ++i) out6[21+i] = JTC13 * J_pM[3+i] + JTC14 * J_pM[9+i] + JTC15 * J_pM[15+i];
174
175 for (int i=0; i<3; ++i) out6[28+i] = JTC16 * J_pM[4+i] + JTC17 * J_pM[10+i] + JTC18 * J_pM[16+i];
176
177 out6[30+5] = (J_pM[5] * cov5[0*5] + J_pM[6+5] * cov5[5] + J_pM[12+5] * cov5[10]) * J_pM[5] + (J_pM[5] * cov5[5] + J_pM[6+5] * cov5[5+1] + J_pM[12+5] * cov5[10+1]) * J_pM[6+5] + (J_pM[5] * cov5[10] + J_pM[6+5] * cov5[10+1] + J_pM[12+5] * cov5[10+2]) * J_pM[12+5];
178
179 // symmetric part
180 out6[6] = out6[1];
181 out6[12] = out6[2]; out6[12+1] = out6[6+2];
182 out6[18] = out6[3]; out6[18+1] = out6[6+3]; out6[18+2] = out6[12+3];
183 out6[24] = out6[4]; out6[24+1] = out6[6+4]; out6[24+2] = out6[12+4]; out6[24+3] = out6[18+4];
184 out6[30] = out6[5]; out6[30+1] = out6[6+5]; out6[30+2] = out6[12+5]; out6[30+3] = out6[18+5]; out6[30+4] = out6[24+5];
185
186}
187
188
189void RKTools::J_MpTxcov7xJ_Mp(const M7x5& J_Mp, const M7x7& cov7, M5x5& out5)
190{
191 // 7D -> 5D
192
193 // J_Mp
194 // 0 0 0 x x
195 // 0 0 0 x x
196 // 0 0 0 x x
197 // 0 x x 0 0
198 // 0 x x 0 0
199 // 0 x x 0 0
200 // 1 0 0 0 0
201
202 // it is assumed that J_Mp is only non-zero here:
203 // [3]
204 // [1*7+3]
205 // [2*7+3]
206
207 // [4]
208 // [1*7+4]
209 // [2*7+4]
210
211 // [3*7+1]
212 // [4*7+1]
213 // [5*7+1]
214
215 // [3*7+2]
216 // [4*7+2]
217 // [5*7+2]
218
219 // [6*7+0] = 1
220
221
222 double JTC0 = (J_Mp[16] * cov7[24] + J_Mp[21] * cov7[31] + J_Mp[26] * cov7[38]);
223 double JTC1 = (J_Mp[16] * cov7[31] + J_Mp[21] * cov7[32] + J_Mp[26] * cov7[39]);
224 double JTC2 = (J_Mp[16] * cov7[38] + J_Mp[21] * cov7[39] + J_Mp[26] * cov7[40]);
225 double JTC3 = (J_Mp[16] * cov7[21] + J_Mp[21] * cov7[28] + J_Mp[26] * cov7[35]);
226 double JTC4 = (J_Mp[16] * cov7[22] + J_Mp[21] * cov7[29] + J_Mp[26] * cov7[36]);
227 double JTC5 = (J_Mp[16] * cov7[23] + J_Mp[21] * cov7[30] + J_Mp[26] * cov7[37]);
228 double JTC6 = (J_Mp[17] * cov7[21] + J_Mp[22] * cov7[28] + J_Mp[27] * cov7[35]);
229 double JTC7 = (J_Mp[17] * cov7[22] + J_Mp[22] * cov7[29] + J_Mp[27] * cov7[36]);
230 double JTC8 = (J_Mp[17] * cov7[23] + J_Mp[22] * cov7[30] + J_Mp[27] * cov7[37]);
231 double JTC9 = (J_Mp[3] * cov7[0] + J_Mp[8] * cov7[7] + J_Mp[13] * cov7[14]);
232 double JTC10 = (J_Mp[3] * cov7[7] + J_Mp[8] * cov7[8] + J_Mp[13] * cov7[15]);
233 double JTC11 = (J_Mp[3] * cov7[14] + J_Mp[8] * cov7[15] + J_Mp[13] * cov7[16]);
234
235 out5[0] = cov7[48];
236 out5[1] = J_Mp[16] * cov7[45] + J_Mp[21] * cov7[46] + J_Mp[26] * cov7[47];
237 out5[2] = J_Mp[17] * cov7[45] + J_Mp[22] * cov7[46] + J_Mp[27] * cov7[47];
238 out5[3] = J_Mp[3] * cov7[42] + J_Mp[8] * cov7[43] + J_Mp[13] * cov7[44];
239 out5[4] = J_Mp[4] * cov7[42] + J_Mp[9] * cov7[43] + J_Mp[14] * cov7[44];
240
241 // loops are vectorizable by the compiler!
242 for (int i=0; i<2; ++i) out5[6+i] = JTC0 * J_Mp[16+i] + JTC1 * J_Mp[21+i] + JTC2 * J_Mp[26+i];
243 for (int i=0; i<2; ++i) out5[8+i] = JTC3 * J_Mp[3+i] + JTC4 * J_Mp[8+i] + JTC5 * J_Mp[13+i];
244
245 out5[12] = (J_Mp[17] * cov7[24] + J_Mp[22] * cov7[31] + J_Mp[27] * cov7[38]) * J_Mp[17] + (J_Mp[17] * cov7[31] + J_Mp[22] * cov7[32] + J_Mp[27] * cov7[39]) * J_Mp[22] + (J_Mp[17] * cov7[38] + J_Mp[22] * cov7[39] + J_Mp[27] * cov7[40]) * J_Mp[27];
246 for (int i=0; i<2; ++i) out5[13+i] = JTC6 * J_Mp[3+i] + JTC7 * J_Mp[8+i] + JTC8 * J_Mp[13+i];
247
248 for (int i=0; i<2; ++i) out5[18+i] = JTC9 * J_Mp[3+i] + JTC10 * J_Mp[8+i] + JTC11 * J_Mp[13+i];
249
250 out5[24] = (J_Mp[4] * cov7[0] + J_Mp[9] * cov7[7] + J_Mp[14] * cov7[14]) * J_Mp[4] + (J_Mp[4] * cov7[7] + J_Mp[9] * cov7[8] + J_Mp[14] * cov7[15]) * J_Mp[9] + (J_Mp[4] * cov7[14] + J_Mp[9] * cov7[15] + J_Mp[14] * cov7[16]) * J_Mp[14];
251
252 // symmetric part
253 out5[5] = out5[1];
254 out5[10] = out5[2]; out5[11] = out5[7];
255 out5[15] = out5[3]; out5[16] = out5[8]; out5[17] = out5[13];
256 out5[20] = out5[4]; out5[21] = out5[9]; out5[22] = out5[14]; out5[23] = out5[19];
257
258}
259
260
261void RKTools::J_MpTxcov6xJ_Mp(const M6x5& J_Mp, const M6x6& cov6, M5x5& out5)
262{
263 // 6D -> 5D
264
265 // J_Mp
266 // 0 0 0 x x
267 // 0 0 0 x x
268 // 0 0 0 x x
269 // x x x 0 0
270 // x x x 0 0
271 // x x x 0 0
272
273 // it is assumed that J_Mp is only non-zero here:
274 // [3]
275 // [1*6+3]
276 // [2*6+3]
277
278 // [4]
279 // [1*6+4]
280 // [2*6+4]
281
282 // [3*6+0]
283 // [4*6+0]
284 // [5*6+0]
285
286 // [3*6+1]
287 // [4*6+1]
288 // [5*6+1]
289
290 // [3*6+2]
291 // [4*6+2]
292 // [5*6+2]
293
294 double JTC0 = (J_Mp[15] * cov6[18+3] + J_Mp[20] * cov6[24+3] + J_Mp[25] * cov6[30+3]);
295 double JTC1 = (J_Mp[15] * cov6[24+3] + J_Mp[20] * cov6[24+4] + J_Mp[25] * cov6[30+4]);
296 double JTC2 = (J_Mp[15] * cov6[30+3] + J_Mp[20] * cov6[30+4] + J_Mp[25] * cov6[30+5]);
297 double JTC3 = (J_Mp[15] * cov6[18] + J_Mp[20] * cov6[24] + J_Mp[25] * cov6[30]);
298 double JTC4 = (J_Mp[15] * cov6[18+1] + J_Mp[20] * cov6[24+1] + J_Mp[25] * cov6[30+1]);
299 double JTC5 = (J_Mp[15] * cov6[18+2] + J_Mp[20] * cov6[24+2] + J_Mp[25] * cov6[30+2]);
300 double JTC6 = (J_Mp[15+1] * cov6[18+3] + J_Mp[20+1] * cov6[24+3] + J_Mp[25+1] * cov6[30+3]);
301 double JTC7 = (J_Mp[15+1] * cov6[24+3] + J_Mp[20+1] * cov6[24+4] + J_Mp[25+1] * cov6[30+4]);
302 double JTC8 = (J_Mp[15+1] * cov6[30+3] + J_Mp[20+1] * cov6[30+4] + J_Mp[25+1] * cov6[30+5]);
303 double JTC9 = (J_Mp[15+1] * cov6[18] + J_Mp[20+1] * cov6[24] + J_Mp[25+1] * cov6[30]);
304 double JTC10 = (J_Mp[15+1] * cov6[18+1] + J_Mp[20+1] * cov6[24+1] + J_Mp[25+1] * cov6[30+1]);
305 double JTC11 = (J_Mp[15+1] * cov6[18+2] + J_Mp[20+1] * cov6[24+2] + J_Mp[25+1] * cov6[30+2]);
306 double JTC12 = (J_Mp[15+2] * cov6[18] + J_Mp[20+2] * cov6[24] + J_Mp[25+2] * cov6[30]);
307 double JTC13 = (J_Mp[15+2] * cov6[18+1] + J_Mp[20+2] * cov6[24+1] + J_Mp[25+2] * cov6[30+1]);
308 double JTC14 = (J_Mp[15+2] * cov6[18+2] + J_Mp[20+2] * cov6[24+2] + J_Mp[25+2] * cov6[30+2]);
309 double JTC15 = (J_Mp[3] * cov6[0] + J_Mp[5+3] * cov6[6] + J_Mp[10+3] * cov6[12]);
310 double JTC16 = (J_Mp[3] * cov6[6] + J_Mp[5+3] * cov6[6+1] + J_Mp[10+3] * cov6[12+1]);
311 double JTC17 = (J_Mp[3] * cov6[12] + J_Mp[5+3] * cov6[12+1] + J_Mp[10+3] * cov6[12+2]);
312
313 // loops are vectorizable by the compiler!
314 for (int i=0; i<3; ++i) out5[i] = JTC0 * J_Mp[15+i] + JTC1 * J_Mp[20+i] + JTC2 * J_Mp[25+i];
315 for (int i=0; i<2; ++i) out5[3+i] = JTC3 * J_Mp[3+i] + JTC4 * J_Mp[8+i] + JTC5 * J_Mp[13+i];
316
317 for (int i=0; i<2; ++i) out5[6+i] = JTC6 * J_Mp[16+i] + JTC7 * J_Mp[21+i] + JTC8 * J_Mp[26+i];
318 for (int i=0; i<2; ++i) out5[8+i] = JTC9 * J_Mp[3+i] + JTC10 * J_Mp[8+i] + JTC11 * J_Mp[13+i];
319
320 out5[10+2] = (J_Mp[15+2] * cov6[18+3] + J_Mp[20+2] * cov6[24+3] + J_Mp[25+2] * cov6[30+3]) * J_Mp[15+2] + (J_Mp[15+2] * cov6[24+3] + J_Mp[20+2] * cov6[24+4] + J_Mp[25+2] * cov6[30+4]) * J_Mp[20+2] + (J_Mp[15+2] * cov6[30+3] + J_Mp[20+2] * cov6[30+4] + J_Mp[25+2] * cov6[30+5]) * J_Mp[25+2];
321 for (int i=0; i<2; ++i) out5[13+i] = JTC12 * J_Mp[3+i] + JTC13 * J_Mp[8+i] + JTC14 * J_Mp[13+i];
322
323 for (int i=0; i<2; ++i) out5[18+i] = JTC15 * J_Mp[3+i] + JTC16 * J_Mp[8+i] + JTC17 * J_Mp[13+i];
324
325 out5[20+4] = (J_Mp[4] * cov6[0] + J_Mp[5+4] * cov6[6] + J_Mp[10+4] * cov6[12]) * J_Mp[4] + (J_Mp[4] * cov6[6] + J_Mp[5+4] * cov6[6+1] + J_Mp[10+4] * cov6[12+1]) * J_Mp[5+4] + (J_Mp[4] * cov6[12] + J_Mp[5+4] * cov6[12+1] + J_Mp[10+4] * cov6[12+2]) * J_Mp[10+4];
326
327 // symmetric part
328 out5[5] = out5[1];
329 out5[10] = out5[2]; out5[10+1] = out5[5+2];
330 out5[15] = out5[3]; out5[15+1] = out5[5+3]; out5[15+2] = out5[10+3];
331 out5[20] = out5[4]; out5[20+1] = out5[5+4]; out5[20+2] = out5[10+4]; out5[20+3] = out5[15+4];
332
333}
334
335void RKTools::J_pMTTxJ_MMTTxJ_MpTT(const M7x5& J_pMT, const M7x7& J_MMT, const M5x7& J_MpT, M5x5& J_pp)
336{
337 // calculates J_pp = J_pM * J_MM * J_Mp
338 // input J_MMT is transposed version of actual jacobian J_MM
339 // input J_pMT is transposed version of actual jacobian J_pM (Master to plane)
340 // input J_MpT is transposed version of actual jacobian J_Mp (plane to Master)
341
342 // J_pMT
343 // 0 0 0 x x
344 // 0 0 0 x x
345 // 0 0 0 x x
346 // 0 x x 0 0
347 // 0 x x 0 0
348 // 0 x x 0 0
349 // 1 0 0 0 0
350
351 // J_MMT if MMproj == true
352 // x x x x x x 0
353 // x x x x x x 0
354 // x x x x x x 0
355 // x x x x x x 0
356 // x x x x x x 0
357 // x x x x x x 0
358 // x x x x x x x
359
360 // J_MpT
361 // 0 0 0 0 0 0 1
362 // 0 0 0 x x x 0
363 // 0 0 0 x x x 0
364 // x x x 0 0 0 0
365 // x x x 0 0 0 0
366
367
368 J_pp[0*5+0] = J_MMT[6*7+6];
369 J_pp[0*5+1] = 0;
370 J_pp[0*5+2] = 0;
371 J_pp[0*5+3] = 0;
372 J_pp[0*5+4] = 0;
373
374 J_pp[1*5+0] = J_pMT[3*5+1] * J_MMT[6*7+3] + J_pMT[4*5+1] * J_MMT[6*7+4] + J_pMT[5*5+1] * J_MMT[6*7+5];
375 J_pp[1*5+1] = ( (J_pMT[3*5+1] * J_MMT[3*7+3] + J_pMT[4*5+1] * J_MMT[3*7+4] + J_pMT[5*5+1] * J_MMT[3*7+5]) * J_MpT[1*7+3]
376 + (J_pMT[3*5+1] * J_MMT[4*7+3] + J_pMT[4*5+1] * J_MMT[4*7+4] + J_pMT[5*5+1] * J_MMT[4*7+5]) * J_MpT[1*7+4]
377 + (J_pMT[3*5+1] * J_MMT[5*7+3] + J_pMT[4*5+1] * J_MMT[5*7+4] + J_pMT[5*5+1] * J_MMT[5*7+5]) * J_MpT[1*7+5]);
378 J_pp[1*5+2] = ( (J_pMT[3*5+1] * J_MMT[3*7+3] + J_pMT[4*5+1] * J_MMT[3*7+4] + J_pMT[5*5+1] * J_MMT[3*7+5]) * J_MpT[2*7+3]
379 + (J_pMT[3*5+1] * J_MMT[4*7+3] + J_pMT[4*5+1] * J_MMT[4*7+4] + J_pMT[5*5+1] * J_MMT[4*7+5]) * J_MpT[2*7+4]
380 + (J_pMT[3*5+1] * J_MMT[5*7+3] + J_pMT[4*5+1] * J_MMT[5*7+4] + J_pMT[5*5+1] * J_MMT[5*7+5]) * J_MpT[2*7+5]);
381 J_pp[1*5+3] = ( (J_pMT[3*5+1] * J_MMT[0*7+3] + J_pMT[4*5+1] * J_MMT[0*7+4] + J_pMT[5*5+1] * J_MMT[0*7+5]) * J_MpT[3*7+0]
382 + (J_pMT[3*5+1] * J_MMT[1*7+3] + J_pMT[4*5+1] * J_MMT[1*7+4] + J_pMT[5*5+1] * J_MMT[1*7+5]) * J_MpT[3*7+1]
383 + (J_pMT[3*5+1] * J_MMT[2*7+3] + J_pMT[4*5+1] * J_MMT[2*7+4] + J_pMT[5*5+1] * J_MMT[2*7+5]) * J_MpT[3*7+2]);
384 J_pp[1*5+4] = ( (J_pMT[3*5+1] * J_MMT[0*7+3] + J_pMT[4*5+1] * J_MMT[0*7+4] + J_pMT[5*5+1] * J_MMT[0*7+5]) * J_MpT[4*7+0]
385 + (J_pMT[3*5+1] * J_MMT[1*7+3] + J_pMT[4*5+1] * J_MMT[1*7+4] + J_pMT[5*5+1] * J_MMT[1*7+5]) * J_MpT[4*7+1]
386 + (J_pMT[3*5+1] * J_MMT[2*7+3] + J_pMT[4*5+1] * J_MMT[2*7+4] + J_pMT[5*5+1] * J_MMT[2*7+5]) * J_MpT[4*7+2]);
387
388 J_pp[2*5+0] = J_pMT[3*5+2] * J_MMT[6*7+3] + J_pMT[4*5+2] * J_MMT[6*7+4] + J_pMT[5*5+2] * J_MMT[6*7+5];
389 J_pp[2*5+1] = ( (J_pMT[3*5+2] * J_MMT[3*7+3] + J_pMT[4*5+2] * J_MMT[3*7+4] + J_pMT[5*5+2] * J_MMT[3*7+5]) * J_MpT[1*7+3]
390 + (J_pMT[3*5+2] * J_MMT[4*7+3] + J_pMT[4*5+2] * J_MMT[4*7+4] + J_pMT[5*5+2] * J_MMT[4*7+5]) * J_MpT[1*7+4]
391 + (J_pMT[3*5+2] * J_MMT[5*7+3] + J_pMT[4*5+2] * J_MMT[5*7+4] + J_pMT[5*5+2] * J_MMT[5*7+5]) * J_MpT[1*7+5]);
392 J_pp[2*5+2] = ( (J_pMT[3*5+2] * J_MMT[3*7+3] + J_pMT[4*5+2] * J_MMT[3*7+4] + J_pMT[5*5+2] * J_MMT[3*7+5]) * J_MpT[2*7+3]
393 + (J_pMT[3*5+2] * J_MMT[4*7+3] + J_pMT[4*5+2] * J_MMT[4*7+4] + J_pMT[5*5+2] * J_MMT[4*7+5]) * J_MpT[2*7+4]
394 + (J_pMT[3*5+2] * J_MMT[5*7+3] + J_pMT[4*5+2] * J_MMT[5*7+4] + J_pMT[5*5+2] * J_MMT[5*7+5]) * J_MpT[2*7+5]);
395 J_pp[2*5+3] = ( (J_pMT[3*5+2] * J_MMT[0*7+3] + J_pMT[4*5+2] * J_MMT[0*7+4] + J_pMT[5*5+2] * J_MMT[0*7+5]) * J_MpT[3*7+0]
396 + (J_pMT[3*5+2] * J_MMT[1*7+3] + J_pMT[4*5+2] * J_MMT[1*7+4] + J_pMT[5*5+2] * J_MMT[1*7+5]) * J_MpT[3*7+1]
397 + (J_pMT[3*5+2] * J_MMT[2*7+3] + J_pMT[4*5+2] * J_MMT[2*7+4] + J_pMT[5*5+2] * J_MMT[2*7+5]) * J_MpT[3*7+2]);
398 J_pp[2*5+4] = ( (J_pMT[3*5+2] * J_MMT[0*7+3] + J_pMT[4*5+2] * J_MMT[0*7+4] + J_pMT[5*5+2] * J_MMT[0*7+5]) * J_MpT[4*7+0]
399 + (J_pMT[3*5+2] * J_MMT[1*7+3] + J_pMT[4*5+2] * J_MMT[1*7+4] + J_pMT[5*5+2] * J_MMT[1*7+5]) * J_MpT[4*7+1]
400 + (J_pMT[3*5+2] * J_MMT[2*7+3] + J_pMT[4*5+2] * J_MMT[2*7+4] + J_pMT[5*5+2] * J_MMT[2*7+5]) * J_MpT[4*7+2]);
401
402 J_pp[3*5+0] = J_pMT[0*5+3] * J_MMT[6*7+0] + J_pMT[1*5+3] * J_MMT[6*7+1] + J_pMT[2*5+3] * J_MMT[6*7+2];
403 J_pp[3*5+1] = ( (J_pMT[0*5+3] * J_MMT[3*7+0] + J_pMT[1*5+3] * J_MMT[3*7+1] + J_pMT[2*5+3] * J_MMT[3*7+2]) * J_MpT[1*7+3]
404 + (J_pMT[0*5+3] * J_MMT[4*7+0] + J_pMT[1*5+3] * J_MMT[4*7+1] + J_pMT[2*5+3] * J_MMT[4*7+2]) * J_MpT[1*7+4]
405 + (J_pMT[0*5+3] * J_MMT[5*7+0] + J_pMT[1*5+3] * J_MMT[5*7+1] + J_pMT[2*5+3] * J_MMT[5*7+2]) * J_MpT[1*7+5]);
406 J_pp[3*5+2] = ( (J_pMT[0*5+3] * J_MMT[3*7+0] + J_pMT[1*5+3] * J_MMT[3*7+1] + J_pMT[2*5+3] * J_MMT[3*7+2]) * J_MpT[2*7+3]
407 + (J_pMT[0*5+3] * J_MMT[4*7+0] + J_pMT[1*5+3] * J_MMT[4*7+1] + J_pMT[2*5+3] * J_MMT[4*7+2]) * J_MpT[2*7+4]
408 + (J_pMT[0*5+3] * J_MMT[5*7+0] + J_pMT[1*5+3] * J_MMT[5*7+1] + J_pMT[2*5+3] * J_MMT[5*7+2]) * J_MpT[2*7+5]);
409 J_pp[3*5+3] = ( (J_pMT[0*5+3] * J_MMT[0*7+0] + J_pMT[1*5+3] * J_MMT[0*7+1] + J_pMT[2*5+3] * J_MMT[0*7+2]) * J_MpT[3*7+0]
410 + (J_pMT[0*5+3] * J_MMT[1*7+0] + J_pMT[1*5+3] * J_MMT[1*7+1] + J_pMT[2*5+3] * J_MMT[1*7+2]) * J_MpT[3*7+1]
411 + (J_pMT[0*5+3] * J_MMT[2*7+0] + J_pMT[1*5+3] * J_MMT[2*7+1] + J_pMT[2*5+3] * J_MMT[2*7+2]) * J_MpT[3*7+2]);
412 J_pp[3*5+4] = ( (J_pMT[0*5+3] * J_MMT[0*7+0] + J_pMT[1*5+3] * J_MMT[0*7+1] + J_pMT[2*5+3] * J_MMT[0*7+2]) * J_MpT[4*7+0]
413 + (J_pMT[0*5+3] * J_MMT[1*7+0] + J_pMT[1*5+3] * J_MMT[1*7+1] + J_pMT[2*5+3] * J_MMT[1*7+2]) * J_MpT[4*7+1]
414 + (J_pMT[0*5+3] * J_MMT[2*7+0] + J_pMT[1*5+3] * J_MMT[2*7+1] + J_pMT[2*5+3] * J_MMT[2*7+2]) * J_MpT[4*7+2]);
415
416 J_pp[4*5+0] = J_pMT[0*5+4] * J_MMT[6*7+0] + J_pMT[1*5+4] * J_MMT[6*7+1] + J_pMT[2*5+4] * J_MMT[6*7+2];
417 J_pp[4*5+1] = ( (J_pMT[0*5+4] * J_MMT[3*7+0] + J_pMT[1*5+4] * J_MMT[3*7+1] + J_pMT[2*5+4] * J_MMT[3*7+2]) * J_MpT[1*7+3]
418 + (J_pMT[0*5+4] * J_MMT[4*7+0] + J_pMT[1*5+4] * J_MMT[4*7+1] + J_pMT[2*5+4] * J_MMT[4*7+2]) * J_MpT[1*7+4]
419 + (J_pMT[0*5+4] * J_MMT[5*7+0] + J_pMT[1*5+4] * J_MMT[5*7+1] + J_pMT[2*5+4] * J_MMT[5*7+2]) * J_MpT[1*7+5]);
420 J_pp[4*5+2] = ( (J_pMT[0*5+4] * J_MMT[3*7+0] + J_pMT[1*5+4] * J_MMT[3*7+1] + J_pMT[2*5+4] * J_MMT[3*7+2]) * J_MpT[2*7+3]
421 + (J_pMT[0*5+4] * J_MMT[4*7+0] + J_pMT[1*5+4] * J_MMT[4*7+1] + J_pMT[2*5+4] * J_MMT[4*7+2]) * J_MpT[2*7+4]
422 + (J_pMT[0*5+4] * J_MMT[5*7+0] + J_pMT[1*5+4] * J_MMT[5*7+1] + J_pMT[2*5+4] * J_MMT[5*7+2]) * J_MpT[2*7+5]);
423 J_pp[4*5+3] = ( (J_pMT[0*5+4] * J_MMT[0*7+0] + J_pMT[1*5+4] * J_MMT[0*7+1] + J_pMT[2*5+4] * J_MMT[0*7+2]) * J_MpT[3*7+0]
424 + (J_pMT[0*5+4] * J_MMT[1*7+0] + J_pMT[1*5+4] * J_MMT[1*7+1] + J_pMT[2*5+4] * J_MMT[1*7+2]) * J_MpT[3*7+1]
425 + (J_pMT[0*5+4] * J_MMT[2*7+0] + J_pMT[1*5+4] * J_MMT[2*7+1] + J_pMT[2*5+4] * J_MMT[2*7+2]) * J_MpT[3*7+2]);
426 J_pp[4*5+4] = ( (J_pMT[0*5+4] * J_MMT[0*7+0] + J_pMT[1*5+4] * J_MMT[0*7+1] + J_pMT[2*5+4] * J_MMT[0*7+2]) * J_MpT[4*7+0]
427 + (J_pMT[0*5+4] * J_MMT[1*7+0] + J_pMT[1*5+4] * J_MMT[1*7+1] + J_pMT[2*5+4] * J_MMT[1*7+2]) * J_MpT[4*7+1]
428 + (J_pMT[0*5+4] * J_MMT[2*7+0] + J_pMT[1*5+4] * J_MMT[2*7+1] + J_pMT[2*5+4] * J_MMT[2*7+2]) * J_MpT[4*7+2]);
429}
430
431
432void RKTools::Np_N_NpT(const M7x7& Np, M7x7& N) {
433
434 // N is symmetric
435
436 // Np
437 // x x x 0 0 0 0
438 // x x x 0 0 0 0
439 // x x x 0 0 0 0
440 // x x x 1 0 0 0
441 // x x x 0 1 0 0
442 // x x x 0 0 1 0
443 // 0 0 0 0 0 0 1
444
445 // calculate:
446 // Np * N * Np^T
447
448 double N00(N[0*7+0]), N11(N[1*7+1]), N22(N[2*7+2]), N33(N[3*7+3]), N44(N[4*7+4]), N55(N[5*7+5]);
449
450 // replace lower left triangle using the original values from upper right triangle plus the cached diagonal values
451 N[0*7+0] = (Np[0*7+0] * N00 + Np[0*7+1] * N[0*7+1] + Np[0*7+2] * N[0*7+2]) * Np[0*7+0] + (Np[0*7+0] * N[0*7+1] + Np[0*7+1] * N11 + Np[0*7+2] * N[1*7+2]) * Np[0*7+1] + (Np[0*7+0] * N[0*7+2] + Np[0*7+1] * N[1*7+2] + Np[0*7+2] * N22) * Np[0*7+2];
452
453 N[1*7+0] = (Np[1*7+0] * N00 + Np[1*7+1] * N[0*7+1] + Np[1*7+2] * N[0*7+2]) * Np[0*7+0] + (Np[1*7+0] * N[0*7+1] + Np[1*7+1] * N11 + Np[1*7+2] * N[1*7+2]) * Np[0*7+1] + (Np[1*7+0] * N[0*7+2] + Np[1*7+1] * N[1*7+2] + Np[1*7+2] * N22) * Np[0*7+2];
454 N[1*7+1] = (Np[1*7+0] * N00 + Np[1*7+1] * N[0*7+1] + Np[1*7+2] * N[0*7+2]) * Np[1*7+0] + (Np[1*7+0] * N[0*7+1] + Np[1*7+1] * N11 + Np[1*7+2] * N[1*7+2]) * Np[1*7+1] + (Np[1*7+0] * N[0*7+2] + Np[1*7+1] * N[1*7+2] + Np[1*7+2] * N22) * Np[1*7+2];
455
456 N[2*7+0] = (Np[2*7+0] * N00 + Np[2*7+1] * N[0*7+1] + Np[2*7+2] * N[0*7+2]) * Np[0*7+0] + (Np[2*7+0] * N[0*7+1] + Np[2*7+1] * N11 + Np[2*7+2] * N[1*7+2]) * Np[0*7+1] + (Np[2*7+0] * N[0*7+2] + Np[2*7+1] * N[1*7+2] + Np[2*7+2] * N22) * Np[0*7+2];
457 N[2*7+1] = (Np[2*7+0] * N00 + Np[2*7+1] * N[0*7+1] + Np[2*7+2] * N[0*7+2]) * Np[1*7+0] + (Np[2*7+0] * N[0*7+1] + Np[2*7+1] * N11 + Np[2*7+2] * N[1*7+2]) * Np[1*7+1] + (Np[2*7+0] * N[0*7+2] + Np[2*7+1] * N[1*7+2] + Np[2*7+2] * N22) * Np[1*7+2];
458 N[2*7+2] = (Np[2*7+0] * N00 + Np[2*7+1] * N[0*7+1] + Np[2*7+2] * N[0*7+2]) * Np[2*7+0] + (Np[2*7+0] * N[0*7+1] + Np[2*7+1] * N11 + Np[2*7+2] * N[1*7+2]) * Np[2*7+1] + (Np[2*7+0] * N[0*7+2] + Np[2*7+1] * N[1*7+2] + Np[2*7+2] * N22) * Np[2*7+2];
459
460 N[3*7+0] = (Np[3*7+0] * N00 + Np[3*7+1] * N[0*7+1] + Np[3*7+2] * N[0*7+2] + N[0*7+3]) * Np[0*7+0] + (Np[3*7+0] * N[0*7+1] + Np[3*7+1] * N11 + Np[3*7+2] * N[1*7+2] + N[1*7+3]) * Np[0*7+1] + (Np[3*7+0] * N[0*7+2] + Np[3*7+1] * N[1*7+2] + Np[3*7+2] * N22 + N[2*7+3]) * Np[0*7+2];
461 N[3*7+1] = (Np[3*7+0] * N00 + Np[3*7+1] * N[0*7+1] + Np[3*7+2] * N[0*7+2] + N[0*7+3]) * Np[1*7+0] + (Np[3*7+0] * N[0*7+1] + Np[3*7+1] * N11 + Np[3*7+2] * N[1*7+2] + N[1*7+3]) * Np[1*7+1] + (Np[3*7+0] * N[0*7+2] + Np[3*7+1] * N[1*7+2] + Np[3*7+2] * N22 + N[2*7+3]) * Np[1*7+2];
462 N[3*7+2] = (Np[3*7+0] * N00 + Np[3*7+1] * N[0*7+1] + Np[3*7+2] * N[0*7+2] + N[0*7+3]) * Np[2*7+0] + (Np[3*7+0] * N[0*7+1] + Np[3*7+1] * N11 + Np[3*7+2] * N[1*7+2] + N[1*7+3]) * Np[2*7+1] + (Np[3*7+0] * N[0*7+2] + Np[3*7+1] * N[1*7+2] + Np[3*7+2] * N22 + N[2*7+3]) * Np[2*7+2];
463 N[3*7+3] = (Np[3*7+0] * N00 + Np[3*7+1] * N[0*7+1] + Np[3*7+2] * N[0*7+2] + N[0*7+3]) * Np[3*7+0] + (Np[3*7+0] * N[0*7+1] + Np[3*7+1] * N11 + Np[3*7+2] * N[1*7+2] + N[1*7+3]) * Np[3*7+1] + (Np[3*7+0] * N[0*7+2] + Np[3*7+1] * N[1*7+2] + Np[3*7+2] * N22 + N[2*7+3]) * Np[3*7+2] + Np[3*7+0] * N[0*7+3] + Np[3*7+1] * N[1*7+3] + Np[3*7+2] * N[2*7+3] + N33;
464
465 N[4*7+0] = (Np[4*7+0] * N00 + Np[4*7+1] * N[0*7+1] + Np[4*7+2] * N[0*7+2] + N[0*7+4]) * Np[0*7+0] + (Np[4*7+0] * N[0*7+1] + Np[4*7+1] * N11 + Np[4*7+2] * N[1*7+2] + N[1*7+4]) * Np[0*7+1] + (Np[4*7+0] * N[0*7+2] + Np[4*7+1] * N[1*7+2] + Np[4*7+2] * N22 + N[2*7+4]) * Np[0*7+2];
466 N[4*7+1] = (Np[4*7+0] * N00 + Np[4*7+1] * N[0*7+1] + Np[4*7+2] * N[0*7+2] + N[0*7+4]) * Np[1*7+0] + (Np[4*7+0] * N[0*7+1] + Np[4*7+1] * N11 + Np[4*7+2] * N[1*7+2] + N[1*7+4]) * Np[1*7+1] + (Np[4*7+0] * N[0*7+2] + Np[4*7+1] * N[1*7+2] + Np[4*7+2] * N22 + N[2*7+4]) * Np[1*7+2];
467 N[4*7+2] = (Np[4*7+0] * N00 + Np[4*7+1] * N[0*7+1] + Np[4*7+2] * N[0*7+2] + N[0*7+4]) * Np[2*7+0] + (Np[4*7+0] * N[0*7+1] + Np[4*7+1] * N11 + Np[4*7+2] * N[1*7+2] + N[1*7+4]) * Np[2*7+1] + (Np[4*7+0] * N[0*7+2] + Np[4*7+1] * N[1*7+2] + Np[4*7+2] * N22 + N[2*7+4]) * Np[2*7+2];
468 N[4*7+3] = (Np[4*7+0] * N00 + Np[4*7+1] * N[0*7+1] + Np[4*7+2] * N[0*7+2] + N[0*7+4]) * Np[3*7+0] + (Np[4*7+0] * N[0*7+1] + Np[4*7+1] * N11 + Np[4*7+2] * N[1*7+2] + N[1*7+4]) * Np[3*7+1] + (Np[4*7+0] * N[0*7+2] + Np[4*7+1] * N[1*7+2] + Np[4*7+2] * N22 + N[2*7+4]) * Np[3*7+2] + Np[4*7+0] * N[0*7+3] + Np[4*7+1] * N[1*7+3] + Np[4*7+2] * N[2*7+3] + N[3*7+4];
469 N[4*7+4] = (Np[4*7+0] * N00 + Np[4*7+1] * N[0*7+1] + Np[4*7+2] * N[0*7+2] + N[0*7+4]) * Np[4*7+0] + (Np[4*7+0] * N[0*7+1] + Np[4*7+1] * N11 + Np[4*7+2] * N[1*7+2] + N[1*7+4]) * Np[4*7+1] + (Np[4*7+0] * N[0*7+2] + Np[4*7+1] * N[1*7+2] + Np[4*7+2] * N22 + N[2*7+4]) * Np[4*7+2] + Np[4*7+0] * N[0*7+4] + Np[4*7+1] * N[1*7+4] + Np[4*7+2] * N[2*7+4] + N44;
470
471 N[5*7+0] = (Np[5*7+0] * N00 + Np[5*7+1] * N[0*7+1] + Np[5*7+2] * N[0*7+2] + N[0*7+5]) * Np[0*7+0] + (Np[5*7+0] * N[0*7+1] + Np[5*7+1] * N11 + Np[5*7+2] * N[1*7+2] + N[1*7+5]) * Np[0*7+1] + (Np[5*7+0] * N[0*7+2] + Np[5*7+1] * N[1*7+2] + Np[5*7+2] * N22 + N[2*7+5]) * Np[0*7+2];
472 N[5*7+1] = (Np[5*7+0] * N00 + Np[5*7+1] * N[0*7+1] + Np[5*7+2] * N[0*7+2] + N[0*7+5]) * Np[1*7+0] + (Np[5*7+0] * N[0*7+1] + Np[5*7+1] * N11 + Np[5*7+2] * N[1*7+2] + N[1*7+5]) * Np[1*7+1] + (Np[5*7+0] * N[0*7+2] + Np[5*7+1] * N[1*7+2] + Np[5*7+2] * N22 + N[2*7+5]) * Np[1*7+2];
473 N[5*7+2] = (Np[5*7+0] * N00 + Np[5*7+1] * N[0*7+1] + Np[5*7+2] * N[0*7+2] + N[0*7+5]) * Np[2*7+0] + (Np[5*7+0] * N[0*7+1] + Np[5*7+1] * N11 + Np[5*7+2] * N[1*7+2] + N[1*7+5]) * Np[2*7+1] + (Np[5*7+0] * N[0*7+2] + Np[5*7+1] * N[1*7+2] + Np[5*7+2] * N22 + N[2*7+5]) * Np[2*7+2];
474 N[5*7+3] = (Np[5*7+0] * N00 + Np[5*7+1] * N[0*7+1] + Np[5*7+2] * N[0*7+2] + N[0*7+5]) * Np[3*7+0] + (Np[5*7+0] * N[0*7+1] + Np[5*7+1] * N11 + Np[5*7+2] * N[1*7+2] + N[1*7+5]) * Np[3*7+1] + (Np[5*7+0] * N[0*7+2] + Np[5*7+1] * N[1*7+2] + Np[5*7+2] * N22 + N[2*7+5]) * Np[3*7+2] + Np[5*7+0] * N[0*7+3] + Np[5*7+1] * N[1*7+3] + Np[5*7+2] * N[2*7+3] + N[3*7+5];
475 N[5*7+4] = (Np[5*7+0] * N00 + Np[5*7+1] * N[0*7+1] + Np[5*7+2] * N[0*7+2] + N[0*7+5]) * Np[4*7+0] + (Np[5*7+0] * N[0*7+1] + Np[5*7+1] * N11 + Np[5*7+2] * N[1*7+2] + N[1*7+5]) * Np[4*7+1] + (Np[5*7+0] * N[0*7+2] + Np[5*7+1] * N[1*7+2] + Np[5*7+2] * N22 + N[2*7+5]) * Np[4*7+2] + Np[5*7+0] * N[0*7+4] + Np[5*7+1] * N[1*7+4] + Np[5*7+2] * N[2*7+4] + N[4*7+5];
476 N[5*7+5] = (Np[5*7+0] * N00 + Np[5*7+1] * N[0*7+1] + Np[5*7+2] * N[0*7+2] + N[0*7+5]) * Np[5*7+0] + (Np[5*7+0] * N[0*7+1] + Np[5*7+1] * N11 + Np[5*7+2] * N[1*7+2] + N[1*7+5]) * Np[5*7+1] + (Np[5*7+0] * N[0*7+2] + Np[5*7+1] * N[1*7+2] + Np[5*7+2] * N22 + N[2*7+5]) * Np[5*7+2] + Np[5*7+0] * N[0*7+5] + Np[5*7+1] * N[1*7+5] + Np[5*7+2] * N[2*7+5] + N55;
477
478 N[6*7+0] = Np[0*7+0] * N[0*7+6] + Np[0*7+1] * N[1*7+6] + Np[0*7+2] * N[2*7+6];
479 N[6*7+1] = Np[1*7+0] * N[0*7+6] + Np[1*7+1] * N[1*7+6] + Np[1*7+2] * N[2*7+6];
480 N[6*7+2] = Np[2*7+0] * N[0*7+6] + Np[2*7+1] * N[1*7+6] + Np[2*7+2] * N[2*7+6];
481 N[6*7+3] = Np[3*7+0] * N[0*7+6] + Np[3*7+1] * N[1*7+6] + Np[3*7+2] * N[2*7+6] + N[3*7+6];
482 N[6*7+4] = Np[4*7+0] * N[0*7+6] + Np[4*7+1] * N[1*7+6] + Np[4*7+2] * N[2*7+6] + N[4*7+6];
483 N[6*7+5] = Np[5*7+0] * N[0*7+6] + Np[5*7+1] * N[1*7+6] + Np[5*7+2] * N[2*7+6] + N[5*7+6];
484 //N[6*7+6] = N[6*7+6]; remains unchanged
485
486
487 // copy the results from the lower left triangle to the upper right triangle as well
488 N[5*7+6] = N[6*7+5];
489 N[4*7+5] = N[5*7+4]; N[4*7+6] = N[6*7+4];
490 N[3*7+4] = N[4*7+3]; N[3*7+5] = N[5*7+3]; N[3*7+6] = N[6*7+3];
491 N[2*7+3] = N[3*7+2]; N[2*7+4] = N[4*7+2]; N[2*7+5] = N[5*7+2]; N[2*7+6] = N[6*7+2];
492 N[1*7+2] = N[2*7+1]; N[1*7+3] = N[3*7+1]; N[1*7+4] = N[4*7+1]; N[1*7+5] = N[5*7+1]; N[1*7+6] = N[6*7+1];
493 N[0*7+1] = N[1*7+0]; N[0*7+2] = N[2*7+0]; N[0*7+3] = N[3*7+0]; N[0*7+4] = N[4*7+0]; N[0*7+5] = N[5*7+0]; N[0*7+6] = N[6*7+0];
494}
495
496
497void RKTools::printDim(const double* mat, unsigned int dimX, unsigned int dimY){
498
499 printOut << dimX << " x " << dimY << " matrix as follows: \n";
500 for (unsigned int i=0; i< dimX; ++i){
501 for (unsigned int j=0; j< dimY; ++j){
502 printf(" %11.5g", mat[i*dimY+j]);
503 }
504 printOut<<"\n";
505 }
506 printOut<<std::endl;
507
508}
509
510
511} /* End of namespace genfit */
512
genfit::RKTools::J_pMTxcov5xJ_pM
void J_pMTxcov5xJ_pM(const M5x7 &J_pM, const M5x5 &cov5, M7x7 &out7)
Definition RKTools.cc:31
genfit::RKTools::J_MpTxcov6xJ_Mp
void J_MpTxcov6xJ_Mp(const M6x5 &J_Mp, const M6x6 &cov6, M5x5 &out5)
Definition RKTools.cc:261
genfit::RKTools::Np_N_NpT
void Np_N_NpT(const M7x7 &Np, M7x7 &N)
Definition RKTools.cc:432
genfit::RKTools::J_pMTTxJ_MMTTxJ_MpTT
void J_pMTTxJ_MMTTxJ_MpTT(const M7x5 &J_pMT, const M7x7 &J_MMT, const M5x7 &J_MpT, M5x5 &J_pp)
Definition RKTools.cc:335
genfit::RKTools::printDim
void printDim(const double *mat, unsigned int dimX, unsigned int dimY)
Definition RKTools.cc:497
genfit::RKTools::J_MpTxcov7xJ_Mp
void J_MpTxcov7xJ_Mp(const M7x5 &J_Mp, const M7x7 &cov7, M5x5 &out5)
Definition RKTools.cc:189
genfit
Defines for I/O streams used for error and debug printing.
Definition AbsFinitePlane.cc:22
genfit::M6x6
RKMatrix< 6, 6 > M6x6
Definition RKTools.h:70
genfit::M5x5
RKMatrix< 5, 5 > M5x5
Definition RKTools.h:69
genfit::M5x6
RKMatrix< 5, 6 > M5x6
Definition RKTools.h:74
genfit::M5x7
RKMatrix< 5, 7 > M5x7
Definition RKTools.h:75
genfit::M7x7
RKMatrix< 7, 7 > M7x7
Definition RKTools.h:71
genfit::M7x5
RKMatrix< 7, 5 > M7x5
Definition RKTools.h:73
genfit::printOut
std::ostream printOut
genfit::M6x5
RKMatrix< 6, 5 > M6x5
Definition RKTools.h:72
Generated by doxygen 1.13.2