MkFitCore/src/KalmanUtilsMPlex.icc

0001 ///////////////////////////////////////////////////////////////////////////////
0002 /// KHMult_imp
0003 ///////////////////////////////////////////////////////////////////////////////
0004
0005 template <typename TfLH, typename TfQF1, typename TfQF2, typename TfLL>
0006 static inline void KHMult_imp(
0007     const TfLH& a, const TfQF1& b00, const TfQF2& b01, TfLL& c, const int nmin, const int nmax) {
0008 #pragma omp simd
0009   for (int n = nmin; n < nmax; ++n) {
0010     c(n, 0, 0) = a(n, 0, 0) * b00(n, 0, 0);
0011     c(n, 0, 1) = a(n, 0, 0) * b01(n, 0, 0);
0012     c(n, 0, 2) = a(n, 0, 1);
0013     c(n, 0, 3) = 0;
0014     c(n, 0, 4) = 0;
0015     c(n, 0, 5) = 0;
0016     c(n, 0, 6) = a(n, 0, 3) * b00(n, 0, 0);
0017     c(n, 0, 7) = a(n, 0, 3) * b01(n, 0, 0);
0018     c(n, 0, 8) = a(n, 0, 4);
0019     c(n, 0, 9) = 0;
0020     c(n, 0, 10) = 0;
0021     c(n, 0, 11) = 0;
0022     c(n, 0, 12) = a(n, 0, 6) * b00(n, 0, 0);
0023     c(n, 0, 13) = a(n, 0, 6) * b01(n, 0, 0);
0024     c(n, 0, 14) = a(n, 0, 7);
0025     c(n, 0, 15) = 0;
0026     c(n, 0, 16) = 0;
0027     c(n, 0, 17) = 0;
0028     c(n, 0, 18) = a(n, 0, 9) * b00(n, 0, 0);
0029     c(n, 0, 19) = a(n, 0, 9) * b01(n, 0, 0);
0030     c(n, 0, 20) = a(n, 0, 10);
0031     c(n, 0, 21) = 0;
0032     c(n, 0, 22) = 0;
0033     c(n, 0, 23) = 0;
0034     c(n, 0, 24) = a(n, 0, 12) * b00(n, 0, 0);
0035     c(n, 0, 25) = a(n, 0, 12) * b01(n, 0, 0);
0036     c(n, 0, 26) = a(n, 0, 13);
0037     c(n, 0, 27) = 0;
0038     c(n, 0, 28) = 0;
0039     c(n, 0, 29) = 0;
0040     c(n, 0, 30) = a(n, 0, 15) * b00(n, 0, 0);
0041     c(n, 0, 31) = a(n, 0, 15) * b01(n, 0, 0);
0042     c(n, 0, 32) = a(n, 0, 16);
0043     c(n, 0, 33) = 0;
0044     c(n, 0, 34) = 0;
0045     c(n, 0, 35) = 0;
0046   }
0047 }
0048
0049 ///////////////////////////////////////////////////////////////////////////////
0050 /// ConvertToCCS_imp
0051 ///////////////////////////////////////////////////////////////////////////////
0052
0053 template <typename TfLV1, typename TfLV2, typename TfLL>
0054 static inline void ConvertToCCS_imp(const TfLV1& a, TfLV2& b, TfLL& c, const int nmin, const int nmax) {
0055 #pragma omp simd
0056   for (int n = nmin; n < nmax; ++n) {
0057     const float pt = hipo(a(n, 0, 3), a(n, 0, 4));
0058     const float p2 = pt * pt + a(n, 0, 5) * a(n, 0, 5);
0059     //
0060     b(n, 0, 0) = a(n, 0, 0);
0061     b(n, 0, 1) = a(n, 0, 1);
0062     b(n, 0, 2) = a(n, 0, 2);
0063     b(n, 0, 3) = 1.0f / pt;
0064     b(n, 0, 4) = getPhi(a(n, 0, 3), a(n, 0, 4));  //fixme: use trig approx
0065     b(n, 0, 5) = getTheta(pt, a(n, 0, 5));
0066     //
0067     c(n, 0, 0) = 1.;
0068     c(n, 0, 1) = 0.;
0069     c(n, 0, 2) = 0.;
0070     c(n, 0, 3) = 0.;
0071     c(n, 0, 4) = 0.;
0072     c(n, 0, 5) = 0.;
0073     c(n, 0, 6) = 0.;
0074     c(n, 0, 7) = 1.;
0075     c(n, 0, 8) = 0.;
0076     c(n, 0, 9) = 0.;
0077     c(n, 0, 10) = 0.;
0078     c(n, 0, 11) = 0.;
0079     c(n, 0, 12) = 0.;
0080     c(n, 0, 13) = 0.;
0081     c(n, 0, 14) = 1.;
0082     c(n, 0, 15) = 0.;
0083     c(n, 0, 16) = 0.;
0084     c(n, 0, 17) = 0.;
0085     c(n, 0, 18) = 0.;
0086     c(n, 0, 19) = 0.;
0087     c(n, 0, 20) = 0.;
0088     c(n, 0, 21) = -a(n, 0, 3) / (pt * pt * pt);
0089     c(n, 0, 22) = -a(n, 0, 4) / (pt * pt * pt);
0090     c(n, 0, 23) = 0.;
0091     c(n, 0, 24) = 0.;
0092     c(n, 0, 25) = 0.;
0093     c(n, 0, 26) = 0.;
0094     c(n, 0, 27) = -a(n, 0, 4) / (pt * pt);
0095     c(n, 0, 28) = a(n, 0, 3) / (pt * pt);
0096     c(n, 0, 29) = 0.;
0097     c(n, 0, 30) = 0.;
0098     c(n, 0, 31) = 0.;
0099     c(n, 0, 32) = 0.;
0100     c(n, 0, 33) = a(n, 0, 3) * a(n, 0, 5) / (pt * p2);
0101     c(n, 0, 34) = a(n, 0, 4) * a(n, 0, 5) / (pt * p2);
0102     c(n, 0, 35) = -pt / p2;
0103   }
0104 }
0105
0106 ///////////////////////////////////////////////////////////////////////////////
0107 /// ConvertToCartesian_imp
0108 ///////////////////////////////////////////////////////////////////////////////
0109
0110 template <typename TfLV1, typename TfLV2, typename TfLL>
0111 static inline void ConvertToCartesian_imp(const TfLV1& a, TfLV2& b, TfLL& c, const int nmin, const int nmax) {
0112 #pragma omp simd
0113   for (int n = nmin; n < nmax; ++n) {
0114     const float cosP = std::cos(a(n, 0, 4));  //fixme: use trig approx
0115     const float sinP = std::sin(a(n, 0, 4));
0116     const float cosT = std::cos(a(n, 0, 5));
0117     const float sinT = std::sin(a(n, 0, 5));
0118     //
0119     b(n, 0, 0) = a(n, 0, 0);
0120     b(n, 0, 1) = a(n, 0, 1);
0121     b(n, 0, 2) = a(n, 0, 2);
0122     b(n, 0, 3) = cosP / a(n, 0, 3);
0123     b(n, 0, 4) = sinP / a(n, 0, 3);
0124     b(n, 0, 5) = cosT / (sinT * a(n, 0, 3));
0125     //
0126     c(n, 0, 0) = 1.;
0127     c(n, 0, 1) = 0.;
0128     c(n, 0, 2) = 0.;
0129     c(n, 0, 3) = 0.;
0130     c(n, 0, 4) = 0.;
0131     c(n, 0, 5) = 0.;
0132     c(n, 0, 6) = 0.;
0133     c(n, 0, 7) = 1.;
0134     c(n, 0, 8) = 0.;
0135     c(n, 0, 9) = 0.;
0136     c(n, 0, 10) = 0.;
0137     c(n, 0, 11) = 0.;
0138     c(n, 0, 12) = 0.;
0139     c(n, 0, 13) = 0.;
0140     c(n, 0, 14) = 1.;
0141     c(n, 0, 15) = 0.;
0142     c(n, 0, 16) = 0.;
0143     c(n, 0, 17) = 0.;
0144     c(n, 0, 18) = 0.;
0145     c(n, 0, 19) = 0.;
0146     c(n, 0, 20) = 0.;
0147     c(n, 0, 21) = -cosP / (a(n, 0, 3) * a(n, 0, 3));
0148     c(n, 0, 22) = -sinP / a(n, 0, 3);
0149     c(n, 0, 23) = 0.;
0150     c(n, 0, 24) = 0.;
0151     c(n, 0, 25) = 0.;
0152     c(n, 0, 26) = 0.;
0153     c(n, 0, 27) = -sinP / (a(n, 0, 3) * a(n, 0, 3));
0154     c(n, 0, 28) = cosP / a(n, 0, 3);
0155     c(n, 0, 29) = 0.;
0156     c(n, 0, 30) = 0.;
0157     c(n, 0, 31) = 0.;
0158     c(n, 0, 32) = 0.;
0159     c(n, 0, 33) = -cosT / (sinT * a(n, 0, 3) * a(n, 0, 3));
0160     c(n, 0, 34) = 0.;
0161     c(n, 0, 35) = -1.0f / (sinT * sinT * a(n, 0, 3));
0162   }
0163 }
0164
0165 ///////////////////////////////////////////////////////////////////////////////
0166 /// MultResidualsAdd_imp
0167 ///////////////////////////////////////////////////////////////////////////////
0168
0169 template <typename TfLH, typename TfLV1, typename Tf2V, typename TfLV2>
0170 static inline void MultResidualsAdd_imp(
0171     const TfLH& a, const TfLV1& b, const Tf2V& c, TfLV2& d, const int nmin, const int nmax) {
0172 #pragma omp simd
0173   for (int n = nmin; n < nmax; ++n) {
0174     d(n, 0, 0) = b(n, 0, 0) + a(n, 0, 0) * c(n, 0, 0) + a(n, 0, 1) * c(n, 0, 1);
0175     d(n, 0, 1) = b(n, 0, 1) + a(n, 0, 3) * c(n, 0, 0) + a(n, 0, 4) * c(n, 0, 1);
0176     d(n, 0, 2) = b(n, 0, 2) + a(n, 0, 6) * c(n, 0, 0) + a(n, 0, 7) * c(n, 0, 1);
0177     d(n, 0, 3) = b(n, 0, 3) + a(n, 0, 9) * c(n, 0, 0) + a(n, 0, 10) * c(n, 0, 1);
0178     d(n, 0, 4) = b(n, 0, 4) + a(n, 0, 12) * c(n, 0, 0) + a(n, 0, 13) * c(n, 0, 1);
0179     d(n, 0, 5) = b(n, 0, 5) + a(n, 0, 15) * c(n, 0, 0) + a(n, 0, 16) * c(n, 0, 1);
0180   }
0181 }
0182
0183 ///////////////////////////////////////////////////////////////////////////////
0184 /// AddIntoUpperLeft3x3_imp
0185 ///////////////////////////////////////////////////////////////////////////////
0186
0187 template <typename TfLS, typename TfHS1, typename TfHS2>
0188 static inline void AddIntoUpperLeft3x3_imp(
0189     const TfLS& A, const TfHS1& B, TfHS2& C, const int aN, const int bN, const int cN, const int nmin, const int nmax) {
0190   // Problem here: (n, i, j) uses indirection -> slow on the GPU
0191 }
0192
0193 ///////////////////////////////////////////////////////////////////////////////
0194 ///  RotateResidualsOnTangentPlane_impl
0195 ///////////////////////////////////////////////////////////////////////////////
0196
0197 template <typename TfQF1, typename TfQF2, typename TfHV, typename Tf2V>
0198 static inline void RotateResidualsOnTangentPlane_impl(
0199     const TfQF1& r00, const TfQF2& r01, const TfHV& a, Tf2V& b, const int nmin, const int nmax) {
0200 #pragma omp simd
0201   for (int n = nmin; n < nmax; ++n) {
0202     b(n, 0, 0) = r00(n, 0, 0) * a(n, 0, 0) + r01(n, 0, 0) * a(n, 0, 1);
0203     b(n, 0, 1) = a(n, 0, 2);
0204   }
0205 }