ECCE @ EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
sPHENIXSeedFinder.cpp
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file sPHENIXSeedFinder.cpp
1 #include "sPHENIXSeedFinder.h"
2 
3 #include "CylinderKalman.h"
4 #include "Pincushion.h"
5 #include "SimpleTrack3D.h"
6 #include "vector_math_inline.h"
7 
8 #include <Eigen/Core>
9 #include <Eigen/Dense>
10 #include <Eigen/LU>
11 
12 #include <algorithm>
13 #include <cfloat>
14 #include <cmath>
15 #include <cstddef>
16 #include <iostream>
17 #include <sys/time.h>
18 #include <utility>
19 #include <xmmintrin.h>
20 
21 class HelixResolution;
22 
23 #define LogDebug(exp) std::cout<<"DEBUG: " <<__FILE__<<": "<<__LINE__<<": "<< exp
24 #define LogError(exp) std::cout<<"ERROR: " <<__FILE__<<": "<<__LINE__<<": "<< exp
25 #define LogWarning(exp) std::cout<<"WARNING: "<<__FILE__<<": "<<__LINE__<<": "<< exp
26 
27 using namespace std;
28 using namespace Eigen;
29 using namespace SeamStress;
30 
31 class hit_triplet {
32 public:
33  hit_triplet(unsigned int h1, unsigned int h2, unsigned int h3,
34  unsigned int t, float c) :
35  hit1(h1), hit2(h2), hit3(h3), track(t), chi2(c) {
36  }
38  }
39 
40  bool operator<(const hit_triplet& other) const {
41  return (hit1 < other.hit1)
42  || ((hit2 < other.hit2) && (hit1 == other.hit1))
43  || ((hit3 < other.hit3) && (hit1 == other.hit1)
44  && (hit2 == other.hit2));
45  }
46 
47  bool operator==(const hit_triplet& other) const {
48  return ((hit1 == other.hit1) && (hit2 == other.hit2)
49  && (hit3 == other.hit3));
50  }
51 
52  unsigned int hit1, hit2, hit3, track;
53  float chi2;
54 };
55 
56 class hitTriplet {
57 public:
58  hitTriplet(unsigned int h1, unsigned int h2, unsigned int h3,
59  unsigned int t, float c) :
60  hit1(h1), hit2(h2), hit3(h3), track(t), chi2(c) {
61  }
63  }
64 
65  bool operator<(const hitTriplet& other) const {
66  return (hit1 < other.hit1)
67  || ((hit2 < other.hit2) && (hit1 == other.hit1))
68  || ((hit3 < other.hit3) && (hit1 == other.hit1)
69  && (hit2 == other.hit2));
70  }
71 
72  bool operator==(const hitTriplet& other) const {
73  return ((hit1 == other.hit1) && (hit2 == other.hit2)
74  && (hit3 == other.hit3));
75  }
76 
77  unsigned int hit1, hit2, hit3, track;
78  float chi2;
79 };
80 
81 void sPHENIXSeedFinder::tripletRejection(vector<SimpleTrack3D>& input,
82  vector<SimpleTrack3D>& /*output*/, vector<bool>& usetrack,
83  vector<float>& next_best_chi2) {
84 
85  vector<hitTriplet> trips;
86  for (unsigned int i = 0; i < input.size(); ++i) {
87  for (unsigned int h1 = 0; h1 < input[i].hits.size(); ++h1) {
88  for (unsigned int h2 = (h1 + 1); h2 < input[i].hits.size(); ++h2) {
89  for (unsigned int h3 = (h2 + 1); h3 < input[i].hits.size(); ++h3) {
90  //if (cut_on_dca == false)
91  {
92  trips.push_back(hitTriplet(
93  input[i].hits[h1].get_id(), input[i].hits[h2].get_id(),
94  input[i].hits[h3].get_id(), i, track_states[i].chi2));
95  }
96  }
97  }
98  }
99  }
100  if (trips.size() == 0) {
101  return;
102  }
103  sort(trips.begin(), trips.end());
104  unsigned int pos = 0;
105  // unsigned int cur_h1 = trips[pos].hit1;
106  // unsigned int cur_h2 = trips[pos].hit2;
107  while (pos < trips.size()) {
108  unsigned int next_pos = pos + 1;
109  if (next_pos >= trips.size()) {
110  break;
111  }
112  while (trips[pos] == trips[next_pos]) {
113  next_pos += 1;
114  if (next_pos >= trips.size()) {
115  break;
116  }
117  }
118  if ((next_pos - pos) > 1) {
119  float best_chi2 = trips[pos].chi2;
120  float next_chi2 = trips[pos + 1].chi2;
121  unsigned int best_pos = pos;
122  for (unsigned int i = (pos + 1); i < next_pos; ++i) {
123  if (input[trips[i].track].hits.size() <
124  input[trips[best_pos].track].hits.size()) {
125  continue;
126  } else if ((input[trips[i].track].hits.size() >
127  input[trips[best_pos].track].hits.size()) ||
128  (input[trips[i].track].hits.back().get_layer() >
129  input[trips[best_pos].track].hits.back().get_layer())) {
130  next_chi2 = best_chi2;
131  best_chi2 = trips[i].chi2;
132  best_pos = i;
133  continue;
134  }
135  if ((trips[i].chi2 < best_chi2) ||
136  (usetrack[trips[best_pos].track] == false)) {
137  next_chi2 = best_chi2;
138  best_chi2 = trips[i].chi2;
139  best_pos = i;
140  } else if (trips[i].chi2 < next_chi2) {
141  next_chi2 = trips[i].chi2;
142  }
143  }
144  for (unsigned int i = pos; i < next_pos; ++i) {
145  if (i != best_pos) {
146  usetrack[trips[i].track] = false;
147  } else {
148  next_best_chi2[trips[i].track] = next_chi2;
149  }
150  }
151  }
152  pos = next_pos;
153  // cur_h1 = trips[pos].hit1;
154  // cur_h2 = trips[pos].hit2;
155  }
156 }
157 
158 sPHENIXSeedFinder::sPHENIXSeedFinder(unsigned int n_phi, unsigned int n_d,
159  unsigned int n_k, unsigned int n_dzdl,
160  unsigned int n_z0,
161  HelixResolution& min_resolution,
162  HelixResolution& max_resolution,
163  HelixRange& range, vector<float>& material,
164  vector<float>& radius, float Bfield)
165  : HelixHough(n_phi, n_d, n_k, n_dzdl, n_z0, min_resolution, max_resolution,
166  range),
167  fast_chi2_cut_par0(12.),
168  fast_chi2_cut_par1(0.),
169  fast_chi2_cut_max(FLT_MAX),
170  chi2_cut(3.),
171  chi2_removal_cut(1.),
172  n_removal_hits(0),
173  seeding(false),
174  verbosity(0),
175  cut_on_dca(false),
176  dca_cut(0.01),
177  vertex_x(0.),
178  vertex_y(0.),
179  vertex_z(0.),
180  required_layers(0),
181  reject_ghosts(false),
182  nfits(0),
183  findtracksiter(0),
184  prev_max_k(0.),
185  prev_max_dzdl(0.),
186  prev_p_inv(0.),
187  seed_layer(0),
188  ca_chi2_cut(2.0),
189  cosang_cut(0.985) {
190  vector<float> detector_material;
191 
192  for (unsigned int i = 0; i < radius.size(); ++i) {
193  detector_radii.push_back(radius[i]);
194  }
195  for (unsigned int i = 0; i < material.size(); ++i) {
196  detector_scatter.push_back(1.41421356237309515 * 0.0136 *
197  sqrt(3. * material[i]));
198  detector_material.push_back(3. * material[i]);
199  }
200 
201  detector_B_field = Bfield;
202 
203  integrated_scatter.assign(detector_scatter.size(), 0.);
204  float total_scatter_2 = 0.;
205  for (unsigned int l = 0; l < detector_scatter.size(); ++l) {
206  total_scatter_2 += detector_scatter[l] * detector_scatter[l];
207  integrated_scatter[l] = sqrt(total_scatter_2);
208  }
209 
210  kalman =
211  new CylinderKalman(detector_radii, detector_material, detector_B_field);
212 
213  vector<SimpleHit3D> one_layer;
214  layer_sorted.assign(n_layers, one_layer);
215  for (unsigned int i = 0; i < 4; ++i) {
216  layer_sorted_1[i].assign(n_layers, one_layer);
217  }
218  temp_comb.assign(n_layers, 0);
219 }
220 
221 sPHENIXSeedFinder::sPHENIXSeedFinder(vector<vector<unsigned int> >& zoom_profile,
222  unsigned int minzoom, HelixRange& range,
223  vector<float>& material, vector<float>& radius,
224  float Bfield, bool parallel,
225  unsigned int num_threads)
226  : HelixHough(zoom_profile, minzoom, range),
227  fast_chi2_cut_par0(12.),
228  fast_chi2_cut_par1(0.),
229  fast_chi2_cut_max(FLT_MAX),
230  chi2_cut(3.),
231  chi2_removal_cut(1.),
232  n_removal_hits(0),
233  seeding(false),
234  verbosity(0),
235  cut_on_dca(false),
236  dca_cut(0.01),
237  vertex_x(0.),
238  vertex_y(0.),
239  vertex_z(0.),
240  required_layers(0),
241  reject_ghosts(false),
242  nfits(0),
243  findtracksiter(0),
244  prev_max_k(0.),
245  prev_max_dzdl(0.),
246  prev_p_inv(0.),
247  seed_layer(0),
248  nthreads(num_threads),
249  vssp(NULL),
250  pins(NULL),
251  is_parallel(parallel),
252  is_thread(false),
253  ca_chi2_cut(2.0),
254  cosang_cut(0.985) {
255  vector<float> detector_material;
256 
257  for (unsigned int i = 0; i < radius.size(); ++i) {
258  detector_radii.push_back(radius[i]);
259  }
260  for (unsigned int i = 0; i < material.size(); ++i) {
261  detector_scatter.push_back(
262  1.41421356237309515 * 0.0136 * sqrt(3. * material[i]));
263  detector_material.push_back(3. * material[i]);
264  }
265 
266  detector_B_field = Bfield;
267 
268  integrated_scatter.assign(detector_scatter.size(), 0.);
269  float total_scatter_2 = 0.;
270  for (unsigned int l = 0; l < detector_scatter.size(); ++l) {
271  total_scatter_2 += detector_scatter[l] * detector_scatter[l];
272  integrated_scatter[l] = sqrt(total_scatter_2);
273  }
274 
275  kalman = new CylinderKalman(detector_radii, detector_material,
277 
278  vector<SimpleHit3D> one_layer;
279  layer_sorted.assign(n_layers, one_layer);
280  for (unsigned int i = 0; i < 4; ++i) {
281  layer_sorted_1[i].assign(n_layers, one_layer);
282  }
283  temp_comb.assign(n_layers, 0);
284 
285  if (is_parallel == true) {
286  Seamstress::init_vector(num_threads, vss);
287 
288  vssp = new vector<Seamstress*>();
289  for (unsigned int i = 0; i < vss.size(); i++) {
290  vssp->push_back(&(vss[i]));
291  }
292 
294 
295  vector<vector<unsigned int> > zoom_profile_new;
296  for (unsigned int i = 1; i < zoom_profile.size(); ++i) {
297  zoom_profile_new.push_back(zoom_profile[i]);
298  }
299 
300  for (unsigned int i = 0; i < nthreads; ++i) {
301  thread_trackers.push_back(
302  new sPHENIXSeedFinder(zoom_profile, minzoom, range,
303  material, radius, Bfield));
304  thread_trackers.back()->setThread();
305  thread_trackers.back()->setStartZoom(1);
306  thread_tracks.push_back(vector<SimpleTrack3D>());
307  thread_ranges.push_back(HelixRange());
308  thread_hits.push_back(vector<SimpleHit3D>());
309  split_output_hits.push_back(new vector<vector<SimpleHit3D> >());
310  split_ranges.push_back(new vector<HelixRange>());
311  split_input_hits.push_back(vector<SimpleHit3D>());
312  }
313  }
314 }
315 
317  if (kalman != NULL)
318  delete kalman;
319  for (unsigned int i = 0; i < vss.size(); i++) {
320  vss[i].stop();
321  }
322  for (unsigned int i = 0; i < thread_trackers.size(); ++i) {
323  delete thread_trackers[i];
324  delete split_output_hits[i];
325  delete split_ranges[i];
326  }
327 
328  if (pins != NULL)
329  delete pins;
330  if (vssp != NULL)
331  delete vssp;
332 }
333 
334 float sPHENIXSeedFinder::kappaToPt(float kappa) {
335  return detector_B_field / 333.6 / kappa;
336 }
337 
339  return detector_B_field / 333.6 / pt;
340 }
341 
342 // hel should be +- 1
343 // static void xyTangent(SimpleHit3D& hit1, SimpleHit3D& hit2, float kappa,
344 // float hel, float& ux_out, float& uy_out, float& ux_in, float& uy_in) {
345 // float x = hit2.get_x() - hit1.get_x();
346 // float y = hit2.get_y() - hit1.get_y();
347 // float D = sqrt(x * x + y * y);
348 // float ak = 0.5 * kappa * D;
349 // float D_inv = 1. / D;
350 // float hk = sqrt(1. - ak * ak);
351 
352 // float kcx = (ak * x + hel * hk * y) * D_inv;
353 // float kcy = (ak * y - hel * hk * x) * D_inv;
354 // float ktx = -(kappa * y - kcy);
355 // float kty = kappa * x - kcx;
356 // float norm = 1. / sqrt(ktx * ktx + kty * kty);
357 // ux_out = ktx * norm;
358 // uy_out = kty * norm;
359 
360 // ktx = kcy;
361 // kty = -kcx;
362 // norm = 1. / sqrt(ktx * ktx + kty * kty);
363 // ux_in = ktx * norm;
364 // uy_in = kty * norm;
365 // }
366 
367 // hel should be +- 1
368 // static float cosScatter(SimpleHit3D& hit1, SimpleHit3D& hit2, SimpleHit3D& hit3,
369 // float kappa, float hel) {
370 // float ux_in = 0.;
371 // float uy_in = 0.;
372 // float ux_out = 0.;
373 // float uy_out = 0.;
374 
375 // float temp1 = 0.;
376 // float temp2 = 0.;
377 
378 // xyTangent(hit1, hit2, kappa, hel, ux_in, uy_in, temp1, temp2);
379 // xyTangent(hit2, hit3, kappa, hel, temp1, temp2, ux_out, uy_out);
380 
381 // return ux_in * ux_out + uy_in * uy_out;
382 // }
383 
384 // static float dzdsSimple(SimpleHit3D& hit1, SimpleHit3D& hit2, float k) {
385 // float x = hit2.get_x() - hit1.get_x();
386 // float y = hit2.get_y() - hit1.get_y();
387 // float D = sqrt(x * x + y * y);
388 // float s = 0.;
389 // float temp1 = k * D * 0.5;
390 // temp1 *= temp1;
391 // float temp2 = D * 0.5;
392 // s += 2. * temp2;
393 // temp2 *= temp1;
394 // s += temp2 / 3.;
395 // temp2 *= temp1;
396 // s += (3. / 20.) * temp2;
397 // temp2 *= temp1;
398 // s += (5. / 56.) * temp2;
399 
400 // return (hit2.get_z() - hit1.get_z()) / s;
401 // }
402 
404  float vy) {
405  float d_out = 0.;
406 
407  // find point at the dca to 0
408  float x0 = track.d * cos(track.phi);
409  float y0 = track.d * sin(track.phi);
410 
411  // change variables so x0,y0 -> 0,0
412  float phi2 = atan2(
413  (1. + track.kappa * track.d) * sin(track.phi) - track.kappa * y0,
414  (1. + track.kappa * track.d) * cos(track.phi) - track.kappa * x0);
415 
416  // translate so that (0,0) -> (x0 - vx , y0 - vy)
417  float cosphi = cos(phi2);
418  float sinphi = sin(phi2);
419  float tx = cosphi + track.kappa * (x0 - vx);
420  float ty = sinphi + track.kappa * (y0 - vy);
421  float dk = sqrt(tx * tx + ty * ty) - 1.;
422  if (track.kappa == 0.) {
423  d_out = (x0 - vx) * cosphi + (y0 - vy) * sinphi;
424  } else {
425  d_out = dk / track.kappa;
426  }
427  return fabs(d_out);
428 }
429 
430 void sPHENIXSeedFinder::finalize(vector<SimpleTrack3D>& input,
431  vector<SimpleTrack3D>& output) {
432 
433  if (is_thread == true) {
434  for (unsigned int i = 0; i < input.size(); ++i) {
435  output.push_back(input[i]);
436  }
437  return;
438  }
439 
440  unsigned int nt = input.size();
441  vector<bool> usetrack;
442  usetrack.assign(input.size(), true);
443  vector<float> next_best_chi2;
444  next_best_chi2.assign(input.size(), 99999.);
445 
446  //LogDebug("input size: ")<<input.size()<<endl;
447 
448  if (reject_ghosts == true) {
449  tripletRejection(input, output, usetrack, next_best_chi2);
450  }
451 
452  vector<HelixKalmanState> states_new;
453 
454  for (unsigned int i = 0; i < nt; ++i) {
455  if (usetrack[i] == true) {
456  if (!(track_states[i].chi2 == track_states[i].chi2)) {
457  continue;
458  }
459 
460  output.push_back(input[i]);
461  output.back().index = (output.size() - 1);
462  states_new.push_back(track_states[i]);
463  isolation_variable.push_back(next_best_chi2[i]);
464  }
465  }
466 
467  //LogDebug("output size: ")<<output.size()<<endl;
468 
469  track_states = states_new;
470  if (smooth_back == true) {
471  for (unsigned int i = 0; i < output.size(); ++i) {
472 
473  HelixKalmanState state = track_states[i];
474 
475  track_states[i].C *= 3.;
476  track_states[i].chi2 = 0.;
477  track_states[i].x_int = 0.;
478  track_states[i].y_int = 0.;
479  track_states[i].z_int = 0.;
480  // track_states[i].position = output[i].hits.size();
481  track_states[i].position = 0;
482  for (int h = (output[i].hits.size() - 1); h >= 0; --h) {
483  SimpleHit3D hit = output[i].hits[h];
484  float err_scale = 1.;
485  int layer = hit.get_layer();
486  if ((layer >= 0) && (layer < (int) (hit_error_scale.size()))) {
487  err_scale = hit_error_scale[layer];
488  }
489  err_scale *= 3.0; // fudge factor, like needed due to non-gaussian errors
490 
491  // \todo location of a rescale fudge factor
492 
493  kalman->addHit(hit, track_states[i]);
494  track_states[i].position = h;
495  }
496 
497  // track_states[i].C = state.C;
498  track_states[i].chi2 = state.chi2;
499  track_states[i].C *= 2. / 3.;
500 
501  output[i].phi = track_states[i].phi;
502  output[i].d = track_states[i].d;
503  output[i].kappa = track_states[i].kappa;
504  output[i].z0 = track_states[i].z0;
505  output[i].dzdl = track_states[i].dzdl;
506  }
507  }
508 
509  if (verbosity > 0) {
510  cout << "# fits = " << nfits << endl;
511  cout << "findTracks called " << findtracksiter << " times" << endl;
512  cout << "CAtime = " << CAtime << endl;
513  cout << "KALime = " << KALtime << endl;
514  }
515 }
516 
517 void sPHENIXSeedFinder::findTracks(vector<SimpleHit3D>& hits,
518  vector<SimpleTrack3D>& tracks, const HelixRange& range) {
519 
520  findtracksiter += 1;
521  findTracksBySegments(hits, tracks, range);
522 }
523 
524 bool sPHENIXSeedFinder::breakRecursion(const vector<SimpleHit3D>& hits,
525  const HelixRange& /*range*/) {
526  if (seeding == true) {
527  return false;
528  }
529  unsigned int layer_mask[4] = { 0, 0, 0, 0 };
530  for (unsigned int i = 0; i < hits.size(); ++i) {
531  if (hits[i].get_layer() < 32) {
532  layer_mask[0] = layer_mask[0] | (1 << hits[i].get_layer());
533  } else if (hits[i].get_layer() < 64) {
534  layer_mask[1] = layer_mask[1] | (1 << (hits[i].get_layer() - 32));
535  } else if (hits[i].get_layer() < 96) {
536  layer_mask[2] = layer_mask[2] | (1 << (hits[i].get_layer() - 64));
537  } else if (hits[i].get_layer() < 128) {
538  layer_mask[3] = layer_mask[3] | (1 << (hits[i].get_layer() - 96));
539  }
540  }
541  unsigned int nlayers = __builtin_popcount(layer_mask[0])
542  + __builtin_popcount(layer_mask[1])
543  + __builtin_popcount(layer_mask[2])
544  + __builtin_popcount(layer_mask[3]);
545 
546  return (nlayers < required_layers);
547 }
548 
549 float sPHENIXSeedFinder::phiError(SimpleHit3D& hit, float /*min_k*/, float max_k,
550  float min_d, float max_d, float /*min_z0*/, float /*max_z0*/, float /*min_dzdl*/,
551  float max_dzdl, bool pairvoting) {
552  float Bfield_inv = 1. / detector_B_field;
553  float p_inv = 0.;
554 
555  if ((prev_max_k == max_k) && (prev_max_dzdl == max_dzdl)) {
556  p_inv = prev_p_inv;
557  } else {
558  prev_max_k = max_k;
559  prev_max_dzdl = max_dzdl;
560  prev_p_inv = 3.33333333333333314e+02 * max_k * Bfield_inv
561  * sqrt(1. - max_dzdl * max_dzdl);
562  p_inv = prev_p_inv;
563  }
564  float total_scatter_2 = 0.;
565  for (int i = seed_layer + 1; i <= (hit.get_layer()); ++i) {
566  float this_scatter = detector_scatter[i - 1]
567  * (detector_radii[i] - detector_radii[i - 1])
568  / detector_radii[i];
569  total_scatter_2 += this_scatter * this_scatter;
570  }
571  float angle = p_inv * sqrt(total_scatter_2) * 1.0;
572  float dsize = 0.5 * (max_d - min_d);
573  float angle_from_d = dsize / detector_radii[hit.get_layer()];
574  float returnval = 0.;
575  if (pairvoting == false) {
576  if (angle_from_d > angle) {
577  returnval = 0.;
578  } else {
579  returnval = (angle - angle_from_d);
580  }
581  } else {
582  returnval = angle;
583  }
584 
585  return returnval;
586 }
587 
588 float sPHENIXSeedFinder::dzdlError(SimpleHit3D& hit, float /*min_k*/, float max_k,
589  float /*min_d*/, float /*max_d*/, float min_z0, float max_z0, float /*min_dzdl*/,
590  float max_dzdl, bool pairvoting) {
591  float Bfield_inv = 1. / detector_B_field;
592  float p_inv = 0.;
593 
594  if ((prev_max_k == max_k) && (prev_max_dzdl == max_dzdl)) {
595  p_inv = prev_p_inv;
596  } else {
597  prev_max_k = max_k;
598  prev_max_dzdl = max_dzdl;
599  prev_p_inv = 3.33333333333333314e+02 * max_k * Bfield_inv
600  * sqrt(1. - max_dzdl * max_dzdl);
601  p_inv = prev_p_inv;
602  }
603  float total_scatter_2 = 0.;
604  for (int i = seed_layer + 1; i <= (hit.get_layer()); ++i) {
605  float this_scatter = detector_scatter[i - 1]
606  * (detector_radii[i] - detector_radii[i - 1])
607  / detector_radii[i];
608  total_scatter_2 += this_scatter * this_scatter;
609  }
610  float angle = p_inv * sqrt(total_scatter_2) * 1.0;
611  float z0size = 0.5 * (max_z0 - min_z0);
612  float angle_from_z0 = z0size / detector_radii[hit.get_layer()];
613  float returnval = 0.;
614  if (pairvoting == false) {
615  if (angle_from_z0 > angle) {
616  returnval = 0.;
617  } else {
618  returnval = (angle - angle_from_z0);
619  }
620  } else {
621  returnval = angle;
622  }
623 
624  return returnval;
625 }
626 
628  SimpleTrack3D& seed) {
629  HelixKalmanState* state = &(seed_states[seed.index]);
630 
631  float dphi = 2. * sqrt(state->C(0, 0));
632  float dd = 2. * sqrt(state->C(1, 1));
633  float dk = 2. * state->C(2, 2);
634  float dz0 = 2. * sqrt(state->C(3, 3));
635  float ddzdl = 2. * sqrt(state->C(4, 4));
636 
637  range.min_phi = seed.phi - dphi;
638  range.max_phi = seed.phi + dphi;
639  if (range.min_phi < 0.) {
640  range.min_phi = 0.;
641  }
642  if (range.max_phi > 2. * M_PI) {
643  range.max_phi = 2. * M_PI;
644  }
645  range.min_d = seed.d - dd;
646  range.max_d = seed.d + dd;
647  range.min_k = seed.kappa - dk;
648  range.max_k = seed.kappa + dk;
649  if (range.min_k < 0.) {
650  range.min_k = 0.;
651  }
652 
653  range.min_k = range.min_k * range.min_k;
654  range.max_k = range.max_k * range.max_k;
655 
656  range.min_dzdl = seed.dzdl - ddzdl;
657  range.max_dzdl = seed.dzdl + ddzdl;
658  range.min_z0 = seed.z0 - dz0;
659  range.max_z0 = seed.z0 + dz0;
660 }
661 
663  vector<float> chi2_hit;
664  return sPHENIXSeedFinder::fitTrack(track, chi2_hit);
665 }
666 
668  vector<float>& chi2_hit) {
669  chi2_hit.clear();
670  vector<float> xyres;
671  vector<float> xyres_inv;
672  vector<float> zres;
673  vector<float> zres_inv;
674  for (unsigned int i = 0; i < track.hits.size(); i++) {
675 
677 
678  xyres.push_back(
679  sqrt(
680  (0.5 * sqrt(12.) * sqrt(track.hits[i].get_size(0, 0)))
681  * (0.5 * sqrt(12.)
682  * sqrt(track.hits[i].get_size(0, 0)))
683  + (0.5 * sqrt(12.)
684  * sqrt(track.hits[i].get_size(1, 1)))
685  * (0.5 * sqrt(12.)
686  * sqrt(
687  track.hits[i].get_size(
688  1, 1)))));
689  xyres_inv.push_back(1. / xyres.back());
690  zres.push_back((0.5 * sqrt(12.) * sqrt(track.hits[i].get_size(2, 2))));
691  zres_inv.push_back(1. / zres.back());
692  }
693 
694  chi2_hit.resize(track.hits.size(), 0.);
695 
696  MatrixXf y = MatrixXf::Zero(track.hits.size(), 1);
697  for (unsigned int i = 0; i < track.hits.size(); i++) {
698  y(i, 0) =
699  (pow(track.hits[i].get_x(), 2) + pow(track.hits[i].get_y(), 2));
700  y(i, 0) *= xyres_inv[i];
701  }
702 
703  MatrixXf X = MatrixXf::Zero(track.hits.size(), 3);
704  for (unsigned int i = 0; i < track.hits.size(); i++) {
705  X(i, 0) = track.hits[i].get_x();
706  X(i, 1) = track.hits[i].get_y();
707  X(i, 2) = -1.;
708  X(i, 0) *= xyres_inv[i];
709  X(i, 1) *= xyres_inv[i];
710  X(i, 2) *= xyres_inv[i];
711  }
712 
713  MatrixXf Xt = X.transpose();
714 
715  MatrixXf prod = Xt * X;
716 
717  MatrixXf Xty = Xt * y;
718  MatrixXf beta = prod.ldlt().solve(Xty);
719 
720  float cx = beta(0, 0) * 0.5;
721  float cy = beta(1, 0) * 0.5;
722  float r = sqrt(cx * cx + cy * cy - beta(2, 0));
723 
724  float phi = atan2(cy, cx);
725  float d = sqrt(cx * cx + cy * cy) - r;
726  float k = 1. / r;
727 
728  MatrixXf diff = y - (X * beta);
729  MatrixXf chi2 = (diff.transpose()) * diff;
730 
731  float dx = d * cos(phi);
732  float dy = d * sin(phi);
733 
734  MatrixXf y2 = MatrixXf::Zero(track.hits.size(), 1);
735  for (unsigned int i = 0; i < track.hits.size(); i++) {
736  y2(i, 0) = track.hits[i].get_z();
737  y2(i, 0) *= zres_inv[i];
738  }
739 
740  MatrixXf X2 = MatrixXf::Zero(track.hits.size(), 2);
741  for (unsigned int i = 0; i < track.hits.size(); i++) {
742  float D = sqrt(
743  pow(dx - track.hits[i].get_x(), 2)
744  + pow(dy - track.hits[i].get_y(), 2));
745  float s = 0.0;
746 
747  if (0.5 * k * D > 0.1) {
748  float v = 0.5 * k * D;
749  if (v >= 0.999999) {
750  v = 0.999999;
751  }
752  s = 2. * asin(v) / k;
753  } else {
754  float temp1 = k * D * 0.5;
755  temp1 *= temp1;
756  float temp2 = D * 0.5;
757  s += 2. * temp2;
758  temp2 *= temp1;
759  s += temp2 / 3.;
760  temp2 *= temp1;
761  s += (3. / 20.) * temp2;
762  temp2 *= temp1;
763  s += (5. / 56.) * temp2;
764  }
765 
766  X2(i, 0) = s;
767  X2(i, 1) = 1.0;
768 
769  X2(i, 0) *= zres_inv[i];
770  X2(i, 1) *= zres_inv[i];
771  }
772 
773  MatrixXf Xt2 = X2.transpose();
774  MatrixXf prod2 = Xt2 * X2;
775 
776  MatrixXf Xty2 = Xt2 * y2;
777  MatrixXf beta2 = prod2.ldlt().solve(Xty2);
778 
779  MatrixXf diff2 = y2 - (X2 * beta2);
780  MatrixXf chi2_z = (diff2.transpose()) * diff2;
781 
782  float z0 = beta2(1, 0);
783  float dzdl = beta2(0, 0) / sqrt(1. + beta2(0, 0) * beta2(0, 0));
784 
785  track.phi = phi;
786  track.d = d;
787  track.kappa = k;
788  track.dzdl = dzdl;
789  track.z0 = z0;
790 
791  if (track.kappa != 0.) {
792  r = 1. / track.kappa;
793  } else {
794  r = 1.0e10;
795  }
796 
797  cx = (track.d + r) * cos(track.phi);
798  cy = (track.d + r) * sin(track.phi);
799 
800  float chi2_tot = 0.;
801  for (unsigned int h = 0; h < track.hits.size(); h++) {
802  float dx1 = track.hits[h].get_x() - cx;
803  float dy1 = track.hits[h].get_y() - cy;
804 
805  float dx2 = track.hits[h].get_x() + cx;
806  float dy2 = track.hits[h].get_y() + cy;
807 
808  float xydiff1 = sqrt(dx1 * dx1 + dy1 * dy1) - r;
809  float xydiff2 = sqrt(dx2 * dx2 + dy2 * dy2) - r;
810  float xydiff = xydiff2;
811  if (fabs(xydiff1) < fabs(xydiff2)) {
812  xydiff = xydiff1;
813  }
814 
815  float ls_xy = xyres[h];
816 
817  chi2_hit[h] = 0.;
818  chi2_hit[h] += xydiff * xydiff / (ls_xy * ls_xy);
819  chi2_hit[h] += diff2(h, 0) * diff2(h, 0);
820 
821  chi2_tot += chi2_hit[h];
822  }
823 
824  unsigned int deg_of_freedom = 2 * track.hits.size() - 5;
825 
826  return (chi2_tot) / ((float) (deg_of_freedom));
827 }
828 
829 void sPHENIXSeedFinder::calculateKappaTangents(float* x1_a, float* y1_a,
830  float* z1_a, float* x2_a, float* y2_a, float* z2_a, float* x3_a,
831  float* y3_a, float* z3_a, float* dx1_a, float* dy1_a, float* dz1_a,
832  float* dx2_a, float* dy2_a, float* dz2_a, float* dx3_a, float* dy3_a,
833  float* dz3_a, float* kappa_a, float* dkappa_a, float* ux_mid_a,
834  float* uy_mid_a, float* ux_end_a, float* uy_end_a, float* dzdl_1_a,
835  float* dzdl_2_a, float* ddzdl_1_a, float* ddzdl_2_a) {
836  static const __m128 two = { 2., 2., 2., 2. };
837 
838  __m128 x1 = _mm_load_ps(x1_a);
839  __m128 x2 = _mm_load_ps(x2_a);
840  __m128 x3 = _mm_load_ps(x3_a);
841  __m128 y1 = _mm_load_ps(y1_a);
842  __m128 y2 = _mm_load_ps(y2_a);
843  __m128 y3 = _mm_load_ps(y3_a);
844  __m128 z1 = _mm_load_ps(z1_a);
845  __m128 z2 = _mm_load_ps(z2_a);
846  __m128 z3 = _mm_load_ps(z3_a);
847 
848  __m128 dx1 = _mm_load_ps(dx1_a);
849  __m128 dx2 = _mm_load_ps(dx2_a);
850  __m128 dx3 = _mm_load_ps(dx3_a);
851  __m128 dy1 = _mm_load_ps(dy1_a);
852  __m128 dy2 = _mm_load_ps(dy2_a);
853  __m128 dy3 = _mm_load_ps(dy3_a);
854  __m128 dz1 = _mm_load_ps(dz1_a);
855  __m128 dz2 = _mm_load_ps(dz2_a);
856  __m128 dz3 = _mm_load_ps(dz3_a);
857 
858  __m128 D12 = _mm_sub_ps(x2, x1);
859  D12 = _mm_mul_ps(D12, D12);
860  __m128 tmp1 = _mm_sub_ps(y2, y1);
861  tmp1 = _mm_mul_ps(tmp1, tmp1);
862  D12 = _mm_add_ps(D12, tmp1);
863  D12 = _vec_sqrt_ps(D12);
864 
865  __m128 D23 = _mm_sub_ps(x3, x2);
866  D23 = _mm_mul_ps(D23, D23);
867  tmp1 = _mm_sub_ps(y3, y2);
868  tmp1 = _mm_mul_ps(tmp1, tmp1);
869  D23 = _mm_add_ps(D23, tmp1);
870  D23 = _vec_sqrt_ps(D23);
871 
872  __m128 D31 = _mm_sub_ps(x1, x3);
873  D31 = _mm_mul_ps(D31, D31);
874  tmp1 = _mm_sub_ps(y1, y3);
875  tmp1 = _mm_mul_ps(tmp1, tmp1);
876  D31 = _mm_add_ps(D31, tmp1);
877  D31 = _vec_sqrt_ps(D31);
878 
879  __m128 k = _mm_mul_ps(D12, D23);
880  k = _mm_mul_ps(k, D31);
881  k = _vec_rec_ps(k);
882  tmp1 = (D12 + D23 + D31) * (D23 + D31 - D12) * (D12 + D31 - D23)
883  * (D12 + D23 - D31);
884  tmp1 = _vec_sqrt_ps(tmp1);
885  k *= tmp1;
886 
887  __m128 tmp2 = _mm_cmpgt_ps(tmp1, zero);
888  tmp1 = _mm_and_ps(tmp2, k);
889  tmp2 = _mm_andnot_ps(tmp2, zero);
890  k = _mm_xor_ps(tmp1, tmp2);
891 
892  _mm_store_ps(kappa_a, k);
893  __m128 k_inv = _vec_rec_ps(k);
894 
895  __m128 D12_inv = _vec_rec_ps(D12);
896  __m128 D23_inv = _vec_rec_ps(D23);
897  __m128 D31_inv = _vec_rec_ps(D31);
898 
899  __m128 dr1 = dx1 * dx1 + dy1 * dy1;
900  dr1 = _vec_sqrt_ps(dr1);
901  __m128 dr2 = dx2 * dx2 + dy2 * dy2;
902  dr2 = _vec_sqrt_ps(dr2);
903  __m128 dr3 = dx3 * dx3 + dy3 * dy3;
904  dr3 = _vec_sqrt_ps(dr3);
905 
906  __m128 dk1 = (dr1 + dr2) * D12_inv * D12_inv;
907  __m128 dk2 = (dr2 + dr3) * D23_inv * D23_inv;
908  __m128 dk = dk1 + dk2;
909  _mm_store_ps(dkappa_a, dk);
910 
911  __m128 ux12 = (x2 - x1) * D12_inv;
912  __m128 uy12 = (y2 - y1) * D12_inv;
913  __m128 ux23 = (x3 - x2) * D23_inv;
914  __m128 uy23 = (y3 - y2) * D23_inv;
915  __m128 ux13 = (x3 - x1) * D31_inv;
916  __m128 uy13 = (y3 - y1) * D31_inv;
917 
918  __m128 cosalpha = ux12 * ux13 + uy12 * uy13;
919  __m128 sinalpha = ux13 * uy12 - ux12 * uy13;
920 
921  __m128 ux_mid = ux23 * cosalpha - uy23 * sinalpha;
922  __m128 uy_mid = ux23 * sinalpha + uy23 * cosalpha;
923  _mm_store_ps(ux_mid_a, ux_mid);
924  _mm_store_ps(uy_mid_a, uy_mid);
925 
926  __m128 ux_end = ux23 * cosalpha + uy23 * sinalpha;
927  __m128 uy_end = uy23 * cosalpha - ux23 * sinalpha;
928 
929  _mm_store_ps(ux_end_a, ux_end);
930  _mm_store_ps(uy_end_a, uy_end);
931 
932  // asin(x) = 2*atan( x/( 1 + sqrt( 1 - x*x ) ) )
933  __m128 v = one - sinalpha * sinalpha;
934  v = _vec_sqrt_ps(v);
935  v += one;
936  v = _vec_rec_ps(v);
937  v *= sinalpha;
938  __m128 s2 = _vec_atan_ps(v);
939  s2 *= two;
940  s2 *= k_inv;
941  tmp1 = _mm_cmpgt_ps(k, zero);
942  tmp2 = _mm_and_ps(tmp1, s2);
943  tmp1 = _mm_andnot_ps(tmp1, D23);
944  s2 = _mm_xor_ps(tmp1, tmp2);
945 
946  // dz/dl = (dz/ds)/sqrt(1 + (dz/ds)^2)
947  // = dz/sqrt(s^2 + dz^2)
948  __m128 del_z_2 = z3 - z2;
949  __m128 dzdl_2 = s2 * s2 + del_z_2 * del_z_2;
950  dzdl_2 = _vec_rsqrt_ps(dzdl_2);
951  dzdl_2 *= del_z_2;
952  __m128 ddzdl_2 = (dz2 + dz3) * D23_inv;
953  _mm_store_ps(dzdl_2_a, dzdl_2);
954  _mm_store_ps(ddzdl_2_a, ddzdl_2);
955 
956  sinalpha = ux13 * uy23 - ux23 * uy13;
957  v = one - sinalpha * sinalpha;
958  v = _vec_sqrt_ps(v);
959  v += one;
960  v = _vec_rec_ps(v);
961  v *= sinalpha;
962  __m128 s1 = _vec_atan_ps(v);
963  s1 *= two;
964  s1 *= k_inv;
965  tmp1 = _mm_cmpgt_ps(k, zero);
966  tmp2 = _mm_and_ps(tmp1, s1);
967  tmp1 = _mm_andnot_ps(tmp1, D12);
968  s1 = _mm_xor_ps(tmp1, tmp2);
969 
970  __m128 del_z_1 = z2 - z1;
971  __m128 dzdl_1 = s1 * s1 + del_z_1 * del_z_1;
972  dzdl_1 = _vec_rsqrt_ps(dzdl_1);
973  dzdl_1 *= del_z_1;
974  __m128 ddzdl_1 = (dz1 + dz2) * D12_inv;
975  _mm_store_ps(dzdl_1_a, dzdl_1);
976  _mm_store_ps(ddzdl_1_a, ddzdl_1);
977 }
978 
979 void sPHENIXSeedFinder::calculateKappaTangents(float* x1_a, float* y1_a,
980  float* z1_a, float* x2_a, float* y2_a, float* z2_a, float* x3_a,
981  float* y3_a, float* z3_a, float* dx1_a, float* dy1_a, float* dz1_a,
982  float* dx2_a, float* dy2_a, float* dz2_a, float* dx3_a, float* dy3_a,
983  float* dz3_a, float* kappa_a, float* dkappa_a, float* ux_mid_a,
984  float* uy_mid_a, float* ux_end_a, float* uy_end_a, float* dzdl_1_a,
985  float* dzdl_2_a, float* ddzdl_1_a, float* ddzdl_2_a, float sinang_cut,
986  float cosang_diff_inv, float* cur_kappa_a, float* cur_dkappa_a,
987  float* cur_ux_a, float* cur_uy_a, float* cur_chi2_a, float* chi2_a) {
988  static const __m128 two = { 2., 2., 2., 2. };
989 
990  __m128 x1 = _mm_load_ps(x1_a);
991  __m128 x2 = _mm_load_ps(x2_a);
992  __m128 x3 = _mm_load_ps(x3_a);
993  __m128 y1 = _mm_load_ps(y1_a);
994  __m128 y2 = _mm_load_ps(y2_a);
995  __m128 y3 = _mm_load_ps(y3_a);
996  __m128 z1 = _mm_load_ps(z1_a);
997  __m128 z2 = _mm_load_ps(z2_a);
998  __m128 z3 = _mm_load_ps(z3_a);
999 
1000  __m128 dx1 = _mm_load_ps(dx1_a);
1001  __m128 dx2 = _mm_load_ps(dx2_a);
1002  __m128 dx3 = _mm_load_ps(dx3_a);
1003  __m128 dy1 = _mm_load_ps(dy1_a);
1004  __m128 dy2 = _mm_load_ps(dy2_a);
1005  __m128 dy3 = _mm_load_ps(dy3_a);
1006  __m128 dz1 = _mm_load_ps(dz1_a);
1007  __m128 dz2 = _mm_load_ps(dz2_a);
1008  __m128 dz3 = _mm_load_ps(dz3_a);
1009 
1010  __m128 D12 = _mm_sub_ps(x2, x1);
1011  D12 = _mm_mul_ps(D12, D12);
1012  __m128 tmp1 = _mm_sub_ps(y2, y1);
1013  tmp1 = _mm_mul_ps(tmp1, tmp1);
1014  D12 = _mm_add_ps(D12, tmp1);
1015  D12 = _vec_sqrt_ps(D12);
1016 
1017  __m128 D23 = _mm_sub_ps(x3, x2);
1018  D23 = _mm_mul_ps(D23, D23);
1019  tmp1 = _mm_sub_ps(y3, y2);
1020  tmp1 = _mm_mul_ps(tmp1, tmp1);
1021  D23 = _mm_add_ps(D23, tmp1);
1022  D23 = _vec_sqrt_ps(D23);
1023 
1024  __m128 D31 = _mm_sub_ps(x1, x3);
1025  D31 = _mm_mul_ps(D31, D31);
1026  tmp1 = _mm_sub_ps(y1, y3);
1027  tmp1 = _mm_mul_ps(tmp1, tmp1);
1028  D31 = _mm_add_ps(D31, tmp1);
1029  D31 = _vec_sqrt_ps(D31);
1030 
1031  __m128 k = _mm_mul_ps(D12, D23);
1032  k = _mm_mul_ps(k, D31);
1033  k = _vec_rec_ps(k);
1034  tmp1 = (D12 + D23 + D31) * (D23 + D31 - D12) * (D12 + D31 - D23)
1035  * (D12 + D23 - D31);
1036  tmp1 = _vec_sqrt_ps(tmp1);
1037  k *= tmp1;
1038 
1039  __m128 tmp2 = _mm_cmpgt_ps(tmp1, zero);
1040  tmp1 = _mm_and_ps(tmp2, k);
1041  tmp2 = _mm_andnot_ps(tmp2, zero);
1042  k = _mm_xor_ps(tmp1, tmp2);
1043 
1044  _mm_store_ps(kappa_a, k);
1045  __m128 k_inv = _vec_rec_ps(k);
1046 
1047  __m128 D12_inv = _vec_rec_ps(D12);
1048  __m128 D23_inv = _vec_rec_ps(D23);
1049  __m128 D31_inv = _vec_rec_ps(D31);
1050 
1051  __m128 dr1 = dx1 * dx1 + dy1 * dy1;
1052  dr1 = _vec_sqrt_ps(dr1);
1053  __m128 dr2 = dx2 * dx2 + dy2 * dy2;
1054  dr2 = _vec_sqrt_ps(dr2);
1055  __m128 dr3 = dx3 * dx3 + dy3 * dy3;
1056  dr3 = _vec_sqrt_ps(dr3);
1057 
1058  __m128 dk1 = (dr1 + dr2) * D12_inv * D12_inv;
1059  __m128 dk2 = (dr2 + dr3) * D23_inv * D23_inv;
1060  __m128 dk = dk1 + dk2;
1061  _mm_store_ps(dkappa_a, dk);
1062 
1063  __m128 ux12 = (x2 - x1) * D12_inv;
1064  __m128 uy12 = (y2 - y1) * D12_inv;
1065  __m128 ux23 = (x3 - x2) * D23_inv;
1066  __m128 uy23 = (y3 - y2) * D23_inv;
1067  __m128 ux13 = (x3 - x1) * D31_inv;
1068  __m128 uy13 = (y3 - y1) * D31_inv;
1069 
1070  __m128 cosalpha = ux12 * ux13 + uy12 * uy13;
1071  __m128 sinalpha = ux13 * uy12 - ux12 * uy13;
1072 
1073  __m128 ux_mid = ux23 * cosalpha - uy23 * sinalpha;
1074  __m128 uy_mid = ux23 * sinalpha + uy23 * cosalpha;
1075  _mm_store_ps(ux_mid_a, ux_mid);
1076  _mm_store_ps(uy_mid_a, uy_mid);
1077 
1078  __m128 ux_end = ux23 * cosalpha + uy23 * sinalpha;
1079  __m128 uy_end = uy23 * cosalpha - ux23 * sinalpha;
1080 
1081  _mm_store_ps(ux_end_a, ux_end);
1082  _mm_store_ps(uy_end_a, uy_end);
1083 
1084  // asin(x) = 2*atan( x/( 1 + sqrt( 1 - x*x ) ) )
1085  __m128 v = one - sinalpha * sinalpha;
1086  v = _vec_sqrt_ps(v);
1087  v += one;
1088  v = _vec_rec_ps(v);
1089  v *= sinalpha;
1090  __m128 s2 = _vec_atan_ps(v);
1091  s2 *= two;
1092  s2 *= k_inv;
1093  tmp1 = _mm_cmpgt_ps(k, zero);
1094  tmp2 = _mm_and_ps(tmp1, s2);
1095  tmp1 = _mm_andnot_ps(tmp1, D23);
1096  s2 = _mm_xor_ps(tmp1, tmp2);
1097 
1098  // dz/dl = (dz/ds)/sqrt(1 + (dz/ds)^2)
1099  // = dz/sqrt(s^2 + dz^2)
1100  __m128 del_z_2 = z3 - z2;
1101  __m128 dzdl_2 = s2 * s2 + del_z_2 * del_z_2;
1102  dzdl_2 = _vec_rsqrt_ps(dzdl_2);
1103  dzdl_2 *= del_z_2;
1104  __m128 ddzdl_2 = (dz2 + dz3) * D23_inv;
1105  _mm_store_ps(dzdl_2_a, dzdl_2);
1106  _mm_store_ps(ddzdl_2_a, ddzdl_2);
1107 
1108  sinalpha = ux13 * uy23 - ux23 * uy13;
1109  v = one - sinalpha * sinalpha;
1110  v = _vec_sqrt_ps(v);
1111  v += one;
1112  v = _vec_rec_ps(v);
1113  v *= sinalpha;
1114  __m128 s1 = _vec_atan_ps(v);
1115  s1 *= two;
1116  s1 *= k_inv;
1117  tmp1 = _mm_cmpgt_ps(k, zero);
1118  tmp2 = _mm_and_ps(tmp1, s1);
1119  tmp1 = _mm_andnot_ps(tmp1, D12);
1120  s1 = _mm_xor_ps(tmp1, tmp2);
1121 
1122  __m128 del_z_1 = z2 - z1;
1123  __m128 dzdl_1 = s1 * s1 + del_z_1 * del_z_1;
1124  dzdl_1 = _vec_rsqrt_ps(dzdl_1);
1125  dzdl_1 *= del_z_1;
1126  __m128 ddzdl_1 = (dz1 + dz2) * D12_inv;
1127  _mm_store_ps(dzdl_1_a, dzdl_1);
1128  _mm_store_ps(ddzdl_1_a, ddzdl_1);
1129 
1130  __m128 c_dk = _mm_load_ps(cur_dkappa_a);
1131  __m128 c_k = _mm_load_ps(cur_kappa_a);
1132  __m128 c_ux = _mm_load_ps(cur_ux_a);
1133  __m128 c_uy = _mm_load_ps(cur_uy_a);
1134  __m128 c_chi2 = _mm_load_ps(cur_chi2_a);
1135  __m128 sinang = _mm_load1_ps(&sinang_cut);
1136  __m128 cosdiff = _mm_load1_ps(&cosang_diff_inv);
1137 
1138  __m128 kdiff = c_k - k;
1139  __m128 n_dk = c_dk + dk + sinang * k;
1140  __m128 chi2_k = kdiff * kdiff / (n_dk * n_dk);
1141  __m128 cos_scatter = c_ux * ux_mid + c_uy * uy_mid;
1142  __m128 chi2_ang = (one - cos_scatter) * (one - cos_scatter) * cosdiff
1143  * cosdiff;
1144  tmp1 = dzdl_1 * sinang;
1145  _vec_fabs_ps(tmp1);
1146  __m128 chi2_dzdl = (dzdl_1 - dzdl_2) / (ddzdl_1 + ddzdl_2 + tmp1);
1147  chi2_dzdl *= chi2_dzdl;
1148  chi2_dzdl *= one_o_2;
1149 
1150  __m128 n_chi2 = c_chi2 + chi2_ang + chi2_k + chi2_dzdl;
1151  _mm_store_ps(chi2_a, n_chi2);
1152 }
1153 
1154 struct TempComb {
1156  }
1159 
1160  inline bool operator<(const TempComb& other) const {
1161  if (track.hits.size() > other.track.hits.size()) {
1162  return true;
1163  } else if (track.hits.size() == other.track.hits.size()) {
1164  return state.chi2 < other.state.chi2;
1165  } else {
1166  return false;
1167  }
1168  }
1169 };
1170 
1171 void sPHENIXSeedFinder::initDummyHits(vector<SimpleHit3D>& dummies,
1172  const HelixRange& range, HelixKalmanState& init_state) {
1173  SimpleTrack3D dummy_track;
1174  dummy_track.hits.push_back(SimpleHit3D());
1175  dummy_track.kappa = 0.5 * (range.min_k + range.max_k);
1176  dummy_track.phi = 0.5 * (range.min_phi + range.max_phi);
1177  dummy_track.d = 0.5 * (range.min_d + range.max_d);
1178  dummy_track.dzdl = 0.5 * (range.min_dzdl + range.max_dzdl);
1179  dummy_track.z0 = 0.5 * (range.min_z0 + range.max_z0);
1180 
1181  init_state.kappa = dummy_track.kappa;
1182  init_state.nu = sqrt(dummy_track.kappa);
1183  init_state.phi = dummy_track.phi;
1184  init_state.d = dummy_track.d;
1185  init_state.dzdl = dummy_track.dzdl;
1186  init_state.z0 = dummy_track.z0;
1187 
1188  init_state.C = Matrix<float, 5, 5>::Zero(5, 5);
1189  init_state.C(0, 0) = pow(range.max_phi - range.min_phi, 2.);
1190  init_state.C(1, 1) = pow(range.max_d - range.min_d, 2.);
1191  init_state.C(2, 2) = pow(10. * sqrt(range.max_k - range.min_k), 2.);
1192  init_state.C(3, 3) = pow(range.max_z0 - range.min_z0, 2.);
1193  init_state.C(4, 4) = pow(range.max_dzdl - range.min_dzdl, 2.);
1194  init_state.chi2 = 0.;
1195  init_state.position = 0;
1196  init_state.x_int = 0.;
1197  init_state.y_int = 0.;
1198  init_state.z_int = 0.;
1199 
1200  for (unsigned int i = 0; i < n_layers; ++i) {
1201  float x, y, z;
1202  projectToLayer(dummy_track, i, x, y, z);
1203  dummies[i].set_x(x);
1204  dummies[i].set_y(x);
1205  dummies[i].set_z(x);
1206  dummies[i].set_layer(i);
1207  }
1208 }
1209 
1210 // static void triplet_rejection(vector<SimpleTrack3D>& input,
1211 // vector<float>& chi2s, vector<bool>& usetrack) {
1212 // vector<hit_triplet> trips;
1213 // for (unsigned int i = 0; i < input.size(); ++i) {
1214 // for (unsigned int h1 = 0; h1 < input[i].hits.size(); ++h1) {
1215 // for (unsigned int h2 = (h1 + 1); h2 < input[i].hits.size(); ++h2) {
1216 // for (unsigned int h3 = (h2 + 1); h3 < input[i].hits.size();
1217 // ++h3) {
1218 // trips.push_back(
1219 // hit_triplet(input[i].hits[h1].get_id(),
1220 // input[i].hits[h2].get_id(),
1221 // input[i].hits[h3].get_id(), i, chi2s[i]));
1222 // }
1223 // }
1224 // }
1225 // }
1226 // if (trips.size() == 0) {
1227 // return;
1228 // }
1229 // sort(trips.begin(), trips.end());
1230 // unsigned int pos = 0;
1231 // unsigned int cur_h1 = trips[pos].hit1;
1232 // unsigned int cur_h2 = trips[pos].hit2;
1233 // while (pos < trips.size()) {
1234 // unsigned int next_pos = pos + 1;
1235 // if (next_pos >= trips.size()) {
1236 // break;
1237 // }
1238 // while (trips[pos] == trips[next_pos]) {
1239 // next_pos += 1;
1240 // if (next_pos >= trips.size()) {
1241 // break;
1242 // }
1243 // }
1244 // if ((next_pos - pos) > 1) {
1245 // float best_chi2 = trips[pos].chi2;
1246 // float next_chi2 = trips[pos + 1].chi2;
1247 // unsigned int best_pos = pos;
1248 // for (unsigned int i = (pos + 1); i < next_pos; ++i) {
1249 // if (input[trips[i].track].hits.size()
1250 // < input[trips[best_pos].track].hits.size()) {
1251 // continue;
1252 // } else if ((input[trips[i].track].hits.size()
1253 // > input[trips[best_pos].track].hits.size())
1254 // || (input[trips[i].track].hits.back().get_layer()
1255 // > input[trips[best_pos].track].hits.back().get_layer())) {
1256 // next_chi2 = best_chi2;
1257 // best_chi2 = trips[i].chi2;
1258 // best_pos = i;
1259 // continue;
1260 // }
1261 // if ((trips[i].chi2 < best_chi2)
1262 // || (usetrack[trips[best_pos].track] == false)) {
1263 // next_chi2 = best_chi2;
1264 // best_chi2 = trips[i].chi2;
1265 // best_pos = i;
1266 // } else if (trips[i].chi2 < next_chi2) {
1267 // next_chi2 = trips[i].chi2;
1268 // }
1269 // }
1270 // for (unsigned int i = pos; i < next_pos; ++i) {
1271 // if (i != best_pos) {
1272 // usetrack[trips[i].track] = false;
1273 // }
1274 // }
1275 // }
1276 // pos = next_pos;
1277 // cur_h1 = trips[pos].hit1;
1278 // cur_h2 = trips[pos].hit2;
1279 // }
1280 // }
1281 
1283  vector<SimpleTrack3D>& tracks, const HelixRange& /*range*/) {
1284 
1285  vector<TrackSegment>* cur_seg = &segments1;
1286  vector<TrackSegment>* next_seg = &segments2;
1287  unsigned int curseg_size = 0;
1288  unsigned int nextseg_size = 0;
1289 
1290  vector<TrackSegment> complete_segments;
1291 
1292  unsigned int allowed_missing = n_layers - req_layers;
1293  for (unsigned int l = 0; l < n_layers; ++l) {
1294  layer_sorted[l].clear();
1295  }
1296  for (unsigned int i = 0; i < hits.size(); ++i) {
1297  unsigned int min = (hits[i].get_layer() - allowed_missing);
1298  if (allowed_missing > (unsigned int) hits[i].get_layer()) {
1299  min = 0;
1300  }
1301  for (int l = min; l <= hits[i].get_layer(); l += 1) {
1302  layer_sorted[l].push_back(hits[i]);
1303  }
1304  }
1305  for (unsigned int l = 0; l < n_layers; ++l) {
1306  if (layer_sorted[l].size() == 0) {
1307  return;
1308  }
1309  }
1310 
1311  timeval t1, t2;
1312  double time1 = 0.;
1313  double time2 = 0.;
1314 
1315  gettimeofday(&t1, NULL);
1316 
1317  float cosang_diff = 1. - cosang_cut;
1318  float cosang_diff_inv = 1. / cosang_diff;
1319  float sinang_cut = sqrt(1. - cosang_cut * cosang_cut);
1320  float easy_chi2_cut = ca_chi2_cut;
1321 
1322  vector<float> inv_layer;
1323  inv_layer.assign(n_layers, 1.);
1324  for (unsigned int l = 3; l < n_layers; ++l) {
1325  inv_layer[l] = 1. / (((float) l) - 2.);
1326  }
1327 
1328  unsigned int hit_counter = 0;
1329  float x1_a[4] __attribute__((aligned(16)));
1330  float x2_a[4] __attribute__((aligned(16)));
1331  float x3_a[4] __attribute__((aligned(16)));
1332  float y1_a[4] __attribute__((aligned(16)));
1333  float y2_a[4] __attribute__((aligned(16)));
1334  float y3_a[4] __attribute__((aligned(16)));
1335  float z1_a[4] __attribute__((aligned(16)));
1336  float z2_a[4] __attribute__((aligned(16)));
1337  float z3_a[4] __attribute__((aligned(16)));
1338 
1339  float dx1_a[4] __attribute__((aligned(16)));
1340  float dx2_a[4] __attribute__((aligned(16)));
1341  float dx3_a[4] __attribute__((aligned(16)));
1342  float dy1_a[4] __attribute__((aligned(16)));
1343  float dy2_a[4] __attribute__((aligned(16)));
1344  float dy3_a[4] __attribute__((aligned(16)));
1345  float dz1_a[4] __attribute__((aligned(16)));
1346  float dz2_a[4] __attribute__((aligned(16)));
1347  float dz3_a[4] __attribute__((aligned(16)));
1348 
1349  float kappa_a[4] __attribute__((aligned(16)));
1350  float dkappa_a[4] __attribute__((aligned(16)));
1351 
1352  float ux_mid_a[4] __attribute__((aligned(16)));
1353  float uy_mid_a[4] __attribute__((aligned(16)));
1354  float ux_end_a[4] __attribute__((aligned(16)));
1355  float uy_end_a[4] __attribute__((aligned(16)));
1356 
1357  float dzdl_1_a[4] __attribute__((aligned(16)));
1358  float dzdl_2_a[4] __attribute__((aligned(16)));
1359  float ddzdl_1_a[4] __attribute__((aligned(16)));
1360  float ddzdl_2_a[4] __attribute__((aligned(16)));
1361 
1362  float cur_kappa_a[4] __attribute__((aligned(16)));
1363  float cur_dkappa_a[4] __attribute__((aligned(16)));
1364  float cur_ux_a[4] __attribute__((aligned(16)));
1365  float cur_uy_a[4] __attribute__((aligned(16)));
1366  float cur_chi2_a[4] __attribute__((aligned(16)));
1367  float chi2_a[4] __attribute__((aligned(16)));
1368 
1369  unsigned int hit1[4];
1370  unsigned int hit2[4];
1371  unsigned int hit3[4];
1372 
1373  TrackSegment temp_segment;
1374  temp_segment.hits.assign(n_layers, 0);
1375  // make segments out of first 3 layers
1376  for (unsigned int i = 0, sizei = layer_sorted[0].size(); i < sizei; ++i) {
1377  for (unsigned int j = 0, sizej = layer_sorted[1].size(); j < sizej;
1378  ++j) {
1379  for (unsigned int k = 0, sizek = layer_sorted[2].size(); k < sizek;
1380  ++k) {
1381  if ((layer_sorted[0][i].get_layer()
1382  >= layer_sorted[1][j].get_layer())
1383  || (layer_sorted[1][j].get_layer()
1384  >= layer_sorted[2][k].get_layer())) {
1385  continue;
1386  }
1387 
1388  x1_a[hit_counter] = layer_sorted[0][i].get_x();
1389  y1_a[hit_counter] = layer_sorted[0][i].get_y();
1390  z1_a[hit_counter] = layer_sorted[0][i].get_z();
1391 
1393 
1394  dx1_a[hit_counter] = 0.5 * sqrt(12.0)
1395  * sqrt(layer_sorted[0][i].get_size(0, 0));
1396  dy1_a[hit_counter] = 0.5 * sqrt(12.0)
1397  * sqrt(layer_sorted[0][i].get_size(1, 1));
1398  dz1_a[hit_counter] = 0.5 * sqrt(12.0)
1399  * sqrt(layer_sorted[0][i].get_size(2, 2));
1400 
1401  x2_a[hit_counter] = layer_sorted[1][j].get_x();
1402  y2_a[hit_counter] = layer_sorted[1][j].get_y();
1403  z2_a[hit_counter] = layer_sorted[1][j].get_z();
1404 
1405  dx2_a[hit_counter] = 0.5 * sqrt(12.0)
1406  * sqrt(layer_sorted[1][j].get_size(0, 0));
1407  dy2_a[hit_counter] = 0.5 * sqrt(12.0)
1408  * sqrt(layer_sorted[1][j].get_size(1, 1));
1409  dz2_a[hit_counter] = 0.5 * sqrt(12.0)
1410  * sqrt(layer_sorted[1][j].get_size(2, 2));
1411 
1412  x3_a[hit_counter] = layer_sorted[2][k].get_x();
1413  y3_a[hit_counter] = layer_sorted[2][k].get_y();
1414  z3_a[hit_counter] = layer_sorted[2][k].get_z();
1415 
1416  dx3_a[hit_counter] = 0.5 * sqrt(12.0)
1417  * sqrt(layer_sorted[2][k].get_size(0, 0));
1418  dy3_a[hit_counter] = 0.5 * sqrt(12.0)
1419  * sqrt(layer_sorted[2][k].get_size(1, 1));
1420  dz3_a[hit_counter] = 0.5 * sqrt(12.0)
1421  * sqrt(layer_sorted[2][k].get_size(2, 2));
1422 
1423  hit1[hit_counter] = i;
1424  hit2[hit_counter] = j;
1425  hit3[hit_counter] = k;
1426 
1427  hit_counter += 1;
1428 
1429  if (hit_counter == 4) {
1430  calculateKappaTangents(x1_a, y1_a, z1_a, x2_a, y2_a, z2_a,
1431  x3_a, y3_a, z3_a, dx1_a, dy1_a, dz1_a, dx2_a, dy2_a,
1432  dz2_a, dx3_a, dy3_a, dz3_a, kappa_a, dkappa_a,
1433  ux_mid_a, uy_mid_a, ux_end_a, uy_end_a, dzdl_1_a,
1434  dzdl_2_a, ddzdl_1_a, ddzdl_2_a);
1435 
1436  for (unsigned int h = 0; h < hit_counter; ++h) {
1437  temp_segment.chi2 = (dzdl_1_a[h] - dzdl_2_a[h])
1438  / (ddzdl_1_a[h] + ddzdl_2_a[h]
1439  + fabs(dzdl_1_a[h] * sinang_cut));
1440  temp_segment.chi2 *= temp_segment.chi2;
1441  if (temp_segment.chi2 > 2.0) {
1442  continue;
1443  }
1444  temp_segment.ux = ux_end_a[h];
1445  temp_segment.uy = uy_end_a[h];
1446  temp_segment.kappa = kappa_a[h];
1447  if (temp_segment.kappa > top_range.max_k) {
1448  continue;
1449  }
1450  temp_segment.dkappa = dkappa_a[h];
1451  temp_segment.hits[0] = hit1[h];
1452  temp_segment.hits[1] = hit2[h];
1453  temp_segment.hits[2] = hit3[h];
1454  temp_segment.n_hits = 3;
1455  unsigned int outer_layer =
1456  layer_sorted[2][temp_segment.hits[2]].get_layer();
1457  if ((outer_layer - 2) > allowed_missing) {
1458  continue;
1459  }
1460  if ((n_layers - 3) <= allowed_missing) {
1461  complete_segments.push_back(temp_segment);
1462  }
1463  if (next_seg->size() == nextseg_size) {
1464  next_seg->push_back(temp_segment);
1465  nextseg_size += 1;
1466  } else {
1467  (*next_seg)[nextseg_size] = temp_segment;
1468  nextseg_size += 1;
1469  }
1470  }
1471 
1472  hit_counter = 0;
1473  }
1474  }
1475  }
1476  }
1477  if (hit_counter != 0) {
1478  calculateKappaTangents(x1_a, y1_a, z1_a, x2_a, y2_a, z2_a, x3_a, y3_a,
1479  z3_a, dx1_a, dy1_a, dz1_a, dx2_a, dy2_a, dz2_a, dx3_a, dy3_a,
1480  dz3_a, kappa_a, dkappa_a, ux_mid_a, uy_mid_a, ux_end_a,
1481  uy_end_a, dzdl_1_a, dzdl_2_a, ddzdl_1_a, ddzdl_2_a);
1482 
1483  for (unsigned int h = 0; h < hit_counter; ++h) {
1484  temp_segment.chi2 = (dzdl_1_a[h] - dzdl_2_a[h])
1485  / (ddzdl_1_a[h] + ddzdl_2_a[h]
1486  + fabs(dzdl_1_a[h] * sinang_cut));
1487  temp_segment.chi2 *= temp_segment.chi2;
1488  if (temp_segment.chi2 > 2.0) {
1489  continue;
1490  }
1491  temp_segment.ux = ux_end_a[h];
1492  temp_segment.uy = uy_end_a[h];
1493  temp_segment.kappa = kappa_a[h];
1494  if (temp_segment.kappa > top_range.max_k) {
1495  continue;
1496  }
1497  temp_segment.dkappa = dkappa_a[h];
1498  temp_segment.hits[0] = hit1[h];
1499  temp_segment.hits[1] = hit2[h];
1500  temp_segment.hits[2] = hit3[h];
1501  temp_segment.n_hits = 3;
1502  unsigned int outer_layer =
1503  layer_sorted[2][temp_segment.hits[2]].get_layer();
1504  if ((outer_layer - 2) > allowed_missing) {
1505  continue;
1506  }
1507  if ((n_layers - 3) <= allowed_missing) {
1508  complete_segments.push_back(temp_segment);
1509  }
1510  if (next_seg->size() == nextseg_size) {
1511  next_seg->push_back(temp_segment);
1512  nextseg_size += 1;
1513  } else {
1514  (*next_seg)[nextseg_size] = temp_segment;
1515  nextseg_size += 1;
1516  }
1517  }
1518 
1519  hit_counter = 0;
1520  }
1521 
1522  swap(cur_seg, next_seg);
1523  swap(curseg_size, nextseg_size);
1524 
1525  // add hits to segments layer-by-layer, cutting out bad segments
1526  unsigned int whichseg[4];
1527  for (unsigned int l = 3; l < n_layers; ++l) {
1528  if (l == (n_layers - 1)) {
1529  easy_chi2_cut *= 0.25;
1530  }
1531  nextseg_size = 0;
1532  for (unsigned int i = 0, sizei = curseg_size; i < sizei; ++i) {
1533  for (unsigned int j = 0, sizej = layer_sorted[l].size(); j < sizej;
1534  ++j) {
1535  if ((layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_layer()
1536  >= layer_sorted[l][j].get_layer())) {
1537  continue;
1538  }
1539 
1540  x1_a[hit_counter] =
1541  layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_x();
1542  y1_a[hit_counter] =
1543  layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_y();
1544  z1_a[hit_counter] =
1545  layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_z();
1546  x2_a[hit_counter] =
1547  layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_x();
1548  y2_a[hit_counter] =
1549  layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_y();
1550  z2_a[hit_counter] =
1551  layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_z();
1552  x3_a[hit_counter] = layer_sorted[l][j].get_x();
1553  y3_a[hit_counter] = layer_sorted[l][j].get_y();
1554  z3_a[hit_counter] = layer_sorted[l][j].get_z();
1555 
1556  dx1_a[hit_counter] =
1557  0.5 * sqrt(12.0)
1558  * sqrt(
1559  layer_sorted[l - 2][(*cur_seg)[i].hits[l
1560  - 2]].get_size(0, 0));
1561  dy1_a[hit_counter] =
1562  0.5 * sqrt(12.0)
1563  * sqrt(
1564  layer_sorted[l - 2][(*cur_seg)[i].hits[l
1565  - 2]].get_size(1, 1));
1566  dz1_a[hit_counter] =
1567  0.5 * sqrt(12.0)
1568  * sqrt(
1569  layer_sorted[l - 2][(*cur_seg)[i].hits[l
1570  - 2]].get_size(2, 2));
1571  dx2_a[hit_counter] =
1572  0.5 * sqrt(12.0)
1573  * sqrt(
1574  layer_sorted[l - 1][(*cur_seg)[i].hits[l
1575  - 1]].get_size(0, 0));
1576  dy2_a[hit_counter] =
1577  0.5 * sqrt(12.0)
1578  * sqrt(
1579  layer_sorted[l - 1][(*cur_seg)[i].hits[l
1580  - 1]].get_size(1, 1));
1581  dz2_a[hit_counter] =
1582  0.5 * sqrt(12.0)
1583  * sqrt(
1584  layer_sorted[l - 1][(*cur_seg)[i].hits[l
1585  - 1]].get_size(2, 2));
1586  dx3_a[hit_counter] = 0.5 * sqrt(12.0)
1587  * sqrt(layer_sorted[l][j].get_size(0, 0));
1588  dy3_a[hit_counter] = 0.5 * sqrt(12.0)
1589  * sqrt(layer_sorted[l][j].get_size(1, 1));
1590  dz3_a[hit_counter] = 0.5 * sqrt(12.0)
1591  * sqrt(layer_sorted[l][j].get_size(2, 2));
1592 
1593  cur_kappa_a[hit_counter] = (*cur_seg)[i].kappa;
1594  cur_dkappa_a[hit_counter] = (*cur_seg)[i].dkappa;
1595  cur_ux_a[hit_counter] = (*cur_seg)[i].ux;
1596  cur_uy_a[hit_counter] = (*cur_seg)[i].uy;
1597  cur_chi2_a[hit_counter] = (*cur_seg)[i].chi2;
1598 
1599  whichseg[hit_counter] = i;
1600  hit1[hit_counter] = j;
1601 
1602  hit_counter += 1;
1603  if (hit_counter == 4) {
1604  calculateKappaTangents(x1_a, y1_a, z1_a, x2_a, y2_a, z2_a,
1605  x3_a, y3_a, z3_a, dx1_a, dy1_a, dz1_a, dx2_a, dy2_a,
1606  dz2_a, dx3_a, dy3_a, dz3_a, kappa_a, dkappa_a,
1607  ux_mid_a, uy_mid_a, ux_end_a, uy_end_a, dzdl_1_a,
1608  dzdl_2_a, ddzdl_1_a, ddzdl_2_a, sinang_cut,
1609  cosang_diff_inv, cur_kappa_a, cur_dkappa_a,
1610  cur_ux_a, cur_uy_a, cur_chi2_a, chi2_a);
1611 
1612  for (unsigned int h = 0; h < hit_counter; ++h) {
1613  if ((chi2_a[h]) * inv_layer[l] < easy_chi2_cut) {
1614  temp_segment.chi2 = chi2_a[h];
1615  temp_segment.ux = ux_end_a[h];
1616  temp_segment.uy = uy_end_a[h];
1617  temp_segment.kappa = kappa_a[h];
1618  if (temp_segment.kappa > top_range.max_k) {
1619  continue;
1620  }
1621  temp_segment.dkappa = dkappa_a[h];
1622  for (unsigned int ll = 0; ll < l; ++ll) {
1623  temp_segment.hits[ll] =
1624  (*cur_seg)[whichseg[h]].hits[ll];
1625  }
1626  temp_segment.hits[l] = hit1[h];
1627  unsigned int outer_layer =
1628  layer_sorted[l][temp_segment.hits[l]].get_layer();
1629  temp_segment.n_hits = l + 1;
1630  if ((n_layers - (l + 1)) <= allowed_missing) {
1631  complete_segments.push_back(temp_segment);
1632  }
1633  if ((outer_layer - l) > allowed_missing) {
1634  continue;
1635  }
1636  if (next_seg->size() == nextseg_size) {
1637  next_seg->push_back(temp_segment);
1638  nextseg_size += 1;
1639  } else {
1640  (*next_seg)[nextseg_size] = temp_segment;
1641  nextseg_size += 1;
1642  }
1643  }
1644  }
1645  hit_counter = 0;
1646  }
1647  }
1648  }
1649  if (hit_counter != 0) {
1650  calculateKappaTangents(x1_a, y1_a, z1_a, x2_a, y2_a, z2_a, x3_a,
1651  y3_a, z3_a, dx1_a, dy1_a, dz1_a, dx2_a, dy2_a, dz2_a, dx3_a,
1652  dy3_a, dz3_a, kappa_a, dkappa_a, ux_mid_a, uy_mid_a,
1653  ux_end_a, uy_end_a, dzdl_1_a, dzdl_2_a, ddzdl_1_a,
1654  ddzdl_2_a, sinang_cut, cosang_diff_inv, cur_kappa_a,
1655  cur_dkappa_a, cur_ux_a, cur_uy_a, cur_chi2_a, chi2_a);
1656 
1657  for (unsigned int h = 0; h < hit_counter; ++h) {
1658  if ((chi2_a[h]) * inv_layer[l] < easy_chi2_cut) {
1659  temp_segment.chi2 = chi2_a[h];
1660  temp_segment.ux = ux_end_a[h];
1661  temp_segment.uy = uy_end_a[h];
1662  temp_segment.kappa = kappa_a[h];
1663  if (temp_segment.kappa > top_range.max_k) {
1664  continue;
1665  }
1666 
1667  temp_segment.dkappa = dkappa_a[h];
1668  for (unsigned int ll = 0; ll < l; ++ll) {
1669  temp_segment.hits[ll] =
1670  (*cur_seg)[whichseg[h]].hits[ll];
1671  }
1672  temp_segment.hits[l] = hit1[h];
1673  unsigned int outer_layer =
1674  layer_sorted[l][temp_segment.hits[l]].get_layer();
1675  temp_segment.n_hits = l + 1;
1676  if ((n_layers - (l + 1)) <= allowed_missing) {
1677  complete_segments.push_back(temp_segment);
1678  }
1679  if ((outer_layer - l) > allowed_missing) {
1680  continue;
1681  }
1682  if (next_seg->size() == nextseg_size) {
1683  next_seg->push_back(temp_segment);
1684  nextseg_size += 1;
1685  } else {
1686  (*next_seg)[nextseg_size] = temp_segment;
1687  nextseg_size += 1;
1688  }
1689  }
1690  }
1691  hit_counter = 0;
1692  }
1693  swap(cur_seg, next_seg);
1694  swap(curseg_size, nextseg_size);
1695  }
1696 
1697  for (unsigned int i = 0; i < complete_segments.size(); ++i) {
1698  if (cur_seg->size() == curseg_size) {
1699  cur_seg->push_back(complete_segments[i]);
1700  curseg_size += 1;
1701  } else {
1702  (*cur_seg)[curseg_size] = complete_segments[i];
1703  curseg_size += 1;
1704  }
1705  }
1706 
1707  gettimeofday(&t2, NULL);
1708  time1 = ((double) (t1.tv_sec) + (double) (t1.tv_usec) / 1000000.);
1709  time2 = ((double) (t2.tv_sec) + (double) (t2.tv_usec) / 1000000.);
1710  CAtime += (time2 - time1);
1711  SimpleTrack3D temp_track;
1712  temp_track.hits.assign(n_layers, SimpleHit3D());
1713  vector<SimpleHit3D> temp_hits;
1714  for (unsigned int i = 0, sizei = curseg_size; i < sizei; ++i) {
1715  temp_track.hits.assign((*cur_seg)[i].n_hits, SimpleHit3D());
1716 
1717  temp_comb.assign((*cur_seg)[i].n_hits, 0);
1718  for (unsigned int l = 0; l < (*cur_seg)[i].n_hits; ++l) {
1719  temp_comb[l] = layer_sorted[l][(*cur_seg)[i].hits[l]].get_id();
1720  }
1721  sort(temp_comb.begin(), temp_comb.end());
1722  set<vector<unsigned int> >::iterator it = combos.find(temp_comb);
1723  if (it != combos.end()) {
1724  continue;
1725  }
1726  if (combos.size() > 10000) {
1727  combos.clear();
1728  }
1729  combos.insert(temp_comb);
1730 
1731  for (unsigned int l = 0; l < (*cur_seg)[i].n_hits; ++l) {
1732  temp_track.hits[l] = layer_sorted[l][(*cur_seg)[i].hits[l]];
1733  }
1734 
1735  gettimeofday(&t1, NULL);
1736 
1737  // unsigned int layer_out = temp_track.hits.size()-1;
1738  // unsigned int layer_mid = layer_out/2;
1739  // temp_track_3hits.hits[0] = temp_track.hits[0];
1740  // temp_track_3hits.hits[1] = temp_track.hits[layer_mid];
1741  // temp_track_3hits.hits[2] = temp_track.hits[layer_out];
1742 
1743  float init_chi2 = fitTrack(temp_track);
1744 
1745  if (init_chi2 > fast_chi2_cut_max) {
1746  if (init_chi2
1749  / kappaToPt(temp_track.kappa)) {
1750  gettimeofday(&t2, NULL);
1751  time1 =
1752  ((double) (t1.tv_sec) + (double) (t1.tv_usec) / 1000000.);
1753  time2 =
1754  ((double) (t2.tv_sec) + (double) (t2.tv_usec) / 1000000.);
1755  KALtime += (time2 - time1);
1756  continue;
1757  }
1758  }
1759  HelixKalmanState state;
1760  state.phi = temp_track.phi;
1761  if (state.phi < 0.) {
1762  state.phi += 2. * M_PI;
1763  }
1764  state.d = temp_track.d;
1765  state.kappa = temp_track.kappa;
1766  state.nu = sqrt(state.kappa);
1767  state.z0 = temp_track.z0;
1768  state.dzdl = temp_track.dzdl;
1769  state.C = Matrix<float, 5, 5>::Zero(5, 5);
1770  state.C(0, 0) = pow(0.01, 2.);
1771  state.C(1, 1) = pow(0.01, 2.);
1772  state.C(2, 2) = pow(0.01 * state.nu, 2.);
1773  state.C(3, 3) = pow(0.05, 2.);
1774  state.C(4, 4) = pow(0.05, 2.);
1775  state.chi2 = 0.;
1776  state.position = 0;
1777  state.x_int = 0.;
1778  state.y_int = 0.;
1779  state.z_int = 0.;
1780 
1781  for (unsigned int h = 0; h < temp_track.hits.size(); ++h) {
1782  kalman->addHit(temp_track.hits[h], state);
1783  nfits += 1;
1784  }
1785 
1786  // fudge factor for non-gaussian hit sizes
1787  state.C *= 3.;
1788  state.chi2 *= 6.;
1789 
1790  gettimeofday(&t2, NULL);
1791  time1 = ((double) (t1.tv_sec) + (double) (t1.tv_usec) / 1000000.);
1792  time2 = ((double) (t2.tv_sec) + (double) (t2.tv_usec) / 1000000.);
1793  KALtime += (time2 - time1);
1794 
1795  if (!(temp_track.kappa == temp_track.kappa)) {
1796  continue;
1797  }
1798  if (temp_track.kappa > top_range.max_k) {
1799  continue;
1800  }
1801  if (!(state.chi2 == state.chi2)) {
1802  continue;
1803  }
1804  if (state.chi2 / (2. * ((float) (temp_track.hits.size())) - 5.)
1805  > chi2_cut) {
1806  continue;
1807  }
1808 
1809  if (cut_on_dca == true) {
1810  if (fabs(temp_track.d) > dca_cut) {
1811  continue;
1812  }
1813  if (fabs(temp_track.z0) > dca_cut) {
1814  continue;
1815  }
1816  }
1817 
1818  /*
1819  cout << " add track with hits size " << temp_track.hits.size()
1820  << " z0 " << temp_track.z0
1821  << endl;
1822  for (unsigned int i = 0; i < temp_track.hits.size(); ++i)
1823  {
1824  cout << " temp_track.hits.get_id() " << temp_track.hits[i].get_id() << endl;
1825  }
1826  */
1827 
1828  tracks.push_back(temp_track);
1829  track_states.push_back(state);
1830  if ((remove_hits == true) && (state.chi2 < chi2_removal_cut)
1831  && (temp_track.hits.size() >= n_removal_hits)) {
1832  for (unsigned int i = 0; i < temp_track.hits.size(); ++i) {
1833  (*hit_used)[temp_track.hits[i].get_id()] = true;
1834  }
1835  }
1836  }
1837 }
1838 
1839 static inline double sign(double x) {
1840  return ((double) (x > 0.)) - ((double) (x < 0.));
1841 }
1842 
1844  float& x, float& y, float& z) {
1845  float phi = seed.phi;
1846  float d = seed.d;
1847  float k = seed.kappa;
1848  float z0 = seed.z0;
1849  float dzdl = seed.dzdl;
1850 
1851  float hitx = seed.hits.back().get_x();
1852  float hity = seed.hits.back().get_y();
1853 
1854  float rad_det = detector_radii[layer];
1855 
1856  float cosphi = cos(phi);
1857  float sinphi = sin(phi);
1858 
1859  k = fabs(k);
1860 
1861  float kd = (d * k + 1.);
1862  float kcx = kd * cosphi;
1863  float kcy = kd * sinphi;
1864  float kd_inv = 1. / kd;
1865  float R2 = rad_det * rad_det;
1866  float a = 0.5 * (k * R2 + (d * d * k + 2. * d)) * kd_inv;
1867  float tmp1 = a * kd_inv;
1868  float P2x = kcx * tmp1;
1869  float P2y = kcy * tmp1;
1870 
1871  float h = sqrt(R2 - a * a);
1872 
1873  float ux = -kcy * kd_inv;
1874  float uy = kcx * kd_inv;
1875 
1876  float x1 = P2x + ux * h;
1877  float y1 = P2y + uy * h;
1878  float x2 = P2x - ux * h;
1879  float y2 = P2y - uy * h;
1880  float diff1 = (x1 - hitx) * (x1 - hitx) + (y1 - hity) * (y1 - hity);
1881  float diff2 = (x2 - hitx) * (x2 - hitx) + (y2 - hity) * (y2 - hity);
1882  float signk = 0.;
1883  if (diff1 < diff2) {
1884  signk = 1.;
1885  } else {
1886  signk = -1.;
1887  }
1888  x = P2x + signk * ux * h;
1889  y = P2y + signk * uy * h;
1890 
1891  double sign_dzdl = sign(dzdl);
1892 // double onedzdl2_inv = 1. / (1. - dzdl * dzdl);
1893  double startx = d * cosphi;
1894  double starty = d * sinphi;
1895  double D = sqrt((startx - x) * (startx - x) + (starty - y) * (starty - y));
1896 // double D_inv = 1. / D;
1897  double v = 0.5 * k * D;
1898  z = 0.;
1899  if (v > 0.1) {
1900  if (v >= 0.999999) {
1901  v = 0.999999;
1902  }
1903  double s = 2. * asin(v) / k;
1904 // double s_inv = 1. / s;
1905 // double sqrtvv = sqrt(1 - v * v);
1906  double dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
1907  z = z0 + sign_dzdl * dz;
1908  } else {
1909  double s = 0.;
1910  double temp1 = k * D * 0.5;
1911  temp1 *= temp1;
1912  double temp2 = D * 0.5;
1913  s += 2. * temp2;
1914  temp2 *= temp1;
1915  s += temp2 / 3.;
1916  temp2 *= temp1;
1917  s += (3. / 20.) * temp2;
1918  temp2 *= temp1;
1919  s += (5. / 56.) * temp2;
1920 // double s_inv = 1. / s;
1921  double dz = sqrt(s * s * dzdl * dzdl / (1. - dzdl * dzdl));
1922  z = z0 + sign_dzdl * dz;
1923  }
1924 }
1925 
1926 void sPHENIXSeedFinder::initSplitting(vector<SimpleHit3D>& hits,
1927  unsigned int min_hits, unsigned int /*max_hits*/) {
1928  initEvent(hits, min_hits);
1929  (*(hits_vec[0])) = hits;
1930  zoomranges.clear();
1931  for (unsigned int z = 0; z <= max_zoom; z++) {
1932  zoomranges.push_back(top_range);
1933  }
1934 }
1935 
1937  vector<SimpleHit3D>& hits, unsigned int /*min_hits*/, unsigned int /*max_hits*/,
1938  vector<SimpleTrack3D>& /*tracks*/) {
1939  unsigned int hits_per_thread = (hits.size() + 2 * nthreads) / nthreads;
1940  unsigned int pos = 0;
1941  while (pos < hits.size()) {
1942  for (unsigned int i = 0; i < nthreads; ++i) {
1943  if (pos >= hits.size()) {
1944  break;
1945  }
1946  for (unsigned int j = 0; j < hits_per_thread; ++j) {
1947  if (pos >= hits.size()) {
1948  break;
1949  }
1950  split_input_hits[i].push_back(hits[pos]);
1951  pos += 1;
1952  }
1953  }
1954  }
1955  for (unsigned int i = 0; i < nthreads; ++i) {
1956  thread_trackers[i]->setTopRange(top_range);
1957  thread_trackers[i]->initSplitting(split_input_hits[i], thread_min_hits,
1958  thread_max_hits);
1959  }
1961  thread_ranges.clear();
1962  thread_hits.clear();
1963 
1964  unsigned int nbins = split_output_hits[0]->size();
1965  for (unsigned int b = 0; b < nbins; ++b) {
1966  thread_ranges.push_back((*(split_ranges[0]))[b]);
1967  thread_hits.push_back(vector<SimpleHit3D>());
1968  for (unsigned int i = 0; i < nthreads; ++i) {
1969  for (unsigned int j = 0; j < (*(split_output_hits[i]))[b].size();
1970  ++j) {
1971  thread_hits.back().push_back((*(split_output_hits[i]))[b][j]);
1972  }
1973  }
1974  }
1975 
1977 }
1978 
1980  unsigned int min_hits, unsigned int max_hits,
1981  vector<SimpleTrack3D>& tracks) {
1982  thread_min_hits = min_hits;
1983  thread_max_hits = max_hits;
1984 
1985  for (unsigned int i = 0; i < nthreads; ++i) {
1986  thread_tracks[i].clear();
1987  thread_trackers[i]->clear();
1988  if (cluster_start_bin != 0) {
1989  thread_trackers[i]->setClusterStartBin(cluster_start_bin - 1);
1990  } else {
1991  thread_trackers[i]->setClusterStartBin(0);
1992  }
1993  }
1994 
1995  initSplitting(hits, min_hits, max_hits);
1996 
1997  if (separate_by_helicity == true) {
1998  for (unsigned int i = 0; i < nthreads; ++i) {
1999  thread_trackers[i]->setSeparateByHelicity(true);
2000  thread_trackers[i]->setOnlyOneHelicity(true);
2001  thread_trackers[i]->setHelicity(true);
2002  split_output_hits[i]->clear();
2003  split_input_hits[i].clear();
2004  }
2005  findHelicesParallelOneHelicity(hits, min_hits, max_hits, tracks);
2006 
2007  for (unsigned int i = 0; i < nthreads; ++i) {
2008  thread_trackers[i]->setSeparateByHelicity(true);
2009  thread_trackers[i]->setOnlyOneHelicity(true);
2010  thread_trackers[i]->setHelicity(false);
2011  split_output_hits[i]->clear();
2012  split_input_hits[i].clear();
2013  }
2014  findHelicesParallelOneHelicity(hits, min_hits, max_hits, tracks);
2015  } else {
2016  for (unsigned int i = 0; i < nthreads; ++i) {
2017  thread_trackers[i]->setSeparateByHelicity(false);
2018  thread_trackers[i]->setOnlyOneHelicity(false);
2019  split_output_hits[i]->clear();
2020  split_input_hits[i].clear();
2021  }
2022 
2023  findHelicesParallelOneHelicity(hits, min_hits, max_hits, tracks);
2024  }
2025 
2026  vector<SimpleTrack3D> temp_tracks;
2027  for (unsigned int i = 0; i < nthreads; ++i) {
2028  vector<HelixKalmanState>* states =
2029  &(thread_trackers[i]->getKalmanStates());
2030  for (unsigned int j = 0; j < thread_tracks[i].size(); ++j) {
2031  track_states.push_back((*states)[j]);
2032  temp_tracks.push_back(thread_tracks[i][j]);
2033  }
2034  }
2035  finalize(temp_tracks, tracks);
2036 }
2037 
2039  unsigned long int w = (*((unsigned long int*) arg));
2041  *(split_ranges[w]), *(split_output_hits[w]), 0);
2042 }
2043 
2045  unsigned long int w = (*((unsigned long int*) arg));
2046 
2047  for (unsigned int i = w; i < thread_ranges.size(); i += nthreads) {
2048  if (thread_hits[i].size() == 0) {
2049  continue;
2050  }
2051  thread_trackers[w]->setTopRange(thread_ranges[i]);
2052  thread_trackers[w]->findHelices(thread_hits[i], thread_min_hits,
2054  }
2055 }