ECCE @ EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
kdfinder.hpp
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file kdfinder.hpp
1 /***********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright 2019-2020 Dmitry Arkhipkim (arkhipkin@bnl.gov). All rights reserved.
5  * All rights reserved.
6  *
7  * THE BSD LICENSE
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * 1. Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * 2. Redistributions in binary form must reproduce the above copyright
16  * notice, this list of conditions and the following disclaimer in the
17  * documentation and/or other materials provided with the distribution.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
20  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
21  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
23  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
24  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
28  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29  *************************************************************************/
30 
44 #ifndef KDFINDER_HPP_
45 #define KDFINDER_HPP_
46 
47 #include "nanoflann.hpp"
48 
49 #include <algorithm>
50 #include <chrono>
51 #include <cmath>
52 #include <fstream>
53 #include <iostream>
54 #include <limits>
55 #include <sstream>
56 #include <thread>
57 #include <tuple>
58 #include <vector>
59 
60 namespace kdfinder
61 {
62  template <typename T>
63  T rnd_gauss(T mean, T stddev)
64  {
65  T u, v, r, gauss;
66  do
67  {
68  u = 2 * ((double) rand() / (RAND_MAX)) - 1;
69  v = 2 * ((double) rand() / (RAND_MAX)) - 1;
70  r = u * u + v * v;
71  } while (r > 1 || r == 0);
72  gauss = u * std::sqrt(-2 * std::log(r) / r);
73  return (mean + gauss * stddev);
74  }
75 
76  template <class T>
77  class TVector
78  {
79  public:
81  : mX1(0)
82  , mX2(0)
83  , mX3(0)
84  {
85  }
86  TVector(T x, T y, T z)
87  : mX1(x)
88  , mX2(y)
89  , mX3(z)
90  {
91  }
93  : mX1(v.x())
94  , mX2(v.y())
95  , mX3(v.z())
96  {
97  }
98  TVector(const T* a)
99  : mX1(a[0])
100  , mX2(a[1])
101  , mX3(a[2])
102  {
103  }
104  ~TVector() {}
105 
106  void setX(T x) { mX1 = x; }
107  void setY(T y) { mX2 = y; }
108  void setZ(T z) { mX3 = z; }
109 
110  void set(T X, T Y, T Z)
111  {
112  mX1 = X;
113  mX2 = Y;
114  mX3 = Z;
115  }
116 
117  void setPhi(T Angle)
118  {
119  double r = magnitude();
120  double th = theta();
121  mX1 = r * std::sin(th) * std::cos(Angle);
122  mX2 = r * std::sin(th) * std::sin(Angle);
123  }
124 
125  void setTheta(T Angle)
126  {
127  double r = magnitude();
128  double ph = phi();
129  mX1 = r * sin(Angle) * cos(ph);
130  mX2 = r * sin(Angle) * sin(ph);
131  mX3 = r * cos(Angle);
132  }
133 
134  void setMag(T Mag)
135  {
136  setMagnitude(Mag);
137  }
138 
140  {
141  double th = theta();
142  double ph = phi();
143  mX1 = r * sin(th) * cos(ph);
144  mX2 = r * sin(th) * sin(ph);
145  mX3 = r * cos(th);
146  }
147 
148  const T& x() const { return mX1; }
149  const T& y() const { return mX2; }
150  const T& z() const { return mX3; }
151 
152  const T* xyz() const { return &mX1; }
153  T* xyz() { return &mX1; }
154 
155  T abs(const TVector<T>& v) { return v.mag(); }
156 
157  T theta() const
158  {
159  return acos(cosTheta());
160  }
161 
162  T cosTheta() const
163  {
164  return mX3 / (mag() + 1e-20);
165  }
166 
167  T phi() const
168  {
169  return std::atan2(mX2, mX1);
170  }
171 
172  T perp() const
173  {
174  return std::sqrt(mX1 * mX1 + mX2 * mX2);
175  }
176 
177  T perp2() const
178  {
179  return mX1 * mX1 + mX2 * mX2;
180  }
181 
182  T magnitude() const
183  {
184  return mag();
185  }
186 
187  T mag() const
188  {
189  return std::sqrt(mX1 * mX1 + mX2 * mX2 + mX3 * mX3);
190  }
191 
192  T mag2() const
193  {
194  return (mX1 * mX1 + mX2 * mX2 + mX3 * mX3);
195  }
196 
198  {
199  // change code to more optimal:
200  // double m = mag();
201  // return 0.5*::log( ( m + z() ) / ( m - z() ) );
202  double tmp = std::tan(theta() / 2.);
203  if (tmp <= 0.)
204  {
205  return 1e20;
206  }
207  return -std::log(tmp);
208  }
209 
211  {
212  mX1 = v.x();
213  mX2 = v.y();
214  mX3 = v.z();
215  return *this;
216  }
217 
218  T operator()(size_t i) const
219  {
220  return (&mX1)[i];
221  }
222 
223  T operator[](size_t i) const
224  {
225  return (&mX1)[i];
226  }
227 
228  T& operator()(size_t i)
229  {
230  return (&mX1)[i];
231  }
232 
233  T& operator[](size_t i)
234  {
235  return (&mX1)[i];
236  }
237 
239  {
240  return std::sqrt((*this) * (*this) + mass * mass);
241  }
242 
243  TVector<T> unit() const
244  {
245  T tmp = mag();
246  if (tmp <= 0.) tmp = 1e-20;
247  return *this / tmp;
248  }
249 
251  {
252  T X = (mX1 < 0.0) ? -mX1 : mX1;
253  T Y = (mX2 < 0.0) ? -mX2 : mX2;
254  T Z = (mX3 < 0.0) ? -mX3 : mX3;
255  if (X < Y)
256  {
257  return X < Z ? TVector<T>(0, mX3, -mX2) : TVector<T>(mX2, -mX1, 0);
258  }
259  return mX2 < mX3 ? TVector<T>(-mX3, 0, mX1) : TVector<T>(mX2, -mX1, 0);
260  }
261 
262  void rotateX(T Angle)
263  {
264  T yPrime = cos(Angle) * mX2 - sin(Angle) * mX3;
265  T zPrime = sin(Angle) * mX2 + cos(Angle) * mX3;
266  mX2 = yPrime;
267  mX3 = zPrime;
268  }
269 
270  void rotateY(T Angle)
271  {
272  T zPrime = cos(Angle) * mX3 - sin(Angle) * mX1;
273  T xPrime = sin(Angle) * mX3 + cos(Angle) * mX1;
274  mX1 = xPrime;
275  mX3 = zPrime;
276  }
277 
278  void rotateZ(T Angle)
279  {
280  T xPrime = cos(Angle) * mX1 - sin(Angle) * mX2;
281  T yPrime = sin(Angle) * mX1 + cos(Angle) * mX2;
282  mX1 = xPrime;
283  mX2 = yPrime;
284  }
285 
287  {
288  return TVector<T>(-mX1, -mX2, -mX3);
289  }
290 
292  {
293  return *this;
294  }
295 
297  {
298  mX1 *= c;
299  mX2 *= c;
300  mX3 *= c;
301  return *this;
302  }
303 
305  {
306  mX1 /= c;
307  mX2 /= c;
308  mX3 /= c;
309  return *this;
310  }
311 
313  {
314  return TVector<T>(mX1 * X, mX2 * Y, mX3 * Z);
315  }
316 
317  T angle(const TVector<T>& vec) const
318  {
319  T norm = this->mag2() * vec.mag2();
320  return norm > 0 ? std::acos(this->dot(vec) / (std::sqrt(norm))) : 0;
321  }
322 
323  TVector<T> cross(const TVector<T>& v) const
324  {
325  return TVector<T>(
326  mX2 * v.z() - mX3 * v.y(),
327  mX3 * v.x() - mX1 * v.z(),
328  mX1 * v.y() - mX2 * v.x());
329  }
330 
331  T dot(const TVector<T>& v) const
332  {
333  return mX1 * v.x() + mX2 * v.y() + mX3 * v.z();
334  }
335 
337  {
338  return this->pseudoProduct(v.x(), v.y(), v.z());
339  }
340 
341  bool operator==(const TVector<T>& v) const
342  {
343  return mX1 == v.x() && mX2 == v.y() && mX3 == v.z();
344  }
345 
346  bool operator!=(const TVector<T>& v) const
347  {
348  return !(*this == v);
349  }
350 
352  {
353  mX1 += v.x();
354  mX2 += v.y();
355  mX3 += v.z();
356  return *this;
357  }
358 
360  {
361  mX1 -= v.x();
362  mX2 -= v.y();
363  mX3 -= v.z();
364  return *this;
365  }
366 
367  int valid(T world = 1.e+5) const
368  {
369  return !bad(world);
370  }
371 
372  int bad(T world = 1.e+5) const
373  {
374  for (size_t i = 0; i < 3; i++)
375  {
376  if (!std::isfinite((&mX1)[i]))
377  {
378  return 10 + i;
379  }
380  if (std::fabs((&mX1)[i]) > world)
381  {
382  return 20 + i;
383  }
384  }
385  return 0;
386  }
387 
388  protected:
390  };
391 
392  template <class T>
393  inline T abs(const TVector<T>& v)
394  {
395  return v.mag();
396  }
397 
398  template <class T>
400  {
401  return v1.cross(v2);
402  }
403 
404  template <class T>
405  inline TVector<T>
407  {
408  return TVector<T>(v1) += v2;
409  }
410 
411  template <class T>
412  inline TVector<T>
414  {
415  return TVector<T>(v1) -= v2;
416  }
417 
418  template <class T>
419  inline T operator*(const TVector<T>& v1, const TVector<T>& v2)
420  {
421  return TVector<T>(v1).dot(v2);
422  }
423 
424  template <class T>
425  inline TVector<T> operator*(const TVector<T>& v, double c)
426  {
427  return TVector<T>(v) *= c;
428  }
429 
430  template <class T>
431  inline TVector<T> operator*(T c, const TVector<T>& v)
432  {
433  return TVector<T>(v) *= c;
434  }
435 
436  template <class T>
437  inline TVector<T> operator/(const TVector<T>& v, double c)
438  {
439  return TVector<T>(v) /= c;
440  }
441 
442  template <class T>
443  class Helix
444  {
445  public:
447  Helix(T c, T dip, T phase, const TVector<T>& o, int h = -1) { setParameters(c, dip, phase, o, h); }
448 
449  // momentum, origin, magnetic field, charge
450  Helix(const TVector<T>& p, const TVector<T>& o, T B, T q)
451  {
452  mH = (q * B <= 0) ? 1 : -1;
453  if (p.y() == 0 && p.x() == 0)
454  {
455  setPhase((M_PI / 4) * (1 - 2. * mH));
456  }
457  else
458  {
459  setPhase(std::atan2(p.y(), p.x()) - mH * M_PI / 2);
460  setDipAngle(std::atan2(p.z(), p.perp()));
461  mOrigin = o;
462  setCurvature(std::fabs((c_light * nanosecond / meter * q * B / tesla) / (abs(p) / GeV * mCosDipAngle) / meter));
463  }
464  }
465 
466  virtual ~Helix() {}
467 
468  T dipAngle() const { return mDipAngle; }
469  T curvature() const { return mCurvature; }
470  T phase() const { return mPhase; }
471  T xcenter() const
472  {
473  if (mSingularity)
474  {
475  return 0;
476  }
477  else
478  {
479  return mOrigin.x() - mCosPhase / mCurvature;
480  }
481  }
482  T ycenter() const
483  {
484  if (mSingularity)
485  {
486  return 0;
487  }
488  else
489  {
490  return mOrigin.y() - mSinPhase / mCurvature;
491  }
492  }
493  T h() const { return mH; }
494 
495  const TVector<T>& origin() const { return mOrigin; }
496 
498  {
499  if (mSingularity)
500  {
501  return (TVector<T>(0, 0, 0));
502  }
503  T pt = GeV * std::fabs(c_light * nanosecond / meter * B / tesla) / (std::fabs(mCurvature) * meter);
504  return (TVector<T>(pt * std::cos(mPhase + mH * M_PI / 2),
505  pt * sin(mPhase + mH * M_PI / 2),
506  pt * tan(mDipAngle)));
507  }
508 
510  {
511  Helix tmp(*this);
512  tmp.moveOrigin(s);
513  return tmp.momentum(B);
514  }
515 
516  int charge(T B) const
517  {
518  return (B > 0 ? -mH : mH);
519  }
520 
522  {
523  // Geometric signed distance
524  T thePath = this->pathLength(x, y);
525  TVector<T> DCA2dPosition = this->at(thePath);
526  DCA2dPosition.setZ(0);
527  TVector<T> position(x, y, 0);
528  TVector<T> DCAVec = (DCA2dPosition - position);
529  TVector<T> momVec;
530  // Deal with straight tracks
531  if (this->mSingularity)
532  {
533  momVec = this->at(1) - this->at(0);
534  momVec.setZ(0);
535  }
536  else
537  {
538  momVec = this->momentumAt(thePath, 1. / tesla);
539  momVec.setZ(0);
540  }
541 
542  T cross = DCAVec.x() * momVec.y() - DCAVec.y() * momVec.x();
543  T theSign = (cross >= 0) ? 1. : -1.;
544  return theSign * DCAVec.perp();
545  }
546 
548  {
549  if (this->mSingularity || std::abs(this->mH) <= 0)
550  {
551  return (this->geometricSignedDistance(x, y));
552  }
553  return (this->geometricSignedDistance(x, y)) / (this->mH);
554  }
555 
557  {
558  T sdca2d = this->geometricSignedDistance(pos.x(), pos.y());
559  T theSign = (sdca2d >= 0) ? 1. : -1.;
560  return (this->distance(pos)) * theSign;
561  }
562 
564  {
565  T sdca2d = this->curvatureSignedDistance(pos.x(), pos.y());
566  T theSign = (sdca2d >= 0) ? 1. : -1.;
567  return (this->distance(pos)) * theSign;
568  }
569 
570  void setParameters(T c, T dip, T phase, const TVector<T>& o, int h)
571  {
572  mH = (h >= 0) ? 1 : -1;
573  mOrigin = o;
574  setDipAngle(dip);
575  setPhase(phase);
576  setCurvature(c);
577  if (mSingularity && mH == -1)
578  {
579  mH = +1;
580  setPhase(mPhase - M_PI);
581  }
582  }
583 
585  T x(T s) const
586  {
587  if (mSingularity)
588  {
589  return mOrigin.x() - s * mCosDipAngle * mSinPhase;
590  }
591  return mOrigin.x() + (std::cos(mPhase + s * mH * mCurvature * mCosDipAngle) - mCosPhase) / mCurvature;
592  }
593 
594  T y(T s) const
595  {
596  if (mSingularity)
597  {
598  return mOrigin.y() + s * mCosDipAngle * mCosPhase;
599  }
600  return mOrigin.y() + (std::sin(mPhase + s * mH * mCurvature * mCosDipAngle) - mSinPhase) / mCurvature;
601  }
602 
603  T z(T s) const
604  {
605  return mOrigin.z() + s * mSinDipAngle;
606  }
607 
608  TVector<T> at(T s) const
609  {
610  return TVector<T>(x(s), y(s), z(s));
611  }
612 
614  T cx(T s = 0) const
615  {
616  if (mSingularity)
617  {
618  return -mCosDipAngle * mSinPhase;
619  }
620  return -std::sin(mPhase + s * mH * mCurvature * mCosDipAngle) * mH * mCosDipAngle;
621  }
622 
623  T cy(T s = 0) const
624  {
625  if (mSingularity)
626  {
627  return mCosDipAngle * mCosPhase;
628  }
629  return std::cos(mPhase + s * mH * mCurvature * mCosDipAngle) * mH * mCosDipAngle;
630  }
631 
632  T cz(T s = 0) const
633  {
634  return mSinDipAngle;
635  }
636 
637  TVector<T> cat(T s) const
638  {
639  return TVector<T>(cx(s), cy(s), cz(s));
640  }
641 
643  T period() const
644  {
645  if (mSingularity)
646  {
648  }
649  return std::fabs(2 * M_PI / (mH * mCurvature * mCosDipAngle));
650  }
651 
653  std::pair<T, T> pathLength(T r) const
654  {
655  std::pair<T, T> value;
656  std::pair<T, T> VALUE(999999999., 999999999.);
657 
658  if (mSingularity)
659  {
660  std::cerr << " singularity!" << std::endl;
661  T t1 = mCosDipAngle * (mOrigin.x() * mSinPhase - mOrigin.y() * mCosPhase);
662  T t12 = mOrigin.y() * mOrigin.y();
663  T t13 = mCosPhase * mCosPhase;
664  T t15 = r * r;
665  T t16 = mOrigin.x() * mOrigin.x();
666  T t20 = -mCosDipAngle * mCosDipAngle * (2.0 * mOrigin.x() * mSinPhase * mOrigin.y() * mCosPhase + t12 - t12 * t13 - t15 + t13 * t16);
667  if (t20 < 0.)
668  {
669  return VALUE;
670  }
671  t20 = std::sqrt(t20);
672  value.first = (t1 - t20) / (mCosDipAngle * mCosDipAngle);
673  value.second = (t1 + t20) / (mCosDipAngle * mCosDipAngle);
674  }
675  else
676  {
677  //std::cout << " no singularity!" << std::endl;
678  T t1 = mOrigin.y() * mCurvature;
679  T t2 = mSinPhase;
680  T t3 = mCurvature * mCurvature;
681  T t4 = mOrigin.y() * t2;
682  T t5 = mCosPhase;
683  T t6 = mOrigin.x() * t5;
684  T t8 = mOrigin.x() * mOrigin.x();
685  T t11 = mOrigin.y() * mOrigin.y();
686  T t14 = r * r;
687  T t15 = t14 * mCurvature;
688  T t17 = t8 * t8;
689  T t19 = t11 * t11;
690  T t21 = t11 * t3;
691  T t23 = t5 * t5;
692  T t32 = t14 * t14;
693  T t35 = t14 * t3;
694  T t38 = 8.0 * t4 * t6 - 4.0 * t1 * t2 * t8 - 4.0 * t11 * mCurvature * t6 +
695  4.0 * t15 * t6 + t17 * t3 + t19 * t3 + 2.0 * t21 * t8 + 4.0 * t8 * t23 -
696  4.0 * t8 * mOrigin.x() * mCurvature * t5 - 4.0 * t11 * t23 -
697  4.0 * t11 * mOrigin.y() * mCurvature * t2 + 4.0 * t11 - 4.0 * t14 +
698  t32 * t3 + 4.0 * t15 * t4 - 2.0 * t35 * t11 - 2.0 * t35 * t8;
699  T t40 = (-t3 * t38);
700  if (t40 < 0.)
701  {
702  //std::cerr << "t40 < 0." << std::endl;
703  return VALUE;
704  }
705  t40 = std::sqrt(t40);
706 
707  T t43 = mOrigin.x() * mCurvature;
708  T t45 = 2.0 * t5 - t35 + t21 + 2.0 - 2.0 * t1 * t2 - 2.0 * t43 - 2.0 * t43 * t5 + t8 * t3;
709  T t46 = mH * mCosDipAngle * mCurvature;
710 
711  value.first = (-mPhase + 2.0 * std::atan((-2.0 * t1 + 2.0 * t2 + t40) / t45)) / t46;
712  value.second = -(mPhase + 2.0 * std::atan((2.0 * t1 - 2.0 * t2 + t40) / t45)) / t46;
713 
714  T p = period();
715  if (!std::isnan(value.first))
716  {
717  if (std::fabs(value.first - p) < std::fabs(value.first))
718  {
719  value.first = value.first - p;
720  }
721  else if (std::fabs(value.first + p) < std::fabs(value.first))
722  {
723  value.first = value.first + p;
724  }
725  }
726  else
727  {
728  std::cerr << "value.first = isnan" << std::endl;
729  }
730 
731  if (!std::isnan(value.second))
732  {
733  if (std::fabs(value.second - p) < std::fabs(value.second))
734  {
735  value.second = value.second - p;
736  }
737  else if (fabs(value.second + p) < fabs(value.second))
738  value.second = value.second + p;
739  }
740  else
741  {
742  std::cerr << "value.second = isnan" << std::endl;
743  }
744  }
745 
746  if (value.first > value.second)
747  {
748  std::swap(value.first, value.second);
749  }
750 
751  return (value);
752  }
753 
755  std::pair<T, T> pathLength(T r, T x, T y)
756  {
757  //std::cout << "--- pathlength at r, x, y --- " << std::endl;
758  T x0 = mOrigin.x();
759  T y0 = mOrigin.y();
760  mOrigin.setX(x0 - x);
761  mOrigin.setY(y0 - y);
762  std::pair<T, T> result = this->pathLength(r);
763  mOrigin.setX(x0);
764  mOrigin.setY(y0);
765  return result;
766  }
767 
769  T pathLength(const TVector<T>& p, bool scanPeriods = true) const
770  {
771  T s;
772  T dx = p.x() - mOrigin.x();
773  T dy = p.y() - mOrigin.y();
774  T dz = p.z() - mOrigin.z();
775  if (mSingularity)
776  {
777  s = mCosDipAngle * (mCosPhase * dy - mSinPhase * dx) + mSinDipAngle * dz;
778  }
779  else
780  {
781  const T MaxPrecisionNeeded = 0.0001;
782  const int MaxIterations = 100;
783 
785  T t41 = mSinDipAngle * mSinDipAngle;
786  T t6, t7, t11, t12, t19;
787 
788  s = fudgePathLength(p);
789 
790  if (scanPeriods)
791  {
792  T ds = period();
793  int j, jmin = 0;
794  T d, dmin = abs(at(s) - p);
795  for (j = 1; j < MaxIterations; j++)
796  {
797  if ((d = abs(at(s + j * ds) - p)) < dmin)
798  {
799  dmin = d;
800  jmin = j;
801  }
802  else
803  {
804  break;
805  }
806  }
807  for (j = -1; - j < MaxIterations; j--)
808  {
809  if ((d = abs(at(s + j * ds) - p)) < dmin)
810  {
811  dmin = d;
812  jmin = j;
813  }
814  else
815  {
816  break;
817  }
818  }
819  if (jmin)
820  {
821  s += jmin * ds;
822  }
823  }
824 
825  T sOld = s;
826  for (int i = 0; i < MaxIterations; i++)
827  {
828  t6 = mPhase + s * mH * mCurvature * mCosDipAngle;
829  t7 = std::cos(t6);
830  t11 = dx - (1 / mCurvature) * (t7 - mCosPhase);
831  t12 = std::sin(t6);
832  t19 = dy - (1 / mCurvature) * (t12 - mSinPhase);
833  s -= (t11 * t12 * mH * mCosDipAngle - t19 * t7 * mH * mCosDipAngle -
834  (dz - s * mSinDipAngle) * mSinDipAngle) /
835  (t12 * t12 * mCosDipAngle * mCosDipAngle + t11 * t7 * t34 +
836  t7 * t7 * mCosDipAngle * mCosDipAngle + t19 * t12 * t34 + t41);
837  if (std::fabs(sOld - s) < MaxPrecisionNeeded)
838  {
839  break;
840  }
841  sOld = s;
842  }
843  }
844  return s;
845  }
846 
848  T pathLength(const TVector<T>& r, const TVector<T>& n) const
849  {
850  T s;
851 
852  if (mSingularity)
853  {
854  T t = n.z() * mSinDipAngle +
855  n.y() * mCosDipAngle * mCosPhase -
856  n.x() * mCosDipAngle * mSinPhase;
857  if (t == 0)
858  {
859  s = NoSolution;
860  }
861  else
862  {
863  s = ((r - mOrigin) * n) / t;
864  }
865  }
866  else
867  {
868  const T MaxPrecisionNeeded = 0.0001;
869  const int MaxIterations = 20;
870 
871  T A = mCurvature * ((mOrigin - r) * n) - n.x() * mCosPhase - n.y() * mSinPhase;
872  T t = mH * mCurvature * mCosDipAngle;
873  T u = n.z() * mCurvature * mSinDipAngle;
874 
875  T a, f, fp;
876  T sOld = s = 0;
877  T shiftOld = 0;
878  T shift;
879  const T angMax = 0.21;
880  T deltas = std::fabs(angMax / (mCurvature * mCosDipAngle));
881 
882  int i;
883  for (i = 0; i < MaxIterations; i++)
884  {
885  a = t * s + mPhase;
886  T sina = std::sin(a);
887  T cosa = std::cos(a);
888  f = A + n.x() * cosa + n.y() * sina + u * s;
889  fp = -n.x() * sina * t + n.y() * cosa * t + u;
890  if (std::fabs(fp) * deltas <= std::fabs(f))
891  { //too big step
892  int sgn = 1;
893  if (fp < 0.)
894  {
895  sgn = -sgn;
896  }
897  if (f < 0.)
898  {
899  sgn = -sgn;
900  }
901  shift = sgn * deltas;
902  if (shift < 0)
903  {
904  shift *= 0.9;
905  } // don't get stuck shifting +/-deltas
906  }
907  else
908  {
909  shift = f / fp;
910  }
911  s -= shift;
912  shiftOld = shift;
913  if (std::fabs(sOld - s) < MaxPrecisionNeeded)
914  {
915  break;
916  }
917  sOld = s;
918  }
919  if (i == MaxIterations)
920  {
921  return NoSolution;
922  }
923  }
924  return s;
925  }
926 
928  T pathLength(T x, T y) const
929  {
930  return fudgePathLength(TVector<T>(x, y, 0));
931  }
932 
934  std::pair<T, T> pathLengths(const Helix&, T minStepSize = 10 * 0.0001, T minRange = 10) const
935  {
936  if (mSingularity != h().mSingularity)
937  {
938  return std::pair<T, T>(NoSolution, NoSolution);
939  }
940 
941  T s1, s2;
942 
943  if (mSingularity)
944  {
945  TVector<T> dv = h().mOrigin - mOrigin;
947  TVector<T> b(-h().mCosDipAngle * h().mSinPhase, h().mCosDipAngle * h().mCosPhase, h().mSinDipAngle);
948  T ab = a * b;
949  T g = dv * a;
950  T k = dv * b;
951  s2 = (k - ab * g) / (ab * ab - 1.);
952  s1 = g + s2 * ab;
953  return std::pair<T, T>(s1, s2);
954  }
955  else
956  {
957  T dx = h().xcenter() - xcenter();
958  T dy = h().ycenter() - ycenter();
959  T dd = std::sqrt(dx * dx + dy * dy);
960  T r1 = 1 / curvature();
961  T r2 = 1 / h().curvature();
962  T cosAlpha = (r1 * r1 + dd * dd - r2 * r2) / (2 * r1 * dd);
963 
964  T s;
965  T x, y;
966  if (std::fabs(cosAlpha) < 1)
967  {
968  T sinAlpha = std::sin(std::acos(cosAlpha));
969  x = xcenter() + r1 * (cosAlpha * dx - sinAlpha * dy) / dd;
970  y = ycenter() + r1 * (sinAlpha * dx + cosAlpha * dy) / dd;
971  s = pathLength(x, y);
972  x = xcenter() + r1 * (cosAlpha * dx + sinAlpha * dy) / dd;
973  y = ycenter() + r1 * (cosAlpha * dy - sinAlpha * dx) / dd;
974  T a = pathLength(x, y);
975  if (h().distance(at(a)) < h().distance(at(s)))
976  {
977  s = a;
978  }
979  }
980  else
981  {
982  int rsign = ((r2 - r1) > dd ? -1 : 1);
983  x = xcenter() + rsign * r1 * dx / dd;
984  y = ycenter() + rsign * r1 * dy / dd;
985  s = pathLength(x, y);
986  }
987 
988  T dmin = h().distance(at(s));
989  T range = std::max(2 * dmin, minRange);
990  T ds = range / 10;
991  T slast = -999999, ss, d;
992  s1 = s - range / 2.;
993  s2 = s + range / 2.;
994 
995  while (ds > minStepSize)
996  {
997  for (ss = s1; ss < s2 + ds; ss += ds)
998  {
999  d = h().distance(at(ss));
1000  if (d < dmin)
1001  {
1002  dmin = d;
1003  s = ss;
1004  }
1005  slast = ss;
1006  }
1007  if (s == s1)
1008  {
1009  d = 0.8 * (s2 - s1);
1010  s1 -= d;
1011  s2 -= d;
1012  }
1013  else if (s == slast)
1014  {
1015  d = 0.8 * (s2 - s1);
1016  s1 += d;
1017  s2 += d;
1018  }
1019  else
1020  {
1021  s1 = s - ds;
1022  s2 = s + ds;
1023  ds /= 10;
1024  }
1025  }
1026  return std::pair<T, T>(s, h().pathLength(at(s)));
1027  }
1028  }
1029 
1031  T distance(const TVector<T>& p, bool scanPeriods = true) const
1032  {
1033  return std::abs(this->at(pathLength(p, scanPeriods)) - p);
1034  }
1035 
1037  bool valid(T world = 1.e+5) const
1038  {
1039  return !bad(world);
1040  }
1041 
1042  int bad(T WorldSize = 1.e+5) const
1043  {
1044  int ierr;
1045  if (!std::isfinite(mDipAngle))
1046  {
1047  return 11;
1048  }
1049  if (!std::isfinite(mCurvature))
1050  {
1051  return 12;
1052  }
1053  ierr = mOrigin.bad(WorldSize);
1054  if (ierr)
1055  {
1056  return 3 + ierr * 100;
1057  }
1058  if (std::fabs(mDipAngle) > 1.58)
1059  {
1060  return 21;
1061  }
1062  T qwe = std::fabs(std::fabs(mDipAngle) - M_PI / 2);
1063  if (qwe < 1. / WorldSize)
1064  {
1065  return 31;
1066  }
1067  if (std::fabs(mCurvature) > WorldSize)
1068  {
1069  return 22;
1070  }
1071  if (mCurvature < 0)
1072  {
1073  return 32;
1074  }
1075  if (std::abs(mH) != 1)
1076  {
1077  return 24;
1078  }
1079  return 0;
1080  }
1081 
1083  virtual void moveOrigin(T s)
1084  {
1085  if (mSingularity)
1086  {
1087  mOrigin = at(s);
1088  }
1089  else
1090  {
1091  TVector<T> newOrigin = at(s);
1092  T newPhase = std::atan2(newOrigin.y() - ycenter(), newOrigin.x() - xcenter());
1093  mOrigin = newOrigin;
1094  setPhase(newPhase);
1095  }
1096  }
1097 
1098  static constexpr T NoSolution = 3.e+33;
1099 
1100  protected:
1101  Helix() {}
1102 
1103  void setCurvature(T val)
1104  {
1105  if (val < 0)
1106  {
1107  mCurvature = -val;
1108  mH = -mH;
1109  setPhase(mPhase + M_PI);
1110  }
1111  else
1112  {
1113  mCurvature = val;
1114  }
1115  if (std::fabs(mCurvature) <= std::numeric_limits<T>::epsilon())
1116  {
1117  mSingularity = true; // straight line
1118  }
1119  else
1120  {
1121  mSingularity = false; // curved
1122  }
1123  }
1124 
1125  void setPhase(T val)
1126  {
1127  mPhase = val;
1128  mCosPhase = std::cos(mPhase);
1129  mSinPhase = std::sin(mPhase);
1130  if (std::fabs(mPhase) > M_PI)
1131  {
1132  mPhase = atan2(mSinPhase, mCosPhase); // force range [-pi,pi]
1133  }
1134  }
1135 
1136  void setDipAngle(T val)
1137  {
1138  mDipAngle = val;
1139  mCosDipAngle = std::cos(mDipAngle);
1140  mSinDipAngle = std::sin(mDipAngle);
1141  }
1142 
1145  {
1146  T s;
1147  T dx = p.x() - mOrigin.x();
1148  T dy = p.y() - mOrigin.y();
1149  if (mSingularity)
1150  {
1151  s = (dy * mCosPhase - dx * mSinPhase) / mCosDipAngle;
1152  }
1153  else
1154  {
1155  s = std::atan2(dy * mCosPhase - dx * mSinPhase, 1 / mCurvature + dx * mCosPhase + dy * mSinPhase) / (mH * mCurvature * mCosDipAngle);
1156  }
1157  return s;
1158  }
1159 
1160  protected:
1161  bool mSingularity = false; // true for straight line case (B=0)
1165  T mPhase = 0;
1166  int mH = 0; // -sign(q*B);
1167 
1172 
1173  static constexpr T meter = 100.0;
1174  static constexpr T meter2 = meter * meter;
1175  static constexpr T second = 1.0;
1176  static constexpr T nanosecond = 1.e-9;
1177  static constexpr T GeV = 1.0;
1178  static constexpr T MeV = 1.e-3;
1179  static constexpr T volt = 1.e-6 * MeV;
1180  static constexpr T c_light = 2.99792458e+8 * meter / second; // c_light * meter / sec
1181  static constexpr T tesla = 1.0;
1182  };
1183 
1184  template <class T>
1185  class Data
1186  {
1187  public:
1188  Data(const std::vector<std::vector<T>>& hits_)
1189  : meanX(0)
1190  , meanY(0)
1191  {
1192  std::copy(hits_.begin(), hits_.end(), back_inserter(hits));
1193  hits_size = hits.size();
1194  }
1195 
1196  void means()
1197  {
1198  meanX = 0.;
1199  meanY = 0.;
1200  for (size_t i = 0; i < hits_size; i++)
1201  {
1202  meanX += hits[i][0];
1203  meanY += hits[i][1];
1204  }
1205  meanX /= hits_size;
1206  meanY /= hits_size;
1207  }
1208 
1209  void center(void)
1210  {
1211  T sX = 0., sY = 0.;
1212  for (size_t i = 0; i < hits_size; i++)
1213  {
1214  sX += hits[i][0];
1215  sY += hits[i][1];
1216  }
1217  sX /= hits_size;
1218  sY /= hits_size;
1219  for (size_t i = 0; i < hits_size; i++)
1220  {
1221  hits[i][0] -= sX;
1222  hits[i][1] -= sY;
1223  }
1224  meanX = 0.;
1225  meanY = 0.;
1226  }
1227 
1228  void scale(void)
1229  {
1230  T sXX = 0., sYY = 0., scaling;
1231  for (size_t i = 0; i < hits_size; i++)
1232  {
1233  sXX += hits[i][0] * hits[i][0];
1234  sYY += hits[i][1] * hits[i][1];
1235  }
1236  scaling = std::sqrt((sXX + sYY) / hits_size / 2.0);
1237  for (size_t i = 0; i < hits_size; i++)
1238  {
1239  hits[i][0] /= scaling;
1240  hits[i][1] /= scaling;
1241  }
1242  }
1243 
1244  std::vector<std::vector<T>> hits;
1245  size_t hits_size;
1248  };
1249 
1250  template <typename T>
1251  struct Circle
1252  {
1254  : a(0)
1255  , b(0)
1256  , r(0)
1257  , s(0)
1258  , g(0)
1259  , Gx(0)
1260  , Gy(0)
1261  , i(0)
1262  , j(0)
1263  , chi2(99999.0)
1264  {
1265  }
1266 
1268  {
1269  a = c->a;
1270  b = c->b;
1271  r = c->r;
1272  s = c->s;
1273  g = c->g;
1274  Gx = c->Gx;
1275  Gy = c->Gy;
1276  i = c->i;
1277  j = c->j;
1278  chi2 = c->chi2;
1279  }
1280 
1281  bool is_good()
1282  {
1283  return !std::isnan(a) && !std::isnan(b) && !std::isnan(r) &&
1285  std::isfinite(j) && j > 0;
1286  }
1287 
1288  T a; // x-center
1289  T b; // y-center
1290  T r; // radius
1291  T s; // sigma
1292  T g;
1295  size_t i; // inner loop iterations
1296  size_t j; // outer loop iterations
1298  };
1299 
1300  template <typename T>
1301  struct Line
1302  {
1304  : a(std::numeric_limits<T>::infinity())
1305  , b(std::numeric_limits<T>::infinity())
1306  , siga(0)
1307  , sigb(0)
1308  , chi2(std::numeric_limits<T>::infinity())
1309  {
1310  }
1311 
1312  Line(T a_, T b_, T siga_, T sigb_, T chi2_)
1313  : a(a_)
1314  , b(b_)
1315  , siga(siga_)
1316  , sigb(sigb_)
1317  , chi2(chi2_)
1318  {
1319  }
1320 
1321  bool is_good()
1322  {
1323  return !std::isnan(a) && !std::isnan(b) && !std::isnan(chi2) &&
1325  }
1326  T a; // a + b * x
1327  T b; //
1328  T siga; // sigma a
1329  T sigb; // sigma b
1331  };
1332 
1333  template <typename T>
1335  {
1336  return std::fabs(std::sqrt(std::pow(x - circle->a, 2) + std::pow(y - circle->b, 2)) - circle->r);
1337  }
1338 
1339  template <typename T>
1341  {
1342  return std::fabs(std::sqrt(std::pow(x - a, 2) + std::pow(y - b, 2)) - r);
1343  }
1344 
1345  template <typename T>
1347  {
1348  // point-slope line : 1 * y = a + b * x
1349  // standard line : b * x - 1 * y + a = 0
1350  return std::fabs((b * x) + (-1 * y) + a) / std::sqrt(std::pow(b, 2) + std::pow(-1, 2));
1351  }
1352 
1353  template <class T>
1355  {
1356  public:
1358 
1359  static Circle<T>* RegularFit(const std::vector<std::vector<T>>& hits, size_t fit = 0)
1360  {
1361  switch (fit)
1362  {
1363  case 2:
1364  return CircleFitByTaubin(hits);
1365  break;
1366  case 3:
1367  return CircleFitByPratt(hits);
1368  break;
1369  case 1:
1370  default:
1371  return CircleFitByHyper(hits);
1372  break;
1373  }
1374  }
1375 
1376  static Circle<T>* RobustFit(const std::vector<std::vector<T>>& hits, Circle<T>* circle = 0 /* estimate */, T lambda = 0.01)
1377  {
1379  return circle;
1380  }
1381 
1382  private:
1383  static T Sigma(Data<T>& data, Circle<T>* circle)
1384  {
1385  T sum = 0., dx, dy;
1386  for (size_t i = 0; i < data.hits_size; i++)
1387  {
1388  dx = data.hits[i][0] - circle->a;
1389  dy = data.hits[i][1] - circle->b;
1390  sum += std::pow(std::sqrt(dx * dx + dy * dy) - circle->r, 2);
1391  }
1392  return std::sqrt(sum / data.hits_size);
1393  }
1394 
1395  static T ChiSqr(Data<T>& data, Circle<T>* circle)
1396  {
1397  T sum = 0., dx, dy;
1398  for (size_t i = 0; i < data.hits_size; i++)
1399  {
1400  dx = data.hits[i][0] - circle->a;
1401  dy = data.hits[i][1] - circle->b;
1402  sum += std::pow(std::sqrt(dx * dx + dy * dy) - circle->r, 2);
1403  }
1404  return (sum / (data.hits_size - 3));
1405  }
1406 
1407  static Circle<T>* CircleFitByHyper(const std::vector<std::vector<T>>& hits)
1408  {
1409  // From: http://people.cas.uab.edu/~mosya/cl/CPPcircle.html
1410  size_t i, iter, IterMAX = 99;
1411 
1412  T Xi, Yi, Zi;
1413  T Mz, Mxy, Mxx, Myy, Mxz, Myz, Mzz, Cov_xy, Var_z;
1414  T A0, A1, A2, A22;
1415  T Dy, xnew, x, ynew, y;
1416  T DET, Xcenter, Ycenter;
1417 
1418  Circle<T>* circle = new Circle<T>();
1419  circle->a = std::numeric_limits<T>::infinity();
1420  circle->b = std::numeric_limits<T>::infinity();
1421  circle->r = std::numeric_limits<T>::infinity();
1422  circle->s = 0;
1423  circle->j = 0;
1424 
1425  Data<T> data(hits);
1426  data.means();
1427 
1428  Mxx = Myy = Mxy = Mxz = Myz = Mzz = 0.;
1429 
1430  for (i = 0; i < data.hits_size; i++)
1431  {
1432  Xi = data.hits[i][0] - data.meanX; // centered x-coordinates
1433  Yi = data.hits[i][1] - data.meanY; // centered y-coordinates
1434  Zi = Xi * Xi + Yi * Yi;
1435  Mxy += Xi * Yi;
1436  Mxx += Xi * Xi;
1437  Myy += Yi * Yi;
1438  Mxz += Xi * Zi;
1439  Myz += Yi * Zi;
1440  Mzz += Zi * Zi;
1441  }
1442  Mxx /= data.hits_size;
1443  Myy /= data.hits_size;
1444  Mxy /= data.hits_size;
1445  Mxz /= data.hits_size;
1446  Myz /= data.hits_size;
1447  Mzz /= data.hits_size;
1448 
1449  Mz = Mxx + Myy;
1450  Cov_xy = Mxx * Myy - Mxy * Mxy;
1451  Var_z = Mzz - Mz * Mz;
1452 
1453  A2 = 4.0 * Cov_xy - 3.0 * Mz * Mz - Mzz;
1454  A1 = Var_z * Mz + 4.0 * Cov_xy * Mz - Mxz * Mxz - Myz * Myz;
1455  A0 = Mxz * (Mxz * Myy - Myz * Mxy) + Myz * (Myz * Mxx - Mxz * Mxy) - Var_z * Cov_xy;
1456  A22 = A2 + A2;
1457 
1458  for (x = 0., y = A0, iter = 0; iter < IterMAX; iter++)
1459  { // usually, 4-6 iterations are enough
1460  Dy = A1 + x * (A22 + 16. * x * x);
1461  xnew = x - y / Dy;
1462  if ((xnew == x) || (!std::isfinite(xnew)))
1463  {
1464  break;
1465  }
1466  ynew = A0 + xnew * (A1 + xnew * (A2 + 4.0 * xnew * xnew));
1467  if (std::abs(ynew) >= std::abs(y))
1468  {
1469  break;
1470  }
1471  x = xnew;
1472  y = ynew;
1473  }
1474 
1475  DET = x * x - x * Mz + Cov_xy;
1476  Xcenter = (Mxz * (Myy - x) - Myz * Mxy) / DET / 2.0;
1477  Ycenter = (Myz * (Mxx - x) - Mxz * Mxy) / DET / 2.0;
1478 
1479  circle->a = Xcenter + data.meanX;
1480  circle->b = Ycenter + data.meanY;
1481  circle->r = std::sqrt(Xcenter * Xcenter + Ycenter * Ycenter + Mz - x - x);
1482  circle->s = Sigma(data, circle);
1483  circle->j = iter;
1484  circle->chi2 = ChiSqr(data, circle);
1485 
1486  return circle;
1487  }
1488 
1489  static Circle<T>* CircleFitByTaubin(const std::vector<std::vector<T>>& hits)
1490  {
1491  // From: http://people.cas.uab.edu/~mosya/cl/CPPcircle.html
1492  size_t i, iter, IterMAX = 99;
1493 
1494  T Xi, Yi, Zi;
1495  T Mz, Mxy, Mxx, Myy, Mxz, Myz, Mzz, Cov_xy, Var_z;
1496  T A0, A1, A2, A22, A3, A33;
1497  T Dy, xnew, x, ynew, y;
1498  T DET, Xcenter, Ycenter;
1499 
1500  Circle<T>* circle = new Circle<T>();
1501 
1502  Data<T> data(hits);
1503  data.means();
1504 
1505  Mxx = Myy = Mxy = Mxz = Myz = Mzz = 0.;
1506 
1507  for (i = 0; i < data.hits_size; i++)
1508  {
1509  Xi = data.hits[i][0] - data.meanX; // centered x-coordinates
1510  Yi = data.hits[i][0] - data.meanY; // centered y-coordinates
1511  Zi = Xi * Xi + Yi * Yi;
1512 
1513  Mxy += Xi * Yi;
1514  Mxx += Xi * Xi;
1515  Myy += Yi * Yi;
1516  Mxz += Xi * Zi;
1517  Myz += Yi * Zi;
1518  Mzz += Zi * Zi;
1519  }
1520  Mxx /= data.hits_size;
1521  Myy /= data.hits_size;
1522  Mxy /= data.hits_size;
1523  Mxz /= data.hits_size;
1524  Myz /= data.hits_size;
1525  Mzz /= data.hits_size;
1526 
1527  Mz = Mxx + Myy;
1528  Cov_xy = Mxx * Myy - Mxy * Mxy;
1529  Var_z = Mzz - Mz * Mz;
1530  A3 = 4.0 * Mz;
1531  A2 = -3.0 * Mz * Mz - Mzz;
1532  A1 = Var_z * Mz + 4.0 * Cov_xy * Mz - Mxz * Mxz - Myz * Myz;
1533  A0 = Mxz * (Mxz * Myy - Myz * Mxy) + Myz * (Myz * Mxx - Mxz * Mxy) - Var_z * Cov_xy;
1534  A22 = A2 + A2;
1535  A33 = A3 + A3 + A3;
1536 
1537  for (x = 0., y = A0, iter = 0; iter < IterMAX; iter++)
1538  { // usually, 4-6 iterations are enough
1539  Dy = A1 + x * (A22 + A33 * x);
1540  xnew = x - y / Dy;
1541  if ((xnew == x) || (!std::isfinite(xnew)))
1542  {
1543  break;
1544  }
1545  ynew = A0 + xnew * (A1 + xnew * (A2 + xnew * A3));
1546  if (std::abs(ynew) >= std::abs(y))
1547  {
1548  break;
1549  }
1550  x = xnew;
1551  y = ynew;
1552  }
1553 
1554  DET = x * x - x * Mz + Cov_xy;
1555  Xcenter = (Mxz * (Myy - x) - Myz * Mxy) / DET / 2.0;
1556  Ycenter = (Myz * (Mxx - x) - Mxz * Mxy) / DET / 2.0;
1557 
1558  circle->a = Xcenter + data.meanX;
1559  circle->b = Ycenter + data.meanY;
1560  circle->r = std::sqrt(Xcenter * Xcenter + Ycenter * Ycenter + Mz);
1561  circle->s = Sigma(data, circle);
1562  circle->j = iter;
1563  circle->chi2 = ChiSqr(data, circle);
1564 
1565  return circle;
1566  }
1567 
1568  static Circle<T>* CircleFitByPratt(const std::vector<std::vector<T>>& hits)
1569  {
1570  // From: http://people.cas.uab.edu/~mosya/cl/CPPcircle.html
1571  size_t i, iter, IterMAX = 99;
1572 
1573  T Xi, Yi, Zi;
1574  T Mz, Mxy, Mxx, Myy, Mxz, Myz, Mzz, Cov_xy, Var_z;
1575  T A0, A1, A2, A22;
1576  T Dy, xnew, x, ynew, y;
1577  T DET, Xcenter, Ycenter;
1578 
1579  Circle<T>* circle = new Circle<T>();
1580 
1581  Data<T> data(hits);
1582  data.means();
1583 
1584  Mxx = Myy = Mxy = Mxz = Myz = Mzz = 0.;
1585 
1586  for (i = 0; i < data.hits_size; i++)
1587  {
1588  Xi = data.hits[i][0] - data.meanX; // centered x-coordinates
1589  Yi = data.hits[i][1] - data.meanY; // centered y-coordinates
1590  Zi = Xi * Xi + Yi * Yi;
1591 
1592  Mxy += Xi * Yi;
1593  Mxx += Xi * Xi;
1594  Myy += Yi * Yi;
1595  Mxz += Xi * Zi;
1596  Myz += Yi * Zi;
1597  Mzz += Zi * Zi;
1598  }
1599  Mxx /= data.hits_size;
1600  Myy /= data.hits_size;
1601  Mxy /= data.hits_size;
1602  Mxz /= data.hits_size;
1603  Myz /= data.hits_size;
1604  Mzz /= data.hits_size;
1605 
1606  Mz = Mxx + Myy;
1607  Cov_xy = Mxx * Myy - Mxy * Mxy;
1608  Var_z = Mzz - Mz * Mz;
1609 
1610  A2 = 4.0 * Cov_xy - 3.0 * Mz * Mz - Mzz;
1611  A1 = Var_z * Mz + 4.0 * Cov_xy * Mz - Mxz * Mxz - Myz * Myz;
1612  A0 = Mxz * (Mxz * Myy - Myz * Mxy) + Myz * (Myz * Mxx - Mxz * Mxy) - Var_z * Cov_xy;
1613  A22 = A2 + A2;
1614 
1615  for (x = 0., y = A0, iter = 0; iter < IterMAX; iter++)
1616  { // usually, 4-6 iterations are enough
1617  Dy = A1 + x * (A22 + 16. * x * x);
1618  xnew = x - y / Dy;
1619  if ((xnew == x) || (!std::isfinite(xnew)))
1620  {
1621  break;
1622  }
1623  ynew = A0 + xnew * (A1 + xnew * (A2 + 4.0 * xnew * xnew));
1624  if (std::abs(ynew) >= std::abs(y))
1625  {
1626  break;
1627  }
1628  x = xnew;
1629  y = ynew;
1630  }
1631 
1632  DET = x * x - x * Mz + Cov_xy;
1633  Xcenter = (Mxz * (Myy - x) - Myz * Mxy) / DET / 2.0;
1634  Ycenter = (Myz * (Mxx - x) - Mxz * Mxy) / DET / 2.0;
1635 
1636  circle->a = Xcenter + data.meanX;
1637  circle->b = Ycenter + data.meanY;
1638  circle->r = std::sqrt(Xcenter * Xcenter + Ycenter * Ycenter + Mz + x + x);
1639  circle->s = Sigma(data, circle);
1640  circle->j = iter;
1641  circle->chi2 = ChiSqr(data, circle);
1642 
1643  return circle;
1644  }
1645 
1646  static T pythag(T a, T b)
1647  {
1648  T absa = std::abs(a), absb = std::abs(b);
1649  if (absa > absb)
1650  {
1651  return absa * std::sqrt(1.0 + std::pow(absb / absa, 2));
1652  }
1653  return (absb == 0.0 ? 0.0 : absb * std::sqrt(1.0 + std::pow(absa / absb, 2)));
1654  }
1655 
1657  {
1658  T Mr = 0., dx, dy;
1659  for (size_t i = 0; i < data.hits_size; i++)
1660  {
1661  dx = data.hits[i][0] - circle.a;
1662  dy = data.hits[i][1] - circle.b;
1663  Mr += std::sqrt(dx * dx + dy * dy);
1664  }
1665  return Mr / data.hits_size;
1666  }
1667 
1668  static void eigen2x2(T a, T b, T c, T& d1, T& d2, T& Vx, T& Vy)
1669  {
1670  T disc, f;
1671  disc = pythag(a - b, 2.0 * c);
1672  d1 = (a + b > 0.) ? (a + b + disc) / 2.0 : (a + b - disc) / 2.0;
1673  d2 = (a * b - c * c) / d1;
1674  if (std::abs(a - d1) > std::abs(b - d1))
1675  {
1676  if ((f = pythag(c, d1 - a)) == 0.)
1677  {
1678  Vx = 1.0;
1679  Vy = 0.;
1680  return;
1681  }
1682  else
1683  {
1684  Vx = c / f;
1685  Vy = (d1 - a) / f;
1686  }
1687  }
1688  else
1689  {
1690  if ((f = pythag(c, d1 - b)) == 0.)
1691  {
1692  Vx = 1.0;
1693  Vy = 0.;
1694  return;
1695  }
1696  else
1697  {
1698  Vx = (d1 - b) / f;
1699  Vy = c / f;
1700  }
1701  }
1702  return;
1703  }
1704 
1706  {
1707  int i, n = data.hits_size;
1708  T sum = 0., dx, dy, r;
1709  std::vector<T> D(n);
1710  T LargeCircle = 10.0, a0, b0, del, s, c, x, y, z, p, t, g, W, Z;
1711  if (std::abs(circle.a) < LargeCircle && std::abs(circle.b) < LargeCircle)
1712  {
1713  for (i = 0; i < n; i++)
1714  {
1715  dx = data.hits[i][0] - circle.a;
1716  dy = data.hits[i][1] - circle.b;
1717  D[i] = std::sqrt(dx * dx + dy * dy);
1718  sum += D[i];
1719  }
1720  r = sum / n;
1721  for (sum = 0., i = 0; i < n; i++)
1722  {
1723  sum += std::pow(D[i] - r, 2);
1724  }
1725  return sum / n;
1726  }
1727  else
1728  {
1729  a0 = circle.a - data.meanX;
1730  b0 = circle.b - data.meanY;
1731  del = 1.0 / std::sqrt(a0 * a0 + b0 * b0);
1732  s = b0 * del;
1733  c = a0 * del;
1734  for (W = Z = 0., i = 0; i < n; i++)
1735  {
1736  x = data.hits[i][0] - data.meanX;
1737  y = data.hits[i][1] - data.meanY;
1738  z = x * x + y * y;
1739  p = x * c + y * s;
1740  t = del * z - 2.0 * p;
1741  g = t / (1.0 + std::sqrt(1.0 + del * t));
1742  W += (z + p * g) / (2.0 + del * g);
1743  Z += z;
1744  }
1745  W /= n;
1746  Z /= n;
1747  return Z - W * (2.0 + del * del * W);
1748  }
1749  }
1750 
1751  static void GradientHessian(Data<T>& data, Circle<T>& circle, T& F1, T& F2, T& A11, T& A22, T& A12)
1752  {
1753  int i, n = data.hits_size;
1754  T LargeCircle = 10.0, dx, dy, r, u, v, Mr, Mu, Mv, Muu, Mvv, Muv, Muur, Mvvr, Muvr;
1755  T a0, b0, del, dd, s, c, x, y, a, b, z, p, t, w, g, g1, gg1, gg2;
1756  T X, Y, R, U, V, Tr, W, AA, BB, AB, AG, BG, GG, UUR, VVR, UVR;
1757 
1758  if (std::abs(circle.a) < LargeCircle && std::abs(circle.b) < LargeCircle)
1759  {
1760  for (Mr = Mu = Mv = Muu = Mvv = Muv = Muur = Mvvr = Muvr = 0., i = 0; i < n; i++)
1761  {
1762  dx = data.hits[i][0] - circle.a;
1763  dy = data.hits[i][1] - circle.b;
1764  r = std::sqrt(dx * dx + dy * dy);
1765  u = dx / r;
1766  v = dy / r;
1767  Mr += r;
1768  Mu += u;
1769  Mv += v;
1770  Muu += u * u;
1771  Mvv += v * v;
1772  Muv += u * v;
1773  Muur += u * u / r;
1774  Mvvr += v * v / r;
1775  Muvr += u * v / r;
1776  }
1777  Mr /= n;
1778  Mu /= n;
1779  Mv /= n;
1780  Muu /= n;
1781  Mvv /= n;
1782  Muv /= n;
1783  Muur /= n;
1784  Mvvr /= n;
1785  Muvr /= n;
1786 
1787  F1 = circle.a + Mu * Mr - data.meanX;
1788  F2 = circle.b + Mv * Mr - data.meanY;
1789 
1790  A11 = 1.0 - Mu * Mu - Mr * Mvvr;
1791  A22 = 1.0 - Mv * Mv - Mr * Muur;
1792  A12 = -Mu * Mv + Mr * Muvr;
1793  }
1794  else
1795  {
1796  a0 = circle.a - data.meanX;
1797  b0 = circle.b - data.meanY;
1798  del = 1.0 / std::sqrt(a0 * a0 + b0 * b0);
1799  dd = del * del;
1800  s = b0 * del;
1801  c = a0 * del;
1802  for (X = Y = R = Tr = W = AA = BB = AB = AG = BG = GG = 0., i = 0; i < n; i++)
1803  {
1804  x = data.hits[i][0] - data.meanX;
1805  y = data.hits[i][1] - data.meanY;
1806  z = x * x + y * y;
1807  p = x * c + y * s;
1808  t = 2.0 * p - del * z;
1809  w = std::sqrt(1.0 - del * t);
1810  g = -t / (1.0 + w);
1811  g1 = 1.0 / (1.0 + del * g);
1812  gg1 = g * g1;
1813  gg2 = g / (2.0 + del * g);
1814  a = (x + g * c) / w;
1815  b = (y + g * s) / w;
1816  X += x * gg1;
1817  Y += y * gg1;
1818  R += z + t * gg2;
1819  Tr += t * gg1;
1820  W += t * gg1 * gg2;
1821  AA += a * a * g1;
1822  BB += b * b * g1;
1823  AB += a * b * g1;
1824  AG += a * gg1;
1825  BG += b * gg1;
1826  GG += g * gg1;
1827  }
1828  X /= n;
1829  Y /= n;
1830  R /= n;
1831  Tr /= n;
1832  W /= n;
1833  AA /= n;
1834  BB /= n;
1835  AB /= n;
1836  AG /= n;
1837  BG /= n;
1838  GG /= n;
1839 
1840  U = (Tr - del * W) * c / 2.0 - X + R * c / 2.0;
1841  V = (Tr - del * W) * s / 2.0 - Y + R * s / 2.0;
1842 
1843  F1 = del * ((dd * R * U - del * W * c + Tr * c) / 2.0 - X);
1844  F2 = del * ((dd * R * V - del * W * s + Tr * s) / 2.0 - Y);
1845 
1846  UUR = ((GG - R / 2.0) * c + 2.0 * (AG - U)) * c + AA;
1847  VVR = ((GG - R / 2.0) * s + 2.0 * (BG - V)) * s + BB;
1848  UVR = ((GG - R / 2.0) * c + (AG - U)) * s + (BG - V) * c + AB;
1849 
1850  A11 = dd * (U * (2.0 * c - dd * U) - R * s * s / 2.0 - VVR * (1.0 + dd * R / 2.0));
1851  A22 = dd * (V * (2.0 * s - dd * V) - R * c * c / 2.0 - UUR * (1.0 + dd * R / 2.0));
1852  A12 = dd * (U * s + V * c + R * s * c / 2.0 - dd * U * V + UVR * (1.0 + dd * R / 2.0));
1853  }
1854  }
1855 
1856  static int CircleFitByChernovHoussam(const std::vector<std::vector<T>>& hits, Circle<T>* circleIni = 0, T LambdaIni = 0.01)
1857  {
1858  if (circleIni == 0)
1859  {
1860  circleIni = RegularFit(hits);
1861  }
1862 
1863  Data<T> data(hits);
1864  int i, n = data.hits_size, iter, inner, IterMAX = 200, check_line = 1, code;
1865 
1866  T lambda;
1867  T F1, F2, A11, A22, A12, dX, dY, Mxx, Myy, Mxy, Mxxy, dx, dy;
1868  T d1, d2, dmin = 1.0, Vx, Vy, dL1, dL2, VLx = 0, VLy = 0, aL = 0, bL = 0, R = 0, G1, G2, sBest, gBest, AB, progress;
1869 
1870  T ParLimit2 = 100.;
1872  T factor1 = 32., factor2 = 32.;
1873  T ccc = 0.4;
1874  T factorUp = 10., factorDown = 0.1;
1875 
1876  Circle<T> Old, New;
1877 
1878  data.means();
1879 
1880  // New = circleIni;
1881  New.initFromCircle(circleIni);
1882 
1883  New.s = SigmaWithLargeCircleOption(data, New);
1884  GradientHessian(data, New, F1, F2, A11, A22, A12);
1885  New.Gx = F1;
1886  New.Gy = F2;
1887  New.g = std::sqrt(F1 * F1 + F2 * F2);
1888 
1889  lambda = LambdaIni;
1890  iter = inner = code = 0;
1891  sBest = gBest = progress = std::numeric_limits<T>::max();
1892  ;
1893 
1894  do
1895  { // NextIteration
1896  bool break_outer = false;
1897 
1898  if (iter > 0)
1899  {
1900  progress = (std::abs(New.a - Old.a) + std::abs(New.b - Old.b)) /
1901  (std::pow(Old.a, 2) + std::pow(Old.b, 2) + 1.0);
1902  }
1903 
1904  Old = New;
1905  if (++iter > IterMAX)
1906  {
1907  break;
1908  }
1909 
1910  eigen2x2(A11, A22, A12, d1, d2, Vx, Vy);
1911  dmin = (d1 < d2) ? d1 : d2;
1912 
1913  AB = std::sqrt(std::pow(Old.a, 2) + std::pow(Old.b, 2)) + 1.0;
1914 
1915  if ((Old.g < factor1 * std::numeric_limits<T>::epsilon()) && (progress < epsilon))
1916  {
1917  break;
1918  }
1919 
1920  if ((Old.s >= sBest) && (Old.g >= gBest))
1921  {
1922  break;
1923  }
1924 
1925  if (sBest > Old.s)
1926  {
1927  sBest = Old.s;
1928  }
1929  if (gBest > Old.g)
1930  {
1931  gBest = Old.g;
1932  }
1933 
1934  G1 = Vx * F1 + Vy * F2;
1935  G2 = Vx * F2 - Vy * F1;
1936 
1937  do
1938  { // try_again
1939 
1940  if (lambda < std::abs(G1) / AB / ccc - d1)
1941  {
1942  lambda = std::abs(G1) / AB / ccc - d1;
1943  }
1944  if (lambda < std::abs(G2) / AB / ccc - d2)
1945  {
1946  lambda = std::abs(G2) / AB / ccc - d2;
1947  }
1948 
1949  dX = Old.Gx * (Vx * Vx / (d1 + lambda) + Vy * Vy / (d2 + lambda)) + Old.Gy * Vx * Vy * (1.0 / (d1 + lambda) - 1.0 / (d2 + lambda));
1950  dY = Old.Gx * Vx * Vy * (1.0 / (d1 + lambda) - 1.0 / (d2 + lambda)) + Old.Gy * (Vx * Vx / (d2 + lambda) + Vy * Vy / (d1 + lambda));
1951 
1952  New.a = Old.a - dX;
1953  New.b = Old.b - dY;
1954 
1955  if ((New.a == Old.a) && (New.b == Old.b))
1956  {
1957  break;
1958  }
1959 
1960  if (std::abs(New.a) > ParLimit2 || std::abs(New.b) > ParLimit2)
1961  {
1962  if (check_line)
1963  {
1964  for (Mxx = Myy = Mxy = 0., i = 0; i < n; i++)
1965  {
1966  dx = data.hits[i][0] - data.meanX;
1967  dy = data.hits[i][1] - data.meanY;
1968  Mxx += dx * dx;
1969  Myy += dy * dy;
1970  Mxy += dx * dy;
1971  }
1972 
1973  eigen2x2(Mxx, Myy, Mxy, dL1, dL2, VLx, VLy);
1974 
1975  for (Mxxy = 0., i = 0; i < n; i++)
1976  {
1977  dx = (data.hits[i][0] - data.meanX) * VLx + (data.hits[i][1] - data.meanY) * VLy;
1978  dy = (data.hits[i][1] - data.meanY) * VLx - (data.hits[i][0] - data.meanX) * VLy;
1979  Mxxy += dx * dx * dy;
1980  }
1981 
1982  R = (Mxxy > 0.) ? ParLimit2 : -ParLimit2;
1983  aL = -VLy * R;
1984  bL = VLx * R;
1985  check_line = 0;
1986  }
1987 
1988  if ((New.a * VLy - New.b * VLx) * R > 0.)
1989  {
1990  New.a = aL;
1991  New.b = bL;
1992  New.s = SigmaWithLargeCircleOption(data, New);
1993  GradientHessian(data, New, F1, F2, A11, A22, A12);
1994  New.Gx = F1;
1995  New.Gy = F2;
1996  New.g = std::sqrt(F1 * F1 + F2 * F2);
1997  lambda = LambdaIni;
1998  sBest = gBest = std::numeric_limits<T>::max();
1999  ;
2000  break;
2001  }
2002  }
2003 
2004  New.s = SigmaWithLargeCircleOption(data, New);
2005  GradientHessian(data, New, F1, F2, A11, A22, A12);
2006  New.Gx = F1;
2007  New.Gy = F2;
2008  New.g = std::sqrt(F1 * F1 + F2 * F2);
2009 
2010  if (New.s < sBest * (1.0 + factor2 * std::numeric_limits<T>::epsilon()))
2011  {
2012  lambda *= factorDown;
2013  break;
2014  }
2015  else
2016  {
2017  if (++inner > IterMAX)
2018  {
2019  break_outer = true;
2020  break;
2021  }
2022  lambda = factorUp * lambda;
2023  continue;
2024  }
2025 
2026  } while (1); // try_again loop
2027 
2028  if (break_outer)
2029  {
2030  break; // break out of two loops
2031  }
2032 
2033  } while (1); // NextIteration loop
2034 
2035  Old.r = OptimalRadius(data, Old);
2036  Old.i = iter;
2037  Old.j = inner;
2038 
2039  circleIni->initFromCircle(&Old);
2040  circleIni->chi2 = ChiSqr(data, circleIni);
2041 
2042  if (iter > IterMAX)
2043  {
2044  code = 1;
2045  }
2046  if (inner > IterMAX)
2047  {
2048  code = 2;
2049  }
2050  if ((dmin <= 0.) && (code == 0))
2051  {
2052  code = 3;
2053  }
2054 
2055  return code;
2056 
2057  } /* CircleFitByChernovHoussam */
2058  };
2059 
2060  template <class T>
2062  {
2063  public:
2065 
2066  static Line<T>* RegularFit(const std::vector<std::vector<T>>& hits)
2067  {
2068  // From: Numerical Recipes in C++: The Art of Scientific Computing 3rd Ed.
2069  size_t n = hits.size();
2070 
2071  T ss, sx = 0., sy = 0., st2 = 0., t, sxoss, sigdat = 0.,
2072  a = 0., b = 0., siga = 0., sigb = 0., chi2 = 0.;
2073  for (size_t i = 0; i < n; i++)
2074  {
2075  sx += hits[i][0];
2076  sy += hits[i][1];
2077  }
2078  ss = n;
2079  sxoss = sx / ss;
2080  for (size_t i = 0; i < n; i++)
2081  {
2082  t = hits[i][0] - sxoss;
2083  st2 += t * t;
2084  b += t * hits[i][1];
2085  }
2086  b /= st2;
2087  a = (sy - sx * b) / ss;
2088  siga = std::sqrt((1.0 + sx * sx / (ss * st2)) / ss);
2089  sigb = std::sqrt(1.0 / st2);
2090  for (size_t i = 0; i < n; i++)
2091  {
2092  chi2 += std::pow(hits[i][1] - a - b * hits[i][0], 2);
2093  }
2094  if (n > 2)
2095  {
2096  sigdat = std::sqrt(chi2 / (n - 2));
2097  }
2098  siga *= sigdat;
2099  sigb *= sigdat;
2100 
2101  chi2 = chi2 / (n - 2);
2102 
2103  Line<T>* line = new Line<T>(a, b, siga, sigb, chi2);
2104 
2105  return line;
2106  }
2107 
2108  static Line<T>* RobustFit(const std::vector<std::vector<T>>& hits, Line<T>* line = 0 /* estimate */)
2109  {
2110  // From: Numerical Recipes in C++: The Art of Scientific Computing 3rd Ed.
2111  size_t n = hits.size();
2112  T abdev = 0;
2113  T b1, b2, f, f1, f2;
2114  T a, b, sigb, chi2;
2115 
2116  if (line == 0)
2117  {
2118  line = RegularFit(hits);
2119  }
2120 
2121  a = line->a;
2122  b = line->b;
2123  sigb = line->sigb;
2124  chi2 = line->chi2;
2125 
2126  // robust fit
2127  b1 = b;
2128  f1 = rofunc(a, b1, abdev, hits);
2129  if (sigb > 0.0)
2130  {
2131  b2 = b + SIGN(3.0 * sigb, f1);
2132  f2 = rofunc(a, b2, abdev, hits);
2133  if (b2 == b1)
2134  {
2135  abdev /= n;
2136  return 0;
2137  }
2138  while (f1 * f2 > 0.0)
2139  {
2140  b = b2 + 1.6 * (b2 - b1);
2141  b1 = b2;
2142  f1 = f2;
2143  b2 = b;
2144  f2 = rofunc(a, b2, abdev, hits);
2145  }
2146  sigb = 0.01 * sigb;
2147  while (std::abs(b2 - b1) > sigb)
2148  {
2149  b = b1 + 0.5 * (b2 - b1);
2150  if (b == b1 || b == b2) break;
2151  f = rofunc(a, b, abdev, hits);
2152  if (f * f1 >= 0.0)
2153  {
2154  f1 = f;
2155  b1 = b;
2156  }
2157  else
2158  {
2159  f2 = f;
2160  b2 = b;
2161  }
2162  }
2163  }
2164  abdev /= n;
2165  line->b = b;
2166  line->sigb = sigb * 100.0; // restore original sigb or not?
2167  line->chi2 = chi2;
2168 
2169  return line;
2170  }
2171 
2172  private:
2173  static T rofunc(T& a, const T b, T& abdev, const std::vector<std::vector<T>>& hits)
2174  {
2175  size_t n = hits.size();
2177  size_t j;
2178  T d, sum = 0.0;
2179  std::vector<T> arr(n);
2180  for (j = 0; j < n; j++)
2181  {
2182  arr[j] = hits[j][1] - b * hits[j][0];
2183  }
2184  if ((n & 1) == 1)
2185  {
2186  a = select((n - 1) >> 1, arr);
2187  }
2188  else
2189  {
2190  j = n >> 1;
2191  a = 0.5 * (select(j - 1, arr) + select(j, arr));
2192  }
2193  abdev = 0.0;
2194  for (j = 0; j < n; j++)
2195  {
2196  d = hits[j][1] - (b * hits[j][0] + a);
2197  abdev += std::abs(d);
2198  if (hits[j][1] != 0.0)
2199  {
2200  d /= std::abs(hits[j][1]);
2201  }
2202  if (std::abs(d) > EPS)
2203  {
2204  sum += (d >= 0.0 ? hits[j][0] : -hits[j][0]);
2205  }
2206  }
2207  return sum;
2208  }
2209  static T select(const int k, std::vector<T>& arr)
2210  {
2211  int i, ir, j, l, mid, n = arr.size();
2212  T a;
2213  l = 0;
2214  ir = n - 1;
2215  for (;;)
2216  {
2217  if (ir <= l + 1)
2218  {
2219  if (ir == l + 1 && arr[ir] < arr[l])
2220  {
2221  SWAP(arr[l], arr[ir]);
2222  }
2223  return arr[k];
2224  }
2225  else
2226  {
2227  mid = (l + ir) >> 1;
2228  SWAP(arr[mid], arr[l + 1]);
2229  if (arr[l] > arr[ir])
2230  {
2231  SWAP(arr[l], arr[ir]);
2232  }
2233  if (arr[l + 1] > arr[ir])
2234  {
2235  SWAP(arr[l + 1], arr[ir]);
2236  }
2237  if (arr[l] > arr[l + 1])
2238  {
2239  SWAP(arr[l], arr[l + 1]);
2240  }
2241  i = l + 1;
2242  j = ir;
2243  a = arr[l + 1];
2244  for (;;)
2245  {
2246  do
2247  {
2248  i++;
2249  } while (arr[i] < a);
2250  do
2251  {
2252  j--;
2253  } while (arr[j] > a);
2254  if (j < i)
2255  {
2256  break;
2257  }
2258  SWAP(arr[i], arr[j]);
2259  }
2260  arr[l + 1] = arr[j];
2261  arr[j] = a;
2262  if (j >= k)
2263  {
2264  ir = j - 1;
2265  }
2266  if (j <= k)
2267  {
2268  l = i;
2269  }
2270  }
2271  }
2272  }
2273 
2274  static inline T SQR(const T a) { return a * a; }
2275  static inline T SIGN(const T& a, const T& b) { return b >= 0 ? (a >= 0 ? a : -a) : (a >= 0 ? -a : a); }
2276  static inline float SIGN(const float& a, const double& b) { return b >= 0 ? (a >= 0 ? a : -a) : (a >= 0 ? -a : a); }
2277  static inline float SIGN(const double& a, const float& b) { return (float) (b >= 0 ? (a >= 0 ? a : -a) : (a >= 0 ? -a : a)); }
2278  static inline void SWAP(T& a, T& b)
2279  {
2280  T dum = a;
2281  a = b;
2282  b = dum;
2283  }
2284  };
2285 
2286  template <typename T>
2288  {
2289  std::vector<std::vector<T>> pts;
2290  inline size_t kdtree_get_point_count() const
2291  {
2292  return pts.size();
2293  }
2294  inline T kdtree_distance(const T* p1, const size_t idx_p2, size_t /*size*/) const
2295  {
2296  const T d0 = p1[0] - pts[idx_p2][0];
2297  const T d1 = p1[1] - pts[idx_p2][1];
2298  const T d2 = p1[2] - pts[idx_p2][2];
2299  return d0 * d0 + d1 * d1 + d2 * d2;
2300  }
2301  inline T kdtree_get_pt(const size_t idx, int dim) const
2302  {
2303  if (dim == 0)
2304  return pts[idx][0];
2305  else if (dim == 1)
2306  return pts[idx][1];
2307  else
2308  return pts[idx][2];
2309  }
2310  template <class BBOX>
2311  bool kdtree_get_bbox(BBOX& /*bb*/) const
2312  {
2313  return false;
2314  }
2315  };
2316 
2317  template <typename T>
2318  struct KDTriplet
2319  {
2323  size_t n1;
2324  size_t n2;
2325  };
2326 
2327  template <class T>
2329  {
2330  public:
2331  TrackCandidate(std::vector<std::vector<T>>& hits, T B)
2332  : mCircle(0)
2333  , mLine(0)
2334  , mB(B)
2335  , mMinR(0)
2336  , mMaxR(0)
2337  {
2338  // copy hits to local container
2339  std::copy(hits.begin(), hits.end(), back_inserter(mHits));
2340  // check if radius of the last hit is closer to 0,0
2341  // if so - reverse hits
2342  size_t n = hits.size();
2343  if (std::sqrt(mHits[0][0] * mHits[0][0] + mHits[0][1] * hits[0][1]) > std::sqrt(mHits[n - 1][0] * mHits[n - 1][0] + mHits[n - 1][1] * mHits[n - 1][1]))
2344  {
2345  std::reverse(mHits.begin(), mHits.end());
2346  }
2347  calcMinMaxR();
2348  }
2349 
2351  {
2352  mHits.clear();
2353  if (mCircle)
2354  {
2355  delete mCircle;
2356  mCircle = 0;
2357  }
2358  if (mLine)
2359  {
2360  delete mLine;
2361  mLine = 0;
2362  }
2363  }
2364 
2365  bool isFitted() const
2366  {
2367  return mCircle && mLine && mCircle->is_good() && mLine->is_good();
2368  }
2369 
2370  void refit()
2371  {
2372  radiusFit();
2373  if (mCircle->is_good())
2374  {
2375  szFit();
2376  }
2377  }
2378 
2379  size_t nhits() const
2380  {
2381  return mHits.size();
2382  }
2383 
2384  std::vector<T>& getFirstHit()
2385  {
2386  return mHits[0];
2387  }
2388 
2389  std::vector<T>& getLastHit()
2390  {
2391  return mHits.back();
2392  }
2393 
2394  std::vector<std::vector<T>>& getHits()
2395  {
2396  return mHits;
2397  }
2398 
2399  void deleteHits()
2400  {
2401  mHits.clear();
2402  std::vector<std::vector<T>>().swap(mHits);
2403  if (mCircle)
2404  {
2405  delete mCircle;
2406  mCircle = 0;
2407  }
2408  if (mLine)
2409  {
2410  delete mLine;
2411  mLine = 0;
2412  }
2413  }
2414 
2415  T sign() const
2416  {
2417  T s = getS(3);
2418  return (s * mB) < 0 ? -1. : 1;
2419  }
2420 
2421  T momentum() const
2422  {
2423  return std::sqrt(std::pow(Pt(), 2) + std::pow(Pl(), 2));
2424  }
2425 
2426  T Pt() const
2427  {
2428  return 0.3 * std::fabs(mB) * mCircle->r * 0.01;
2429  }
2430 
2431  T Pl() const
2432  {
2433  T Pl = Pt() * mLine->b;
2434  if ((mHits[1][2] - mHits[0][2]) > 0)
2435  {
2436  if (Pl < 0)
2437  {
2438  Pl *= -1;
2439  }
2440  }
2441  else
2442  {
2443  if (Pl > 0)
2444  {
2445  Pl *= -1;
2446  }
2447  }
2448  return Pl;
2449  }
2450 
2451  // momentum for i-th hit
2452  std::vector<T> getMomForHit(size_t i)
2453  {
2454  T pt = Pt();
2455  T pl = Pl();
2456  std::vector<T>& myHit = mHits[i];
2457  T phi = calcAlpha(i);
2458  T ptX = -pt * std::sin(phi);
2459  T ptY = +pt * std::cos(phi);
2460  if (i > 0)
2461  {
2462  std::vector<T>& myHitBefore = mHits[i - 1];
2463  T difX = myHit[0] - myHitBefore[0];
2464  T difY = myHit[1] - myHitBefore[1];
2465  if ((ptX * difX + ptY * difY) < 0)
2466  {
2467  ptX *= -1;
2468  ptY *= -1;
2469  }
2470  }
2471  else if (!mHits.empty())
2472  {
2473  std::vector<T>& myHitAfter = mHits[1];
2474  T difX = myHitAfter[0] - myHit[0];
2475  T difY = myHitAfter[1] - myHit[1];
2476  if ((ptX * difX + ptY * difY) < 0)
2477  {
2478  ptX *= -1;
2479  ptY *= -1;
2480  }
2481  }
2482  std::vector<T> result(3);
2483  result[0] = ptX;
2484  result[1] = ptY;
2485  result[2] = pl;
2486  return result;
2487  }
2488 
2489  std::vector<T> getPosForHit(size_t i)
2490  {
2491  TVector<T> v0(mCircle->r, 0, 0);
2492  v0.rotateZ(calcAlpha(i));
2493  v0.setZ(calcZPosByS(getS(i)));
2494  std::vector<T> result(3);
2495  result[0] = v0.x() + mCircle->a;
2496  result[1] = v0.y() + mCircle->b;
2497  result[2] = v0.z();
2498  return result;
2499  }
2500 
2501  std::vector<T>& getHit(size_t i)
2502  {
2503  return mHits[i];
2504  }
2505 
2507  {
2508  // FIXME: need proper solution here
2509  T length = 0;
2510  if (isFitted())
2511  {
2512  std::vector<T> p = getMomForHit(0);
2513  std::vector<T> o = getPosForHit(0);
2514  std::vector<T> oL = getPosForHit(mHits.size() - 1);
2515  Helix<T> helix(TVector<T>(p[0], p[1], p[2]), TVector<T>(o[0], o[1], o[2]), mB, sign());
2516  length = helix.pathLength(TVector<T>(oL[0], oL[1], oL[2]), false);
2517  }
2518  else
2519  {
2520  for (size_t i = 1, ilen = mHits.size(); i < ilen; i++)
2521  {
2522  length += std::sqrt(std::pow(mHits[i][0] - mHits[i - 1][0], 2) +
2523  std::pow(mHits[i][1] - mHits[i - 1][1], 2) +
2524  std::pow(mHits[i][2] - mHits[i - 1][2], 2));
2525  }
2526  }
2527  return length;
2528  }
2529 
2530  T getB() const
2531  {
2532  return mB;
2533  }
2534 
2535  T minR() const
2536  {
2537  return mMinR;
2538  }
2539 
2540  T maxR() const
2541  {
2542  return mMaxR;
2543  }
2544 
2545  T radius() const
2546  {
2547  return mCircle->r;
2548  }
2549 
2550  T radius_err() const
2551  {
2552  return mCircle->s;
2553  }
2554 
2555  T tanl() const
2556  {
2557  return mLine->b;
2558  }
2559 
2560  T tanl_err() const
2561  {
2562  return mLine->sigb;
2563  }
2564 
2565  T dip() const
2566  {
2567  return std::cos(std::atan(mLine->b));
2568  }
2569 
2571  {
2572  return DistanceToCircle<T>(mCircle, x, y);
2573  }
2574 
2575  T distanceToCircle(const std::vector<T>& hit)
2576  {
2577  return DistanceToCircle<T>(mCircle, hit[0], hit[1]);
2578  }
2579 
2581  {
2582  // default sorting minR -> maxR for both tracks
2583  if (minR() > candidate->maxR())
2584  {
2585  // add hits to front
2586  std::vector<std::vector<T>>& hits = candidate->getHits(); // FIXME: reserve extra capacity for hits
2587  std::reverse(hits.begin(), hits.end());
2588  std::reverse(mHits.begin(), mHits.end());
2589  std::copy(hits.begin(), hits.end(), back_inserter(mHits));
2590  std::reverse(mHits.begin(), mHits.end());
2591  }
2592  else
2593  {
2594  // add hits to back
2595  std::vector<std::vector<T>>& hits = candidate->getHits(); // FIXME: reserve extra capacity for hits
2596  std::copy(hits.begin(), hits.end(), back_inserter(mHits));
2597  }
2598  candidate->deleteHits();
2599  refit();
2600  calcMinMaxR();
2601  }
2602 
2603  void print()
2604  {
2605  std::cout << "--- track params ---" << std::endl;
2606  if (mCircle && mCircle->is_good())
2607  {
2608  std::cout << " x0: " << mCircle->a << ", y0: " << mCircle->b << ", r: " << mCircle->r << ", sig: " << mCircle->s << std::endl;
2609  }
2610  else
2611  {
2612  std::cout << " xy fit either not done or failed" << std::endl;
2613  }
2614  if (mLine && mLine->is_good())
2615  {
2616  std::cout << " z0: " << mLine->a << ", tanl: " << mLine->b << ", sig_z0: " << mLine->siga << ", sig_tanl: " << mLine->sigb << ", chi2: " << mLine->chi2 << std::endl;
2617  }
2618  else
2619  {
2620  std::cout << " sz fit either not done or failed" << std::endl;
2621  }
2622  if (mCircle->is_good() && mLine->is_good())
2623  {
2624  std::cout << " Length: " << approxLength() << ", Pt: " << Pt() << ", Pl: " << Pl() << ", mom: " << momentum() << std::endl;
2625  }
2626  }
2627 
2628  void print_xy()
2629  {
2630  std::cout << "--- xy track params ---" << std::endl;
2631  std::cout << " x0: " << mCircle->a << ", y0: " << mCircle->b << ", r: " << mCircle->r << ", sig: " << mCircle->s << std::endl;
2632  for (size_t i = 0; i < mHits.size(); i++)
2633  {
2634  std::cout << i << "-th hit, x: " << mHits[i][0] << ", y: " << mHits[i][1] << std::endl;
2635  }
2636  }
2637 
2638  void print_sz()
2639  {
2640  std::cout << "--- sz track params ---" << std::endl;
2641  for (size_t i = 0; i < mHits.size(); i++)
2642  {
2643  std::cout << i << "-th hit, s: " << getS(i) << ", z: " << mHits[i][2] << std::endl;
2644  }
2645  }
2646 
2648 
2649  private:
2651  {
2652  if (mHits.empty())
2653  {
2654  return;
2655  }
2656  // min-max radius, used in track merging
2657  T radius = std::sqrt(mHits[0][0] * mHits[0][0] + mHits[0][1] * mHits[0][1]);
2658  mMinR = mMaxR = radius;
2659  for (size_t i = 1, ilen = mHits.size(); i < ilen; i++)
2660  {
2661  radius = std::sqrt(mHits[i][0] * mHits[i][0] + mHits[i][1] * mHits[i][1]);
2662  if (radius < mMinR)
2663  {
2664  mMinR = radius;
2665  }
2666  if (radius > mMaxR)
2667  {
2668  mMaxR = radius;
2669  }
2670  }
2671  }
2672 
2673  void radiusFit()
2674  {
2675  if (mCircle)
2676  {
2677  delete mCircle;
2678  }
2679  // approximate fit first (fast!)
2681 
2682  // check for outliers, refit with outliers weighted down
2683  if (mCircle->chi2 > 5.0)
2684  {
2686  }
2687  }
2688 
2689  void szFit()
2690  {
2691  // linear fit in sz
2692  if (mLine)
2693  {
2694  delete mLine;
2695  }
2696  if (!mCircle || !mCircle->is_good())
2697  {
2698  return; // can't fit sz without xy fit
2699  }
2700  std::vector<std::vector<T>> szhits;
2701  szhits.reserve(mHits.size());
2702  for (size_t i = 0, ilen = mHits.size(); i < ilen; i++)
2703  {
2704  std::vector<T> szhit(2);
2705  szhit[0] = getS(i); // S-angle for i-th hit
2706  szhit[1] = mHits[i][2]; // z-coordinate for i-th hit
2707  szhits.emplace_back(szhit);
2708  }
2709 
2710  // approximate fit first (fast!)
2711  mLine = LinearFit<T>::RegularFit(szhits);
2712 
2713  // check chi2 for possible outliers, refit with outliers weighted down
2714  if (mLine->chi2 > 5.0)
2715  {
2716  LinearFit<T>::RobustFit(szhits, mLine);
2717  }
2718  }
2719 
2720  T getS(size_t i) const
2721  {
2722  T cx0 = mCircle->a, cy0 = mCircle->b;
2723  T dphi = phi_mpi_pi(std::atan2(mHits[0][1] - cy0, mHits[0][0] - cx0) - std::atan2(mHits[i][1] - cy0, mHits[i][0] - cx0));
2724  return mCircle->r * dphi;
2725  }
2726 
2727  T calcAlpha(size_t i)
2728  {
2729  return (M_PI + std::atan2(-(mHits[i][1] - mCircle->b), -(mHits[i][0] - mCircle->a)));
2730  }
2731 
2733  {
2734  return (s * mLine->b + mLine->a);
2735  }
2736 
2737  T phi_mpi_pi(T x) const
2738  {
2739  while (x >= M_PI)
2740  {
2741  x -= M_PI * 2.0;
2742  }
2743  while (x < -M_PI)
2744  {
2745  x += M_PI * 2.0;
2746  }
2747  return x;
2748  }
2749 
2750  /* members of the class */
2751 
2752  Circle<T>* mCircle; // xy fit results
2753  Line<T>* mLine; // sz fit results
2754  std::vector<std::vector<T>> mHits;
2758  };
2759 
2760  template <typename T>
2761  bool pointsort(std::vector<T> a, std::vector<T> b)
2762  {
2763  return ((a[0] * a[0] + a[1] * a[1]) > (b[0] * b[0] + b[1] * b[1]));
2764  }
2765 
2766  template <typename T>
2768  {
2769  return ((a.dist1 + a.dist2) < (b.dist1 + b.dist2));
2770  }
2771 
2772  template <typename T>
2774  {
2775  T ad = std::sqrt(x1 * x1 + y1 * y1 + z1 * z1), bd = std::sqrt(x2 * x2 + y2 * y2 + z2 * z2);
2776  T c0 = (x1 / ad + x2 / bd), c1 = (y1 / ad + y2 / bd), c2 = (z1 / ad + z2 / bd);
2777  T d0 = (x1 / ad - x2 / bd), d1 = (y1 / ad - y2 / bd), d2 = (z1 / ad - z2 / bd);
2778  return (T)(2.0 * std::atan(std::sqrt(d0 * d0 + d1 * d1 + d2 * d2) / std::sqrt(c0 * c0 + c1 * c1 + c2 * c2)));
2779  }
2780 
2781  template <typename T>
2782  void make_triplets(size_t triplet_begin, size_t triplet_end, std::vector<std::vector<double>>& hits,
2784  std::vector<std::vector<KDTriplet<T>>>& triplets,
2785  T search_radius = 10, T search_angle = M_PI / 8)
2786  {
2787  search_radius *= search_radius; // KD-tree is using search_radius^2
2788  nanoflann::SearchParams params;
2789  params.sorted = true;
2790  std::vector<std::pair<size_t, T>> ret_matches;
2791  double query_pt[3] = {0., 0., 0.};
2792  size_t nMatches = 0;
2793  T dist1, dist2, angle;
2794  T current_search_radius, current_search_angle;
2795 
2796  for (size_t h = triplet_begin; h < triplet_end; h++)
2797  {
2798  std::vector<T>& hit = hits[h];
2799  current_search_radius = search_radius;
2800  current_search_angle = search_angle;
2801 
2802  query_pt[0] = hits[h][0];
2803  query_pt[1] = hits[h][1];
2804  query_pt[2] = hits[h][2];
2805  nMatches = index.radiusSearch(&query_pt[0], current_search_radius, ret_matches, params);
2806 
2807  if (nMatches < 5)
2808  {
2809  // if too few matches, increase search radius, retry search
2810  current_search_radius += 0.5 * search_radius;
2811  current_search_angle += M_PI / 32;
2812  nMatches = index.radiusSearch(&query_pt[0], current_search_radius, ret_matches, params);
2813  }
2814 
2815  if (nMatches < 5)
2816  {
2817  // if STILL too few matches, increase search radius AGAIN, retry search
2818  current_search_radius += 0.5 * search_radius;
2819  current_search_angle += M_PI / 32;
2820  nMatches = index.radiusSearch(&query_pt[0], current_search_radius, ret_matches, params);
2821  }
2822 
2823  if (nMatches < 3)
2824  {
2825  continue; // well, we tried, but can't make any triplets
2826  }
2827 
2828  for (size_t i = 1; i < nMatches; i++)
2829  { // skip original hit
2830 
2831  if (!triplets[h].empty() && i > 6)
2832  {
2833  break;
2834  } // optimization: reduces time to process large events by 1/3
2835 
2836  std::vector<T>& hit1 = hits[ret_matches[i].first];
2837  dist1 = ret_matches[i].second; // hit distance to the point
2838  for (size_t j = i + 1; j < nMatches; j++)
2839  {
2840  if (!triplets[h].empty() && i > 6)
2841  {
2842  break;
2843  } // optimization: reduces time to process large events by 1/3
2844 
2845  std::vector<T>& hit2 = hits[ret_matches[j].first];
2846  dist2 = ret_matches[j].second;
2847  angle = M_PI - angle_between_vectors(hit1[0] - hit[0], hit1[1] - hit[1], hit1[2] - hit[2],
2848  hit2[0] - hit[0], hit2[1] - hit[1], hit2[2] - hit[2]);
2849  if (angle > current_search_angle)
2850  {
2851  continue; // discard curvy triplets
2852  }
2853 
2854  KDTriplet<T> t;
2855  t.dist1 = dist1;
2856  t.dist2 = dist2;
2857  t.angle = angle;
2858  t.n1 = ret_matches[i].first;
2859  t.n2 = ret_matches[j].first;
2860  triplets[h].push_back(t);
2861  }
2862  }
2863 
2864  // sort triplets by smallest distance
2865  if (!triplets.empty())
2866  {
2867  std::sort(triplets[h].begin(), triplets[h].end(), tripletsort<T>);
2868  }
2869  }
2870  }
2871 
2872  template <typename T>
2873  std::vector<std::vector<std::vector<T>>> find_tracks(std::vector<std::vector<T>>& points, std::vector<std::vector<T>>& unused_hits,
2874  T search_radius = 10, T search_angle = M_PI / 8, size_t min_track_size = 10,
2875  size_t nthreads = 1,
2876  bool stats = false)
2877  {
2878  auto start0 = std::chrono::system_clock::now(); // total time
2879 
2880  const size_t TOTALHITS = points.size();
2881 
2882  // map of used hits
2883  std::vector<bool> used_hits(TOTALHITS, false);
2884 
2885  // data points storage container
2886  KDPointCloud<T> cloud;
2887 
2888  // generate / import points:
2889  auto start1 = std::chrono::system_clock::now(); // data import time
2890  cloud.pts.resize(TOTALHITS);
2891  for (size_t i = 0, ilen = TOTALHITS; i < ilen; i++)
2892  {
2893  cloud.pts[i] = points[i];
2894  }
2895  auto end1 = std::chrono::system_clock::now(); // data import time
2896 
2897  // alias points as hits
2898  std::vector<std::vector<double>>& hits = cloud.pts;
2899 
2900  // declare kdtree index
2902  KDPointCloud<T>, 3>
2903  index(3, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10));
2904 
2905  // build kdtree index
2906  auto start3 = std::chrono::system_clock::now(); // kd-index build time
2907  index.buildIndex();
2908  auto end3 = std::chrono::system_clock::now(); // kd-index build time
2909 
2910  // make triplets
2911  auto start4 = std::chrono::system_clock::now(); // make triplets time
2912 
2913  std::vector<std::vector<KDTriplet<T>>> triplets(TOTALHITS);
2914  if (nthreads > 1)
2915  {
2916  std::vector<std::thread> t(nthreads);
2917  size_t dnthreads = TOTALHITS / nthreads, triplet_begin = 0, triplet_end = 0;
2918  for (size_t i = 0; i < nthreads; i++)
2919  {
2920  triplet_begin = i * dnthreads;
2921  triplet_end = (i + 1) * dnthreads;
2922  if (triplet_end > TOTALHITS)
2923  {
2924  triplet_end = TOTALHITS;
2925  }
2926  if (i == (nthreads - 1))
2927  {
2928  triplet_end = TOTALHITS;
2929  }
2930  t[i] = std::thread(make_triplets<T>, triplet_begin, triplet_end, std::ref(hits),
2931  std::ref(index), std::ref(triplets), search_radius, search_angle);
2932  }
2933  for (size_t i = 0; i < nthreads; i++)
2934  {
2935  t[i].join();
2936  }
2937  }
2938  else
2939  {
2940  make_triplets<T>(0, TOTALHITS, hits, index, triplets, search_radius, search_angle);
2941  }
2942 
2943  auto end4 = std::chrono::system_clock::now(); // make triplets time
2944 
2945  // count triplets
2946  size_t triplet_ctr = 0;
2947  if (stats)
2948  {
2949  for (size_t i = 0, ilen = triplets.size(); i < ilen; i++)
2950  {
2951  triplet_ctr += triplets[i].size();
2952  }
2953  }
2954 
2955  // reconstruct track candidates
2956  auto start5 = std::chrono::system_clock::now(); // construct track candidates
2957 
2958  std::vector<std::vector<size_t>> reco_tracks;
2959  size_t n0, n1;
2960  bool found;
2961  for (size_t i = 0; i < TOTALHITS; i++)
2962  {
2963  if (used_hits[i] != false)
2964  {
2965  continue;
2966  }
2967  if (triplets[i].size() == 0)
2968  {
2969  continue;
2970  }
2971 
2972  KDTriplet<T>& triplet = triplets[i][0];
2973  std::vector<size_t> track;
2974  track.emplace_back(i);
2975  used_hits[i] = true;
2976 
2977  if (used_hits[triplet.n1] == false)
2978  {
2979  n0 = i;
2980  n1 = triplet.n1;
2981  do
2982  {
2983  found = false;
2984  track.emplace(track.begin(), n1);
2985  used_hits[n1] = true;
2986  if (triplets[n1].size() == 0)
2987  { // reached hit with no triplets
2988  break; // break out of do-while
2989  }
2990  // search for a good triplet
2991  for (size_t t = 0; t < triplets[n1].size(); t++)
2992  {
2993  if (triplets[n1][t].n1 == n0 && used_hits[triplets[n1][t].n2] == false)
2994  {
2995  n0 = n1;
2996  n1 = triplets[n1][t].n2;
2997  found = true;
2998  break; // found candidate via n2, break out of for-loop
2999  }
3000  else if (triplets[n1][t].n2 == n0 && used_hits[triplets[n1][t].n1] == false)
3001  {
3002  n0 = n1;
3003  n1 = triplets[n1][t].n1;
3004  found = true;
3005  break; // found candidate via n1, break out of for-loop
3006  }
3007  }
3008  } while (found == true);
3009  }
3010 
3011  if (used_hits[triplet.n2] == false)
3012  {
3013  n0 = i;
3014  n1 = triplet.n2;
3015  do
3016  {
3017  found = false;
3018  track.emplace_back(n1);
3019  used_hits[n1] = true;
3020  if (triplets[n1].size() == 0)
3021  { // reached hit with no triplets
3022  break; // break out of do-while
3023  }
3024  // search for a good triplet
3025  for (size_t t = 0; t < triplets[n1].size(); t++)
3026  {
3027  if (triplets[n1][t].n1 == n0 && used_hits[triplets[n1][t].n2] == false)
3028  {
3029  n0 = n1;
3030  n1 = triplets[n1][t].n2;
3031  found = true;
3032  break; // found candidate via n2, break out of for-loop
3033  }
3034  else if (triplets[n1][t].n2 == n0 && used_hits[triplets[n1][t].n1] == false)
3035  {
3036  n0 = n1;
3037  n1 = triplets[n1][t].n1;
3038  found = true;
3039  break; // found candidate via n1, break out of for-loop
3040  }
3041  }
3042  } while (found == true);
3043  }
3044 
3045  if ((TOTALHITS < 1000 && track.size() >= 5) || track.size() >= min_track_size)
3046  {
3047  // record track, keep hits marked as used
3048  reco_tracks.emplace_back(track);
3049  }
3050  else
3051  {
3052  // release unused hits
3053  for (size_t i = 0, ilen = track.size(); i < ilen; i++)
3054  {
3055  used_hits[track[i]] = false;
3056  }
3057  }
3058  }
3059  auto end5 = std::chrono::system_clock::now();
3060 
3061  // convert hit ids to xyz
3062 
3063  auto start6 = std::chrono::system_clock::now();
3064  std::vector<std::vector<std::vector<T>>> final_tracks;
3065  final_tracks.reserve(reco_tracks.size());
3066  for (size_t i = 0, ilen = reco_tracks.size(); i < ilen; i++)
3067  {
3068  std::vector<std::vector<T>> final_track;
3069  for (size_t j = 0, jlen = reco_tracks[i].size(); j < jlen; j++)
3070  {
3071  final_track.emplace_back(hits[reco_tracks[i][j]]);
3072  }
3073  final_tracks.emplace_back(final_track);
3074  }
3075  auto end6 = std::chrono::system_clock::now();
3076 
3077  size_t un_hits = 0, us_hits = 0;
3078  for (size_t i = 0, ilen = used_hits.size(); i < ilen; i++)
3079  {
3080  if (used_hits[i])
3081  {
3082  us_hits += 1;
3083  }
3084  else
3085  {
3086  un_hits += 1;
3087  unused_hits.emplace_back(hits[i]);
3088  }
3089  }
3090 
3091  auto end0 = std::chrono::system_clock::now(); // total time
3092 
3093  if (stats)
3094  {
3095  double us_pct = roundf(((double) us_hits / (double) used_hits.size() * 100.0) * 1000) / 1000;
3096  double un_pct = roundf(((double) un_hits / (double) used_hits.size() * 100.0) * 1000) / 1000;
3097 
3098  std::cout << "---------- KDfinder stats ----------\n";
3099  std::chrono::duration<double> elapsed_seconds1 = end1 - start1;
3100  std::cout << " data import: " << elapsed_seconds1.count() << "s\n";
3101  std::chrono::duration<double> elapsed_seconds3 = end3 - start3;
3102  std::cout << " kd index build: " << elapsed_seconds3.count() << "s\n";
3103  std::chrono::duration<double> elapsed_seconds4 = end4 - start4;
3104  std::cout << " triplet build: " << elapsed_seconds4.count() << "s\n";
3105  std::chrono::duration<double> elapsed_seconds5 = end5 - start5;
3106  std::cout << " track reco: " << elapsed_seconds5.count() << "s\n";
3107  std::chrono::duration<double> elapsed_seconds6 = end6 - start6;
3108  std::cout << " hit converter: " << elapsed_seconds6.count() << "s\n";
3109  std::chrono::duration<double> elapsed_seconds0 = end0 - start0;
3110  std::cout << " = total time: " << elapsed_seconds0.count() << "s\n";
3111  std::cout << " --- objects --- \n";
3112  std::cout << " triplets created: " << triplet_ctr << std::endl;
3113  std::cout << " tracks created: " << final_tracks.size() << std::endl;
3114  std::cout << " --- hits --- \n";
3115  std::cout << " hits used: " << us_hits << "\t\t or " << us_pct << "%"
3116  << "\n";
3117  std::cout << " hits unused: " << un_hits << "\t\t or " << un_pct << "%"
3118  << "\n";
3119  }
3120 
3121  return final_tracks;
3122  }
3123 
3124  template <typename T>
3125  std::vector<std::vector<std::vector<T>>> find_tracks_iterative(std::vector<std::vector<T>>& hits, std::vector<std::vector<T>>& unused_hits,
3126  T search_radius1 = 10, T search_angle1 = M_PI / 8, size_t min_track_size1 = 10,
3127  T search_radius2 = 12, T search_angle2 = M_PI / 8, size_t min_track_size2 = 6,
3128  size_t nthreads = 1,
3129  bool stats = false)
3130  {
3131  // allows to make two iterations => gets better results in reasonable time
3132 
3133  std::vector<std::vector<std::vector<T>>> tracks = kdfinder::find_tracks<T>(hits, unused_hits,
3134  search_radius1, search_angle1, min_track_size1, nthreads, stats);
3135  if (unused_hits.size() > 10)
3136  {
3137  hits.clear();
3138  hits.insert(hits.begin(), std::make_move_iterator(unused_hits.begin()), std::make_move_iterator(unused_hits.end()));
3139  unused_hits.erase(unused_hits.begin(), unused_hits.end());
3140  std::vector<std::vector<std::vector<T>>> extra_tracks = kdfinder::find_tracks<T>(hits, unused_hits,
3141  search_radius2, search_angle2, min_track_size2, nthreads, stats);
3142  if (!extra_tracks.empty())
3143  {
3144  tracks.insert(tracks.end(), std::make_move_iterator(extra_tracks.begin()), std::make_move_iterator(extra_tracks.end()));
3145  extra_tracks.erase(extra_tracks.begin(), extra_tracks.end());
3146  }
3147  }
3148 
3149  return tracks;
3150  }
3151 
3152  template <typename T>
3153  std::vector<TrackCandidate<T>*> get_track_candidates(std::vector<std::vector<std::vector<T>>>& tracks, T B, bool stats = false)
3154  {
3155  auto begin1 = std::chrono::system_clock::now();
3156 
3157  std::vector<TrackCandidate<T>*> candidates;
3158  for (size_t i = 0, ilen = tracks.size(); i < ilen; i++)
3159  {
3160  TrackCandidate<T>* trk = new TrackCandidate<T>(tracks[i], B);
3161  trk->refit();
3162  if (trk->isFitted())
3163  {
3164  candidates.emplace_back(trk);
3165  }
3166  else
3167  {
3168  delete trk;
3169  trk = nullptr;
3170  }
3171  }
3172 
3173  auto end1 = std::chrono::system_clock::now();
3174 
3175  if (stats)
3176  {
3177  std::cout << "---------- KDcandidates stats ----------\n";
3178  std::chrono::duration<double> elapsed_seconds1 = end1 - begin1;
3179  std::cout << " conversion time : " << elapsed_seconds1.count() << "s\n";
3180  std::cout << " input tracks : " << tracks.size() << "\n";
3181  std::cout << " candidates : " << candidates.size() << "\n";
3182  }
3183 
3184  return candidates;
3185  }
3186 
3187  template <typename T>
3189  {
3190  return (a->nhits() > b->nhits());
3191  }
3192 
3193  template <typename T>
3195  {
3196  return (a->minR() > b->minR());
3197  }
3198 
3199  template <typename T>
3201  {
3202  return (o->nhits() == 0);
3203  }
3204 
3205  template <typename T>
3206  std::vector<TrackCandidate<T>*> merge_track_candidates(
3207  std::vector<TrackCandidate<T>*>& candidates,
3208  T c_tanl = 5.0 /* n sigma */, T c_xy = 5.0 /* n sigma */, T c_dist = 60.0 /* 3D distance between hits */,
3209  bool stats = false)
3210  {
3211  auto start = std::chrono::system_clock::now();
3212  size_t tracks_before_merge = candidates.size();
3213  if (candidates.size() < 2)
3214  {
3215  return candidates; // need 2+ tracks to check for merging possibility
3216  }
3217 
3218  std::sort(candidates.begin(), candidates.end(), candidatesortradius<T>);
3219 
3220  for (size_t i = 0, ilen = candidates.size(); i < ilen; i++)
3221  {
3222  if (!candidates[i]->nhits())
3223  {
3224  continue;
3225  }
3226  if (!candidates[i]->isFitted())
3227  {
3228  continue;
3229  }
3230  if ((candidates[i]->maxR() - candidates[i]->minR()) < 10.0)
3231  {
3232  continue;
3233  }
3234 
3235  for (size_t j = i + 1, jlen = candidates.size(); j < jlen; j++)
3236  {
3237  if (!candidates[i]->nhits())
3238  {
3239  break;
3240  }
3241  if (!candidates[j]->nhits())
3242  {
3243  continue;
3244  }
3245  if (!candidates[j]->isFitted())
3246  {
3247  continue;
3248  }
3249  if (candidates[j]->maxR() > candidates[i]->minR())
3250  {
3251  continue;
3252  }
3253  if ((candidates[j]->maxR() - candidates[j]->minR()) < 10.0)
3254  {
3255  continue;
3256  }
3257 
3258  T distTanlErr = candidates[i]->nhits() > candidates[j]->nhits() ? candidates[i]->tanl_err() : candidates[j]->tanl_err();
3259  if (std::fabs(candidates[i]->tanl() - candidates[j]->tanl()) > (c_tanl * distTanlErr))
3260  {
3261  continue;
3262  }
3263 
3264  T distToCircle = candidates[i]->nhits() > candidates[j]->nhits() ? std::fabs(candidates[i]->distanceToCircle(candidates[j]->getLastHit())) : std::fabs(candidates[j]->distanceToCircle(candidates[i]->getFirstHit()));
3265  T circleErr = candidates[i]->nhits() > candidates[j]->nhits() ? candidates[i]->radius_err() : candidates[j]->radius_err();
3266  if (std::fabs(distToCircle) > (c_xy * circleErr))
3267  {
3268  continue;
3269  }
3270 
3271  std::vector<T>& hit1 = candidates[i]->getFirstHit();
3272  std::vector<T>& hit2 = candidates[j]->getLastHit();
3273  T dist = std::sqrt(std::pow(hit1[0] - hit2[0], 2) + std::pow(hit1[1] - hit2[1], 2) + std::pow(hit1[2] - hit2[2], 2));
3274  if (dist > c_dist)
3275  {
3276  continue;
3277  }
3278 
3279  candidates[i]->mergeCandidate(candidates[j]);
3280  // don't break out of the loop yet, there might be more candidates coming
3281  }
3282  }
3283 
3284  candidates.erase(std::remove_if(candidates.begin(), candidates.end(), ismergedcandidate<T>), candidates.end());
3285 
3286  auto stop = std::chrono::system_clock::now();
3287  if (stats)
3288  {
3289  size_t tracks_after_merge = candidates.size();
3290  std::cout << "---------- KDmerger stats ----------\n";
3291  std::chrono::duration<double> elapsed_seconds = stop - start;
3292  std::cout << " tracks before: " << tracks_before_merge << "\n";
3293  std::cout << " tracks merged: " << (tracks_before_merge - tracks_after_merge) << " = " << roundf((double) (tracks_before_merge - tracks_after_merge) / (double) tracks_before_merge * 100.0 * 100) / 100.0 << "%\n";
3294  std::cout << " tracks after : " << tracks_after_merge << "\n";
3295  std::cout << " time: " << elapsed_seconds.count() << "s\n";
3296  }
3297 
3298  return candidates;
3299  }
3300 
3301  template <typename T>
3302  bool elementsort(const std::tuple<T, Helix<T>*, size_t> a, const std::tuple<T, Helix<T>*, size_t> b)
3303  {
3304  return (std::get<0>(a) < std::get<0>(b));
3305  }
3306 
3307  template <typename T>
3308  bool vertexsort(const std::pair<std::vector<T>, std::vector<size_t>> a, const std::pair<std::vector<T>, std::vector<size_t>> b)
3309  {
3310  return ((std::get<1>(a)).size() > (std::get<1>(b)).size());
3311  }
3312 
3313  template <typename T>
3314  std::vector<std::pair<std::vector<T>, std::vector<size_t>>>
3315  find_vertex_seeds(std::vector<TrackCandidate<T>*>& candidates, T x0 = 0, T y0 = 0,
3316  T c_z_dist = 0.5 /* cm */, T c_xy_dist = 2.0 /* cm */, T c_min_tracks = 3, bool stats = false)
3317  {
3318  auto start = std::chrono::system_clock::now();
3319 
3320  std::vector<std::pair<std::vector<T>, std::vector<size_t>>> vertices;
3321 
3322  std::vector<std::tuple<T, Helix<T>*, size_t>> elements;
3323  std::vector<std::tuple<T, Helix<T>*, size_t>> sequence;
3324  Helix<T>* helix;
3325  T dca_z, dca_xy;
3326 
3327  size_t stat_candidates = candidates.size(),
3328  stat_elements = 0,
3329  stat_good_sequences = 0,
3330  stat_bad_sequences = 0;
3331 
3332  for (size_t i = 0, ilen = candidates.size(); i < ilen; i++)
3333  {
3334  // make helix out of a candidate, get DCA of ( 0, 0 ), push to elements
3335  std::vector<T> o = candidates[i]->getPosForHit(0);
3336  std::vector<T> p = candidates[i]->getMomForHit(0);
3337  helix = new Helix<T>(TVector<T>(p[0], p[1], p[2]), TVector<T>(o[0], o[1], o[2]), candidates[i]->getB(), candidates[i]->sign());
3338  T s = helix->pathLength(x0, y0);
3339  TVector<T> pos = helix->at(s);
3340  dca_z = pos.z();
3341  dca_xy = pos.perp();
3342  if (dca_xy > c_xy_dist)
3343  {
3344  continue;
3345  }
3346  elements.push_back(std::make_tuple(dca_z, helix, i));
3347  }
3348  stat_elements = elements.size();
3349 
3350  // sort elements by dca_z
3351  std::sort(elements.begin(), elements.end(), elementsort<T>);
3352 
3353  // scan z-direction, find sequences
3354  for (size_t i = 0, ilen = elements.size(); i < ilen; i++)
3355  {
3356  if (sequence.empty())
3357  {
3358  // push helix and candidate to sequence
3359  sequence.push_back(elements[i]);
3360  }
3361  else
3362  {
3363  // get last element of sequence, check z distance to the previous element in sequence
3364  T dz = std::fabs(std::get<0>(elements[i]) - std::get<0>(sequence.back()));
3365 
3366  if (dz > c_z_dist)
3367  {
3368  // stop sequence
3369 
3370  if (sequence.size() >= c_min_tracks)
3371  {
3372  // calculate average x, y, z of vertex, push mean(x,y,z) and candidates to vertices array
3373  std::vector<T> mean(6, 0);
3374  std::vector<size_t> track_ids;
3375  track_ids.reserve(sequence.size());
3376 
3377  // mean:
3378  for (size_t k = 0, klen = sequence.size(); k < klen; k++)
3379  {
3380  Helix<T>* helix = std::get<1>(sequence[k]);
3381  T s = helix->pathLength(x0, y0);
3382  TVector<T> pos = helix->at(s);
3383  mean[0] += pos.x();
3384  mean[1] += pos.y();
3385  mean[2] += pos.z();
3386  track_ids.push_back(std::get<2>(sequence[k]));
3387  }
3388  mean[0] /= sequence.size();
3389  mean[1] /= sequence.size();
3390  mean[2] /= sequence.size();
3391 
3392  // deviation:
3393  for (size_t k = 0, klen = sequence.size(); k < klen; k++)
3394  {
3395  Helix<T>* helix = std::get<1>(sequence[k]);
3396  T s = helix->pathLength(0, 0);
3397  TVector<T> pos = helix->at(s);
3398  mean[3] += std::fabs(mean[0] - pos.x());
3399  mean[4] += std::fabs(mean[1] - pos.y());
3400  mean[5] += std::fabs(mean[2] - pos.z());
3401  }
3402  mean[3] /= sequence.size();
3403  mean[4] /= sequence.size();
3404  mean[5] /= sequence.size();
3405 
3406  vertices.push_back(std::make_pair(mean, track_ids));
3407  stat_good_sequences += 1;
3408  }
3409  else
3410  {
3411  stat_bad_sequences += 1;
3412  }
3413 
3414  // cleanup => delete all sequence ptrs, clear sequence
3415  for (auto it = sequence.begin(); it != sequence.end(); ++it)
3416  {
3417  delete std::get<1>(*it);
3418  }
3419  sequence.clear();
3420  }
3421  else
3422  {
3423  // start sequence => add element to sequence
3424  sequence.push_back(elements[i]);
3425  }
3426  }
3427  }
3428 
3429  // sort vertices by number of tracks per vertex
3430  std::sort(vertices.begin(), vertices.end(), vertexsort<T>);
3431 
3432  auto stop = std::chrono::system_clock::now();
3433 
3434  if (stats)
3435  {
3436  std::cout << "---------- KDvertexer stats ----------\n";
3437  std::cout << " input candidates: " << stat_candidates << "\n";
3438  std::cout << " proj. candidates: " << stat_elements << "\n";
3439  std::cout << " good pre-vertices: " << stat_good_sequences << "\n";
3440  std::cout << " weak pre-vertices: " << stat_bad_sequences << "\n";
3441  std::chrono::duration<double> elapsed_seconds = stop - start;
3442  std::cout << " time: " << elapsed_seconds.count() << "s\n";
3443  }
3444 
3445  return vertices;
3446  }
3447 
3448  template <typename T>
3450  {
3451  T r = 0, g = 0, b = 0, p = std::fabs(pt), maxp = 4.5, colval = std::min(1.0, p / maxp), colvaltimes4 = colval * 4.0;
3452  if (colval < 0.25)
3453  {
3454  b = g = colvaltimes4;
3455  b += 1.0 - colvaltimes4;
3456  }
3457  else if (colval < 0.5)
3458  {
3459  b = g = 1.0 - (colvaltimes4 - 1.0);
3460  g += colvaltimes4 - 1.0;
3461  }
3462  else if (colval < 0.75)
3463  {
3464  g = r = colvaltimes4 - 2.0;
3465  g += 1.0 - (colvaltimes4 - 2.0);
3466  }
3467  else
3468  {
3469  g = r = 1.0 - (colvaltimes4 - 3.0);
3470  r += colvaltimes4 - 3.0;
3471  }
3472  if (rand() % 1000 < 10)
3473  {
3474  r = 1.0;
3475  }
3476  return ((int(r * 255) & 0xff) << 16) + ((int(g * 255) & 0xff) << 8) + (int(b * 255) & 0xff);
3477  }
3478 
3479  template <typename T>
3480  std::string export_candidates_json(std::vector<TrackCandidate<T>*>& candidates, std::vector<std::vector<T>>& hits,
3481  size_t run = 0, size_t event_number = 0, size_t evt_time = 0, double B = -0.5)
3482  {
3483  std::stringstream ofs;
3484 
3485  ofs << "{ \n"
3486  << " \"EVENT\": { \n"
3487  << " \"runid\": " << run << ", \n"
3488  << " \"evtid\": " << event_number << ", \n"
3489  << " \"time\": " << evt_time << ", \n"
3490  << " \"B\": " << B << " \n"
3491  << " }, \n";
3492 
3493  ofs << "\"META\": { \n"
3494  << " \"HITS\": { \n"
3495  << " \"TPC\": { \n"
3496  << " \"type\": \"3D\", \n"
3497  << " \"options\": { \n"
3498  << " \"size\": 2, \n"
3499  << " \"color\": 16777215 \n"
3500  << " }\n"
3501  << " }\n"
3502  << " },\n"
3503  << " \"TRACKS\": { \n"
3504  << " \"TPC\": { \n"
3505  << " \"r_min\": 0,\n"
3506  << " \"r_max\": 2000,\n"
3507  << " \"size\": 2, \n"
3508  << " \"thickness\": 2 \n"
3509  << " }\n"
3510  << " }\n"
3511  << "},\n"
3512  << " \"TRACKS\": {\n"
3513  << " \"TPC\": [\n";
3514 
3515  for (int i = 0, ilen = candidates.size(); i < ilen; i++)
3516  {
3517  std::vector<T> hit = candidates[i]->getPosForHit(0);
3518  std::vector<T> mom = candidates[i]->getMomForHit(0);
3519  size_t nhits = candidates[i]->nhits();
3520  T pt = candidates[i]->Pt();
3521  T sign = candidates[i]->sign();
3522  T length = candidates[i]->approxLength();
3523  size_t color = get_track_color<T>(pt);
3524  ofs << "{ \"color\": " << color << ", \"pt\": " << pt << ", \"xyz\":[" << hit[0] << "," << hit[1] << "," << hit[2] << "], \"pxyz\":[" << mom[0] << "," << mom[1] << "," << mom[2] << "],\"l\":" << length << ",\"nh\":" << nhits << ",\"q\":" << sign << "}";
3525  if (i != (ilen - 1))
3526  {
3527  ofs << ",";
3528  }
3529  else
3530  {
3531  ofs << "\n";
3532  }
3533  }
3534 
3535  ofs << " ]\n"
3536  << " },\n"
3537  << " \"HITS\": {\n"
3538  << " \"TPC\": [\n";
3539 
3540  for (int i = 0, ilen = hits.size(); i < ilen; i++)
3541  {
3542  ofs << "[ " << hits[i][0] << "," << hits[i][1] << "," << hits[i][2] << " ]";
3543  if (i != (ilen - 1))
3544  {
3545  ofs << ",";
3546  }
3547  else
3548  {
3549  ofs << "\n";
3550  }
3551  }
3552 
3553  ofs << " ]\n"
3554  << " }\n"
3555  << "}\n";
3556 
3557  return ofs.str();
3558  }
3559 
3560  template <typename T>
3561  std::string export_json(std::vector<std::vector<std::vector<T>>>& tracks, std::vector<std::vector<T>>& hits,
3562  size_t run = 0, size_t event_number = 0, size_t evt_time = 0, double B = -0.5)
3563  {
3564  std::stringstream ofs;
3565 
3566  ofs << "{ \n"
3567  << " \"EVENT\": { \n"
3568  << " \"runid\": " << run << ", \n"
3569  << " \"evtid\": " << event_number << ", \n"
3570  << " \"time\": " << evt_time << ", \n"
3571  << " \"B\": " << B << " \n"
3572  << " }, \n";
3573 
3574  ofs << "\"META\": { \n"
3575  << " \"HITS\": { \n"
3576  << " \"TPC\": { \n"
3577  << " \"type\": \"3D\", \n"
3578  << " \"options\": { \n"
3579  << " \"size\": 2, \n"
3580  << " \"color\": 16777215 \n"
3581  << " }\n"
3582  << " }\n"
3583  << " },\n"
3584  << " \"TRACKS\": { \n"
3585  << " \"TPC\": { \n"
3586  << " \"size\": 2, \n"
3587  << " \"thickness\": 2, \n"
3588  << " \"color\": 255 \n"
3589  << " }\n"
3590  << " }\n"
3591  << "},\n"
3592  << " \"TRACKS\": {\n"
3593  << " \"TPC\": [\n";
3594 
3595  for (int i = 0, ilen = tracks.size(); i < ilen; i++)
3596  {
3597  ofs << "{ \"nh\": " << tracks[i].size() << ", \"pts\": [ ";
3598  for (int j = 0, jlen = tracks[i].size(); j < jlen; j++)
3599  {
3600  ofs << "[ " << tracks[i][j][0] << "," << tracks[i][j][1] << "," << tracks[i][j][2] << " ]";
3601  if (j != (jlen - 1))
3602  {
3603  ofs << ",";
3604  }
3605  }
3606  ofs << " ] }\n";
3607  if (i != (ilen - 1))
3608  {
3609  ofs << ",";
3610  }
3611  }
3612 
3613  ofs << " ]\n"
3614  << " },\n"
3615  << " \"HITS\": {\n"
3616  << " \"TPC\": [\n";
3617 
3618  for (int i = 0, ilen = hits.size(); i < ilen; i++)
3619  {
3620  ofs << "[ " << hits[i][0] << "," << hits[i][1] << "," << hits[i][2] << " ]";
3621  if (i != (ilen - 1))
3622  {
3623  ofs << ",";
3624  }
3625  else
3626  {
3627  ofs << "\n";
3628  }
3629  }
3630 
3631  ofs << " ]\n"
3632  << " }\n"
3633  << "}\n";
3634 
3635  return ofs.str();
3636  }
3637 
3638  template <typename T>
3639  std::string export_candidates_json_old(std::vector<TrackCandidate<T>*>& candidates, std::vector<ulong>& trigger_ids,
3640  size_t run = 0, size_t event_number = 0, size_t evt_time = 0, T B = -0.5, bool round = false)
3641  {
3642  std::stringstream ofs;
3643  ofs << "{"
3644  << "\"R\": " << run << ", "
3645  << "\"Evt\": " << event_number << ", "
3646  << "\"B\": " << B << ","
3647  << "\"tm\": " << evt_time << ", "
3648  << "\"trig\": [";
3649 
3650  for (size_t i = 0; i < trigger_ids.size(); i++)
3651  {
3652  if (i != 0)
3653  {
3654  ofs << ",";
3655  }
3656  ofs << trigger_ids[i];
3657  }
3658 
3659  ofs << "],\n"
3660  << "\"tracks\": [\n";
3661 
3662  for (int i = 0, ilen = candidates.size(); i < ilen; i++)
3663  {
3664  std::vector<T> hit = candidates[i]->getPosForHit(0);
3665  std::vector<T> mom = candidates[i]->getMomForHit(0);
3666  size_t nhits = candidates[i]->nhits();
3667  T pt = candidates[i]->Pt();
3668  T sign = candidates[i]->sign();
3669  T length = candidates[i]->approxLength();
3670  size_t color = get_track_color<T>(pt);
3671 
3672  if (round)
3673  {
3674  hit[0] = std::floor(hit[0] * 10) / 10;
3675  hit[1] = std::floor(hit[0] * 10) / 10;
3676  hit[2] = std::floor(hit[0] * 10) / 10;
3677  mom[0] = std::floor(mom[0] * 1000) / 1000;
3678  mom[1] = std::floor(mom[1] * 1000) / 1000;
3679  mom[2] = std::floor(mom[2] * 1000) / 1000;
3680  length = std::floor(length * 100) / 100;
3681  }
3682 
3683  ofs << "{ \"color\": " << color << ", \"pt\": " << pt << ", \"xyz\":[" << hit[0] << "," << hit[1] << "," << hit[2] << "], \"pxyz\":[" << mom[0] << "," << mom[1] << "," << mom[2] << "],\"l\":" << length << ",\"nh\":" << nhits << ",\"q\":" << sign << "}";
3684  if (i != (ilen - 1))
3685  {
3686  ofs << ",";
3687  }
3688  else
3689  {
3690  ofs << "\n";
3691  }
3692  }
3693 
3694  ofs << "] }";
3695  return ofs.str();
3696  }
3697 
3698 } /* namespace kdfinder */
3699 
3700 #endif /* KDFINDER_H_ */