KFPTrackVector.cxx 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412
  1. //----------------------------------------------------------------------------
  2. // Implementation of the KFParticle class
  3. // .
  4. // @author I.Kisel, I.Kulakov, M.Zyzak
  5. // @version 1.0
  6. // @since 20.08.13
  7. //
  8. //
  9. // -= Copyright &copy ALICE HLT and CBM L1 Groups =-
  10. //____________________________________________________________________________
  11. #include "KFPTrackVector.h"
  12. #include <iostream>
  13. void KFPTrackVector::SetParameter(const float_v& value, int iP, int iTr)
  14. {
  15. /** Copies the SIMD vector "value" to the parameter vector KFPTrackVector::fP[iP]
  16. ** starting at the position "iTr".
  17. ** \param[in] value - SIMD vector with the values to be stored
  18. ** \param[in] iP - number of the parameter vector
  19. ** \param[in] iTr - starting position in the parameter vector where the values should be stored
  20. **/
  21. // gather caused errors at XeonPhi, temporarly replaced with the simple copying
  22. // if( (iTr+float_vLen) < Size())
  23. // reinterpret_cast<float_v&>(fP[iP][iTr]) = value;
  24. // else
  25. // {
  26. // const uint_v index(uint_v::IndexesFromZero());
  27. // (reinterpret_cast<float_v&>(fP[iP][iTr])).gather(reinterpret_cast<const float*>(&value), index, float_m(index<(Size() - iTr)));
  28. // }
  29. if( (iTr+float_vLen) < Size())
  30. reinterpret_cast<float_v&>(fP[iP][iTr]) = value;
  31. else
  32. for(int i=0; i<float_v::Size; i++)
  33. {
  34. if(iTr + i >= Size()) continue;
  35. fP[iP][iTr+i] = value[i];
  36. }
  37. }
  38. void KFPTrackVector::SetCovariance(const float_v& value, int iC, int iTr)
  39. {
  40. /** Copies the SIMD vector "value" to the element of the covariance matrix vector KFPTrackVector::fC[iC]
  41. ** starting at the position "iTr".
  42. ** \param[in] value - SIMD vector with the values to be stored
  43. ** \param[in] iC - number of the element of the covariance matrix
  44. ** \param[in] iTr - starting position in the parameter vector where the values should be stored
  45. **/
  46. // gather caused errors at XeonPhi, temporarly replaced with the simple copying
  47. // if( (iTr+float_vLen) < Size())
  48. // reinterpret_cast<float_v&>(fC[iC][iTr]) = value;
  49. // else
  50. // {
  51. // const uint_v index(uint_v::IndexesFromZero());
  52. // (reinterpret_cast<float_v&>(fC[iC][iTr])).gather(reinterpret_cast<const float*>(&value), index, float_m(index<(Size() - iTr)));
  53. // }
  54. if( (iTr+float_vLen) < Size())
  55. reinterpret_cast<float_v&>(fC[iC][iTr]) = value;
  56. else
  57. for(int i=0; i<float_v::Size; i++)
  58. {
  59. if(iTr + i >= Size()) continue;
  60. fC[iC][iTr+i] = value[i];
  61. }
  62. }
  63. void KFPTrackVector::Resize(const int n)
  64. {
  65. /** Resizes all vectors in the class to a given value.
  66. ** \param[in] n - new size of the vector
  67. **/
  68. for(int i=0; i<6; i++)
  69. fP[i].resize(n);
  70. for(int i=0; i<21; i++)
  71. fC[i].resize(n);
  72. #ifdef NonhomogeneousField
  73. for(int i=0; i<10; i++)
  74. fField[i].resize(n);
  75. #endif
  76. // fChi2.resize(n);
  77. // fNDF.resize(n);
  78. fId.resize(n);
  79. fPDG.resize(n);
  80. fQ.resize(n);
  81. fPVIndex.resize(n);
  82. fNPixelHits.resize(n);
  83. }
  84. void KFPTrackVector::Set(KFPTrackVector& v, int vSize, int offset)
  85. {
  86. /** Copies "vSize" tracks from the KFPTrackVector "v" to the current object.
  87. ** Tracks are put starting from the "offset" position.
  88. ** \param[in] v - external KFPTrackVector with input tracks to be copied
  89. ** \param[in] vSize - number of tracks to be copied from "v"
  90. ** \param[in] offset - offset position in the current object, starting from which input tracks will be stored
  91. **/
  92. for(int iV=0; iV<vSize; iV++)
  93. {
  94. for(int i=0; i<6; i++)
  95. fP[i][offset+iV] = v.fP[i][iV];
  96. for(int i=0; i<21; i++)
  97. fC[i][offset+iV] = v.fC[i][iV];
  98. #ifdef NonhomogeneousField
  99. for(int i=0; i<10; i++)
  100. fField[i][offset+iV] = v.fField[i][iV];
  101. #endif
  102. // fChi2[offset+iV] = v.fChi2[iV];
  103. // fNDF[offset+iV] = v.fNDF[iV];
  104. fId[offset+iV] = v.fId[iV];
  105. fPDG[offset+iV] = v.fPDG[iV];
  106. fQ[offset+iV] = v.fQ[iV];
  107. fPVIndex[offset+iV] = v.fPVIndex[iV];
  108. fNPixelHits[offset+iV] = v.fNPixelHits[iV];
  109. }
  110. }
  111. void KFPTrackVector::SetTracks(const KFPTrackVector& track, const kfvector_uint& trackIndex, const int nIndexes)
  112. {
  113. /** The current object is resised to "nIndexes", tracks with indices "trackIndex" are copied to the current object.
  114. ** \param[in] track - input vector of tracks
  115. ** \param[in] trackIndex - indices of tracks in a vector "track", which should be stored to the current object
  116. ** \param[in] nIndexes - number of tracks to be copied, defines the new size of the current object
  117. **/
  118. if(nIndexes == 0) return;
  119. Resize(nIndexes);
  120. for(int iP=0; iP<6; iP++)
  121. {
  122. int iElement = 0;
  123. for(iElement=0; iElement<nIndexes-float_vLen; iElement += float_vLen)
  124. {
  125. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  126. float_v& vec = reinterpret_cast<float_v&>(fP[iP][iElement]);
  127. vec.gather(&(track.fP[iP][0]), index);
  128. }
  129. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  130. float_v& vec = reinterpret_cast<float_v&>(fP[iP][iElement]);
  131. vec.gather(&(track.fP[iP][0]), index, simd_cast<float_m>(iElement+uint_v::IndexesFromZero()<nIndexes));
  132. }
  133. for(int iC=0; iC<21; iC++)
  134. {
  135. int iElement=0;
  136. for(iElement=0; iElement<nIndexes-float_vLen; iElement += float_vLen)
  137. {
  138. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  139. float_v& vec = reinterpret_cast<float_v&>(fC[iC][iElement]);
  140. vec.gather(&(track.fC[iC][0]), index);
  141. }
  142. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  143. float_v& vec = reinterpret_cast<float_v&>(fC[iC][iElement]);
  144. vec.gather(&(track.fC[iC][0]), index, simd_cast<float_m>(iElement+uint_v::IndexesFromZero()<nIndexes));
  145. }
  146. #ifdef NonhomogeneousField
  147. for(int iP=0; iP<10; iP++)
  148. {
  149. int iElement = 0;
  150. for(iElement=0; iElement<nIndexes-float_vLen; iElement += float_vLen)
  151. {
  152. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  153. float_v& vec = reinterpret_cast<float_v&>(fField[iP][iElement]);
  154. vec.gather(&(track.fField[iP][0]), index);
  155. }
  156. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  157. float_v& vec = reinterpret_cast<float_v&>(fField[iP][iElement]);
  158. vec.gather(&(track.fField[iP][0]), index, simd_cast<float_m>(iElement+uint_v::IndexesFromZero()<nIndexes));
  159. }
  160. #endif
  161. {
  162. int iElement=0;
  163. for(iElement=0; iElement<nIndexes-float_vLen; iElement += float_vLen)
  164. {
  165. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  166. int_v& vec = reinterpret_cast<int_v&>(fId[iElement]);
  167. vec.gather(&(track.fId[0]), index);
  168. }
  169. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  170. int_v& vec = reinterpret_cast<int_v&>(fId[iElement]);
  171. vec.gather(&(track.fId[0]), index, int_m(iElement+uint_v::IndexesFromZero()<nIndexes));
  172. }
  173. {
  174. int iElement=0;
  175. for(iElement=0; iElement<nIndexes-float_vLen; iElement += float_vLen)
  176. {
  177. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  178. int_v& vec = reinterpret_cast<int_v&>(fPDG[iElement]);
  179. vec.gather(&(track.fPDG[0]), index);
  180. }
  181. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  182. int_v& vec = reinterpret_cast<int_v&>(fPDG[iElement]);
  183. vec.gather(&(track.fPDG[0]), index, int_m(iElement+uint_v::IndexesFromZero()<nIndexes));
  184. }
  185. {
  186. int iElement=0;
  187. for(iElement=0; iElement<nIndexes-float_vLen; iElement += float_vLen)
  188. {
  189. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  190. int_v& vec = reinterpret_cast<int_v&>(fQ[iElement]);
  191. vec.gather(&(track.fQ[0]), index);
  192. }
  193. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  194. int_v& vec = reinterpret_cast<int_v&>(fQ[iElement]);
  195. vec.gather(&(track.fQ[0]), index, int_m(iElement+uint_v::IndexesFromZero()<nIndexes));
  196. }
  197. {
  198. int iElement=0;
  199. for(iElement=0; iElement<nIndexes-float_vLen; iElement += float_vLen)
  200. {
  201. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  202. int_v& vec = reinterpret_cast<int_v&>(fPVIndex[iElement]);
  203. vec.gather(&(track.fPVIndex[0]), index);
  204. }
  205. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  206. int_v& vec = reinterpret_cast<int_v&>(fPVIndex[iElement]);
  207. vec.gather(&(track.fPVIndex[0]), index, int_m(iElement+uint_v::IndexesFromZero()<nIndexes));
  208. }
  209. {
  210. int iElement=0;
  211. for(iElement=0; iElement<nIndexes-float_vLen; iElement += float_vLen)
  212. {
  213. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  214. int_v& vec = reinterpret_cast<int_v&>(fNPixelHits[iElement]);
  215. vec.gather(&(track.fNPixelHits[0]), index);
  216. }
  217. const uint_v& index = reinterpret_cast<const uint_v&>(trackIndex[iElement]);
  218. int_v& vec = reinterpret_cast<int_v&>(fNPixelHits[iElement]);
  219. vec.gather(&(track.fNPixelHits[0]), index, int_m(iElement+uint_v::IndexesFromZero()<nIndexes));
  220. }
  221. }
  222. void KFPTrackVector::GetTrack(KFPTrack& track, const int n)
  223. {
  224. /** Copies track with index "n" for the current object to the KFPTrack object "track".
  225. ** \param[out] track - KFPTrack object, where track with index "n" is copied
  226. ** \param[in] n - index of the track to be copied
  227. **/
  228. track.SetParameters(fP[0][n],fP[1][n],fP[2][n],fP[3][n],fP[4][n],fP[5][n]);
  229. for(int i=0; i<21; i++)
  230. track.SetCovariance(i,fC[i][n]);
  231. // track.SetChi2(fChi2[n]);
  232. // track.SetNDF(fNDF[n]);
  233. track.SetId(fId[n]);
  234. track.SetCharge(fQ[n]);
  235. #ifdef NonhomogeneousField
  236. for(int i=0; i<10; i++)
  237. track.SetFieldCoeff( fField[i][n], i);
  238. #endif
  239. }
  240. void KFPTrackVector::RotateXY( float_v alpha, int firstElement )
  241. {
  242. /** Rotates SIMD vector of tracks starting from the position "firstElement" onto the angles "alpha" in the XY plane.
  243. ** Rotation matrix is:
  244. \verbatim
  245. { cos(A), -sin(A), 0, 0, 0, 0 }
  246. { sin(A), cos(A), 0, 0, 0, 0 }
  247. { 0, 0, 1, 0, 0, 0 }
  248. { 0, 0, 0, cos(A), -sin(A), 0 }
  249. { 0, 0, 0, sin(A), cos(A), 0 }
  250. { 0, 0, 0, 0, 0, 1 }
  251. \endverbatim
  252. ** \param[in] alpha - rotation angles
  253. ** \param[in] firstElement - track index, starting from which SIMD vector of tracks will be rotated
  254. **/
  255. const float_v cA = KFPMath::Cos( alpha );
  256. const float_v sA = KFPMath::Sin( alpha );
  257. const float_v xInit = reinterpret_cast<const float_v&>(fP[0][firstElement]);
  258. const float_v yInit = reinterpret_cast<const float_v&>(fP[1][firstElement]);
  259. float_v& x = reinterpret_cast<float_v&>(fP[0][firstElement]);
  260. float_v& y = reinterpret_cast<float_v&>(fP[1][firstElement]);
  261. x = -(xInit*sA + yInit*cA);
  262. y = xInit*cA - yInit*sA;
  263. const float_v pxInit = reinterpret_cast<const float_v&>(fP[3][firstElement]);
  264. const float_v pyInit = reinterpret_cast<const float_v&>(fP[4][firstElement]);
  265. float_v& px = reinterpret_cast<float_v&>(fP[3][firstElement]);
  266. float_v& py = reinterpret_cast<float_v&>(fP[4][firstElement]);
  267. px = -(pxInit*sA + pyInit*cA);
  268. py = pxInit*cA - pyInit*sA;
  269. float_v cov[21];
  270. for(int iC=0; iC<21; iC++)
  271. cov[iC] = reinterpret_cast<const float_v&>(fC[iC][firstElement]);
  272. reinterpret_cast<float_v&>(fC[0][firstElement]) = cA*cA* cov[2] + 2* cA* cov[1]* sA + cov[0]*sA* sA;
  273. reinterpret_cast<float_v&>(fC[1][firstElement]) = -(cA*cA * cov[1]) + cA* (-cov[0] + cov[2])* sA + cov[1]*sA* sA;
  274. reinterpret_cast<float_v&>(fC[2][firstElement]) = cA*cA* cov[0] - 2* cA* cov[1]* sA + cov[2]*sA* sA;
  275. reinterpret_cast<float_v&>(fC[3][firstElement]) = -(cA* cov[4]) - cov[3]* sA;
  276. reinterpret_cast<float_v&>(fC[4][firstElement]) = cA* cov[3] - cov[4]* sA;
  277. // reinterpret_cast<float_v&>(fC[5][firstElement]) = cov[5];
  278. reinterpret_cast<float_v&>(fC[6][firstElement]) = cA*cA* cov[11] + cA *(cov[10] + cov[7])* sA + cov[6]*sA* sA;
  279. reinterpret_cast<float_v&>(fC[7][firstElement]) = -(cA*cA * cov[10]) + cA* (cov[11] - cov[6])* sA + cov[7] *sA*sA;
  280. reinterpret_cast<float_v&>(fC[8][firstElement]) = -(cA *cov[12]) - cov[8] *sA;
  281. reinterpret_cast<float_v&>(fC[9][firstElement]) = cA*cA* cov[14] + 2 *cA* cov[13]* sA + cov[9]* sA*sA;
  282. reinterpret_cast<float_v&>(fC[10][firstElement]) = -(cA*cA* cov[7]) + cA* (cov[11] - cov[6])* sA + cov[10]*sA* sA;
  283. reinterpret_cast<float_v&>(fC[11][firstElement]) = cA*cA* cov[6] - cA* (cov[10] + cov[7]) *sA + cov[11]*sA* sA;
  284. reinterpret_cast<float_v&>(fC[12][firstElement]) = cA* cov[8] - cov[12]* sA;
  285. reinterpret_cast<float_v&>(fC[13][firstElement]) = -(cA*cA* cov[13]) + cA* (cov[14] - cov[9])* sA + cov[13]* sA*sA;
  286. reinterpret_cast<float_v&>(fC[14][firstElement]) = cA*cA* cov[9] - 2* cA* cov[13]* sA + cov[14]* sA*sA;
  287. reinterpret_cast<float_v&>(fC[15][firstElement]) = -(cA* cov[16]) - cov[15]* sA;
  288. reinterpret_cast<float_v&>(fC[16][firstElement]) = cA* cov[15] - cov[16]* sA;
  289. // reinterpret_cast<float_v&>(fC[17][firstElement]) = cov[17];
  290. reinterpret_cast<float_v&>(fC[18][firstElement]) = -(cA* cov[19]) - cov[18]* sA;
  291. reinterpret_cast<float_v&>(fC[19][firstElement]) = cA* cov[18] - cov[19]* sA;
  292. // reinterpret_cast<float_v&>(fC[20][firstElement]) = cov[20];
  293. } // RotateXY
  294. void KFPTrackVector::PrintTrack(int n)
  295. {
  296. /** Prints parameters of the track with index "n".
  297. ** \param[in] n - index of track to be printed
  298. **/
  299. for(int i=0; i<6; i++)
  300. std::cout << fP[i][n] << " ";
  301. std::cout << std::endl;
  302. for(int i=0; i<21; i++)
  303. std::cout << fC[i][n] << " ";
  304. std::cout << std::endl;
  305. std::cout << fId[n] << " " << fPDG[n] << " " << fQ[n] << " " << fPVIndex[n] << " " << fNPixelHits[n] << std::endl;
  306. }
  307. void KFPTrackVector::Print()
  308. {
  309. /** Prints all field of the current object. **/
  310. std::cout << "NTracks " << Size() << std::endl;
  311. if( Size()==0 ) return;
  312. std::cout << "Parameters: " << std::endl;
  313. for(int iP=0; iP<6; iP++)
  314. {
  315. std::cout << " iP " << iP << ": ";
  316. for(int iTr=0; iTr<Size(); iTr++)
  317. std::cout << Parameter(iP)[iTr]<< " ";
  318. std::cout << std::endl;
  319. }
  320. std::cout << "Cov matrix: " << std::endl;
  321. for(int iC=0; iC<21; iC++)
  322. {
  323. std::cout << " iC " << iC << ": ";
  324. for(int iTr=0; iTr<Size(); iTr++)
  325. std::cout << Covariance(iC)[iTr]<< " ";
  326. std::cout << std::endl;
  327. }
  328. std::cout << "Id: " << std::endl;
  329. for(int iTr=0; iTr<Size(); iTr++)
  330. std::cout << Id()[iTr] << " ";
  331. std::cout << std::endl;
  332. std::cout << "Pdg: " << std::endl;
  333. for(int iTr=0; iTr<Size(); iTr++)
  334. std::cout << PDG()[iTr] << " ";
  335. std::cout << std::endl;
  336. std::cout << "Q: " << std::endl;
  337. for(int iTr=0; iTr<Size(); iTr++)
  338. std::cout << Q()[iTr] << " ";
  339. std::cout << std::endl;
  340. std::cout << "PV index: " << std::endl;
  341. for(int iTr=0; iTr<Size(); iTr++)
  342. std::cout << PVIndex()[iTr] << " ";
  343. std::cout << std::endl;
  344. std::cout << "fNPixelHits: " << std::endl;
  345. for(int iTr=0; iTr<Size(); iTr++)
  346. std::cout << NPixelHits()[iTr] << " ";
  347. std::cout << std::endl;
  348. #ifdef NonhomogeneousField
  349. std::cout << "Field: " << std::endl;
  350. for(int iF=0; iF<6; iF++)
  351. {
  352. std::cout << " iF " << iF << ": ";
  353. for(int iTr=0; iTr<Size(); iTr++)
  354. std::cout << FieldCoefficient(iF)[iTr]<< " ";
  355. std::cout << std::endl;
  356. }
  357. #endif
  358. std::cout << "Last particle index: " << std::endl;
  359. std::cout << LastElectron() << " "
  360. << LastMuon() << " "
  361. << LastPion() << " "
  362. << LastKaon() << " "
  363. << LastProton() << " "
  364. << LastDeuteron() << " "
  365. << LastTritium() << " "
  366. << LastHe3() << " "
  367. << LastHe4() << std::endl;
  368. }