Belle II Software  release-05-01-25
Lpar.cc
1 //-----------------------------------------------------------------------------
2 // $Id$
3 //-----------------------------------------------------------------------------
4 // Filename : Lpar.cc
5 // Section : TRG CDC
6 // Owner : Yoshihito Iwasaki
7 // Email : yoshihito.iwasaki@kek.jp
8 //-----------------------------------------------------------------------------
9 // Description :
10 //-----------------------------------------------------------------------------
11 // $Log$
12 //-----------------------------------------------------------------------------
13 
14 #include <iostream>
15 #include <cmath>
16 #include "CLHEP/Vector/Sqr.h"
17 #include "trg/cdc/Lpar.h"
18 
19 namespace Belle2 {
25 //
26 // constants, enums and typedefs
27 //
28 
29 //
30 // static data member definitions
31 //
32 
33  const double TRGCDCLpar::BELLE_ALPHA(222.37606);
34 
35 //
36 // constructors and destructor
37 //
38 // TRGCDCLpar::TRGCDCLpar(double x1, double y1, double x2, double y2, double x3, double y3) {
39 // circle(x1, y1, x2, y2, x3, y3);
40 // }
42  {
43  m_cu = l.kappa();
44  if (l.alpha() != 0 && l.beta() != 0)
45  m_fi = atan2(l.alpha(), -l.beta());
46  else m_fi = 0;
47  if (m_fi < 0) m_fi += 2 * M_PI;
48  m_da = 2 * l.gamma() / (1 + sqrt(1 + 4 * l.kappa() * l.gamma()));
49  m_cfi = cos(m_fi);
50  m_sfi = sin(m_fi);
51  }
52 
53 // TRGCDCLpar::TRGCDCLpar( const TRGCDCLpar& )
54 // {
55 // }
56 
58  {
59  }
60 
61 //
62 // assignment operators
63 //
64 // const TRGCDCLpar& TRGCDCLpar::operator=( const TRGCDCLpar& )
65 // {
66 // }
67 
68 //
69 // comparison operators
70 //
71 // bool TRGCDCLpar::operator==( const TRGCDCLpar& ) const
72 // {
73 // }
74 
75 // bool TRGCDCLpar::operator!=( const TRGCDCLpar& ) const
76 // {
77 // }
78 
79 //
80 // member functions
81 //
82  void TRGCDCLpar::circle(double x1, double y1, double x2, double y2,
83  double x3, double y3)
84  {
85  double delta = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
86  if (delta == 0) {
87  //
88  // three points are on a line.
89  //
90  m_kappa = 0;
91  double r12sq = (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2);
92  if (r12sq > 0) {
93  double r12 = sqrt(r12sq);
94  m_beta = -(x1 - x2) / r12;
95  m_alpha = (y1 - y2) / r12;
96  m_gamma = - (m_alpha * x1 + m_beta * y1);
97  } else {
98  double r13sq = (x1 - x3) * (x1 - x3) + (y1 - y3) * (y1 - y3);
99  if (r13sq > 0) {
100  double r13 = sqrt(r13sq);
101  m_beta = -(x1 - x3) / r13;
102  m_alpha = (y1 - y3) / r13;
103  m_gamma = - (m_alpha * x3 + m_beta * y3);
104  } else {
105  double r23sq = (x2 - x3) * (x2 - x3) + (y2 - y3) * (y2 - y3);
106  if (r23sq > 0) {
107  double r23 = sqrt(r23sq);
108  m_beta = -(x2 - x3) / r23;
109  m_alpha = (y2 - y3) / r23;
110  m_gamma = - (m_alpha * x3 + m_beta * y3);
111  } else {
112  m_alpha = 1;
113  m_beta = 0;
114  m_gamma = 0;
115  }
116  }
117  }
118  } else {
119  double r1sq = x1 * x1 + y1 * y1;
120  double r2sq = x2 * x2 + y2 * y2;
121  double r3sq = x3 * x3 + y3 * y3;
122  double a = 0.5 * ((y1 - y3) * (r1sq - r2sq) - (y1 - y2) * (r1sq - r3sq)) / delta;
123  double b = 0.5 * (- (x1 - x3) * (r1sq - r2sq) + (x1 - x2) * (r1sq - r3sq)) / delta;
124  double csq = (x1 - a) * (x1 - a) + (y1 - b) * (y1 - b);
125  double c = sqrt(csq);
126 //cnv double csq2 = (x2-a)*(x2-a) + (y2-b)*(y2-b);
127 //cnv double csq3 = (x3-a)*(x3-a) + (y3-b)*(y3-b);
128  m_kappa = 1 / (2 * c);
129  m_alpha = - 2 * a * m_kappa;
130  m_beta = - 2 * b * m_kappa;
131  m_gamma = (a * a + b * b - c * c) * m_kappa;
132  }
133  }
134 
135  CLHEP::HepMatrix TRGCDCLpar::dldc() const
136 #ifdef BELLE_OPTIMIZED_RETURN
137  return vret(3, 4);
138  {
139 #else
140  {
141  CLHEP::HepMatrix vret(3, 4);
142 #endif
143  Cpar cp(*this);
144  double xi = cp.xi();
145  double s = cp.sfi();
146  double c = cp.cfi();
147  vret(1, 1) = 2 * cp.da() * s;
148  vret(1, 2) = -2 * cp.da() * c;
149  vret(1, 3) = cp.da() * cp.da();
150  vret(1, 4) = 1;
151  vret(2, 1) = xi * c;
152  vret(2, 2) = xi * s;
153  vret(2, 3) = 0;
154  vret(2, 4) = 0;
155  vret(3, 1) = 2 * cp.cu() * s;
156  vret(3, 2) = -2 * cp.cu() * c;
157  vret(3, 3) = xi;
158  vret(3, 4) = 0;
159  return vret;
160  }
161 
162  bool TRGCDCLpar::xy(double r, double& x, double& y, int dir) const
163  {
164  double t_kr2g = kr2g(r);
165  double t_xi2 = xi2();
166  double ro = r * r * t_xi2 - t_kr2g * t_kr2g;
167  if (ro < 0) return false;
168  double rs = sqrt(ro);
169  if (dir == 0) {
170  x = (- m_alpha * t_kr2g - m_beta * rs) / t_xi2;
171  y = (- m_beta * t_kr2g + m_alpha * rs) / t_xi2;
172  } else {
173  x = (- m_alpha * t_kr2g + m_beta * rs) / t_xi2;
174  y = (- m_beta * t_kr2g - m_alpha * rs) / t_xi2;
175  }
176  return true;
177  }
178 
179  double TRGCDCLpar::x(double r) const
180  {
181  double t_x, t_y;
182  xy(r, t_x, t_y);
183  return t_x;
184  }
185 
186  double TRGCDCLpar::y(double r) const
187  {
188  double t_x, t_y;
189  xy(r, t_x, t_y);
190  return t_y;
191  }
192 
193  double TRGCDCLpar::phi(double r, int dir) const
194  {
195  double x, y;
196  if (!xy(r, x, y, dir)) return -1;
197  double p = atan2(y, x);
198  if (p < 0) p += (2 * M_PI);
199  return p;
200  }
201 
202  void TRGCDCLpar::xhyh(double x, double y, double& xh, double& yh) const
203  {
204  double ddm = dr(x, y);
205  if (ddm == 0) {
206  xh = x;
207  yh = y;
208  return;
209  }
210  double kdp1 = 1 + 2 * kappa() * ddm;
211  xh = x - ddm * (2 * kappa() * x + alpha()) / kdp1;
212  yh = y - ddm * (2 * kappa() * y + beta()) / kdp1;
213  }
214 
215  double TRGCDCLpar::s(double x, double y) const
216  {
217  double xh, yh, xx, yy;
218  xhyh(x, y, xh, yh);
219  double fk = fabs(kappa());
220  if (fk == 0) return 0;
221  yy = 2 * fk * (alpha() * yh - beta() * xh);
222  xx = 2 * kappa() * (alpha() * xh + beta() * yh) + xi2();
223  double sp = atan2(yy, xx);
224  if (sp < 0) sp += (2 * M_PI);
225  return sp / 2 / fk;
226  }
227 
228  double TRGCDCLpar::s(double r, int dir) const
229  {
230  double d0 = da();
231  if (fabs(r) < fabs(d0)) return -1;
232  double b = fabs(kappa()) * sqrt((r * r - d0 * d0) / (1 + 2 * kappa() * d0));
233  if (fabs(b) > 1) return -1;
234  if (dir == 0)return asin(b) / fabs(kappa());
235  return (M_PI - asin(b)) / fabs(kappa());
236  }
237 
238  CLHEP::HepVector TRGCDCLpar::center() const
239 #ifdef BELLE_OPTIMIZED_RETURN
240  return v(3);
241  {
242 #else
243  {
244  CLHEP::HepVector v(3);
245 #endif
246  v(1) = xc();
247  v(2) = yc();
248  v(3) = 0;
249  return (v);
250  }
251 
253  int intersect(const TRGCDCLpar& lp1, const TRGCDCLpar& lp2, CLHEP::HepVector& v1, CLHEP::HepVector& v2)
254  {
255  CLHEP::HepVector cen1(lp1.center());
256  CLHEP::HepVector cen2(lp2.center());
257  double dx = cen1(1) - cen2(1);
258  double dy = cen1(2) - cen2(2);
259  double dc = sqrt(dx * dx + dy * dy);
260  if (dc < fabs(0.5 / lp1.kappa()) + fabs(0.5 / lp2.kappa())) {
261  double a1 = sqr(lp1.alpha()) + sqr(lp1.beta());
262  double a2 = sqr(lp2.alpha()) + sqr(lp2.beta());
263  double a3 = lp1.alpha() * lp2.alpha() + lp1.beta() * lp2.beta();
264  double det = lp1.alpha() * lp2.beta() - lp1.beta() * lp2.alpha();
265  if (fabs(det) > 1e-12) {
266  double c1 = a2 * sqr(lp1.kappa()) + a1 * sqr(lp2.kappa()) -
267  2.0 * a3 * lp1.kappa() * lp2.kappa();
268  if (c1 != 0) {
269  double cinv = 1.0 / c1;
270  double c2 = sqr(a3) - 0.5 * (a1 + a2) - 2.0 * a3 *
271  (lp1.gamma() * lp2.kappa() + lp2.gamma() * lp1.kappa());
272  double c3 = a2 * sqr(lp1.gamma()) + a1 * sqr(lp2.gamma()) -
273  2.0 * a3 * lp1.gamma() * lp2.gamma();
274  double root = sqr(c2) - 4.0 * c1 * c3;
275  if (root >= 0) {
276  root = sqrt(root);
277  double rad2[2];
278  rad2[0] = 0.5 * cinv * (-c2 - root);
279  rad2[1] = 0.5 * cinv * (-c2 + root);
280  double ab1 = -(lp2.beta() * lp1.gamma() - lp1.beta() * lp2.gamma());
281  double ab2 = (lp2.alpha() * lp1.gamma() - lp1.alpha() * lp2.gamma());
282  double ac1 = -(lp2.beta() * lp1.kappa() - lp1.beta() * lp2.kappa());
283  double ac2 = (lp2.alpha() * lp1.kappa() - lp1.alpha() * lp2.kappa());
284  double dinv = 1.0 / det;
285  v1(1) = dinv * (ab1 + ac1 * rad2[0]);
286  v1(2) = dinv * (ab2 + ac2 * rad2[0]);
287  v1(3) = 0;
288  v2(1) = dinv * (ab1 + ac1 * rad2[1]);
289  v2(2) = dinv * (ab2 + ac2 * rad2[1]);
290  v2(3) = 0;
291 //cnv double d1 = lp1.d(v1(1),v1(2));
292 //cnv double d2 = lp2.d(v1(1),v1(2));
293 //cnv double d3 = lp1.d(v2(1),v2(2));
294 //cnv double d4 = lp2.d(v2(1),v2(2));
295 //cnv double r = sqrt(rad2[0]);
296  TRGCDCLpar::Cpar cp1(lp1);
297  TRGCDCLpar::Cpar cp2(lp2);
298 // for(int j=0;j<2;j++) {
299 //jb double s1,s2;
300 //jb if(j==0) {
301 //jb s1 = lp1.s(v1(1),v1(2));
302 //jb s2 = lp2.s(v1(1),v1(2));
303 //jb } else {
304 //jb s1 = lp1.s(v2(1),v2(2));
305 //jb s2 = lp2.s(v2(1),v2(2));
306 //jb }
307 //cnv double phi1 = cp1.fi() + 2 * cp1.cu() * s1;
308 //cnv double phi2 = cp2.fi() + 2 * cp2.cu() * s2;
309 // double f = (1 + 2 * cp1.cu() * cp1.da()) *
310 // (1 + 2 * cp2.cu() * cp2.da()) * cos(cp1.fi()-cp2.fi());
311 // f -= 2 * (lp1.gamma() * lp2.kappa() + lp2.gamma() * lp1.kappa());
312 //cnv double cosphi12 = f;
313 // }
314  return 2;
315  }
316  }
317  }
318  }
319  return 0;
320  }
321 
322 //
323 // const member functions
324 //
325 
326 //
327 // static member functions
328 //
329 
331  std::ostream& operator<<(std::ostream& o, TRGCDCLpar& s)
332  {
333  return o << " al=" << s.m_alpha << " be=" << s.m_beta
334  << " ka=" << s.m_kappa << " ga=" << s.m_gamma;
335  }
336 
338 } // namespace Belle2
339 
Belle2::TRGCDCLpar::Cpar::Cpar
Cpar(const TRGCDCLpar &)
constructor of Cpar class
Definition: Lpar.cc:41
Belle2::TRGCDCLpar::dr
double dr(double x, double y) const
const member functions
Definition: Lpar.h:264
Belle2::TRGCDCLpar::intersect
friend int intersect(const TRGCDCLpar &, const TRGCDCLpar &, CLHEP::HepVector &, CLHEP::HepVector &)
intersection
Definition: Lpar.cc:253
Belle2::TRGCDCLpar::xy
bool xy(double, double &, double &, int dir=0) const
private const member functions
Definition: Lpar.cc:162
Belle2::TRGCDCLpar::beta
double beta() const
private const member functions
Definition: Lpar.h:142
Belle2::TRGCDCLpar::center
CLHEP::HepVector center() const
const member functions
Definition: Lpar.cc:238
Belle2::TRGCDCLpar::y
double y(double r) const
private const member functions
Definition: Lpar.cc:186
Belle2::TRGCDCLpar::kr2g
double kr2g(double r) const
private const member functions
Definition: Lpar.h:152
Belle2::TRGCDCLpar::circle
void circle(double x1, double y1, double x2, double y2, double x3, double y3)
circle
Definition: Lpar.cc:82
Belle2::TRGCDCLpar::Cpar::m_cfi
double m_cfi
parameter of Cpar class
Definition: Lpar.h:121
Belle2::TRGCDCLpar::~TRGCDCLpar
virtual ~TRGCDCLpar()
Destructor.
Definition: Lpar.cc:57
Belle2::TRGCDCLpar::m_alpha
double m_alpha
data members
Definition: Lpar.h:175
Belle2::TRGCDCLpar::operator<<
friend std::ostream & operator<<(std::ostream &o, TRGCDCLpar &)
ostream operator
Definition: Lpar.cc:331
Belle2::TRGCDCLpar::alpha
double alpha() const
private const member functions
Definition: Lpar.h:140
Belle2::TRGCDCLpar::Cpar::m_cu
double m_cu
parameter of Cpar class
Definition: Lpar.h:113
Belle2::TRGCDCLpar
TRGCDCLpar class.
Definition: Lpar.h:36
Belle2::TRGCDCLpar::phi
double phi(double r, int dir=0) const
const member functions
Definition: Lpar.cc:193
Belle2::TRGCDCLpar::x
double x(double r) const
private const member functions
Definition: Lpar.cc:179
Belle2::TRGCDCLpar::BELLE_ALPHA
static const double BELLE_ALPHA
belle alpha
Definition: Lpar.h:184
Belle2::TRGCDCLpar::m_gamma
double m_gamma
data members
Definition: Lpar.h:179
Belle2::TRGCDCLpar::d0
double d0(double x, double y) const
private const member functions
Definition: Lpar.h:251
Belle2::TRGCDCLpar::yc
double yc() const
private const member functions
Definition: Lpar.h:168
Belle2
Abstract base class for different kinds of events.
Definition: MillepedeAlgorithm.h:19
Belle2::TRGCDCLpar::da
double da() const
private const member functions
Definition: Lpar.h:170
Belle2::TRGCDCLpar::gamma
double gamma() const
private const member functions
Definition: Lpar.h:144
Belle2::TRGCDCLpar::xc
double xc() const
private const member functions
Definition: Lpar.h:166
Belle2::TRGCDCLpar::kappa
double kappa() const
const member functions
Definition: Lpar.h:58
Belle2::TRGCDCLpar::Cpar::m_da
double m_da
parameter of Cpar class
Definition: Lpar.h:117
Belle2::TRGCDCLpar::Cpar::m_fi
double m_fi
parameter of Cpar class
Definition: Lpar.h:115
Belle2::TRGCDCLpar::xi2
double xi2() const
private const member functions
Definition: Lpar.h:160
Belle2::TRGCDCLpar::Cpar
Private class cpar.
Definition: Lpar.h:95
Belle2::TRGCDCLpar::Cpar::m_sfi
double m_sfi
parameter of Cpar class
Definition: Lpar.h:119
Belle2::TRGCDCLpar::m_kappa
double m_kappa
data members
Definition: Lpar.h:181
Belle2::TRGCDCLpar::dldc
CLHEP::HepMatrix dldc() const
private const member functions
Definition: Lpar.cc:135
Belle2::TRGCDCLpar::m_beta
double m_beta
data members
Definition: Lpar.h:177
Belle2::TRGCDCLpar::s
double s(double x, double y) const
const member functions
Definition: Lpar.cc:215
Belle2::TRGCDCLpar::xhyh
void xhyh(double x, double y, double &xh, double &yh) const
private const member functions
Definition: Lpar.cc:202