trackReps/src/RKTools.cc

0001 /* Copyright 2008-2010, Technische Universitaet Muenchen,
0002    Authors: Christian Hoeppner & Sebastian Neubert & Johannes Rauch
0003
0004    This file is part of GENFIT.
0005
0006    GENFIT is free software: you can redistribute it and/or modify
0007    it under the terms of the GNU Lesser General Public License as published
0008    by the Free Software Foundation, either version 3 of the License, or
0009    (at your option) any later version.
0010
0011    GENFIT is distributed in the hope that it will be useful,
0012    but WITHOUT ANY WARRANTY; without even the implied warranty of
0013    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0014    GNU Lesser General Public License for more details.
0015
0016    You should have received a copy of the GNU Lesser General Public License
0017    along with GENFIT.  If not, see <http://www.gnu.org/licenses/>.
0018 */
0019
0020 #include <RKTools.h>
0021 #include "IO.h"
0022
0023 #include <TMatrixD.h>
0024 #include <TMatrixDSym.h>
0025
0026 #include <iostream>
0027
0028 namespace genfit {
0029
0030
0031 void RKTools::J_pMTxcov5xJ_pM(const M5x7& J_pM, const M5x5& cov5, M7x7& out7){
0032   // 5D -> 7D
0033
0034   // J_pM
0035   // 0 0 0 0 0 0 1
0036   // 0 0 0 x x x 0
0037   // 0 0 0 x x x 0
0038   // x x x 0 0 0 0
0039   // x x x 0 0 0 0
0040
0041   // it is assumed that J_pM is only non-zero here:
0042   // [6] = 1
0043
0044   // [1*7+3]
0045   // [1*7+4]
0046   // [1*7+5]
0047
0048   // [2*7+3]
0049   // [2*7+4]
0050   // [2*7+5]
0051
0052   // [3*7+0]
0053   // [3*7+1]
0054   // [3*7+2]
0055
0056   // [4*7+0]
0057   // [4*7+1]
0058   // [4*7+2]
0059
0060   double JTC0  = J_pM[21] * cov5[18] + J_pM[28] * cov5[23];
0061   double JTC1  = J_pM[21] * cov5[23] + J_pM[28] * cov5[24];
0062   double JTC2  = J_pM[21] * cov5[16] + J_pM[28] * cov5[21];
0063   double JTC3  = J_pM[21] * cov5[17] + J_pM[28] * cov5[22];
0064   double JTC4  = J_pM[22] * cov5[18] + J_pM[29] * cov5[23];
0065   double JTC5  = J_pM[22] * cov5[23] + J_pM[29] * cov5[24];
0066   double JTC6  = J_pM[22] * cov5[16] + J_pM[29] * cov5[21];
0067   double JTC7  = J_pM[22] * cov5[17] + J_pM[29] * cov5[22];
0068   double JTC8  = J_pM[23] * cov5[18] + J_pM[30] * cov5[23];
0069   double JTC9  = J_pM[23] * cov5[23] + J_pM[30] * cov5[24];
0070   double JTC10 = J_pM[23] * cov5[16] + J_pM[30] * cov5[21];
0071   double JTC11 = J_pM[23] * cov5[17] + J_pM[30] * cov5[22];
0072   double JTC12 = J_pM[10] * cov5[6]  + J_pM[17] * cov5[11];
0073   double JTC13 = J_pM[10] * cov5[11] + J_pM[17] * cov5[12];
0074   double JTC14 = J_pM[11] * cov5[6]  + J_pM[18] * cov5[11];
0075   double JTC15 = J_pM[11] * cov5[11] + J_pM[18] * cov5[12];
0076
0077   // loops are vectorizable by the compiler!
0078   for (int i=0; i<3; ++i) out7[i]  = JTC0 * J_pM[21+i] + JTC1 * J_pM[28+i];
0079   for (int i=0; i<3; ++i) out7[3+i]  = JTC2 * J_pM[10+i] + JTC3 * J_pM[17+i];
0080   out7[6]  =  J_pM[21] * cov5[15] + J_pM[28] * cov5[20];
0081
0082   for (int i=0; i<2; ++i) out7[8+i]  = JTC4 * J_pM[22+i] + JTC5 * J_pM[29+i];
0083   for (int i=0; i<3; ++i) out7[10+i] = JTC6 * J_pM[10+i] + JTC7 * J_pM[17+i];
0084   out7[13] = J_pM[22] * cov5[15] + J_pM[29] * cov5[20];
0085
0086   out7[16] = JTC8 * J_pM[23] + JTC9 * J_pM[30];
0087   for (int i=0; i<3; ++i) out7[17+i] = JTC10 * J_pM[10+i] + JTC11 * J_pM[17+i];
0088   out7[20] = J_pM[23] * cov5[15] + J_pM[30] * cov5[20];
0089
0090   for (int i=0; i<3; ++i) out7[24+i] = JTC12 * J_pM[10+i] + JTC13 * J_pM[17+i];
0091   out7[27] = J_pM[10] * cov5[5] + J_pM[17] * cov5[10];
0092
0093   for (int i=0; i<2; ++i) out7[32+i] = JTC14 * J_pM[11+i] + JTC15 * J_pM[18+i];
0094   out7[34] = J_pM[11] * cov5[5] + J_pM[18] * cov5[10];
0095
0096   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];
0097   out7[41] = J_pM[12] * cov5[5] + J_pM[19] * cov5[10];
0098
0099   out7[48] = cov5[0];
0100
0101   // symmetric part
0102   out7[7]  = out7[1];
0103   out7[14] = out7[2];  out7[15] = out7[9];
0104   out7[21] = out7[3];  out7[22] = out7[10];  out7[23] = out7[17];
0105   out7[28] = out7[4];  out7[29] = out7[11];  out7[30] = out7[18];  out7[31] = out7[25];
0106   out7[35] = out7[5];  out7[36] = out7[12];  out7[37] = out7[19];  out7[38] = out7[26];  out7[39] = out7[33];
0107   out7[42] = out7[6];  out7[43] = out7[13];  out7[44] = out7[20];  out7[45] = out7[27];  out7[46] = out7[34];  out7[47] = out7[41];
0108
0109 }
0110
0111
0112 void RKTools::J_pMTxcov5xJ_pM(const M5x6& J_pM, const M5x5& cov5, M6x6& out6){
0113   // 5D -> 6D
0114
0115   // J_pM
0116   // 0 0 0 x x x
0117   // 0 0 0 x x x
0118   // 0 0 0 x x x
0119   // x x x 0 0 0
0120   // x x x 0 0 0
0121
0122   // it is assumed that J_pM is only non-zero here:
0123   // [3]
0124   // [4]
0125   // [5]
0126
0127   // [1*6+3]
0128   // [1*6+4]
0129   // [1*6+5]
0130
0131   // [2*6+3]
0132   // [2*6+4]
0133   // [2*6+5]
0134
0135   // [3*6+0]
0136   // [3*6+1]
0137   // [3*6+2]
0138
0139   // [4*6+0]
0140   // [4*6+1]
0141   // [4*6+2]
0142
0143   double JTC0  = J_pM[18] * cov5[15+3] + J_pM[24] * cov5[20+3];
0144   double JTC1  = J_pM[18] * cov5[20+3] + J_pM[24] * cov5[20+4];
0145   double JTC2  = J_pM[18] * cov5[15] + J_pM[24] * cov5[20];
0146   double JTC3  = J_pM[18] * cov5[15+1] + J_pM[24] * cov5[20+1];
0147   double JTC4  = J_pM[18] * cov5[15+2] + J_pM[24] * cov5[20+2];
0148   double JTC5  = J_pM[18+1] * cov5[15+3] + J_pM[24+1] * cov5[20+3];
0149   double JTC6  = J_pM[18+1] * cov5[20+3] + J_pM[24+1] * cov5[20+4];
0150   double JTC7  = J_pM[18+1] * cov5[15] + J_pM[24+1] * cov5[20];
0151   double JTC8  = J_pM[18+1] * cov5[15+1] + J_pM[24+1] * cov5[20+1];
0152   double JTC9  = J_pM[18+1] * cov5[15+2] + J_pM[24+1] * cov5[20+2];
0153   double JTC10 = J_pM[18+2] * cov5[15] + J_pM[24+2] * cov5[20];
0154   double JTC11 = J_pM[18+2] * cov5[15+1] + J_pM[24+2] * cov5[20+1];
0155   double JTC12 = J_pM[18+2] * cov5[15+2] + J_pM[24+2] * cov5[20+2];
0156   double JTC13 = J_pM[3] * cov5[0*5] + J_pM[6+3] * cov5[5] + J_pM[12+3] * cov5[10];
0157   double JTC14 = J_pM[3] * cov5[5] + J_pM[6+3] * cov5[5+1] + J_pM[12+3] * cov5[10+1];
0158   double JTC15 = J_pM[3] * cov5[10] + J_pM[6+3] * cov5[10+1] + J_pM[12+3] * cov5[10+2];
0159   double JTC16 = J_pM[4] * cov5[0*5] + J_pM[6+4] * cov5[5] + J_pM[12+4] * cov5[10];
0160   double JTC17 = J_pM[4] * cov5[5] + J_pM[6+4] * cov5[5+1] + J_pM[12+4] * cov5[10+1];
0161   double JTC18 = J_pM[4] * cov5[10] + J_pM[6+4] * cov5[10+1] + J_pM[12+4] * cov5[10+2];
0162
0163   // loops are vectorizable by the compiler!
0164   for (int i=0; i<3; ++i) out6[i] = JTC0 * J_pM[18+i] + JTC1 * J_pM[24+i];
0165   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];
0166
0167   for (int i=0; i<2; ++i) out6[7+i] = JTC5 * J_pM[19+i] + JTC6 * J_pM[25+i];
0168   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];
0169
0170   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];
0171   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];
0172
0173   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];
0174
0175   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];
0176
0177   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];
0178
0179   // symmetric part
0180   out6[6] = out6[1];
0181   out6[12] = out6[2];  out6[12+1] = out6[6+2];
0182   out6[18] = out6[3];  out6[18+1] = out6[6+3];  out6[18+2] = out6[12+3];
0183   out6[24] = out6[4];  out6[24+1] = out6[6+4];  out6[24+2] = out6[12+4];  out6[24+3] = out6[18+4];
0184   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];
0185
0186 }
0187
0188
0189 void RKTools::J_MpTxcov7xJ_Mp(const M7x5& J_Mp, const M7x7& cov7, M5x5& out5)
0190 {
0191   // 7D -> 5D
0192
0193   // J_Mp
0194   // 0 0 0 x x
0195   // 0 0 0 x x
0196   // 0 0 0 x x
0197   // 0 x x 0 0
0198   // 0 x x 0 0
0199   // 0 x x 0 0
0200   // 1 0 0 0 0
0201
0202   // it is assumed that J_Mp is only non-zero here:
0203   // [3]
0204   // [1*7+3]
0205   // [2*7+3]
0206
0207   // [4]
0208   // [1*7+4]
0209   // [2*7+4]
0210
0211   // [3*7+1]
0212   // [4*7+1]
0213   // [5*7+1]
0214
0215   // [3*7+2]
0216   // [4*7+2]
0217   // [5*7+2]
0218
0219   // [6*7+0] = 1
0220
0221
0222   double JTC0  = (J_Mp[16] * cov7[24] + J_Mp[21] * cov7[31] + J_Mp[26] * cov7[38]);
0223   double JTC1  = (J_Mp[16] * cov7[31] + J_Mp[21] * cov7[32] + J_Mp[26] * cov7[39]);
0224   double JTC2  = (J_Mp[16] * cov7[38] + J_Mp[21] * cov7[39] + J_Mp[26] * cov7[40]);
0225   double JTC3  = (J_Mp[16] * cov7[21] + J_Mp[21] * cov7[28] + J_Mp[26] * cov7[35]);
0226   double JTC4  = (J_Mp[16] * cov7[22] + J_Mp[21] * cov7[29] + J_Mp[26] * cov7[36]);
0227   double JTC5  = (J_Mp[16] * cov7[23] + J_Mp[21] * cov7[30] + J_Mp[26] * cov7[37]);
0228   double JTC6  = (J_Mp[17] * cov7[21] + J_Mp[22] * cov7[28] + J_Mp[27] * cov7[35]);
0229   double JTC7  = (J_Mp[17] * cov7[22] + J_Mp[22] * cov7[29] + J_Mp[27] * cov7[36]);
0230   double JTC8  = (J_Mp[17] * cov7[23] + J_Mp[22] * cov7[30] + J_Mp[27] * cov7[37]);
0231   double JTC9  = (J_Mp[3] * cov7[0] + J_Mp[8] * cov7[7] + J_Mp[13] * cov7[14]);
0232   double JTC10 = (J_Mp[3] * cov7[7] + J_Mp[8] * cov7[8] + J_Mp[13] * cov7[15]);
0233   double JTC11 = (J_Mp[3] * cov7[14] + J_Mp[8] * cov7[15] + J_Mp[13] * cov7[16]);
0234
0235   out5[0] = cov7[48];
0236   out5[1] = J_Mp[16] * cov7[45] + J_Mp[21] * cov7[46] + J_Mp[26] * cov7[47];
0237   out5[2] = J_Mp[17] * cov7[45] + J_Mp[22] * cov7[46] + J_Mp[27] * cov7[47];
0238   out5[3] = J_Mp[3] * cov7[42] + J_Mp[8] * cov7[43] + J_Mp[13] * cov7[44];
0239   out5[4] = J_Mp[4] * cov7[42] + J_Mp[9] * cov7[43] + J_Mp[14] * cov7[44];
0240
0241   // loops are vectorizable by the compiler!
0242   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];
0243   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];
0244
0245   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];
0246   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];
0247
0248   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];
0249
0250   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];
0251
0252   // symmetric part
0253   out5[5] = out5[1];
0254   out5[10] = out5[2];  out5[11] = out5[7];
0255   out5[15] = out5[3];  out5[16] = out5[8];  out5[17] = out5[13];
0256   out5[20] = out5[4];  out5[21] = out5[9];  out5[22] = out5[14];  out5[23] = out5[19];
0257
0258 }
0259
0260
0261 void RKTools::J_MpTxcov6xJ_Mp(const M6x5& J_Mp, const M6x6& cov6, M5x5& out5)
0262 {
0263   // 6D -> 5D
0264
0265   // J_Mp
0266   // 0 0 0 x x
0267   // 0 0 0 x x
0268   // 0 0 0 x x
0269   // x x x 0 0
0270   // x x x 0 0
0271   // x x x 0 0
0272
0273   // it is assumed that J_Mp is only non-zero here:
0274   // [3]
0275   // [1*6+3]
0276   // [2*6+3]
0277
0278   // [4]
0279   // [1*6+4]
0280   // [2*6+4]
0281
0282   // [3*6+0]
0283   // [4*6+0]
0284   // [5*6+0]
0285
0286   // [3*6+1]
0287   // [4*6+1]
0288   // [5*6+1]
0289
0290   // [3*6+2]
0291   // [4*6+2]
0292   // [5*6+2]
0293
0294   double JTC0  = (J_Mp[15] * cov6[18+3] + J_Mp[20] * cov6[24+3] + J_Mp[25] * cov6[30+3]);
0295   double JTC1  = (J_Mp[15] * cov6[24+3] + J_Mp[20] * cov6[24+4] + J_Mp[25] * cov6[30+4]);
0296   double JTC2  = (J_Mp[15] * cov6[30+3] + J_Mp[20] * cov6[30+4] + J_Mp[25] * cov6[30+5]);
0297   double JTC3  = (J_Mp[15] * cov6[18] + J_Mp[20] * cov6[24] + J_Mp[25] * cov6[30]);
0298   double JTC4  = (J_Mp[15] * cov6[18+1] + J_Mp[20] * cov6[24+1] + J_Mp[25] * cov6[30+1]);
0299   double JTC5  = (J_Mp[15] * cov6[18+2] + J_Mp[20] * cov6[24+2] + J_Mp[25] * cov6[30+2]);
0300   double JTC6  = (J_Mp[15+1] * cov6[18+3] + J_Mp[20+1] * cov6[24+3] + J_Mp[25+1] * cov6[30+3]);
0301   double JTC7  = (J_Mp[15+1] * cov6[24+3] + J_Mp[20+1] * cov6[24+4] + J_Mp[25+1] * cov6[30+4]);
0302   double JTC8  = (J_Mp[15+1] * cov6[30+3] + J_Mp[20+1] * cov6[30+4] + J_Mp[25+1] * cov6[30+5]);
0303   double JTC9  = (J_Mp[15+1] * cov6[18] + J_Mp[20+1] * cov6[24] + J_Mp[25+1] * cov6[30]);
0304   double JTC10 = (J_Mp[15+1] * cov6[18+1] + J_Mp[20+1] * cov6[24+1] + J_Mp[25+1] * cov6[30+1]);
0305   double JTC11 = (J_Mp[15+1] * cov6[18+2] + J_Mp[20+1] * cov6[24+2] + J_Mp[25+1] * cov6[30+2]);
0306   double JTC12 = (J_Mp[15+2] * cov6[18] + J_Mp[20+2] * cov6[24] + J_Mp[25+2] * cov6[30]);
0307   double JTC13 = (J_Mp[15+2] * cov6[18+1] + J_Mp[20+2] * cov6[24+1] + J_Mp[25+2] * cov6[30+1]);
0308   double JTC14 = (J_Mp[15+2] * cov6[18+2] + J_Mp[20+2] * cov6[24+2] + J_Mp[25+2] * cov6[30+2]);
0309   double JTC15 = (J_Mp[3] * cov6[0] + J_Mp[5+3] * cov6[6] + J_Mp[10+3] * cov6[12]);
0310   double JTC16 = (J_Mp[3] * cov6[6] + J_Mp[5+3] * cov6[6+1] + J_Mp[10+3] * cov6[12+1]);
0311   double JTC17 = (J_Mp[3] * cov6[12] + J_Mp[5+3] * cov6[12+1] + J_Mp[10+3] * cov6[12+2]);
0312
0313   // loops are vectorizable by the compiler!
0314   for (int i=0; i<3; ++i) out5[i] = JTC0 * J_Mp[15+i] + JTC1 * J_Mp[20+i] + JTC2 * J_Mp[25+i];
0315   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];
0316
0317   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];
0318   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];
0319
0320   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];
0321   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];
0322
0323   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];
0324
0325   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];
0326
0327   // symmetric part
0328   out5[5] = out5[1];
0329   out5[10] = out5[2];  out5[10+1] = out5[5+2];
0330   out5[15] = out5[3];  out5[15+1] = out5[5+3];  out5[15+2] = out5[10+3];
0331   out5[20] = out5[4];  out5[20+1] = out5[5+4];  out5[20+2] = out5[10+4];  out5[20+3] = out5[15+4];
0332
0333 }
0334
0335 void RKTools::J_pMTTxJ_MMTTxJ_MpTT(const M7x5& J_pMT, const M7x7& J_MMT, const M5x7& J_MpT, M5x5& J_pp)
0336 {
0337     // calculates  J_pp = J_pM * J_MM * J_Mp
0338     // input J_MMT is transposed version of actual jacobian J_MM
0339     // input J_pMT is transposed version of actual jacobian J_pM (Master to plane)
0340     // input J_MpT is transposed version of actual jacobian J_Mp (plane to Master)
0341
0342   // J_pMT
0343   // 0 0 0 x x
0344   // 0 0 0 x x
0345   // 0 0 0 x x
0346   // 0 x x 0 0
0347   // 0 x x 0 0
0348   // 0 x x 0 0
0349   // 1 0 0 0 0
0350
0351   // J_MMT if MMproj == true
0352   // x x x x x x 0
0353   // x x x x x x 0
0354   // x x x x x x 0
0355   // x x x x x x 0
0356   // x x x x x x 0
0357   // x x x x x x 0
0358   // x x x x x x x
0359
0360   // J_MpT
0361   // 0 0 0 0 0 0 1
0362   // 0 0 0 x x x 0
0363   // 0 0 0 x x x 0
0364   // x x x 0 0 0 0
0365   // x x x 0 0 0 0
0366
0367
0368   J_pp[0*5+0] = J_MMT[6*7+6];
0369   J_pp[0*5+1] = 0;
0370   J_pp[0*5+2] = 0;
0371   J_pp[0*5+3] = 0;
0372   J_pp[0*5+4] = 0;
0373
0374   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];
0375   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]
0376                  + (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]
0377                  + (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]);
0378   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]
0379                  + (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]
0380                  + (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]);
0381   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]
0382                  + (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]
0383                  + (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]);
0384   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]
0385                  + (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]
0386                  + (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]);
0387
0388   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];
0389   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]
0390                  + (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]
0391                  + (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]);
0392   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]
0393                  + (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]
0394                  + (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]);
0395   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]
0396                  + (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]
0397                  + (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]);
0398   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]
0399                  + (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]
0400                  + (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]);
0401
0402   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];
0403   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]
0404                  + (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]
0405                  + (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]);
0406   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]
0407                  + (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]
0408                  + (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]);
0409   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]
0410                  + (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]
0411                  + (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]);
0412   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]
0413                  + (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]
0414                  + (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]);
0415
0416   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];
0417   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]
0418                  + (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]
0419                  + (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]);
0420   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]
0421                  + (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]
0422                  + (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]);
0423   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]
0424                  + (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]
0425                  + (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]);
0426   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]
0427                  + (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]
0428                  + (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]);
0429 }
0430
0431
0432 void RKTools::Np_N_NpT(const M7x7& Np, M7x7& N) {
0433
0434   // N is symmetric
0435
0436   // Np
0437   // x x x 0 0 0 0
0438   // x x x 0 0 0 0
0439   // x x x 0 0 0 0
0440   // x x x 1 0 0 0
0441   // x x x 0 1 0 0
0442   // x x x 0 0 1 0
0443   // 0 0 0 0 0 0 1
0444
0445   // calculate:
0446   // Np * N * Np^T
0447
0448   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]);
0449
0450   // replace lower left triangle using the original values from upper right triangle plus the cached diagonal values
0451   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];
0452
0453   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];
0454   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];
0455
0456   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];
0457   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];
0458   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];
0459
0460   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];
0461   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];
0462   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];
0463   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;
0464
0465   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];
0466   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];
0467   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];
0468   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];
0469   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;
0470
0471   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];
0472   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];
0473   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];
0474   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];
0475   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];
0476   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;
0477
0478   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];
0479   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];
0480   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];
0481   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];
0482   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];
0483   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];
0484   //N[6*7+6] = N[6*7+6]; remains unchanged
0485
0486
0487   // copy the results from the lower left triangle to the upper right triangle as well
0488   N[5*7+6] = N[6*7+5];
0489   N[4*7+5] = N[5*7+4];  N[4*7+6] = N[6*7+4];
0490   N[3*7+4] = N[4*7+3];  N[3*7+5] = N[5*7+3];  N[3*7+6] = N[6*7+3];
0491   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];
0492   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];
0493   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];
0494 }
0495
0496
0497 void RKTools::printDim(const double* mat, unsigned int dimX, unsigned int dimY){
0498
0499   printOut << dimX << " x " << dimY << " matrix as follows: \n";
0500   for (unsigned int i=0; i< dimX; ++i){
0501     for (unsigned int j=0; j< dimY; ++j){
0502       printf("  %11.5g", mat[i*dimY+j]);
0503     }
0504     printOut<<"\n";
0505   }
0506   printOut<<std::endl;
0507
0508 }
0509
0510
0511 } /* End of namespace genfit */
0512