GENFIT  Rev:NoNumberAvailable
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 
28 static const int flagSlowMatrix = 1 << 10; // Replace custom matrix multiplications with general equivalents
29 static const int debugFlags = 0;// | flagSlowMatrix;
30 
31 namespace genfit {
32 
33 
34 void RKTools::J_pMTxcov5xJ_pM(const M5x7& J_pM, const M5x5& cov5, M7x7& out7){
35  if (debugFlags & flagSlowMatrix) {
36  TMatrixD JpM(5, 7, (const double*)&J_pM);
37  TMatrixDSym J5(5, (const double*)&cov5);
38  J5.SimilarityT(JpM);
39  std::copy(J5.GetMatrixArray(), J5.GetMatrixArray() + 7*7, out7.begin());
40  return;
41  }
42 
43 
44  // 5D -> 7D
45 
46  // J_pM
47  // 0 0 0 0 0 0 1
48  // 0 0 0 x x x 0
49  // 0 0 0 x x x 0
50  // x x x 0 0 0 0
51  // x x x 0 0 0 0
52 
53  // it is assumed that J_pM is only non-zero here:
54  // [6] = 1
55 
56  // [1*7+3]
57  // [1*7+4]
58  // [1*7+5]
59 
60  // [2*7+3]
61  // [2*7+4]
62  // [2*7+5]
63 
64  // [3*7+0]
65  // [3*7+1]
66  // [3*7+2]
67 
68  // [4*7+0]
69  // [4*7+1]
70  // [4*7+2]
71 
72  double JTC0 = J_pM[21] * cov5[18] + J_pM[28] * cov5[23];
73  double JTC1 = J_pM[21] * cov5[23] + J_pM[28] * cov5[24];
74  double JTC2 = J_pM[21] * cov5[16] + J_pM[28] * cov5[21];
75  double JTC3 = J_pM[21] * cov5[17] + J_pM[28] * cov5[22];
76  double JTC4 = J_pM[22] * cov5[18] + J_pM[29] * cov5[23];
77  double JTC5 = J_pM[22] * cov5[23] + J_pM[29] * cov5[24];
78  double JTC6 = J_pM[22] * cov5[16] + J_pM[29] * cov5[21];
79  double JTC7 = J_pM[22] * cov5[17] + J_pM[29] * cov5[22];
80  double JTC8 = J_pM[23] * cov5[18] + J_pM[30] * cov5[23];
81  double JTC9 = J_pM[23] * cov5[23] + J_pM[30] * cov5[24];
82  double JTC10 = J_pM[23] * cov5[16] + J_pM[30] * cov5[21];
83  double JTC11 = J_pM[23] * cov5[17] + J_pM[30] * cov5[22];
84  double JTC12 = J_pM[10] * cov5[6] + J_pM[17] * cov5[11];
85  double JTC13 = J_pM[10] * cov5[11] + J_pM[17] * cov5[12];
86  double JTC14 = J_pM[11] * cov5[6] + J_pM[18] * cov5[11];
87  double JTC15 = J_pM[11] * cov5[11] + J_pM[18] * cov5[12];
88 
89  // loops are vectorizable by the compiler!
90  for (int i=0; i<3; ++i) out7[i] = JTC0 * J_pM[21+i] + JTC1 * J_pM[28+i];
91  for (int i=0; i<3; ++i) out7[3+i] = JTC2 * J_pM[10+i] + JTC3 * J_pM[17+i];
92  out7[6] = J_pM[21] * cov5[15] + J_pM[28] * cov5[20];
93 
94  for (int i=0; i<2; ++i) out7[8+i] = JTC4 * J_pM[22+i] + JTC5 * J_pM[29+i];
95  for (int i=0; i<3; ++i) out7[10+i] = JTC6 * J_pM[10+i] + JTC7 * J_pM[17+i];
96  out7[13] = J_pM[22] * cov5[15] + J_pM[29] * cov5[20];
97 
98  out7[16] = JTC8 * J_pM[23] + JTC9 * J_pM[30];
99  for (int i=0; i<3; ++i) out7[17+i] = JTC10 * J_pM[10+i] + JTC11 * J_pM[17+i];
100  out7[20] = J_pM[23] * cov5[15] + J_pM[30] * cov5[20];
101 
102  for (int i=0; i<3; ++i) out7[24+i] = JTC12 * J_pM[10+i] + JTC13 * J_pM[17+i];
103  out7[27] = J_pM[10] * cov5[5] + J_pM[17] * cov5[10];
104 
105  for (int i=0; i<2; ++i) out7[32+i] = JTC14 * J_pM[11+i] + JTC15 * J_pM[18+i];
106  out7[34] = J_pM[11] * cov5[5] + J_pM[18] * cov5[10];
107 
108  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];
109  out7[41] = J_pM[12] * cov5[5] + J_pM[19] * cov5[10];
110 
111  out7[48] = cov5[0];
112 
113  // symmetric part
114  out7[7] = out7[1];
115  out7[14] = out7[2]; out7[15] = out7[9];
116  out7[21] = out7[3]; out7[22] = out7[10]; out7[23] = out7[17];
117  out7[28] = out7[4]; out7[29] = out7[11]; out7[30] = out7[18]; out7[31] = out7[25];
118  out7[35] = out7[5]; out7[36] = out7[12]; out7[37] = out7[19]; out7[38] = out7[26]; out7[39] = out7[33];
119  out7[42] = out7[6]; out7[43] = out7[13]; out7[44] = out7[20]; out7[45] = out7[27]; out7[46] = out7[34]; out7[47] = out7[41];
120 
121 }
122 
123 
124 void RKTools::J_pMTxcov5xJ_pM(const M5x6& J_pM, const M5x5& cov5, M6x6& out6){
125  if (debugFlags & flagSlowMatrix) {
126  TMatrixD JpM(5, 6, (const double*)&J_pM);
127  TMatrixDSym J5(5, (const double*)&cov5);
128  J5.SimilarityT(JpM);
129  std::copy(J5.GetMatrixArray(), J5.GetMatrixArray() + 6*6, out6.begin());
130  return;
131  }
132 
133  // 5D -> 6D
134 
135  // J_pM
136  // 0 0 0 x x x
137  // 0 0 0 x x x
138  // 0 0 0 x x x
139  // x x x 0 0 0
140  // x x x 0 0 0
141 
142  // it is assumed that J_pM is only non-zero here:
143  // [3]
144  // [4]
145  // [5]
146 
147  // [1*6+3]
148  // [1*6+4]
149  // [1*6+5]
150 
151  // [2*6+3]
152  // [2*6+4]
153  // [2*6+5]
154 
155  // [3*6+0]
156  // [3*6+1]
157  // [3*6+2]
158 
159  // [4*6+0]
160  // [4*6+1]
161  // [4*6+2]
162 
163  double JTC0 = J_pM[18] * cov5[15+3] + J_pM[24] * cov5[20+3];
164  double JTC1 = J_pM[18] * cov5[20+3] + J_pM[24] * cov5[20+4];
165  double JTC2 = J_pM[18] * cov5[15] + J_pM[24] * cov5[20];
166  double JTC3 = J_pM[18] * cov5[15+1] + J_pM[24] * cov5[20+1];
167  double JTC4 = J_pM[18] * cov5[15+2] + J_pM[24] * cov5[20+2];
168  double JTC5 = J_pM[18+1] * cov5[15+3] + J_pM[24+1] * cov5[20+3];
169  double JTC6 = J_pM[18+1] * cov5[20+3] + J_pM[24+1] * cov5[20+4];
170  double JTC7 = J_pM[18+1] * cov5[15] + J_pM[24+1] * cov5[20];
171  double JTC8 = J_pM[18+1] * cov5[15+1] + J_pM[24+1] * cov5[20+1];
172  double JTC9 = J_pM[18+1] * cov5[15+2] + J_pM[24+1] * cov5[20+2];
173  double JTC10 = J_pM[18+2] * cov5[15] + J_pM[24+2] * cov5[20];
174  double JTC11 = J_pM[18+2] * cov5[15+1] + J_pM[24+2] * cov5[20+1];
175  double JTC12 = J_pM[18+2] * cov5[15+2] + J_pM[24+2] * cov5[20+2];
176  double JTC13 = J_pM[3] * cov5[0*5] + J_pM[6+3] * cov5[5] + J_pM[12+3] * cov5[10];
177  double JTC14 = J_pM[3] * cov5[5] + J_pM[6+3] * cov5[5+1] + J_pM[12+3] * cov5[10+1];
178  double JTC15 = J_pM[3] * cov5[10] + J_pM[6+3] * cov5[10+1] + J_pM[12+3] * cov5[10+2];
179  double JTC16 = J_pM[4] * cov5[0*5] + J_pM[6+4] * cov5[5] + J_pM[12+4] * cov5[10];
180  double JTC17 = J_pM[4] * cov5[5] + J_pM[6+4] * cov5[5+1] + J_pM[12+4] * cov5[10+1];
181  double JTC18 = J_pM[4] * cov5[10] + J_pM[6+4] * cov5[10+1] + J_pM[12+4] * cov5[10+2];
182 
183  // loops are vectorizable by the compiler!
184  for (int i=0; i<3; ++i) out6[i] = JTC0 * J_pM[18+i] + JTC1 * J_pM[24+i];
185  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];
186 
187  for (int i=0; i<2; ++i) out6[7+i] = JTC5 * J_pM[19+i] + JTC6 * J_pM[25+i];
188  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];
189 
190  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];
191  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];
192 
193  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];
194 
195  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];
196 
197  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];
198 
199  // symmetric part
200  out6[6] = out6[1];
201  out6[12] = out6[2]; out6[12+1] = out6[6+2];
202  out6[18] = out6[3]; out6[18+1] = out6[6+3]; out6[18+2] = out6[12+3];
203  out6[24] = out6[4]; out6[24+1] = out6[6+4]; out6[24+2] = out6[12+4]; out6[24+3] = out6[18+4];
204  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];
205 
206 }
207 
208 
209 void RKTools::J_MpTxcov7xJ_Mp(const M7x5& J_Mp, const M7x7& cov7, M5x5& out5)
210 {
211  if (debugFlags & flagSlowMatrix) {
212  TMatrixD JMp(7, 5, (const double*)&J_Mp);
213  TMatrixD J7(7, 7, (const double*)&cov7);
214  TMatrixD result(JMp, TMatrixD::kTransposeMult,
215  TMatrixD(J7, TMatrixD::kMult, JMp));
216  std::copy(result.GetMatrixArray(), result.GetMatrixArray() + 5*5, out5.begin());
217  return;
218  }
219 
220  // 7D -> 5D
221 
222  // J_Mp
223  // 0 0 0 x x
224  // 0 0 0 x x
225  // 0 0 0 x x
226  // 0 x x 0 0
227  // 0 x x 0 0
228  // 0 x x 0 0
229  // 1 0 0 0 0
230 
231  // it is assumed that J_Mp is only non-zero here:
232  // [3]
233  // [1*7+3]
234  // [2*7+3]
235 
236  // [4]
237  // [1*7+4]
238  // [2*7+4]
239 
240  // [3*7+1]
241  // [4*7+1]
242  // [5*7+1]
243 
244  // [3*7+2]
245  // [4*7+2]
246  // [5*7+2]
247 
248  // [6*7+0] = 1
249 
250 
251  double JTC0 = (J_Mp[16] * cov7[24] + J_Mp[21] * cov7[31] + J_Mp[26] * cov7[38]);
252  double JTC1 = (J_Mp[16] * cov7[31] + J_Mp[21] * cov7[32] + J_Mp[26] * cov7[39]);
253  double JTC2 = (J_Mp[16] * cov7[38] + J_Mp[21] * cov7[39] + J_Mp[26] * cov7[40]);
254  double JTC3 = (J_Mp[16] * cov7[21] + J_Mp[21] * cov7[28] + J_Mp[26] * cov7[35]);
255  double JTC4 = (J_Mp[16] * cov7[22] + J_Mp[21] * cov7[29] + J_Mp[26] * cov7[36]);
256  double JTC5 = (J_Mp[16] * cov7[23] + J_Mp[21] * cov7[30] + J_Mp[26] * cov7[37]);
257  double JTC6 = (J_Mp[17] * cov7[21] + J_Mp[22] * cov7[28] + J_Mp[27] * cov7[35]);
258  double JTC7 = (J_Mp[17] * cov7[22] + J_Mp[22] * cov7[29] + J_Mp[27] * cov7[36]);
259  double JTC8 = (J_Mp[17] * cov7[23] + J_Mp[22] * cov7[30] + J_Mp[27] * cov7[37]);
260  double JTC9 = (J_Mp[3] * cov7[0] + J_Mp[8] * cov7[7] + J_Mp[13] * cov7[14]);
261  double JTC10 = (J_Mp[3] * cov7[7] + J_Mp[8] * cov7[8] + J_Mp[13] * cov7[15]);
262  double JTC11 = (J_Mp[3] * cov7[14] + J_Mp[8] * cov7[15] + J_Mp[13] * cov7[16]);
263 
264  out5[0] = cov7[48];
265  out5[1] = J_Mp[16] * cov7[45] + J_Mp[21] * cov7[46] + J_Mp[26] * cov7[47];
266  out5[2] = J_Mp[17] * cov7[45] + J_Mp[22] * cov7[46] + J_Mp[27] * cov7[47];
267  out5[3] = J_Mp[3] * cov7[42] + J_Mp[8] * cov7[43] + J_Mp[13] * cov7[44];
268  out5[4] = J_Mp[4] * cov7[42] + J_Mp[9] * cov7[43] + J_Mp[14] * cov7[44];
269 
270  // loops are vectorizable by the compiler!
271  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];
272  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];
273 
274  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];
275  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];
276 
277  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];
278 
279  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];
280 
281  // symmetric part
282  out5[5] = out5[1];
283  out5[10] = out5[2]; out5[11] = out5[7];
284  out5[15] = out5[3]; out5[16] = out5[8]; out5[17] = out5[13];
285  out5[20] = out5[4]; out5[21] = out5[9]; out5[22] = out5[14]; out5[23] = out5[19];
286 
287 }
288 
289 
290 void RKTools::J_MpTxcov6xJ_Mp(const M6x5& J_Mp, const M6x6& cov6, M5x5& out5)
291 {
292  if (debugFlags & flagSlowMatrix) {
293  TMatrixD JMp(6, 5, (const double*)&J_Mp);
294  TMatrixDSym J7(6, (const double*)&cov6);
295  TMatrixD result(JMp, TMatrixD::kTransposeMult,
296  TMatrixD(J7, TMatrixD::kMult, JMp));
297  std::copy(result.GetMatrixArray(), result.GetMatrixArray() + 5*5, out5.begin());
298  return;
299  }
300 
301  // 6D -> 5D
302 
303  // J_Mp
304  // 0 0 0 x x
305  // 0 0 0 x x
306  // 0 0 0 x x
307  // x x x 0 0
308  // x x x 0 0
309  // x x x 0 0
310 
311  // it is assumed that J_Mp is only non-zero here:
312  // [3]
313  // [1*6+3]
314  // [2*6+3]
315 
316  // [4]
317  // [1*6+4]
318  // [2*6+4]
319 
320  // [3*6+0]
321  // [4*6+0]
322  // [5*6+0]
323 
324  // [3*6+1]
325  // [4*6+1]
326  // [5*6+1]
327 
328  // [3*6+2]
329  // [4*6+2]
330  // [5*6+2]
331 
332  double JTC0 = (J_Mp[15] * cov6[18+3] + J_Mp[20] * cov6[24+3] + J_Mp[25] * cov6[30+3]);
333  double JTC1 = (J_Mp[15] * cov6[24+3] + J_Mp[20] * cov6[24+4] + J_Mp[25] * cov6[30+4]);
334  double JTC2 = (J_Mp[15] * cov6[30+3] + J_Mp[20] * cov6[30+4] + J_Mp[25] * cov6[30+5]);
335  double JTC3 = (J_Mp[15] * cov6[18] + J_Mp[20] * cov6[24] + J_Mp[25] * cov6[30]);
336  double JTC4 = (J_Mp[15] * cov6[18+1] + J_Mp[20] * cov6[24+1] + J_Mp[25] * cov6[30+1]);
337  double JTC5 = (J_Mp[15] * cov6[18+2] + J_Mp[20] * cov6[24+2] + J_Mp[25] * cov6[30+2]);
338  double JTC6 = (J_Mp[15+1] * cov6[18+3] + J_Mp[20+1] * cov6[24+3] + J_Mp[25+1] * cov6[30+3]);
339  double JTC7 = (J_Mp[15+1] * cov6[24+3] + J_Mp[20+1] * cov6[24+4] + J_Mp[25+1] * cov6[30+4]);
340  double JTC8 = (J_Mp[15+1] * cov6[30+3] + J_Mp[20+1] * cov6[30+4] + J_Mp[25+1] * cov6[30+5]);
341  double JTC9 = (J_Mp[15+1] * cov6[18] + J_Mp[20+1] * cov6[24] + J_Mp[25+1] * cov6[30]);
342  double JTC10 = (J_Mp[15+1] * cov6[18+1] + J_Mp[20+1] * cov6[24+1] + J_Mp[25+1] * cov6[30+1]);
343  double JTC11 = (J_Mp[15+1] * cov6[18+2] + J_Mp[20+1] * cov6[24+2] + J_Mp[25+1] * cov6[30+2]);
344  double JTC12 = (J_Mp[15+2] * cov6[18] + J_Mp[20+2] * cov6[24] + J_Mp[25+2] * cov6[30]);
345  double JTC13 = (J_Mp[15+2] * cov6[18+1] + J_Mp[20+2] * cov6[24+1] + J_Mp[25+2] * cov6[30+1]);
346  double JTC14 = (J_Mp[15+2] * cov6[18+2] + J_Mp[20+2] * cov6[24+2] + J_Mp[25+2] * cov6[30+2]);
347  double JTC15 = (J_Mp[3] * cov6[0] + J_Mp[5+3] * cov6[6] + J_Mp[10+3] * cov6[12]);
348  double JTC16 = (J_Mp[3] * cov6[6] + J_Mp[5+3] * cov6[6+1] + J_Mp[10+3] * cov6[12+1]);
349  double JTC17 = (J_Mp[3] * cov6[12] + J_Mp[5+3] * cov6[12+1] + J_Mp[10+3] * cov6[12+2]);
350 
351  // loops are vectorizable by the compiler!
352  for (int i=0; i<3; ++i) out5[i] = JTC0 * J_Mp[15+i] + JTC1 * J_Mp[20+i] + JTC2 * J_Mp[25+i];
353  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];
354 
355  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];
356  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];
357 
358  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];
359  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];
360 
361  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];
362 
363  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];
364 
365  // symmetric part
366  out5[5] = out5[1];
367  out5[10] = out5[2]; out5[10+1] = out5[5+2];
368  out5[15] = out5[3]; out5[15+1] = out5[5+3]; out5[15+2] = out5[10+3];
369  out5[20] = out5[4]; out5[20+1] = out5[5+4]; out5[20+2] = out5[10+4]; out5[20+3] = out5[15+4];
370 
371 }
372 
373 
374 void RKTools::J_MMTxcov7xJ_MM(const M7x7& J_MM, M7x7& cov7){
375  if (debugFlags & flagSlowMatrix) {
376  TMatrixD JMM(7, 7, (const double*)&J_MM);
377  TMatrixDSym J7(7, (const double*)&cov7);
378  J7.SimilarityT(JMM);
379  std::copy(J7.GetMatrixArray(), J7.GetMatrixArray() + 7*7, cov7.begin());
380  return;
381  }
382 
383  // it is assumed that the last column of J_MM is [0,0,0,0,0,0,1]
384 
385  double JTC0 = J_MM[0] * cov7[0] + J_MM[7] * cov7[7] + J_MM[14] * cov7[14] + J_MM[21] * cov7[21] + J_MM[28] * cov7[28] + J_MM[35] * cov7[35] + J_MM[42] * cov7[42];
386  double JTC1 = J_MM[0] * cov7[7] + J_MM[7] * cov7[7+1] + J_MM[14] * cov7[14+1] + J_MM[21] * cov7[21+1] + J_MM[28] * cov7[28+1] + J_MM[35] * cov7[35+1] + J_MM[42] * cov7[42+1];
387  double JTC2 = J_MM[0] * cov7[14] + J_MM[7] * cov7[14+1] + J_MM[14] * cov7[14+2] + J_MM[21] * cov7[21+2] + J_MM[28] * cov7[28+2] + J_MM[35] * cov7[35+2] + J_MM[42] * cov7[42+2];
388  double JTC3 = J_MM[0] * cov7[21] + J_MM[7] * cov7[21+1] + J_MM[14] * cov7[21+2] + J_MM[21] * cov7[21+3] + J_MM[28] * cov7[28+3] + J_MM[35] * cov7[35+3] + J_MM[42] * cov7[42+3];
389  double JTC4 = J_MM[0] * cov7[28] + J_MM[7] * cov7[28+1] + J_MM[14] * cov7[28+2] + J_MM[21] * cov7[28+3] + J_MM[28] * cov7[28+4] + J_MM[35] * cov7[35+4] + J_MM[42] * cov7[42+4];
390  double JTC5 = J_MM[0] * cov7[35] + J_MM[7] * cov7[35+1] + J_MM[14] * cov7[35+2] + J_MM[21] * cov7[35+3] + J_MM[28] * cov7[35+4] + J_MM[35] * cov7[35+5] + J_MM[42] * cov7[42+5];
391  double JTC6 = J_MM[0] * cov7[42] + J_MM[7] * cov7[42+1] + J_MM[14] * cov7[42+2] + J_MM[21] * cov7[42+3] + J_MM[28] * cov7[42+4] + J_MM[35] * cov7[42+5] + J_MM[42] * cov7[42+6];
392 
393  double JTC7 = J_MM[1] * cov7[0] + J_MM[7+1] * cov7[7] + J_MM[14+1] * cov7[14] + J_MM[21+1] * cov7[21] + J_MM[28+1] * cov7[28] + J_MM[35+1] * cov7[35] + J_MM[42+1] * cov7[42];
394  double JTC8 = J_MM[1] * cov7[7] + J_MM[7+1] * cov7[7+1] + J_MM[14+1] * cov7[14+1] + J_MM[21+1] * cov7[21+1] + J_MM[28+1] * cov7[28+1] + J_MM[35+1] * cov7[35+1] + J_MM[42+1] * cov7[42+1];
395  double JTC9 = J_MM[1] * cov7[14] + J_MM[7+1] * cov7[14+1] + J_MM[14+1] * cov7[14+2] + J_MM[21+1] * cov7[21+2] + J_MM[28+1] * cov7[28+2] + J_MM[35+1] * cov7[35+2] + J_MM[42+1] * cov7[42+2];
396  double JTC10 = J_MM[1] * cov7[21] + J_MM[7+1] * cov7[21+1] + J_MM[14+1] * cov7[21+2] + J_MM[21+1] * cov7[21+3] + J_MM[28+1] * cov7[28+3] + J_MM[35+1] * cov7[35+3] + J_MM[42+1] * cov7[42+3];
397  double JTC11 = J_MM[1] * cov7[28] + J_MM[7+1] * cov7[28+1] + J_MM[14+1] * cov7[28+2] + J_MM[21+1] * cov7[28+3] + J_MM[28+1] * cov7[28+4] + J_MM[35+1] * cov7[35+4] + J_MM[42+1] * cov7[42+4];
398  double JTC12 = J_MM[1] * cov7[35] + J_MM[7+1] * cov7[35+1] + J_MM[14+1] * cov7[35+2] + J_MM[21+1] * cov7[35+3] + J_MM[28+1] * cov7[35+4] + J_MM[35+1] * cov7[35+5] + J_MM[42+1] * cov7[42+5];
399  double JTC13 = J_MM[1] * cov7[42] + J_MM[7+1] * cov7[42+1] + J_MM[14+1] * cov7[42+2] + J_MM[21+1] * cov7[42+3] + J_MM[28+1] * cov7[42+4] + J_MM[35+1] * cov7[42+5] + J_MM[42+1] * cov7[42+6];
400 
401  double JTC14 = J_MM[2] * cov7[0] + J_MM[7+2] * cov7[7] + J_MM[14+2] * cov7[14] + J_MM[21+2] * cov7[21] + J_MM[28+2] * cov7[28] + J_MM[35+2] * cov7[35] + J_MM[42+2] * cov7[42];
402  double JTC15 = J_MM[2] * cov7[7] + J_MM[7+2] * cov7[7+1] + J_MM[14+2] * cov7[14+1] + J_MM[21+2] * cov7[21+1] + J_MM[28+2] * cov7[28+1] + J_MM[35+2] * cov7[35+1] + J_MM[42+2] * cov7[42+1];
403  double JTC16 = J_MM[2] * cov7[14] + J_MM[7+2] * cov7[14+1] + J_MM[14+2] * cov7[14+2] + J_MM[21+2] * cov7[21+2] + J_MM[28+2] * cov7[28+2] + J_MM[35+2] * cov7[35+2] + J_MM[42+2] * cov7[42+2];
404  double JTC17 = J_MM[2] * cov7[21] + J_MM[7+2] * cov7[21+1] + J_MM[14+2] * cov7[21+2] + J_MM[21+2] * cov7[21+3] + J_MM[28+2] * cov7[28+3] + J_MM[35+2] * cov7[35+3] + J_MM[42+2] * cov7[42+3];
405  double JTC18 = J_MM[2] * cov7[28] + J_MM[7+2] * cov7[28+1] + J_MM[14+2] * cov7[28+2] + J_MM[21+2] * cov7[28+3] + J_MM[28+2] * cov7[28+4] + J_MM[35+2] * cov7[35+4] + J_MM[42+2] * cov7[42+4];
406  double JTC19 = J_MM[2] * cov7[35] + J_MM[7+2] * cov7[35+1] + J_MM[14+2] * cov7[35+2] + J_MM[21+2] * cov7[35+3] + J_MM[28+2] * cov7[35+4] + J_MM[35+2] * cov7[35+5] + J_MM[42+2] * cov7[42+5];
407  double JTC20 = J_MM[2] * cov7[42] + J_MM[7+2] * cov7[42+1] + J_MM[14+2] * cov7[42+2] + J_MM[21+2] * cov7[42+3] + J_MM[28+2] * cov7[42+4] + J_MM[35+2] * cov7[42+5] + J_MM[42+2] * cov7[42+6];
408 
409  double JTC21 = J_MM[3] * cov7[0] + J_MM[7+3] * cov7[7] + J_MM[14+3] * cov7[14] + J_MM[21+3] * cov7[21] + J_MM[28+3] * cov7[28] + J_MM[35+3] * cov7[35] + J_MM[42+3] * cov7[42];
410  double JTC22 = J_MM[3] * cov7[7] + J_MM[7+3] * cov7[7+1] + J_MM[14+3] * cov7[14+1] + J_MM[21+3] * cov7[21+1] + J_MM[28+3] * cov7[28+1] + J_MM[35+3] * cov7[35+1] + J_MM[42+3] * cov7[42+1];
411  double JTC23 = J_MM[3] * cov7[14] + J_MM[7+3] * cov7[14+1] + J_MM[14+3] * cov7[14+2] + J_MM[21+3] * cov7[21+2] + J_MM[28+3] * cov7[28+2] + J_MM[35+3] * cov7[35+2] + J_MM[42+3] * cov7[42+2];
412  double JTC24 = J_MM[3] * cov7[21] + J_MM[7+3] * cov7[21+1] + J_MM[14+3] * cov7[21+2] + J_MM[21+3] * cov7[21+3] + J_MM[28+3] * cov7[28+3] + J_MM[35+3] * cov7[35+3] + J_MM[42+3] * cov7[42+3];
413  double JTC25 = J_MM[3] * cov7[28] + J_MM[7+3] * cov7[28+1] + J_MM[14+3] * cov7[28+2] + J_MM[21+3] * cov7[28+3] + J_MM[28+3] * cov7[28+4] + J_MM[35+3] * cov7[35+4] + J_MM[42+3] * cov7[42+4];
414  double JTC26 = J_MM[3] * cov7[35] + J_MM[7+3] * cov7[35+1] + J_MM[14+3] * cov7[35+2] + J_MM[21+3] * cov7[35+3] + J_MM[28+3] * cov7[35+4] + J_MM[35+3] * cov7[35+5] + J_MM[42+3] * cov7[42+5];
415  double JTC27 = J_MM[3] * cov7[42] + J_MM[7+3] * cov7[42+1] + J_MM[14+3] * cov7[42+2] + J_MM[21+3] * cov7[42+3] + J_MM[28+3] * cov7[42+4] + J_MM[35+3] * cov7[42+5] + J_MM[42+3] * cov7[42+6];
416 
417  double JTC28 = J_MM[4] * cov7[0] + J_MM[7+4] * cov7[7] + J_MM[14+4] * cov7[14] + J_MM[21+4] * cov7[21] + J_MM[28+4] * cov7[28] + J_MM[35+4] * cov7[35] + J_MM[42+4] * cov7[42];
418  double JTC29 = J_MM[4] * cov7[7] + J_MM[7+4] * cov7[7+1] + J_MM[14+4] * cov7[14+1] + J_MM[21+4] * cov7[21+1] + J_MM[28+4] * cov7[28+1] + J_MM[35+4] * cov7[35+1] + J_MM[42+4] * cov7[42+1];
419  double JTC30 = J_MM[4] * cov7[14] + J_MM[7+4] * cov7[14+1] + J_MM[14+4] * cov7[14+2] + J_MM[21+4] * cov7[21+2] + J_MM[28+4] * cov7[28+2] + J_MM[35+4] * cov7[35+2] + J_MM[42+4] * cov7[42+2];
420  double JTC31 = J_MM[4] * cov7[21] + J_MM[7+4] * cov7[21+1] + J_MM[14+4] * cov7[21+2] + J_MM[21+4] * cov7[21+3] + J_MM[28+4] * cov7[28+3] + J_MM[35+4] * cov7[35+3] + J_MM[42+4] * cov7[42+3];
421  double JTC32 = J_MM[4] * cov7[28] + J_MM[7+4] * cov7[28+1] + J_MM[14+4] * cov7[28+2] + J_MM[21+4] * cov7[28+3] + J_MM[28+4] * cov7[28+4] + J_MM[35+4] * cov7[35+4] + J_MM[42+4] * cov7[42+4];
422  double JTC33 = J_MM[4] * cov7[35] + J_MM[7+4] * cov7[35+1] + J_MM[14+4] * cov7[35+2] + J_MM[21+4] * cov7[35+3] + J_MM[28+4] * cov7[35+4] + J_MM[35+4] * cov7[35+5] + J_MM[42+4] * cov7[42+5];
423  double JTC34 = J_MM[4] * cov7[42] + J_MM[7+4] * cov7[42+1] + J_MM[14+4] * cov7[42+2] + J_MM[21+4] * cov7[42+3] + J_MM[28+4] * cov7[42+4] + J_MM[35+4] * cov7[42+5] + J_MM[42+4] * cov7[42+6];
424 
425  double out7_40 = (J_MM[5] * cov7[0] + J_MM[7+5] * cov7[7] + J_MM[14+5] * cov7[14] + J_MM[21+5] * cov7[21] + J_MM[28+5] * cov7[28] + J_MM[35+5] * cov7[35] + J_MM[42+5] * cov7[42]) * J_MM[5] + (J_MM[5] * cov7[7] + J_MM[7+5] * cov7[7+1] + J_MM[14+5] * cov7[14+1] + J_MM[21+5] * cov7[21+1] + J_MM[28+5] * cov7[28+1] + J_MM[35+5] * cov7[35+1] + J_MM[42+5] * cov7[42+1]) * J_MM[7+5] + (J_MM[5] * cov7[14] + J_MM[7+5] * cov7[14+1] + J_MM[14+5] * cov7[14+2] + J_MM[21+5] * cov7[21+2] + J_MM[28+5] * cov7[28+2] + J_MM[35+5] * cov7[35+2] + J_MM[42+5] * cov7[42+2]) * J_MM[14+5] + (J_MM[5] * cov7[21] + J_MM[7+5] * cov7[21+1] + J_MM[14+5] * cov7[21+2] + J_MM[21+5] * cov7[21+3] + J_MM[28+5] * cov7[28+3] + J_MM[35+5] * cov7[35+3] + J_MM[42+5] * cov7[42+3]) * J_MM[21+5] + (J_MM[5] * cov7[28] + J_MM[7+5] * cov7[28+1] + J_MM[14+5] * cov7[28+2] + J_MM[21+5] * cov7[28+3] + J_MM[28+5] * cov7[28+4] + J_MM[35+5] * cov7[35+4] + J_MM[42+5] * cov7[42+4]) * J_MM[28+5] + (J_MM[5] * cov7[35] + J_MM[7+5] * cov7[35+1] + J_MM[14+5] * cov7[35+2] + J_MM[21+5] * cov7[35+3] + J_MM[28+5] * cov7[35+4] + J_MM[35+5] * cov7[35+5] + J_MM[42+5] * cov7[42+5]) * J_MM[35+5] + (J_MM[5] * cov7[42] + J_MM[7+5] * cov7[42+1] + J_MM[14+5] * cov7[42+2] + J_MM[21+5] * cov7[42+3] + J_MM[28+5] * cov7[42+4] + J_MM[35+5] * cov7[42+5] + J_MM[42+5] * cov7[42+6]) * J_MM[42+5];
426 
427  // last row
428  cov7[6] = JTC6;
429  cov7[7+6] = JTC13;
430  cov7[14+6] = JTC20;
431  cov7[21+6] = JTC27;
432  cov7[28+6] = JTC34;
433  cov7[35+6] = J_MM[5] * cov7[42] + J_MM[7+5] * cov7[42+1] + J_MM[14+5] * cov7[42+2] + J_MM[21+5] * cov7[42+3] + J_MM[28+5] * cov7[42+4] + J_MM[35+5] * cov7[42+5] + J_MM[42+5] * cov7[42+6];
434  //cov7[42+6] = cov7[42+6];
435 
436  // loops are vectorizable by the compiler!
437  for (int i=0; i<6; ++i) cov7[i] = JTC0 * J_MM[i] + JTC1 * J_MM[7+i] + JTC2 * J_MM[14+i] + JTC3 * J_MM[21+i] + JTC4 * J_MM[28+i] + JTC5 * J_MM[35+i] + JTC6 * J_MM[42+i];
438  for (int i=1; i<6; ++i) cov7[7+i] = JTC7 * J_MM[i] + JTC8 * J_MM[7+i] + JTC9 * J_MM[14+i] + JTC10 * J_MM[21+i] + JTC11 * J_MM[28+i] + JTC12 * J_MM[35+i] + JTC13 * J_MM[42+i];
439  for (int i=2; i<6; ++i) cov7[14+i] = JTC14 * J_MM[i] + JTC15 * J_MM[7+i] + JTC16 * J_MM[14+i] + JTC17 * J_MM[21+i] + JTC18 * J_MM[28+i] + JTC19 * J_MM[35+i] + JTC20 * J_MM[42+i];
440  for (int i=3; i<6; ++i) cov7[21+i] = JTC21 * J_MM[i] + JTC22 * J_MM[7+i] + JTC23 * J_MM[14+i] + JTC24 * J_MM[21+i] + JTC25 * J_MM[28+i] + JTC26 * J_MM[35+i] + JTC27 * J_MM[42+i];
441  for (int i=4; i<6; ++i) cov7[28+i] = JTC28 * J_MM[i] + JTC29 * J_MM[7+i] + JTC30 * J_MM[14+i] + JTC31 * J_MM[21+i] + JTC32 * J_MM[28+i] + JTC33 * J_MM[35+i] + JTC34 * J_MM[42+i];
442  cov7[35+5] = out7_40;
443 
444  // symmetric part
445  cov7[7] = cov7[1];
446  cov7[14] = cov7[2]; cov7[14+1] = cov7[9];
447  cov7[21] = cov7[3]; cov7[21+1] = cov7[10]; cov7[21+2] = cov7[17];
448  cov7[28] = cov7[4]; cov7[28+1] = cov7[11]; cov7[28+2] = cov7[18]; cov7[28+3] = cov7[25];
449  cov7[35] = cov7[5]; cov7[35+1] = cov7[12]; cov7[35+2] = cov7[19]; cov7[35+3] = cov7[26]; cov7[35+4] = cov7[33];
450  cov7[42] = cov7[6]; cov7[42+1] = cov7[13]; cov7[42+2] = cov7[20]; cov7[42+3] = cov7[27]; cov7[42+4] = cov7[34]; cov7[42+5] = cov7[41];
451 
452 }
453 
454 
455 void RKTools::J_MMxJ_MM(M7x7& J_MM, const M7x7& J_MM_old){
456 
457  // J and J_old are
458  // 1 0 0 0 0 0 0
459  // 0 1 0 0 0 0 0
460  // 0 0 1 0 0 0 0
461  // x x x x x x 0
462  // x x x x x x 0
463  // x x x x x x 0
464  // x x x x x x x
465 
466  M7x7 J_MM_temp = J_MM;
467 
468  /*J_MM[0] = 1;
469  J_MM[1] = 0;
470  J_MM[2] = 0;
471  J_MM[3] = 0;
472  J_MM[4] = 0;
473  J_MM[5] = 0;
474  J_MM[6] = 0;
475 
476  J_MM[1*7+0] = 0;
477  J_MM[1*7+1] = 1;
478  J_MM[1*7+2] = 0;
479  J_MM[1*7+3] = 0;
480  J_MM[1*7+4] = 0;
481  J_MM[1*7+5] = 0;
482  J_MM[1*7+6] = 0;
483 
484  J_MM[2*7+0] = 0;
485  J_MM[2*7+1] = 0;
486  J_MM[2*7+2] = 1;
487  J_MM[2*7+3] = 0;
488  J_MM[2*7+4] = 0;
489  J_MM[2*7+5] = 0;
490  J_MM[2*7+6] = 0;*/
491 
492  J_MM[21] = J_MM_old[21] + J_MM_old[21+3] * J_MM_temp[21] + J_MM_old[21+4] * J_MM_temp[28] + J_MM_old[21+5] * J_MM_temp[35];
493  J_MM[21+1] = J_MM_old[21+1] + J_MM_old[21+3] * J_MM_temp[21+1] + J_MM_old[21+4] * J_MM_temp[28+1] + J_MM_old[21+5] * J_MM_temp[35+1];
494  J_MM[21+2] = J_MM_old[21+2] + J_MM_old[21+3] * J_MM_temp[21+2] + J_MM_old[21+4] * J_MM_temp[28+2] + J_MM_old[21+5] * J_MM_temp[35+2];
495  J_MM[21+3] = J_MM_old[21+3] * J_MM_temp[21+3] + J_MM_old[21+4] * J_MM_temp[28+3] + J_MM_old[21+5] * J_MM_temp[35+3];
496  J_MM[21+4] = J_MM_old[21+3] * J_MM_temp[21+4] + J_MM_old[21+4] * J_MM_temp[28+4] + J_MM_old[21+5] * J_MM_temp[35+4];
497  J_MM[21+5] = J_MM_old[21+3] * J_MM_temp[21+5] + J_MM_old[21+4] * J_MM_temp[28+5] + J_MM_old[21+5] * J_MM_temp[35+5];
498  //J_MM[21+6] = 0;
499 
500  J_MM[28] = J_MM_old[28] + J_MM_old[28+3] * J_MM_temp[21] + J_MM_old[28+4] * J_MM_temp[28] + J_MM_old[28+5] * J_MM_temp[35];
501  J_MM[28+1] = J_MM_old[28+1] + J_MM_old[28+3] * J_MM_temp[21+1] + J_MM_old[28+4] * J_MM_temp[28+1] + J_MM_old[28+5] * J_MM_temp[35+1];
502  J_MM[28+2] = J_MM_old[28+2] + J_MM_old[28+3] * J_MM_temp[21+2] + J_MM_old[28+4] * J_MM_temp[28+2] + J_MM_old[28+5] * J_MM_temp[35+2];
503  J_MM[28+3] = J_MM_old[28+3] * J_MM_temp[21+3] + J_MM_old[28+4] * J_MM_temp[28+3] + J_MM_old[28+5] * J_MM_temp[35+3];
504  J_MM[28+4] = J_MM_old[28+3] * J_MM_temp[21+4] + J_MM_old[28+4] * J_MM_temp[28+4] + J_MM_old[28+5] * J_MM_temp[35+4];
505  J_MM[28+5] = J_MM_old[28+3] * J_MM_temp[21+5] + J_MM_old[28+4] * J_MM_temp[28+5] + J_MM_old[28+5] * J_MM_temp[35+5];
506  //J_MM[28+6] = 0;
507 
508  J_MM[35] = J_MM_old[35] + J_MM_old[35+3] * J_MM_temp[21] + J_MM_old[35+4] * J_MM_temp[28] + J_MM_old[35+5] * J_MM_temp[35] ;
509  J_MM[35+1] = J_MM_old[35+1] + J_MM_old[35+3] * J_MM_temp[21+1] + J_MM_old[35+4] * J_MM_temp[28+1] + J_MM_old[35+5] * J_MM_temp[35+1];
510  J_MM[35+2] = J_MM_old[35+2] + J_MM_old[35+3] * J_MM_temp[21+2] + J_MM_old[35+4] * J_MM_temp[28+2] + J_MM_old[35+5] * J_MM_temp[35+2];
511  J_MM[35+3] = J_MM_old[35+3] * J_MM_temp[21+3] + J_MM_old[35+4] * J_MM_temp[28+3] + J_MM_old[35+5] * J_MM_temp[35+3];
512  J_MM[35+4] = J_MM_old[35+3] * J_MM_temp[21+4] + J_MM_old[35+4] * J_MM_temp[28+4] + J_MM_old[35+5] * J_MM_temp[35+4];
513  J_MM[35+5] = J_MM_old[35+3] * J_MM_temp[21+5] + J_MM_old[35+4] * J_MM_temp[28+5] + J_MM_old[35+5] * J_MM_temp[35+5];
514  //J_MM[35+6] = 0;
515 
516  J_MM[42] = J_MM_old[42] + J_MM_old[42+3] * J_MM_temp[21] + J_MM_old[42+4] * J_MM_temp[28] + J_MM_old[42+5] * J_MM_temp[35] + J_MM_old[42+6] * J_MM_temp[42];
517  J_MM[42+1] = J_MM_old[42+1] + J_MM_old[42+3] * J_MM_temp[21+1] + J_MM_old[42+4] * J_MM_temp[28+1] + J_MM_old[42+5] * J_MM_temp[35+1] + J_MM_old[42+6] * J_MM_temp[42+1];
518  J_MM[42+2] = J_MM_old[42+2] + J_MM_old[42+3] * J_MM_temp[21+2] + J_MM_old[42+4] * J_MM_temp[28+2] + J_MM_old[42+5] * J_MM_temp[35+2] + J_MM_old[42+6] * J_MM_temp[42+2];
519  J_MM[42+3] = J_MM_old[42+3] * J_MM_temp[21+3] + J_MM_old[42+4] * J_MM_temp[28+3] + J_MM_old[42+5] * J_MM_temp[35+3] + J_MM_old[42+6] * J_MM_temp[42+3];
520  J_MM[42+4] = J_MM_old[42+3] * J_MM_temp[21+4] + J_MM_old[42+4] * J_MM_temp[28+4] + J_MM_old[42+5] * J_MM_temp[35+4] + J_MM_old[42+6] * J_MM_temp[42+4];
521  J_MM[42+5] = J_MM_old[42+3] * J_MM_temp[21+5] + J_MM_old[42+4] * J_MM_temp[28+5] + J_MM_old[42+5] * J_MM_temp[35+5] + J_MM_old[42+6] * J_MM_temp[42+5];
522  J_MM[42+6] = J_MM_old[42+6] * J_MM_temp[42+6];
523 
524 }
525 
526 
527 void RKTools::J_pMTTxJ_MMTTxJ_MpTT(const M7x5& J_pMT, const M7x7& J_MMT, const M5x7& J_MpT, M5x5& J_pp)
528 {
529  // calculates J_pp = J_pM * J_MM * J_Mp
530  // input J_MMT is transposed version of actual jacobian J_MM
531  // input J_pMT is transposed version of actual jacobian J_pM (Master to plane)
532  // input J_MpT is transposed version of actual jacobian J_Mp (plane to Master)
533 
534  if (debugFlags & flagSlowMatrix) {
535  TMatrixD JpMT(7, 5, (const double*)&J_pMT);
536  TMatrixD J7(7, 7, (const double*)&J_MMT);
537  TMatrixD JMpT(5, 7, (const double*)&J_MpT);
538  TMatrixD result(TMatrixD::kTransposed,
539  TMatrixD(JMpT, TMatrixD::kMult,
540  TMatrixD(J7, TMatrixD::kMult, JpMT)));
541 
542  std::copy(result.GetMatrixArray(), result.GetMatrixArray() + 5*5, J_pp.begin());
543  return;
544  }
545 
546 
547  // J_pMT
548  // 0 0 0 x x
549  // 0 0 0 x x
550  // 0 0 0 x x
551  // 0 x x 0 0
552  // 0 x x 0 0
553  // 0 x x 0 0
554  // 1 0 0 0 0
555 
556  // J_MMT if MMproj == true
557  // x x x x x x 0
558  // x x x x x x 0
559  // x x x x x x 0
560  // x x x x x x 0
561  // x x x x x x 0
562  // x x x x x x 0
563  // x x x x x x x
564 
565  // J_MpT
566  // 0 0 0 0 0 0 1
567  // 0 0 0 x x x 0
568  // 0 0 0 x x x 0
569  // x x x 0 0 0 0
570  // x x x 0 0 0 0
571 
572 
573  J_pp[0*5+0] = J_MMT[6*7+6];
574  J_pp[0*5+1] = 0;
575  J_pp[0*5+2] = 0;
576  J_pp[0*5+3] = 0;
577  J_pp[0*5+4] = 0;
578 
579  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];
580  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]
581  + (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]
582  + (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]);
583  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]
584  + (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]
585  + (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]);
586  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]
587  + (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]
588  + (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]);
589  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]
590  + (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]
591  + (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]);
592 
593  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];
594  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]
595  + (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]
596  + (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]);
597  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]
598  + (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]
599  + (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]);
600  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]
601  + (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]
602  + (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]);
603  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]
604  + (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]
605  + (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]);
606 
607  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];
608  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]
609  + (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]
610  + (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]);
611  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]
612  + (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]
613  + (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]);
614  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]
615  + (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]
616  + (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]);
617  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]
618  + (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]
619  + (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]);
620 
621  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];
622  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]
623  + (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]
624  + (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]);
625  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]
626  + (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]
627  + (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]);
628  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]
629  + (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]
630  + (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]);
631  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]
632  + (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]
633  + (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]);
634 }
635 
636 
637 void RKTools::Np_N_NpT(const M7x7& Np, M7x7& N) {
638 
639  // N is symmetric
640 
641  // Np
642  // x x x 0 0 0 0
643  // x x x 0 0 0 0
644  // x x x 0 0 0 0
645  // x x x 1 0 0 0
646  // x x x 0 1 0 0
647  // x x x 0 0 1 0
648  // 0 0 0 0 0 0 1
649 
650  // calculate:
651  // Np * N * Np^T
652 
653  if (debugFlags & flagSlowMatrix) {
654  TMatrixDSym n(7,N.begin());
655  TMatrixD np(7,7,Np.begin());
656  n.Similarity(np);
657  std::copy(n.GetMatrixArray(), n.GetMatrixArray() + 7*7, N.begin());
658  return;
659  }
660 
661  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]);
662 
663  // replace lower left triangle using the original values from upper right triangle plus the cached diagonal values
664  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];
665 
666  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];
667  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];
668 
669  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];
670  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];
671  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];
672 
673  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];
674  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];
675  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];
676  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;
677 
678  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];
679  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];
680  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];
681  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];
682  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;
683 
684  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];
685  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];
686  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];
687  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];
688  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];
689  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;
690 
691  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];
692  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];
693  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];
694  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];
695  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];
696  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];
697  //N[6*7+6] = N[6*7+6]; remains unchanged
698 
699 
700  // copy the results from the lower left triangle to the upper right triangle as well
701  N[5*7+6] = N[6*7+5];
702  N[4*7+5] = N[5*7+4]; N[4*7+6] = N[6*7+4];
703  N[3*7+4] = N[4*7+3]; N[3*7+5] = N[5*7+3]; N[3*7+6] = N[6*7+3];
704  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];
705  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];
706  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];
707 }
708 
709 
710 void RKTools::printDim(const double* mat, unsigned int dimX, unsigned int dimY){
711 
712  printOut << dimX << " x " << dimY << " matrix as follows: \n";
713  for (unsigned int i=0; i< dimX; ++i){
714  for (unsigned int j=0; j< dimY; ++j){
715  printf(" %11.5g", mat[i*dimY+j]);
716  }
717  printOut<<"\n";
718  }
719  printOut<<std::endl;
720 
721 }
722 
723 
724 } /* End of namespace genfit */
725 
RKTools.h
genfit::RKMatrix
Definition: RKTools.h:33
genfit::printOut
std::ostream printOut
flagSlowMatrix
static const int flagSlowMatrix
Definition: RKTools.cc:28
debugFlags
static const int debugFlags
Definition: RKTools.cc:29
genfit
Defines for I/O streams used for error and debug printing.
Definition: AbsFinitePlane.cc:22
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:527
genfit::RKTools::J_MpTxcov6xJ_Mp
void J_MpTxcov6xJ_Mp(const M6x5 &J_Mp, const M6x6 &cov6, M5x5 &out5)
Definition: RKTools.cc:290
genfit::RKTools::J_MMxJ_MM
void J_MMxJ_MM(M7x7 &J_MM, const M7x7 &J_MM_old)
Definition: RKTools.cc:455
genfit::RKTools::printDim
void printDim(const double *mat, unsigned int dimX, unsigned int dimY)
Definition: RKTools.cc:710
IO.h
genfit::RKTools::J_MpTxcov7xJ_Mp
void J_MpTxcov7xJ_Mp(const M7x5 &J_Mp, const M7x7 &cov7, M5x5 &out5)
Definition: RKTools.cc:209
genfit::RKTools::Np_N_NpT
void Np_N_NpT(const M7x7 &Np, M7x7 &N)
Definition: RKTools.cc:637
genfit::RKTools::J_MMTxcov7xJ_MM
void J_MMTxcov7xJ_MM(const M7x7 &J_MM, M7x7 &cov7)
Definition: RKTools.cc:374
genfit::RKMatrix::begin
double * begin()
Definition: RKTools.h:45
genfit::RKTools::J_pMTxcov5xJ_pM
void J_pMTxcov5xJ_pM(const M5x7 &J_pM, const M5x5 &cov5, M7x7 &out7)
Definition: RKTools.cc:34