EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
CbmRichRingFinderHoughSimd.cxx
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file CbmRichRingFinderHoughSimd.cxx
1 
8 //#include "../L1/L1Algo/L1Types.h"
9 #include "../L1/L1Algo/vectors/P4_F32vec4.h"
10 #include <emmintrin.h>
11 #include "CbmRichRingFinderHoughSimd.h"
12 
13 CbmRichRingFinderHoughSimd::CbmRichRingFinderHoughSimd()
14 {
15 
16 }
17 
18 void CbmRichRingFinderHoughSimd::HoughTransformGroup(
19  unsigned short int indmin,
20  unsigned short int indmax,
21  Int_t iPart)
22 {
23 // register Float_t r12, r13, r23;
24 // register Float_t rx0, rx1, rx2, ry0, ry1,ry2; //rx[3], ry[3];//, x[3], y[3];
25  //register Float_t xc, yc, r;
26  //register Float_t xcs, ycs; // xcs = xc - fCurMinX
27  register Int_t* intX, *intY, *intR;
28  register Int_t* indXY;
29 
30  register unsigned short int iH11, iH12, iH13, iH14, iH2, iH3;
31  register Int_t nofHitsNorm = fHitInd[0].size() + 1;
32  register Int_t iPmulNofHits;
33 
34  //register Float_t t5, t10, t19, det, t6, t7;
35  register Float_t dx = 1.0f/fDx, dy = 1.0f/fDy, dr = 1.0f/fDr;
36  //register Float_t iH1X, iH1Y, iH2X, iH2Y, iH3X, iH3Y;
37 
38  fvec xcs, ycs;
39  fvec fCurMinXV = fvec(fCurMinX), fCurMinYV = fvec(fCurMinY);
40  fvec xc, yc, r;
41  fvec r12, r13, r23;
42  fvec rx0, rx1, rx2, ry0, ry1,ry2; //rx[3], ry[3];//, x[3], y[3];
43  fvec t5, t10, t19, det, t6, t7;
44  fvec iH1X, iH1Y, iH2X, iH2Y, iH3X, iH3Y;
45 // fvec intXfv, intYfv, intRfv;
46 
47  __m128i intXv, intYv, intRv, indXYv;
48  __m128i fNofBinsXv = _mm_set1_epi32(fNofBinsX);
49  __m128i fNofBinsXYv = _mm_set1_epi32(fNofBinsXY + 1);
50  __m128i m128i_0 = _mm_set1_epi32(0);
51  fvec fMinDistanceSqv = fvec(fMinDistanceSq);
52  fvec fMaxDistanceSqv = fvec(fMaxDistanceSq);
53  __m128i fNofBinsRv = _mm_set1_epi32(fNofBinsR + 1);
54  __m128i m128i_10 = _mm_set1_epi32(10);
55 
56  fvec dxv = fvec(1.0f/fDx), dyv = fvec(1.0f/fDy), drv = fvec(1.0f/fDr);
57  fvec fvec05 = fvec(0.5f);
58  fvec fvec0 = fvec(0.f);
59  Int_t nofHits = fHitInd[iPart].size();
60  if (nofHits <= fMinNofHitsInArea) return;
61  iPmulNofHits = iPart * nofHitsNorm;
62 
63  vector<unsigned short> hitIndPart;
64  hitIndPart.assign(fHitInd[iPart].begin(), fHitInd[iPart].end());
65 
66  CbmRichHoughHitVec h2, h3;
67 
68  for (unsigned short int iHit1 = 0; iHit1 < (nofHits & ~3); iHit1+=4) {
69  iH11 = hitIndPart[iHit1];
70  iH12 = hitIndPart[iHit1+1];
71  iH13 = hitIndPart[iHit1+2];
72  iH14 = hitIndPart[iHit1+3];
73 
74  iH1X = fvec(fData[iH11]->fX, fData[iH12]->fX, fData[iH13]->fX, fData[iH14]->fX);
75  iH1Y = fvec(fData[iH11]->fY, fData[iH12]->fY, fData[iH13]->fY, fData[iH14]->fY);
76 
77  for (unsigned short int iHit2 = iHit1 + 1; iHit2 < nofHits; iHit2++) {
78  iH2 = hitIndPart[iHit2];
79  h2 = fDataV[iH2];
80  iH2X = h2.fX;
81  iH2Y = h2.fY;
82 
83  rx0 = iH1X - iH2X;//rx12
84  ry0 = iH1Y- iH2Y;//ry12
85  r12 = rx0 * rx0 + ry0 * ry0;
86  if ( _mm_comineq_ss(_mm_cmpgt_ss(r12, fMinDistanceSqv), fvec0) == 1) continue;
87  if ( _mm_comineq_ss(_mm_cmplt_ss(r12, fMaxDistanceSqv), fvec0) == 1) continue;
88 
89 
90  t10 = fvec(fData[iH11]->fX2plusY2 - fData[iH2]->fX2plusY2,
91  fData[iH12]->fX2plusY2 - fData[iH2]->fX2plusY2,
92  fData[iH13]->fX2plusY2 - fData[iH2]->fX2plusY2,
93  fData[iH14]->fX2plusY2 - fData[iH2]->fX2plusY2);
94  for (unsigned short int iHit3 = iHit2 + 1; iHit3 < nofHits; iHit3++) {
95  //iH3 = hitIndPart[iHit3];
96  h3 = fDataV[ hitIndPart[iHit3] ];
97 // iH3X = h3.fX;
98 // iH3Y = h3.fY;
99  t5 = h2.fX2plusY2 - h3.fX2plusY2;
100 
101  rx1 = iH1X - iH3X;//rx13
102  ry1 = iH1Y - iH3Y;//ry13
103  r13 = rx1 * rx1 + ry1 * ry1;
104  if ( _mm_comineq_ss(_mm_cmpgt_ss(r13, fMinDistanceSqv), fvec0) == 1) continue;
105  if ( _mm_comineq_ss(_mm_cmplt_ss(r13, fMaxDistanceSqv), fvec0) == 1) continue;
106 
107  rx2 = iH2X - h3.fX;//rx23
108  ry2 = iH2Y - h3.fY;//ry23
109  r23 = rx2 * rx2 + ry2 * ry2;
110  if ( _mm_comineq_ss(_mm_cmpgt_ss(r23, fMinDistanceSqv), fvec0) == 1) continue;
111  if ( _mm_comineq_ss(_mm_cmplt_ss(r23, fMaxDistanceSqv), fvec0) == 1) continue;
112 
113  t19 = fvec05 / (rx2*ry0 - rx0*ry2);
114 
115  xc = (t5 * ry0 - t10 * ry2) * t19;
116  xcs = xc - fCurMinXV;
117  //intX = int( xcs *dx);
118  //if (intX < 0 || intX >= fNofBinsX ) continue;
119 
120  yc = (t10 * rx2 - t5 * rx0) * t19;
121  ycs = yc - fCurMinYV;
122  //intY = int( ycs *dy);
123  //if (intY < 0 || intY >= fNofBinsY ) continue;
124 
125  //radius calculation
126  t6 = iH1X - xc;
127  t7 = iH1Y - yc;
128  r = sqrt(t6 * t6 + t7 * t7);
129 
130  //intR = int(r *dr);
131  //if (intR < 0 || intR > fNofBinsR) continue;
132  //indXY = intX * fNofBinsX + intY;
133 // intXfv = xcs *dxv;
134 // intYfv = ycs *dyv;
135 // intRfv = r *drv;
136  intRv = _mm_cvtps_epi32(r *drv);
137 // if (_mm_movemask_epi8( _mm_cmpgt_epi32(intRv, m128i_10) ) == 0x0000)continue;
138 // if (_mm_movemask_epi8( _mm_cmplt_epi32(intRv, fNofBinsRv) ) == 0x0000) continue;
139 
140  intXv = _mm_cvtps_epi32(xcs *dxv);
141  intYv = _mm_cvtps_epi32(ycs *dyv);
142  indXYv = intXv * fNofBinsXv + intYv;
143 
144 // if (_mm_movemask_epi8( _mm_cmpgt_epi32(indXYv, m128i_0) ) == 0x0000) continue;
145 // if (_mm_movemask_epi8( _mm_cmplt_epi32(indXYv, fNofBinsXYv) ) == 0x0000) continue;
146 
147 
148  indXY = (int*)&indXYv;
149  intR = (int*)&intRv;
150 
151  if (intR[0] > 9 && intR[0] < fNofBinsR &&
152  indXY[0] > -1 && indXY[0] < fNofBinsXY){
153  fHist[indXY[0]]++;
154  fHistR[intR[0]]++;
155  }
156  if (intR[1] > 9 && intR[1] < fNofBinsR &&
157  indXY[1] > -1 && indXY[1] < fNofBinsXY){
158  fHist[indXY[1]]++;
159  fHistR[intR[1]]++;
160  }
161 
162  if (intR[2] > 9 && intR[2] < fNofBinsR &&
163  indXY[2] > -1 && indXY[2] < fNofBinsXY){
164  fHist[indXY[2]]++;
165  fHistR[intR[2]]++;
166  }
167 
168  if (intR[3] > 9 && intR[3] < fNofBinsR &&
169  indXY[3] > -1 && indXY[3] < fNofBinsXY){
170  fHist[indXY[3]]++;
171  fHistR[intR[3]]++;
172  }
173  }//iHit1
174  }//iHit2
175  }//iHit3
176 }
177 
178 
179 
180 void CbmRichRingFinderHoughSimd::HoughTransformReconstruction()
181 {
182  Int_t indmin, indmax;
183  Float_t x0, y0;
184 
185  fDataV.clear();
186  fDataV.reserve(fData.size());
187  for (UInt_t iHit = 0; iHit < fData.size(); iHit++){
188  CbmRichHoughHitVec h;
189  h.fX = fvec(fData[iHit].fX);
190  h.fY = fvec(fData[iHit].fY);
191  h.fX2plusY2 = fvec(fData[iHit].fX2plusY2);
192  fDataV.push_back(h);
193  }
194 
195  for (UInt_t iHit = 0; iHit < fData.size(); iHit++)
196  {
197  if (fData[iHit].fIsUsed == true) continue;
198 
199  x0 = fData[iHit].fX;
200  y0 = fData[iHit].fY;
201 
202  fCurMinX = x0 - fMaxDistance;
203  fCurMinY = y0 - fMaxDistance;
204 
205  DefineLocalAreaAndHits(x0, y0, &indmin, &indmax);
206  HoughTransform(indmin, indmax);
207  FindPeak(indmin, indmax);
208 
209  }//main loop over hits
210 }