EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
CellularAutomaton_v1.cc
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file CellularAutomaton_v1.cc
1 #include "CellularAutomaton_v1.h"
2 
3 #include "HelixHoughSpace.h" // for HelixHoughSpace
4 #include "HelixKalmanFilter.h" // for HelixKalmanFilter
5 
6 #include <HelixHough/HelixKalmanState.h> // for HelixKalmanState
7 #include <HelixHough/SimpleHit3D.h> // for SimpleHit3D
8 
9 #include <phool/PHObject.h> // for PHObject
10 #include <phool/phool.h> // for PHWHERE
11 
12 #include <Eigen/Core>
13 
14 #include <algorithm> // for sort
15 #include <cassert>
16 #include <cfloat>
17 #include <cmath>
18 #include <cstdlib> // for exit, NULL
19 #include <iostream>
20 #include <memory> // for allocator_traits<>::value_type
21 #include <sys/time.h>
22 #include <utility> // for swap, pair, make_pair
23 
24 using namespace std;
25 using namespace Eigen;
26 
27 //#define _DEBUG_
28 // if _FULL_TEST_ is not chosen, only triplets are translated into SimpleTrack3D
29 #define _FULL_TEST_
30 
31 
32 CellularAutomaton_v1::CellularAutomaton_v1(std::vector<SimpleTrack3D>& input_tracks, std::vector<float>& detector_radii, std::vector<float>& detector_materials)
33  :
34  _hough_space(nullptr),
35  _kalman(nullptr),
36  in_tracks(std::vector<SimpleTrack3D>()),
37  ca_tracks(std::vector<SimpleTrack3D>()),
38  ca_track_states(std::vector<HelixKalmanState>()),
39  temp_combo(std::vector<unsigned int>()),
40  combos(std::set<std::vector<unsigned int> >()),
41  layer_sorted(std::vector<std::vector<SimpleHit3D> >()),
42  nlayers(10),
43  rlayers(8),
44  allowed_missing_inner_hits(0),
45  ca_cos_ang_cut(0.985),
46  ca_chi2_cut(2.0),
47  ca_chi2_layer_cut(2.0),
48  ca_phi_cut(M_PI/144.),
49  ca_z_cut(20),
50  ca_dcaxy_cut(0.02),
51  fast_chi2_cut_max(FLT_MAX),
52  fast_chi2_cut_par0(12.),
53  fast_chi2_cut_par1(0.),
54  _pt_rescale(1.0),
55  _mag_field(1.4),
56  _detector_radii(std::vector<float>()),
57  _detector_scatter(std::vector<float>()),
58  _detector_materials(std::vector<float>()),
59  _integrated_scatter(std::vector<float>()),
60  _hits_used(std::map<unsigned int, bool>()),
61  _hits_map(std::map<unsigned int,SimpleHit3D>()),
62  CAtime(0.),
63  KALtime(0.),
64  forward(false),
65  remove_hits(false),
66  remove_inner_hits(false),
67  require_inner_hits(false),
68  triplet_mode(true),
69  seeding_mode(false),
70  verbose(0)
71 {
72  set_input_tracks(input_tracks);
73  set_detector_radii(detector_radii);
74  set_detector_materials(detector_materials);
75 
76 }
77 
78 void CellularAutomaton_v1::Reset(){
79 
80  delete _hough_space;
81  delete _kalman;
82  in_tracks.clear();
83  ca_tracks.clear();
84  ca_track_states.clear();
85 
86  temp_combo.clear();
87 
88  _detector_radii.clear();
89  _detector_scatter.clear();
90  _detector_materials.clear();
91  _integrated_scatter.clear();
92 
93 }
94 
95 
96 int CellularAutomaton_v1::run(std::vector<SimpleTrack3D>& output_tracks, std::vector<HelixKalmanState>& output_track_states, std::map<unsigned int, bool>& hits_used)
97 {
98  if (remove_hits){
99 // cout<< "hits_used size "<< hits_used.size()<<endl;
100 // cout<< "_hits_used size "<<_hits_used.size()<<endl;
101  _hits_used.swap(hits_used);
102  }
103 
104  int code = 0;
105  if(verbose > 0) cout<< "CellularAutomaton:: initializing..."<<endl;
106  code = init();
107  if(verbose > 0) cout<<code<<endl;
108  if (!code)
109  {
110  cout << PHWHERE << "::Error - Initialization failed. " << endl;
111  exit(1);
112  }
113  code = 0;
114  if(verbose > 0) cout<<"CellularAutomaton:: processing tracks... "<<endl;
115  code = process_tracks();
116  if (!code)
117  {
118  cout << PHWHERE << "::Error - Processing tracks failed. " << endl;
119  exit(1);
120  }
121  code = 0;
122  if(verbose > 0) cout<<"CellularAutomaton:: outputting ca tracks..."<<endl;
123  code = get_ca_tracks(output_tracks, output_track_states);
124  if (!code)
125  {
126  cout << PHWHERE << "::Error - Outputting tracks failed. " << endl;
127  exit(1);
128  }
129 
130  if(remove_hits){
131  _hits_used.swap(hits_used);
132  if(verbose > 0) cout<< "hits_used size "<< hits_used.size()<<endl;
133  if(verbose > 0) cout<< "_hits_used size "<<_hits_used.size()<<endl;
134  }
135 
136  for (unsigned int i = 0; i<in_tracks.size(); ++i) in_tracks[i].reset();
137  in_tracks.clear();
138  for (unsigned int i = 0; i<ca_tracks.size(); ++i) ca_tracks[i].reset();
139  ca_tracks.clear();
140  ca_track_states.clear();
141  layer_sorted.clear();
142  return 1;
143 }
144 
145 int CellularAutomaton_v1::init()
146 {
147  if (!_hough_space)
148  {
149  cout << PHWHERE << "::Error - Hough Space is not set. " << endl;
150  exit(1);
151  }
152 /*
153  if (!in_tracks)
154  {
155  cout << PHWHERE << "::Error - Input tracks are not set" << endl;
156  exit(1);
157  }
158 */
159  if (!_detector_radii.size())
160  {
161  cout << PHWHERE << "::Error - Detector radii are not set" << endl;
162  exit(1);
163  }
164 
165  if (!_detector_materials.size())
166  {
167  cout << PHWHERE << "::Error - Detector materials are not set" << endl;
168  exit(1);
169  }
170 
171  temp_combo.clear();
172  temp_combo.assign(nlayers,0);
173 
174  combos.clear();
175 
176  std::vector<SimpleHit3D> one_layer;
177  layer_sorted.clear();
178  layer_sorted.assign(nlayers, one_layer);
179 
180  ca_tracks.clear();
181  ca_track_states.clear();
182  set_cylinder_kalman();
183 
184  return 1;
185 }
186 
187 
188 void CellularAutomaton_v1::set_hough_space(HelixHoughSpace* hough_space) {
189 
190  _hough_space = dynamic_cast<HelixHoughSpace*> (hough_space->CloneMe());
191  assert(_hough_space);
192 }
193 
194 void CellularAutomaton_v1::set_mag_field(float mag_field) {
195  _mag_field = mag_field;
196 }
197 
198 void CellularAutomaton_v1::set_pt_rescale(float pt_rescale){
199  _pt_rescale = pt_rescale;
200 }
201 
202 void CellularAutomaton_v1::set_detector_radii(std::vector<float>& radii)
203 {
204  for (unsigned int i = 0; i < radii.size(); ++i) {
205  _detector_radii.push_back(radii[i]);
206  }
207 }
208 
209 void CellularAutomaton_v1::set_detector_materials(std::vector<float>& materials)
210 {
211  for (unsigned int i = 0; i < materials.size(); ++i) {
212  _detector_scatter.push_back(1.41421356237309515 * 0.0136 *
213  sqrt(3. * materials[i]));
214  _detector_materials.push_back(3. * materials[i]);
215  }
216 
217  _integrated_scatter.assign(_detector_scatter.size(), 0.);
218  float total_scatter_2 = 0.;
219  for (unsigned int l = 0; l < _detector_scatter.size(); ++l) {
220  total_scatter_2 += _detector_scatter[l] * _detector_scatter[l];
221  _integrated_scatter[l] = sqrt(total_scatter_2);
222  }
223 }
224 
225 void CellularAutomaton_v1::set_cylinder_kalman(){
226  _kalman =
227  new HelixKalmanFilter(_detector_radii, _detector_materials, _mag_field);
228 }
229 
230 void CellularAutomaton_v1::set_input_tracks(std::vector<SimpleTrack3D>& input_tracks)
231 {
232 /*
233  for (unsigned int i = 0; i < input_tracks.size(); ++i)
234  {
235  in_tracks.push_back(input_tracks[i]);
236  }
237 
238 */
239  in_tracks = input_tracks;
240  //cout<<"Setting input tracks : size = " << in_tracks.size()<<endl;
241 
242 }
243 
244 int CellularAutomaton_v1::get_ca_tracks(std::vector<SimpleTrack3D>& output_tracks, std::vector<HelixKalmanState>& output_track_states)
245 {
246  // push back new ca processed tracks into _tracks
247 
248  if (ca_tracks.size() != ca_track_states.size())
249  return 0;
250 
251  for (unsigned int i = 0; i <ca_tracks.size(); ++i)
252  {
253  output_tracks.push_back(ca_tracks[i]);
254  output_track_states.push_back(ca_track_states[i]);
255  }
256 
257  //cout<<"newly added ca tracks : "<< ca_tracks.size() <<" tracks. "<<endl;
258  return 1;
259 }
260 
261 
262 int CellularAutomaton_v1::process_tracks()
263 {
264 
265  for (unsigned int i = 0; i < in_tracks.size(); ++i)
266  { // loop over input tracks
267  if(verbose > 10) cout<<"track candidate "<<i<<endl;
268 
269  if (triplet_mode)
270  {
271  process_single_triplet(in_tracks[i]);
272 /*
273  SimpleTrack3D itrack = in_tracks[i]; // hit triplets with track parameters from HT
274  int code = 0;
275  unsigned int missing_layers = 0;
276  bool last_layer = false;
277  code = triplet_to_segment(itrack); // from hit triplets to a segment with estimated kappa & dzdl
278  for (unsigned int n=3; n < nlayers; ++n ) // extend segments{
279  if (n== nlayers) last_layer = true;
280  code = process_single_track(itrack,n, last_layer); // search layer n
281  if (code==0) ++missing_layers;
282  if (missing_layers > (nlayers-rlayers)) break;
283  }
284 */
285  }
286  else
287  {
288  process_single_track(in_tracks[i]);
289  }
290  }
291 
292  return 1;
293 
294 }
295 
296 
297 int CellularAutomaton_v1::process_single_triplet(SimpleTrack3D& track){ // track : from hough transform
298 
299  std::vector<TrackSegment> segments1;
300  std::vector<TrackSegment> segments2;
301 
302  std::vector<TrackSegment>* cur_seg = &segments1;
303  std::vector<TrackSegment>* next_seg = &segments2;
304  unsigned int cur_seg_size = 0;
305  unsigned int next_seg_size = 0;
306 
307  std::vector<TrackSegment> complete_segments;
308 
309  // from hit triplets to a segment with estimated kappa & dzdl
310 // triplet_to_segments(track, cur_segments); // from hit triplets to a segment with estimated kappa & dzdl
311  for (unsigned int l = 0; l < 3; ++l) {
312  layer_sorted[l].clear();
313  }
314 
315  if(verbose > 10) cout<<"track.hits.size "<<track.hits.size()<<endl;
316  for (unsigned int i = 0; i < track.hits.size(); ++i) {
317  SimpleHit3D hit = track.hits[i];
318  unsigned int layer = (unsigned int) hit.get_layer();
319  if(verbose > 10) cout<<"layer "<<layer<< endl;
320  if (!forward) layer = nlayers-layer-1;
321  if (layer > (nlayers-1)) continue;
322  unsigned int min = (layer - allowed_missing_inner_hits);
323  if (allowed_missing_inner_hits > layer) {
324  min = 0;
325  }
326  for (unsigned int l = min; l <= layer; ++l) {
327  layer_sorted[l].push_back(hit);
328  if(verbose > 10) cout<<"adding hit in layer "<<l<<endl;
329  }
330  }
331 
332 // for (unsigned int l = 0; l < nlayers; ++l) {
333  for (unsigned int l = 0; l< 3; ++l){
334  if(verbose > 10) cout<<"layer_sorted["<<l<<"].size = "<< layer_sorted[l].size()<<endl;
335  if (layer_sorted[l].size() == 0) {
336  return 0;
337  }
338  }
339 
340 #ifdef _FULL_TEST_
341  float ca_cos_ang_cut_diff = 1. - ca_cos_ang_cut;
342  float ca_cos_ang_cut_diff_inv = 1. / ca_cos_ang_cut_diff;
343 #endif
344  float ca_sin_ang_cut = sqrt(1. - ca_cos_ang_cut * ca_cos_ang_cut);
345 
346  std::vector<float> inv_layer;
347  inv_layer.assign(nlayers, 1.);
348  for (unsigned int l = 3; l < nlayers; ++l) {
349  inv_layer[l] = 1. / (((float)l) - 2.);
350  }
351 
352  // l = 3, 4, 5, 6, 7
353  // 1, 1/2, 1/3, 1/4, 1/5
354  // 1, 1/3, 1/5, 1/7, 1/9
355 
356  float x1, x2, x3;
357  float y1, y2, y3;
358  float z1, z2, z3;
359  float dx1, dx2, dx3;
360  float dy1, dy2, dy3;
361  float dz1, dz2, dz3;
362 
363  float kappa;
364  float dkappa;
365 
366  float ux_mid;
367  float uy_mid;
368  float ux_end;
369  float uy_end;
370 
371  float dzdl_1;
372  float dzdl_2;
373  float ddzdl_1;
374  float ddzdl_2;
375 
376 #ifdef _FULL_TEST_
377  float cur_kappa;
378  float cur_dkappa;
379  float cur_ux;
380  float cur_uy;
381  float cur_chi2;
382  float chi2;
383 #endif
384  unsigned int hit1;
385  unsigned int hit2;
386  unsigned int hit3;
387 
388  TrackSegment temp_segment;
389  temp_segment.hits.assign(nlayers, 0);
390 
391  for (unsigned int i = 0; i < layer_sorted[0].size(); ++i) {
392  for (unsigned int j = 0; j < layer_sorted[1].size(); ++j) {
393  for (unsigned int k = 0; k < layer_sorted[2].size() ; ++k) {
394 
395  unsigned int layer0 = layer_sorted[0][i].get_layer();
396  unsigned int layer1 = layer_sorted[1][j].get_layer();
397  unsigned int layer2 = layer_sorted[2][k].get_layer();
398  if (!forward)
399  {
400  layer0 = nlayers-layer0-1;
401  layer1 = nlayers-layer1-1;
402  layer2 = nlayers-layer2-1;
403  }
404 
405  x1 = layer_sorted[0][i].get_x();
406  y1 = layer_sorted[0][i].get_y();
407  z1 = layer_sorted[0][i].get_z();
408 
409  dx1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[0][i].get_size(0,0));
410  dy1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[0][i].get_size(1,1));
411  dz1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[0][i].get_size(2,2));
412 
413  x2 = layer_sorted[1][j].get_x();
414  y2 = layer_sorted[1][j].get_y();
415  z2 = layer_sorted[1][j].get_z();
416 
417  dx2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[1][j].get_size(0,0));
418  dy2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[1][j].get_size(1,1));
419  dz2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[1][j].get_size(2,2));
420 
421  x3 = layer_sorted[2][k].get_x();
422  y3 = layer_sorted[2][k].get_y();
423  z3 = layer_sorted[2][k].get_z();
424  dx3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[2][k].get_size(0,0));
425  dy3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[2][k].get_size(1,1));
426  dz3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[2][k].get_size(2,2));
427 
428  hit1 = i;
429  hit2 = j;
430  hit3 = k;
431 
432  calculate_kappa_tangents(x1, y1, z1, x2, y2, z2, x3, y3,
433  z3, dx1, dy1, dz1, dx2, dy2, dz2,
434  dx3, dy3, dz3, kappa, dkappa,
435  ux_mid, uy_mid, ux_end, uy_end,
436  dzdl_1, dzdl_2, ddzdl_1, ddzdl_2);
437 
438 #ifdef _DEBUG_
439  cout<<"Triplet : "<<endl;
440  cout<<"kappa "<<kappa<< " dkappa "<<dkappa<<" ux_mid "<<ux_mid<<" uy_mid "<<" ux_end "<<ux_end<<
441  " uy_end " <<uy_end<<" dzdl_1 "<<dzdl_1<<" dzdl_2 "<<dzdl_2<<" ddzdl_1 "<<ddzdl_1<<" ddzdl_2 "<<ddzdl_2<<endl;
442 #endif
443 
444  temp_segment.chi2 = pow(
445  (dzdl_1 - dzdl_2) /
446  (ddzdl_1 + ddzdl_2 + fabs(dzdl_1 * ca_sin_ang_cut)),2);
447 
448  if (temp_segment.chi2 > ca_chi2_layer_cut) continue;
449  temp_segment.ux = ux_end;
450  temp_segment.uy = uy_end;
451  temp_segment.kappa = kappa;
452 // if (temp_segment.kappa > _hough_space->get_kappa_max()) continue;
453  temp_segment.dkappa = dkappa;
454 
455  temp_segment.hits[0] = hit1;
456  temp_segment.hits[1] = hit2;
457  temp_segment.hits[2] = hit3;
458  temp_segment.n_hits = 3;
459 
460 
461  unsigned int outer_layer =
462  layer_sorted[2][temp_segment.hits[2]].get_layer();
463  if (!forward) outer_layer = nlayers-outer_layer-1;
464  // make sure we have required number of layers with hits
465  if ((outer_layer - 2) > allowed_missing_inner_hits) continue;
466  // finish up if required number of layers is reached,
467  if ((nlayers - 3) <= allowed_missing_inner_hits) {
468  complete_segments.push_back(temp_segment);
469  }
470  if (next_seg->size() == next_seg_size) { // first new segment
471  next_seg->push_back(temp_segment);
472  next_seg_size += 1;
473  } else { // next new segments
474  (*next_seg)[next_seg_size] = temp_segment;
475  next_seg_size += 1;
476  }
477 
478  }
479  }
480  }
481 
482  swap(cur_seg, next_seg);
483  swap(cur_seg_size, next_seg_size);
484 
485 
486  //cout<<"number of complete segments : " << complete_segments.size()<<endl;
487  //cout<<"number of current segments : "<< cur_seg_size<<endl;
488 
489  // copy complete segments over to current segments
490  for (unsigned int i = 0; i < complete_segments.size(); ++i) {
491  if (cur_seg->size() == cur_seg_size) {
492  cur_seg->push_back(complete_segments[i]);
493  ++cur_seg_size;
494  } else {
495  (*cur_seg)[cur_seg_size] = complete_segments[i];
496  ++cur_seg_size;
497  }
498  }
499 
500  std::set<unsigned int> comp1;
501  std::set<unsigned int> comp2;
502 
503 // cout<<"number of segments generated "<< cur_seg_size<<endl;
504  if (cur_seg_size==0 || cur_seg_size>10000) return 1;
505  for (unsigned int i = cur_seg_size-1; i>0; --i){
506  if ((*cur_seg)[i].n_hits==0) continue;
507  comp1.clear();
508  temp_combo.assign((*cur_seg)[i].n_hits, 0);
509  for (unsigned int l = 0; l < (*cur_seg)[i].n_hits; ++l) {
510  temp_combo[l] = layer_sorted[l][(*cur_seg)[i].hits[l]].get_id();
511  comp1.insert(temp_combo[l]);
512  }
513 
514  sort(temp_combo.begin(), temp_combo.end());
515  set<vector<unsigned int> >::iterator it = combos.find(temp_combo);
516  if (it != combos.end()) {
517  (*cur_seg)[i].n_hits = 0.;
518  }
519  if (combos.size() > 10000) {
520  combos.clear();
521  }
522  combos.insert(temp_combo);
523 
524  for (int j = i-1; j>=0; --j){
525  comp2.clear();
526  for (unsigned int m = 0; m < (*cur_seg)[j].n_hits; ++m){
527  comp2.insert(layer_sorted[m][(*cur_seg)[j].hits[m]].get_id());
528  };
529  for (std::set<unsigned int>::iterator it=comp1.begin(); it!=comp1.end(); ++it){
530  auto it2 = comp2.find(*it);
531  if (it2 != comp2.end()) comp2.erase(*it2);
532  }
533  if (comp2.empty()) {
534  (*cur_seg)[j].n_hits = 0;
535  }
536  if (j==0) break;
537  }
538  }
539 
540  comp1.clear();
541  comp2.clear();
542 
543  unsigned int nsegs = cur_seg_size;
544  for (unsigned int i = 0; i<cur_seg_size; i++) {
545  if ((*cur_seg)[i].n_hits ==0) --nsegs;
546  }
547  //cout<<"number of segments from triplets to be processed "<< nsegs<<endl;
548 
549 
550 #ifdef _FULL_TEST_
551 
552  unsigned int which_seg;
553  // seperate counting from triplets
554 // unsigned int allowed_missing = nlayers - rlayers;
556 
557  std::map<unsigned int, unsigned int> missing_layers_map; // segment_id, missing_layers
558  std::map<unsigned int, unsigned int> missing_layers_map_next;
559  missing_layers_map.clear();
560  missing_layers_map_next.clear();
561  for (unsigned int n = 0 ; n < cur_seg_size; ++n) missing_layers_map.insert(make_pair(n,0));
562  unsigned int added_next_segments = 0;
563 
564  // Extend segments, start from searching the 4th layer
565  for (unsigned int l=3; l < nlayers; ++l ) {
567  next_seg_size = 0;
568  next_seg->clear();
569  // Loop over current segments
570  for (unsigned int i = 0; i < cur_seg_size; ++i) {
571  if ((*cur_seg)[i].n_hits ==0) continue;
572  added_next_segments = 0;
573  auto search = missing_layers_map.find(i);
574  unsigned int missing_layers = search->second;
576 // next_segments(cur_segments, next_segments, n ); // search layer n
577  // loop over all hits on layer n
578  // if multiple legitimate hits in next layer, duplicate current segment, copy over missing layer and assign new segment_id
580  // keep cluster ids in hits[l] for good hits of a segment
581  // for a layer without a good hit, store with hits[l] = 999
582 
583  if ( (l-2) < 3){
584  x1 = layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_x();
585  y1 = layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_y();
586  z1 = layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_z();
587  dx1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_size(0,0));
588  dy1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_size(1,1));
589  dz1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_size(2,2));
590 
591  }else {
592  // get positions from hits_map
593  auto searchl2 = _hits_map.find((*cur_seg)[i].hits[l-2]);
594  SimpleHit3D clusterl2 = searchl2->second;
595  x1 = clusterl2.get_x();
596  y1 = clusterl2.get_y();
597  z1 = clusterl2.get_z();
598  dx1 = 0.5*sqrt(12.0)*sqrt(clusterl2.get_size(0,0));
599  dy1 = 0.5*sqrt(12.0)*sqrt(clusterl2.get_size(1,1));
600  dz1 = 0.5*sqrt(12.0)*sqrt(clusterl2.get_size(2,2));
601  }
602 
603  if ( (l-1) < 3) {
604  x2 = layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_x();
605  y2 = layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_y();
606  z2 = layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_z();
607  dx2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_size(0,0));
608  dy2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_size(1,1));
609  dz2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_size(2,2));
610  } else {
611  // get positions from hits_map
612  auto searchl1 = _hits_map.find((*cur_seg)[i].hits[l-1]);
613  SimpleHit3D clusterl1 = searchl1->second;
614  x2 = clusterl1.get_x();
615  y2 = clusterl1.get_y();
616  z2 = clusterl1.get_z();
617  dx2 = 0.5*sqrt(12.0)*sqrt(clusterl1.get_size(0,0));
618  dy2 = 0.5*sqrt(12.0)*sqrt(clusterl1.get_size(1,1));
619  dz2 = 0.5*sqrt(12.0)*sqrt(clusterl1.get_size(2,2));
620  }
621 
622 
623  // convert segment to track to process it through kalman filter or just call fit_track
624 
625  SimpleTrack3D init_track;
626  bool fit_layer = (l >= 6);
627  if (fit_layer){
628  //init_track.hits.assign(l, SimpleHit3D());
629  init_track.hits.assign((*cur_seg)[i].n_hits, SimpleHit3D());
630 
631  for (unsigned int ll = 0; ll < (*cur_seg)[i].n_hits; ++ll) {
632  if (ll<3){
633  init_track.hits[ll] = layer_sorted[ll][(*cur_seg)[i].hits[ll]];
634  }else {
635  auto search1 = _hits_map.find((*cur_seg)[i].hits[ll]);
636  SimpleHit3D cluster = search1->second;
637  init_track.hits[ll] = cluster;
638  }
639  }
640  }
641 
642  for (std::map<unsigned int,SimpleHit3D>::iterator jt = _hits_map.begin();
643  jt!= _hits_map.end();
644  ++jt) {
645 
646  SimpleHit3D hit3d = jt->second;
647  unsigned int layer = hit3d.get_layer();
648  if (layer != l) continue;
649  which_seg = i;
650  hit1 = hit3d.get_id();
651 
652  x3 = hit3d.get_x();
653  y3 = hit3d.get_y();
654  z3 = hit3d.get_z();
655 
656  float phi_prev =shift_phi_range(atan2(y2,x2));
657  float phi_cur = shift_phi_range(atan2(y3,x3));
658  float phi_diff = phi_cur-phi_prev;
659  if (phi_cur< M_PI/2. && phi_prev > 3*M_PI/2.) phi_diff += 2.*M_PI;
660  else if (phi_cur>3*M_PI/2 && phi_prev<M_PI/2.) phi_diff -= 2.*M_PI;
661  if (!seeding_mode){
662  if ((fabs(phi_diff)> ca_phi_cut || abs(z3-z2)> ca_z_cut)) continue;
663  } else {
664  if ((fabs(phi_diff)> ca_phi_cut || abs(z3-z2)> ca_z_cut) && cur_seg_size!=1) continue;
665  }
666 
667 // auto search = _hits_used.find(hit1);
668 // if(search != _hits_used.end() && search->second ) continue;
669 
670  if (fit_layer)
671  {
672  // fit init_track to get kappa to compare with new kappa
673  // copy init_track over to temp_track and add a hit in kalman filter
674  SimpleTrack3D temp_track;
675  temp_track.hits.assign(init_track.hits.size()+1, SimpleHit3D());
676 
677  for (unsigned int ll = 0; ll < init_track.hits.size(); ++ll) {
678  temp_track.hits[ll] = init_track.hits[ll];
679  }
680  temp_track.hits[init_track.hits.size()] = hit3d;
681 
682  // track fitting instead of computing from triplets
683 
684  float temp_chi2 = temp_track.fit_track();
685 // cout<<"chi2 from fit_track "<<init_chi2 <<endl;
686  if (temp_chi2 != temp_chi2) continue;
687  if (temp_track.kappa != temp_track.kappa ) continue;
688  if (temp_track.z0 != temp_track.z0) continue;
689 
690  HelixKalmanState state;
691  state.phi = temp_track.phi;
692  if (state.phi < 0.) {
693  state.phi += 2. * M_PI;
694  }
695  state.d = temp_track.d;
696  state.kappa = temp_track.kappa;
697  state.nu = sqrt(state.kappa);
698  state.z0 = temp_track.z0;
699  state.dzdl = temp_track.dzdl;
700  state.C = Matrix<float, 5, 5>::Zero(5, 5);
701  state.C(0, 0) = pow(0.01, 2.);
702  state.C(1, 1) = pow(0.01, 2.);
703  state.C(2, 2) = pow(0.01 * state.nu, 2.);
704  state.C(3, 3) = pow(0.05, 2.);
705  state.C(4, 4) = pow(0.05, 2.);
706  state.chi2 = 0.;
707  state.position = 0;
708  state.x_int = 0.;
709  state.y_int = 0.;
710  state.z_int = 0.;
711 
712  // place holder for kalman filter
713 /*
714  unsigned int nfits = 0;
715  for (unsigned int h = 0; h < temp_track.hits.size(); ++h) {
716  _kalman->addHit(temp_track.hits[h], state);
717  nfits += 1;
718  cout<<"nfits "<<nfits<<endl;
719  }
720  cout<<"z0 after kalman "<<state.z0<<endl;
721 
722  // fudge factor for non-gaussian hit sizes
723  state.C *= 3.;
724  state.chi2 *= 6.;
725  // kappa cut here drives both efficiency and ghost track rates down at the same time
726  if (!(temp_track.kappa == temp_track.kappa) ) {
727  continue;
728  }
729  if (!(state.chi2 == state.chi2)) {
730  continue;
731  }
732 
733 */
734  if (state.chi2 / (2. * ((float)(temp_track.hits.size())) - 5.) < ca_chi2_cut /* 10. */) {
735  // translate temp_track into temp_segment (only hit info is saved) and save in next segments
736  for (unsigned int ll = 0; ll < l; ++ll) {
737  temp_segment.hits[ll] = (*cur_seg)[which_seg].hits[ll];
738  }
739  temp_segment.hits[l] = hit1;
740  temp_segment.n_hits = l + 1;
741 
742  if (next_seg->size() == next_seg_size) { // first new segment
743  next_seg->push_back(temp_segment);
744  missing_layers_map_next.insert(make_pair(next_seg->size()-1, missing_layers));
745  next_seg_size += 1;
746  } else { // next new segments
747  #ifdef _DEBUG_
748  cout<<"Next segment size inconsistent "<<endl;
749  #endif
750  (*next_seg)[next_seg_size] = temp_segment;
751  missing_layers_map_next.insert(make_pair(next_seg->size()-1, missing_layers));
752  next_seg_size += 1;
753  }
754  ++added_next_segments;
755 #ifdef _DEBUG_
756  cout<<"segment "<< which_seg << " added segment "<<added_next_segments<<endl;
757 #endif
758 
759  }// chi2 from track fitting and/or kalman filter : looser cuts than computing kappa from triplets
760 
761  }
762  else
763  {
764  // x3 = layer_sorted[l][j].get_x();
765  // y3 = layer_sorted[l][j].get_y();
766  // z3 = layer_sorted[l][j].get_z();
767 
768  // dx1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_size(0,0));
769  // dy1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_size(1,1));
770  // dz1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_size(2,2));
771  // dx2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_size(0,0));
772  // dy2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_size(1,1));
773  // dz2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_size(2,2));
774 
775  // dx3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l][j].get_size(0,0));
776  // dy3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l][j].get_size(1,1));
777  // dz3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l][j].get_size(2,2));
778  dx3 = 0.5*sqrt(12.0)*sqrt(hit3d.get_size(0,0));
779  dy3 = 0.5*sqrt(12.0)*sqrt(hit3d.get_size(1,1));
780  dz3 = 0.5*sqrt(12.0)*sqrt(hit3d.get_size(2,2));
781 
782  cur_kappa = (*cur_seg)[i].kappa;
783  cur_dkappa = (*cur_seg)[i].dkappa;
784  cur_ux = (*cur_seg)[i].ux;
785  cur_uy = (*cur_seg)[i].uy;
786  cur_chi2 = (*cur_seg)[i].chi2;
787 
788  chi2=9999;
789  calculate_kappa_tangents(
790  x1, y1, z1, x2, y2, z2, x3, y3, z3,
791  dx1, dy1, dz1, dx2, dy2, dz2, dx3, dy3, dz3,
792  kappa, dkappa, ux_mid, uy_mid, ux_end, uy_end,
793  dzdl_1, dzdl_2, ddzdl_1, ddzdl_2,
794  ca_sin_ang_cut, ca_cos_ang_cut_diff_inv,
795  cur_kappa, cur_dkappa, cur_ux, cur_uy, cur_chi2, chi2);
796 #ifdef _DEBUG_
797  cout<<"Extended layers for segment "<<which_seg<<endl;
798  cout<<"kappa "<<kappa<< " dkappa "<<dkappa
799  <<" ux_mid "<<ux_mid<<" uy_mid "<<" ux_end "<<ux_end<<" uy_end " <<uy_end
800  <<" dzdl_1 "<<dzdl_1<<" dzdl_2 "<<dzdl_2<<" ddzdl_1 "<<ddzdl_1<<" ddzdl_2 "<<ddzdl_2
801  <<" cur_chi2 "<<cur_chi2<<" chi2 "<<chi2<<" chi2*inv_layer "<<l <<" "<<chi2*inv_layer[l]
802  <<" chi2_cut " <<ca_chi2_layer_cut<<endl;
803 #endif
804 
805 
806  // if segment with good chi2, store it in next segment
807  if (chi2 * inv_layer[l] < ca_chi2_layer_cut ) {
808  temp_segment.chi2 = chi2;
809  temp_segment.ux = ux_end;
810  temp_segment.uy = uy_end;
811  temp_segment.kappa = kappa;
812  if (seeding_mode){
813  if (temp_segment.kappa > _hough_space->get_kappa_max() &&cur_seg_size!=1) continue;
814  } else {
815  if (temp_segment.kappa > _hough_space->get_kappa_max()) continue;
816  }
817 
818  temp_segment.dkappa = dkappa;
819  for (unsigned int ll = 0; ll < l; ++ll) {
820  temp_segment.hits[ll] = (*cur_seg)[which_seg].hits[ll];
821  }
822  temp_segment.hits[l] = hit1;
823  //unsigned int outer_layer =
824  // layer_sorted[l][temp_segment.hits[l]].get_layer();
825  //if (!forward) outer_layer = nlayers-outer_layer-1;
826  temp_segment.n_hits = l + 1;
827  // finish up if required number of layers is reached
828  //if ((nlayers - (l + 1)) <= allowed_missing) {
829  // complete_segments.push_back(temp_segment);
830  //}
831  // make sure we have required number of layers with hits
832  //if ((outer_layer - l) > allowed_missing) {
833  //continue;
834  //}
835 
836  // Discard segment if too many missing layers
837  if (next_seg->size() == next_seg_size) { // first new segment
838  next_seg->push_back(temp_segment);
839  missing_layers_map_next.insert(make_pair(next_seg->size()-1, missing_layers));
840  next_seg_size += 1;
841  } else { // next new segments
842  #ifdef _DEBUG_
843  cout<<"Next segment size inconsistent "<<endl;
844  #endif
845  (*next_seg)[next_seg_size] = temp_segment;
846  missing_layers_map_next.insert(make_pair(next_seg->size()-1, missing_layers));
847  next_seg_size += 1;
848  }
849  ++added_next_segments;
850 #ifdef _DEBUG_
851  cout<<"segment "<< which_seg << " added segment "<<added_next_segments<<endl;
852 #endif
853  } // chi2 cut from segment building method -> change to switch - case block
854  }
855 
856  }// clusters on layer l
857 
858 /*
859  if (added_next_segments==0){
860  ++missing_layers;
861  if (missing_layers <= allowed_missing){
862  if (next_seg->size() == next_seg_size) { // first new segment
863  next_seg->push_back((*cur_seg)[i]);
864  missing_layers_map_next.insert(make_pair(next_seg->size()-1, missing_layers));
865  next_seg_size += 1;
866  } else { // next new segments
867  (*next_seg)[next_seg_size] = (*cur_seg)[i];
868  missing_layers_map_next.insert(make_pair(next_seg->size()-1, missing_layers));
869  next_seg_size += 1;
870  }
871  }
872  }
873 */
874 
875 
876  } // current segments
877  swap(cur_seg, next_seg);
878  swap(cur_seg_size, next_seg_size);
879  missing_layers_map.swap(missing_layers_map_next);
880  // Current segments now hold extended segments up to layer l, and missing layers info is in missing_layers_map
881 
882  }// next segment
883 
884 
885 #endif
886 
887  SimpleTrack3D temp_track;
888  temp_track.hits.assign(nlayers, SimpleHit3D());
889 
890  std::vector<SimpleTrack3D> best_track;
891  std::vector<HelixKalmanState> best_track_state;
892  float best_chi2 = 9999;
893  for (unsigned int i = 0; i< cur_seg_size; ++i) {
894 
895  if ((*cur_seg)[i].n_hits ==0) continue;
896 
897 #ifdef _DEBUG_
898  cout<<"segment " <<i <<endl;
899 #endif
900  temp_track.hits.assign((*cur_seg)[i].n_hits, SimpleHit3D());
901 // float seg_kappa = (*cur_seg)[i].kappa;
902 
903  for (unsigned int l = 0; l < (*cur_seg)[i].n_hits; ++l) {
904  if (l<3){
905  temp_track.hits[l] = layer_sorted[l][(*cur_seg)[i].hits[l]];
906  }else {
907  auto search = _hits_map.find((*cur_seg)[i].hits[l]);
908  SimpleHit3D cluster = search->second;
909  temp_track.hits[l] = cluster;
910  }
911  }
912 
913  float init_chi2 = temp_track.fit_track();
914  if(verbose > 10) cout<<"chi2 from fit_track "<<init_chi2 <<" kappa "<< temp_track.kappa <<endl;
915 
916 #ifdef _DEBUG_
917  cout <<" kappa " <<temp_track.kappa <<" phi "<<temp_track.phi<<" d "<<temp_track.d
918  <<" z0 "<<temp_track.z0<<" dzdl "<<temp_track.dzdl<< endl;
919 #endif
920 
921  if (seeding_mode){
922  if (temp_track.kappa != temp_track.kappa && cur_seg_size!=1) continue;
923  if (temp_track.z0 != temp_track.z0 && cur_seg_size != 1) continue;
924  } else {
925  if (temp_track.kappa != temp_track.kappa) continue;
926  if (temp_track.z0 != temp_track.z0) continue;
927  }
928 
929  HelixKalmanState state;
930  state.phi = temp_track.phi;
931  if (state.phi < 0.) {
932  state.phi += 2. * M_PI;
933  }
934  state.d = temp_track.d;
935  state.kappa = temp_track.kappa;
936  state.nu = sqrt(state.kappa);
937  state.z0 = temp_track.z0;
938  state.dzdl = temp_track.dzdl;
939  state.C = Matrix<float, 5, 5>::Zero(5, 5);
940  state.C(0, 0) = pow(0.01, 2.);
941  state.C(1, 1) = pow(0.01, 2.);
942  state.C(2, 2) = pow(0.01 * state.nu, 2.);
943  state.C(3, 3) = pow(0.05, 2.);
944  state.C(4, 4) = pow(0.05, 2.);
945  state.chi2 = 0.;
946  state.position = 0;
947  state.x_int = 0.;
948  state.y_int = 0.;
949  state.z_int = 0.;
950 
951  unsigned int nfits = 0;
952  for (unsigned int h = 0; h < temp_track.hits.size(); ++h) {
953  _kalman->addHit(temp_track.hits[h], state);
954  nfits += 1;
955 #ifdef _DEBUG_
956  cout<<"nfits "<<nfits<<endl;
957 #endif
958  }
959 #ifdef _DEBUG_
960  cout<<"z0 after kalman "<<state.z0<<endl;
961 #endif
962  // fudge factor for non-gaussian hit sizes
963  state.C *= 3.;
964  state.chi2 *= 6.;
965  // kappa cut *here* drives both efficiency and ghost track rates down at the same time
966  if (seeding_mode){
967  if (!(temp_track.kappa == temp_track.kappa) && (cur_seg_size !=1)) continue;
968  if (!(state.chi2 == state.chi2) && (cur_seg_size !=1)) continue;
969  } else {
970  if (!(temp_track.kappa == temp_track.kappa)) continue;
971  if (!(state.chi2 == state.chi2)) continue;
972  }
973 
974 /*
975  if (fabs(temp_track.d) > ca_dcaxy_cut) continue;
976  if (fabs(temp_track.z0) > dca_cut) continue;
977 */
978 
979  // no chi2 cut for tests on triplets
980 
981  //cout<<"state.chi2 from kalman "<<state.chi2<<endl;
982  if (seeding_mode){
983  if (state.chi2 / (2. * ((float)(temp_track.hits.size())) - 5.) > ca_chi2_cut && cur_seg_size !=1) continue;
984  } else {
985  if (state.chi2 / (2. * ((float)(temp_track.hits.size())) - 5.) > ca_chi2_cut) continue;
986  }
987 
988  if (best_chi2 > state.chi2 || (seeding_mode && cur_seg_size==1)){
989  if (!best_track.empty()){
990  best_track.pop_back();
991  best_track_state.pop_back();
992  }
993  best_track.push_back(temp_track);
994  best_track_state.push_back(state);
995  }
996  }
997 
998  if (best_track.empty()) return 1;
999 
1000  if(verbose > 10) cout<<"best_track.size "<<best_track.size()<<endl;
1001  ca_tracks.push_back(best_track.back());
1002  ca_track_states.push_back(best_track_state.back());
1003  if(verbose > 10) cout <<"ca track added, chi2 = "<< (best_track_state.back().chi2)/(2. * ((float)(temp_track.hits.size())) - 5.) <<" z0 = "<<best_track_state.back().z0<<endl;
1004 
1005 
1006  // if ((remove_hits == true) && (state.chi2 < chi2_removal_cut) &&
1007  // (temp_track.hits.size() >= n_removal_hits)) {
1008  temp_track = best_track.back();
1009 
1010  if (remove_hits){
1011  for (unsigned int i = 0; i < temp_track.hits.size(); ++i) {
1012  if (!remove_inner_hits && temp_track.hits[i].get_layer()<3) continue;
1013  auto search = _hits_used.find(temp_track.hits[i].get_id());
1014  if(search != _hits_used.end())
1015  {
1016  _hits_used.find(temp_track.hits[i].get_id())->second = true;
1017  }
1018  }
1019  }
1020 
1021  segments1.clear();
1022  segments2.clear();
1023  return 1;
1024 }
1025 
1026 int CellularAutomaton_v1::process_single_track(SimpleTrack3D& track)
1027 {
1028 
1029  std::vector<TrackSegment> segments1;
1030  std::vector<TrackSegment> segments2;
1031 
1032 
1033  std::vector<TrackSegment>* cur_seg = &segments1;
1034  std::vector<TrackSegment>* next_seg = &segments2;
1035  unsigned int cur_seg_size = 0;
1036  unsigned int next_seg_size = 0;
1037 
1038  std::vector<TrackSegment> complete_segments;
1039 
1040  unsigned int allowed_missing = nlayers - rlayers;
1041  cout<<"allowed missing "<< allowed_missing<<endl;
1042 
1043 
1044  // l is not actual layer number, it is ith layer
1045  for (unsigned int l = 0; l < nlayers; ++l) {
1046  layer_sorted[l].clear();
1047  }
1048 
1049 // cout<<"track.hits.size "<<track.hits.size()<<endl;
1050  for (unsigned int i = 0; i < track.hits.size(); ++i) {
1051  SimpleHit3D hit = track.hits[i];
1052  unsigned int layer = (unsigned int) hit.get_layer();
1053  if (layer > (nlayers-1)) continue;
1054  if (!forward) layer = nlayers-layer-1;
1055  unsigned int min = (layer - allowed_missing);
1056  if (allowed_missing > layer) {
1057  min = 0;
1058  }
1059  for (unsigned int l = min; l <= layer; ++l) {
1060  layer_sorted[l].push_back(hit);
1061  }
1062 
1063  }
1064 
1065  for (unsigned int l = 0; l < nlayers; ++l) {
1066 // cout<<"layer_sorted["<<l<<"].size = "<< layer_sorted[l].size()<<endl;
1067  if (layer_sorted[l].size() == 0) {
1068  return 0;
1069  }
1070  }
1071 
1072  timeval t1, t2;
1073  double time1 = 0.;
1074  double time2 = 0.;
1075 
1076  gettimeofday(&t1, nullptr);
1077 
1078  float ca_cos_ang_cut_diff = 1. - ca_cos_ang_cut;
1079  float ca_cos_ang_cut_diff_inv = 1. / ca_cos_ang_cut_diff;
1080  float ca_sin_ang_cut = sqrt(1. - ca_cos_ang_cut * ca_cos_ang_cut);
1081 
1082  std::vector<float> inv_layer;
1083  inv_layer.assign(nlayers, 1.);
1084  for (unsigned int l = 3; l < nlayers; ++l) {
1085  inv_layer[l] = 1. / (((float)l) - 2.);
1086  }
1087 
1088  float x1, x2, x3;
1089  float y1, y2, y3;
1090  float z1, z2, z3;
1091  float dx1, dx2, dx3;
1092  float dy1, dy2, dy3;
1093  float dz1, dz2, dz3;
1094 
1095  float kappa;
1096  float dkappa;
1097 
1098  float ux_mid;
1099  float uy_mid;
1100  float ux_end;
1101  float uy_end;
1102 
1103  float dzdl_1;
1104  float dzdl_2;
1105  float ddzdl_1;
1106  float ddzdl_2;
1107 
1108 
1109  float cur_kappa;
1110  float cur_dkappa;
1111  float cur_ux;
1112  float cur_uy;
1113  float cur_chi2;
1114  float chi2;
1115 
1116  unsigned int hit1;
1117  unsigned int hit2;
1118  unsigned int hit3;
1119 
1120  TrackSegment temp_segment;
1121  temp_segment.hits.assign(nlayers, 0);
1122 
1123  for (unsigned int i = 0; i < layer_sorted[0].size(); ++i) {
1124  for (unsigned int j = 0; j < layer_sorted[1].size(); ++j) {
1125  for (unsigned int k = 0; k < layer_sorted[2].size() ; ++k) {
1126 
1127  unsigned int layer0 = layer_sorted[0][i].get_layer();
1128  unsigned int layer1 = layer_sorted[1][j].get_layer();
1129  unsigned int layer2 = layer_sorted[2][k].get_layer();
1130  if (!forward)
1131  {
1132  layer0 = nlayers-layer0-1;
1133  layer1 = nlayers-layer1-1;
1134  layer2 = nlayers-layer2-1;
1135  }
1136  if ((layer0 >= layer1) || (layer1 >= layer2)) {
1137  continue;
1138  }
1139 
1140  x1 = layer_sorted[0][i].get_x();
1141  y1 = layer_sorted[0][i].get_y();
1142  z1 = layer_sorted[0][i].get_z();
1143 
1144  // sigma ?= half pictch
1145  dx1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[0][i].get_size(0,0));
1146  dy1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[0][i].get_size(1,1));
1147  dz1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[0][i].get_size(2,2));
1148 
1149  x2 = layer_sorted[1][j].get_x();
1150  y2 = layer_sorted[1][j].get_y();
1151  z2 = layer_sorted[1][j].get_z();
1152 
1153  dx2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[1][j].get_size(0,0));
1154  dy2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[1][j].get_size(1,1));
1155  dz2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[1][j].get_size(2,2));
1156 
1157  x3 = layer_sorted[2][k].get_x();
1158  y3 = layer_sorted[2][k].get_y();
1159  z3 = layer_sorted[2][k].get_z();
1160  dx3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[2][k].get_size(0,0));
1161  dy3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[2][k].get_size(1,1));
1162  dz3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[2][k].get_size(2,2));
1163 
1164  // layer of hit
1165  hit1 = i;
1166  hit2 = j;
1167  hit3 = k;
1168 
1169  calculate_kappa_tangents(x1, y1, z1, x2, y2, z2, x3, y3,
1170  z3, dx1, dy1, dz1, dx2, dy2, dz2,
1171  dx3, dy3, dz3, kappa, dkappa,
1172  ux_mid, uy_mid, ux_end, uy_end,
1173  dzdl_1, dzdl_2, ddzdl_1, ddzdl_2);
1174 
1175 #ifdef _DEBUG_
1176  cout<<"Triplet : "<<endl;
1177  cout<<"kappa "<<kappa<< " dkappa "<<dkappa<<" ux_mid "<<ux_mid<<" uy_mid "<<" ux_end "<<ux_end<<
1178  " uy_end " <<uy_end<<" dzdl_1 "<<dzdl_1<<" dzdl_2 "<<dzdl_2<<" ddzdl_1 "<<ddzdl_1<<" ddzdl_2 "<<ddzdl_2<<endl;
1179 #endif
1180 
1181  temp_segment.chi2 = pow(
1182  (dzdl_1 - dzdl_2) /
1183  (ddzdl_1 + ddzdl_2 + fabs(dzdl_1 * ca_sin_ang_cut)),2);
1184  if (temp_segment.chi2 > ca_chi2_layer_cut) continue;
1185  temp_segment.ux = ux_end;
1186  temp_segment.uy = uy_end;
1187  temp_segment.kappa = kappa;
1188 // if (temp_segment.kappa > _hough_space->get_kappa_max()) continue;
1189  temp_segment.dkappa = dkappa;
1190  temp_segment.hits[0] = hit1;
1191  temp_segment.hits[1] = hit2;
1192  temp_segment.hits[2] = hit3;
1193  temp_segment.n_hits = 3;
1194  unsigned int outer_layer =
1195  layer_sorted[2][temp_segment.hits[2]].get_layer();
1196  if (!forward) outer_layer = nlayers-outer_layer-1;
1197  // make sure we have required number of layers with hits
1198  if ((outer_layer - 2) > allowed_missing) continue;
1199  // finish up if required number of layers is reached,
1200  if ((nlayers - 3) <= allowed_missing) {
1201  complete_segments.push_back(temp_segment);
1202  }
1203  if (next_seg->size() == next_seg_size) { // first new segment
1204  next_seg->push_back(temp_segment);
1205  next_seg_size += 1;
1206  } else { // next new segments
1207  (*next_seg)[next_seg_size] = temp_segment;
1208  next_seg_size += 1;
1209  }
1210 
1211  }
1212  }
1213  }
1214 
1215  swap(cur_seg, next_seg);
1216  swap(cur_seg_size, next_seg_size);
1217 
1218  // cout<<"number of segments from first 3 layers : " << cur_seg_size<<endl;
1219  unsigned int which_seg;
1220 
1221  // add hits to segments layer-by-layer, cutting out bad segments
1222  for (unsigned int l = 3; l < nlayers; ++l) {
1223  if (l == (nlayers - 1)) {
1224 // ca_chi2_cut_layer = 0.25* ca_chi2_cut;// 2.*0.25 = 0.8 less loose cut after adding all clusters
1225  }
1226  next_seg_size = 0;
1227  for (unsigned int i = 0; i < cur_seg_size; ++i) {
1228  for (unsigned int j = 0; j < layer_sorted[l].size(); ++j) {
1229 
1230  unsigned int layer0 = layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_layer();
1231  unsigned int layer1 = layer_sorted[l][j].get_layer();
1232  if (!forward)
1233  {
1234  layer0 = nlayers-layer0-1;
1235  layer1 = nlayers-layer1-1;
1236  }
1237  if (layer0 >= layer1) continue;
1238 
1239  x1 = layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_x();
1240  y1 = layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_y();
1241  z1 = layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_z();
1242  x2 = layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_x();
1243  y2 = layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_y();
1244  z2 = layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_z();
1245  x3 = layer_sorted[l][j].get_x();
1246  y3 = layer_sorted[l][j].get_y();
1247  z3 = layer_sorted[l][j].get_z();
1248 
1249  dx1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_size(0,0));
1250  dy1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_size(1,1));
1251  dz1 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_size(2,2));
1252  dx2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_size(0,0));
1253  dy2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_size(1,1));
1254  dz2 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_size(2,2));
1255  dx3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l][j].get_size(0,0));
1256  dy3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l][j].get_size(1,1));
1257  dz3 = 0.5*sqrt(12.0)*sqrt(layer_sorted[l][j].get_size(2,2));
1258 
1259  cur_kappa = (*cur_seg)[i].kappa;
1260  cur_dkappa = (*cur_seg)[i].dkappa;
1261  cur_ux = (*cur_seg)[i].ux;
1262  cur_uy = (*cur_seg)[i].uy;
1263  cur_chi2 = (*cur_seg)[i].chi2;
1264 
1265  which_seg = i;
1266  hit1 = j;
1267 
1268  calculate_kappa_tangents(
1269  x1, y1, z1, x2, y2, z2, x3, y3, z3,
1270  dx1, dy1, dz1, dx2, dy2, dz2, dx3, dy3, dz3,
1271  kappa, dkappa, ux_mid, uy_mid, ux_end, uy_end,
1272  dzdl_1, dzdl_2, ddzdl_1, ddzdl_2,
1273  ca_sin_ang_cut, ca_cos_ang_cut_diff_inv,
1274  cur_kappa, cur_dkappa, cur_ux, cur_uy, cur_chi2, chi2);
1275 
1276 #ifdef _DEBUG_
1277  cout<<"Extended layers for segment "<<which_seg<<endl;
1278  cout<<"kappa "<<kappa<< " dkappa "<<dkappa
1279  <<" ux_mid "<<ux_mid<<" uy_mid "<<" ux_end "<<ux_end<<" uy_end " <<uy_end
1280  <<" dzdl_1 "<<dzdl_1<<" dzdl_2 "<<dzdl_2<<" ddzdl_1 "<<ddzdl_1<<" ddzdl_2 "<<ddzdl_2
1281  <<" cur_chi2 "<<cur_chi2<<" chi2 "<<chi2<<" chi2*inv_layer "<<l <<" "<<chi2*inv_layer[l]
1282  <<" chi2_cut" <<ca_chi2_layer_cut<<endl;
1283 #endif
1284  if (chi2 * inv_layer[l] < ca_chi2_layer_cut) {
1285  temp_segment.chi2 = chi2;
1286  temp_segment.ux = ux_end;
1287  temp_segment.uy = uy_end;
1288  temp_segment.kappa = kappa;
1289  //if (temp_segment.kappa > _hough_space->get_kappa_max()) continue;
1290  temp_segment.dkappa = dkappa;
1291  for (unsigned int ll = 0; ll < l; ++ll) {
1292  temp_segment.hits[ll] = (*cur_seg)[which_seg].hits[ll];
1293  }
1294  temp_segment.hits[l] = hit1;
1295  unsigned int outer_layer =
1296  layer_sorted[l][temp_segment.hits[l]].get_layer();
1297  if (!forward) outer_layer = nlayers-outer_layer-1;
1298  temp_segment.n_hits = l + 1;
1299  // finish up if required number of layers is reached
1300  if ((nlayers - (l + 1)) <= allowed_missing) {
1301  complete_segments.push_back(temp_segment);
1302  }
1303  // make sure we have required number of layers with hits
1304  if ((outer_layer - l) > allowed_missing) {
1305  continue;
1306  }
1307  if (next_seg->size() == next_seg_size) { // first new segment
1308  next_seg->push_back(temp_segment);
1309  next_seg_size += 1;
1310  } else { // next new segments
1311  (*next_seg)[next_seg_size] = temp_segment;
1312  next_seg_size += 1;
1313  }
1314  }
1315  }// j
1316  }// i
1317 
1318  swap(cur_seg, next_seg);
1319  swap(cur_seg_size, next_seg_size);
1320 
1321  }//l = 3
1322 
1323  //cout<<"number of complete segments : " << complete_segments.size()<<endl;
1324  //cout<<"number of current segments : "<< cur_seg_size<<endl;
1325 
1326  // copy complete segments over to current segments
1327  for (unsigned int i = 0; i < complete_segments.size(); ++i) {
1328  if (cur_seg->size() == cur_seg_size) {
1329  cur_seg->push_back(complete_segments[i]);
1330  ++cur_seg_size;
1331  } else {
1332  (*cur_seg)[cur_seg_size] = complete_segments[i];
1333  ++cur_seg_size;
1334  }
1335  }
1336 
1337  gettimeofday(&t2, nullptr);
1338  time1 = ((double)(t1.tv_sec) + (double)(t1.tv_usec) / 1000000.);
1339  time2 = ((double)(t2.tv_sec) + (double)(t2.tv_usec) / 1000000.);
1340  CAtime += (time2 - time1);
1341 
1342  std::set<unsigned int> comp1;
1343  std::set<unsigned int> comp2;
1344 
1345  //cout<<"number of segments generated "<< cur_seg_size<<endl;
1346  if (cur_seg_size==0 || cur_seg_size>10000) return 1;
1347  for (unsigned int i = cur_seg_size-1; i>0; --i){
1348  if ((*cur_seg)[i].n_hits==0) continue;
1349  comp1.clear();
1350  temp_combo.assign((*cur_seg)[i].n_hits, 0);
1351  for (unsigned int l = 0; l < (*cur_seg)[i].n_hits; ++l) {
1352  temp_combo[l] = layer_sorted[l][(*cur_seg)[i].hits[l]].get_id();
1353  comp1.insert(temp_combo[l]);
1354  }
1355 
1356  sort(temp_combo.begin(), temp_combo.end());
1357  set<vector<unsigned int> >::iterator it = combos.find(temp_combo);
1358  if (it != combos.end()) {
1359  (*cur_seg)[i].n_hits = 0.;
1360  }
1361 
1362 // if (combos.size() > 10000) {
1363  if (combos.size() > 100000) {
1364  combos.clear();
1365  }
1366  combos.insert(temp_combo);
1367 
1368  for (int j = i-1; j>=0; --j){
1369  comp2.clear();
1370  for (unsigned int m = 0; m < (*cur_seg)[j].n_hits; ++m){
1371  comp2.insert(layer_sorted[m][(*cur_seg)[j].hits[m]].get_id());
1372  };
1373  for (std::set<unsigned int>::iterator it=comp1.begin(); it!=comp1.end(); ++it){
1374  auto it2 = comp2.find(*it);
1375  if (it2 != comp2.end()) comp2.erase(*it2);
1376  }
1377  if (comp2.empty()) {
1378  (*cur_seg)[j].n_hits = 0;
1379  }
1380  if (j==0) break;
1381  }
1382  }
1383 
1384  unsigned int nsegs = cur_seg_size;
1385  for (unsigned int i = 0; i<cur_seg_size; i++) {
1386  if ((*cur_seg)[i].n_hits ==0) --nsegs;
1387  }
1388  //cout<<"number of segments to be processed "<< nsegs<<endl;
1389 
1390  SimpleTrack3D temp_track;
1391  temp_track.hits.assign(nlayers, SimpleHit3D());
1392 
1393  std::vector<SimpleTrack3D> best_track;
1394  std::vector<HelixKalmanState> best_track_state;
1395  float best_chi2 = 9999;
1396  for (unsigned int i = 0; i< cur_seg_size; ++i) {
1397 
1398  if ((*cur_seg)[i].n_hits ==0) continue;
1399 
1400 #ifdef _DEBUG_
1401  cout<<"segment " <<i <<endl;
1402 #endif
1403  temp_track.hits.assign((*cur_seg)[i].n_hits, SimpleHit3D());
1404 
1405  for (unsigned int l = 0; l < (*cur_seg)[i].n_hits; ++l) {
1406  temp_track.hits[l] = layer_sorted[l][(*cur_seg)[i].hits[l]];
1407  }
1408 
1409  unsigned int ninner_hits =0;
1410  for (unsigned int n = 0; n < temp_track.hits.size(); ++n){
1411  if (temp_track.hits[n].get_layer()<3)
1412  ++ninner_hits;
1413  }
1414 
1415  if(require_inner_hits && ninner_hits<2) continue;
1416 
1417  gettimeofday(&t1, nullptr);
1418 
1419  float init_chi2 = temp_track.fit_track();
1420 #ifdef _DEBUG_
1421  cout<<"chi2 from fit_track "<<init_chi2
1422  <<" kappa " <<temp_track.kappa <<" phi "<<temp_track.phi<<" d "<<temp_track.d
1423  <<" z0 "<<temp_track.z0<<" dzdl "<<temp_track.dzdl<< endl;
1424 #endif
1425 
1426  // not being used for the time being
1427  if (init_chi2 > fast_chi2_cut_max) {
1428  if (init_chi2 > fast_chi2_cut_par0 +
1429  fast_chi2_cut_par1 / kappa_to_pt(temp_track.kappa)) {
1430  gettimeofday(&t2, nullptr);
1431  time1 = ((double)(t1.tv_sec) + (double)(t1.tv_usec) / 1000000.);
1432  time2 = ((double)(t2.tv_sec) + (double)(t2.tv_usec) / 1000000.);
1433  KALtime += (time2 - time1);
1434  continue;
1435  }
1436  }
1437 
1438  HelixKalmanState state;
1439  state.phi = temp_track.phi;
1440  if (state.phi < 0.) {
1441  state.phi += 2. * M_PI;
1442  }
1443  state.d = temp_track.d;
1444  state.kappa = temp_track.kappa;
1445  state.nu = sqrt(state.kappa);
1446  state.z0 = temp_track.z0;
1447  state.dzdl = temp_track.dzdl;
1448  state.C = Matrix<float, 5, 5>::Zero(5, 5);
1449  state.C(0, 0) = pow(0.01, 2.);
1450  state.C(1, 1) = pow(0.01, 2.);
1451  state.C(2, 2) = pow(0.01 * state.nu, 2.);
1452  state.C(3, 3) = pow(0.05, 2.);
1453  state.C(4, 4) = pow(0.05, 2.);
1454  state.chi2 = 0.;
1455  state.position = 0;
1456  state.x_int = 0.;
1457  state.y_int = 0.;
1458  state.z_int = 0.;
1459 
1460  unsigned int nfits = 0;
1461  for (unsigned int h = 0; h < temp_track.hits.size(); ++h) {
1462  _kalman->addHit(temp_track.hits[h], state);
1463  nfits += 1;
1464  }
1465  if(verbose > 0) cout<<"z0 after kalman "<<state.z0<<endl;
1466 
1467  // fudge factor for non-gaussian hit sizes
1468  state.C *= 3.;
1469  state.chi2 *= 6.;
1470 
1471  gettimeofday(&t2, nullptr);
1472  time1 = ((double)(t1.tv_sec) + (double)(t1.tv_usec) / 1000000.);
1473  time2 = ((double)(t2.tv_sec) + (double)(t2.tv_usec) / 1000000.);
1474  KALtime += (time2 - time1);
1475 
1476  if (!(temp_track.kappa == temp_track.kappa)) {
1477  continue;
1478  }
1479 /*
1480  if (temp_track.kappa > _hough_space->get_kappa_max()) {
1481  continue;
1482  }
1483 */
1484  if (!(state.chi2 == state.chi2)) {
1485  continue;
1486  }
1487  if (state.chi2 / (2. * ((float)(temp_track.hits.size())) - 5.) > ca_chi2_cut) {
1488  continue;
1489  }
1490 /*
1491  if (cut_on_dca == true) {
1492  if (fabs(temp_track.d) > dca_cut) {
1493  continue;
1494  }
1495  if (fabs(temp_track.z0) > dca_cut) {
1496  continue;
1497  }
1498  }
1499 */
1500  if (best_chi2 > state.chi2){
1501  if (!best_track.empty()){
1502  best_track.pop_back();
1503  best_track_state.pop_back();
1504  }
1505  best_track.push_back(temp_track);
1506  best_track_state.push_back(state);
1507  }
1508  }
1509 
1510  if (best_track.empty()) return 1;
1511 
1512  ca_tracks.push_back(best_track.back());
1513  ca_track_states.push_back(best_track_state.back());
1514  if(verbose > 0) cout <<"ca track added, chi2 = "<< (best_track_state.back().chi2)/(2. * ((float)(temp_track.hits.size())) - 5.) <<" z0 = "<<best_track_state.back().z0<<endl;
1515 
1516 
1517 // if ((remove_hits == true) && (state.chi2 < chi2_removal_cut) &&
1518 // (temp_track.hits.size() >= n_removal_hits)) {
1519  temp_track = best_track.back();
1520 
1521  if (remove_hits){
1522  for (unsigned int i = 0; i < temp_track.hits.size(); ++i) {
1523  if (!remove_inner_hits && temp_track.hits[i].get_layer()<3) continue;
1524  auto search = _hits_used.find(temp_track.hits[i].get_id());
1525  if(search != _hits_used.end())
1526  {
1527  _hits_used.find(temp_track.hits[i].get_id())->second = true;
1528  }
1529  }
1530  }
1531 
1532  segments1.clear();
1533  segments2.clear();
1534 
1535 
1536  return 1;
1537 }
1538 
1539 int CellularAutomaton_v1::calculate_kappa_tangents(
1540  float x1, float y1, float z1, float x2, float y2, float z2,
1541  float x3, float y3, float z3,
1542  float dx1, float dy1, float dz1, float dx2, float dy2, float dz2,
1543  float dx3, float dy3, float dz3,
1544  float& kappa, float& dkappa,
1545  float& ux_mid, float& uy_mid, float& ux_end, float& uy_end,
1546  float& dzdl_1, float& dzdl_2, float& ddzdl_1, float& ddzdl_2)
1547 {
1548 
1549  float D12 = sqrt(pow(x2-x1,2)+pow(y2-y1,2));
1550  float D23 = sqrt(pow(x3-x2,2)+pow(y3-y2,2));
1551  float D13 = sqrt(pow(x3-x1,2)+pow(y3-y1,2));
1552  kappa = 1./(D12*D23*D13);
1553  float num = (D12+D23+D13)*(D23+D13-D12)*(D12+D13-D23)*(D12+D23-D13);
1554  if (num<0) num = 0;
1555  num = sqrt(num);
1556  kappa *=num;
1557 
1558  float kappa_inv = 1/kappa;
1559  float D12_inv = 1./D12;
1560  float D23_inv = 1./D23;
1561  float D13_inv = 1./D13;
1562 
1563  float dr1 = sqrt(pow(dx1,2)+pow(dy1,2));
1564  float dr2 = sqrt(pow(dx2,2)+pow(dy2,2));
1565  float dr3 = sqrt(pow(dx3,2)+pow(dy3,2));
1566 
1567  float dk1 = (dr1+dr2)* D12_inv*D12_inv;
1568  float dk2 = (dr2+dr3)* D23_inv*D23_inv;
1569  dkappa = dk1+dk2;
1570 
1571  float ux12 = (x2-x1)*D12_inv;
1572  float uy12 = (y2-y1)*D12_inv;
1573  float ux23 = (x3-x2)*D23_inv;
1574  float uy23 = (y3-y2)*D23_inv;
1575  float ux13 = (x3-x1)*D13_inv;
1576  float uy13 = (y3-y1)*D13_inv;
1577 
1578  // cos(alpha) = cos(alpha12 - alpha13)
1579  // sin(alpha) = sin(alpha12 - alpha13)
1580  float cosalpha = ux12*ux13 + uy12*uy13;
1581  float sinalpha = uy12*ux13 - ux12*uy13;
1582  // alpha23 + alpha
1583  ux_mid = ux23 * cosalpha - uy23 * sinalpha;
1584  uy_mid = ux23 * sinalpha + uy23 * cosalpha;
1585  // alpha23 - alpha
1586  ux_end = ux23 * cosalpha + uy23 * sinalpha;
1587  uy_end = uy23 * cosalpha - ux23 * sinalpha;
1588 
1589  // dzdl = dz/sqrt(ds^2 + dz^2)
1590  float ds23 = 2.*kappa_inv*atan(sinalpha/(1.+ sqrt(1.-pow(sinalpha,2))));
1591  if (kappa<=0) ds23 = D23;
1592 
1593  float dz23 = z3 - z2;
1594  dzdl_2 = dz23/sqrt(pow(ds23,2) + pow(dz23,2));
1595  ddzdl_2 = (dz2 + dz3)*D23_inv;
1596 
1597  // sin(alpha) = sin(alpha13 -alpha23)
1598  sinalpha = ux13 *uy23 - ux23 * uy13;
1599  float ds12 = 2.*kappa_inv*atan(sinalpha/(1.+sqrt(1.-pow(sinalpha,2))));
1600  if (kappa<=0) ds12 = D12;
1601 
1602  float dz12 = z2 - z1;
1603  dzdl_1 = dz12/sqrt(pow(ds12,2) + pow(dz12,2));
1604  ddzdl_1 = (dz1 + dz2) * D12_inv;
1605 
1606  return 1;
1607 
1608 }
1609 
1610 int CellularAutomaton_v1::calculate_kappa_tangents(
1611  float x1, float y1, float z1, float x2, float y2, float z2,
1612  float x3, float y3, float z3,
1613  float dx1, float dy1, float dz1, float dx2, float dy2, float dz2,
1614  float dx3, float dy3, float dz3,
1615  float& kappa, float& dkappa,
1616  float& ux_mid, float& uy_mid, float& ux_end, float& uy_end,
1617  float& dzdl_1, float& dzdl_2, float& ddzdl_1, float& ddzdl_2,
1618  float ca_sin_ang_cut, float ca_cos_ang_cut_diff_inv,
1619  float cur_kappa, float cur_dkappa, float cur_ux, float cur_uy,
1620  float cur_chi2, float& chi2)
1621 {
1622 
1623 
1624  float D12 = sqrt(pow(x2-x1,2)+pow(y2-y1,2));
1625  float D23 = sqrt(pow(x3-x2,2)+pow(y3-y2,2));
1626  float D13 = sqrt(pow(x3-x1,2)+pow(y3-y1,2));
1627  kappa = 1./(D12*D23*D13);
1628  float num = (D12+D23+D13)*(D23+D13-D12)*(D12+D13-D23)*(D12+D23-D13);
1629  if (num<0) num = 0;
1630  num = sqrt(num);
1631  kappa *=num;
1632 
1633  float kappa_inv = 1/kappa;
1634  float D12_inv = 1./D12;
1635  float D23_inv = 1./D23;
1636  float D13_inv = 1./D13;
1637 
1638  float dr1 = sqrt(pow(dx1,2)+pow(dy1,2));
1639  float dr2 = sqrt(pow(dx2,2)+pow(dy2,2));
1640  float dr3 = sqrt(pow(dx3,2)+pow(dy3,2));
1641 
1642  float dk1 = (dr1+dr2)* D12_inv*D12_inv;
1643  float dk2 = (dr2+dr3)* D23_inv*D23_inv;
1644  dkappa = dk1+dk2;
1645 
1646  float ux12 = (x2-x1)*D12_inv;
1647  float uy12 = (y2-y1)*D12_inv;
1648  float ux23 = (x3-x2)*D23_inv;
1649  float uy23 = (y3-y2)*D23_inv;
1650  float ux13 = (x3-x1)*D13_inv;
1651  float uy13 = (y3-y1)*D13_inv;
1652 
1653  // cos(alpha) = cos(alpha12 - alpha13)
1654  // sin(alpha) = sin(alpha12 - alpha13)
1655  float cosalpha = ux12*ux13 + uy12*uy13;
1656  float sinalpha = uy12*ux13 - ux12*uy13;
1657  // alpha23 + alpha
1658  ux_mid = ux23 * cosalpha - uy23 * sinalpha;
1659  uy_mid = ux23 * sinalpha + uy23 * cosalpha;
1660  // alpha23 - alpha
1661  ux_end = ux23 * cosalpha + uy23 * sinalpha;
1662  uy_end = uy23 * cosalpha - ux23 * sinalpha;
1663 
1664  // dzdl = dz/sqrt(ds^2 + dz^2)
1665  float ds23 = 2.*kappa_inv*atan(sinalpha/(1.+ sqrt(1.-pow(sinalpha,2))));
1666  if (kappa<=0) ds23 = D23;
1667 
1668  float dz23 = z3 - z2;
1669  dzdl_2 = dz23/sqrt(pow(ds23,2) + pow(dz23,2));
1670  ddzdl_2 = (dz2 + dz3)*D23_inv;
1671 
1672  // sin(alpha) = sin(alpha13 -alpha23)
1673  sinalpha = ux13 *uy23 - ux23 * uy13;
1674  float ds12 = 2.*kappa_inv*atan(sinalpha/(1.+sqrt(1.-pow(sinalpha,2))));
1675  if (kappa<=0) ds12 = D12;
1676 
1677  float dz12 = z2 - z1;
1678  dzdl_1 = dz12/sqrt(pow(ds12,2) + pow(dz12,2));
1679  ddzdl_1 = (dz1 + dz2) * D12_inv;
1680 
1681  float kappa_diff = cur_kappa - kappa;
1682  float n_dk = cur_dkappa + dkappa + ca_sin_ang_cut * kappa;
1683  float chi2_kappa = pow(kappa_diff,2)/pow(n_dk,2);
1684 
1685  float cos_scatter = cur_ux * ux_mid + cur_uy * uy_mid;
1686  float chi2_ang = pow((1-cos_scatter)*ca_cos_ang_cut_diff_inv,2);
1687 
1688  float sin_scatter = dzdl_1 * ca_sin_ang_cut;
1689  float chi2_dzdl = 0.5*pow((dzdl_1-dzdl_2)/(ddzdl_1+ddzdl_2+fabs(sin_scatter)),2);
1690 
1691  chi2 = cur_chi2 + chi2_ang + chi2_kappa + chi2_dzdl;
1692  return 1;
1693 }
1694 
1695 
1696 float CellularAutomaton_v1::kappa_to_pt(float kappa) {
1697  return _pt_rescale * _mag_field / 333.6 / kappa;
1698 }
1699 
1700 float CellularAutomaton_v1::shift_phi_range(float _phi){
1701 
1702  if (_phi < 0.) _phi += 2.*M_PI;
1703  return _phi;
1704 }
1705 
1706