MpdHelix.cxx 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597
  1. /***************************************************************************
  2. * Armen Kechechyan, September 2009
  3. ***************************************************************************
  4. * Description:
  5. * Parametrization of a helix
  6. **************************************************************************/
  7. #if !defined(NO_NUMERIC_LIMITS)
  8. # include <limits>
  9. # if !defined(NO_NAMESPACES)
  10. using std::numeric_limits;
  11. # endif
  12. #endif
  13. #define FOR_HELIX
  14. #include "CLHEP/Units/PhysicalConstants.h"
  15. #include "CLHEP/Units/SystemOfUnits.h"
  16. using namespace CLHEP;
  17. #include "MpdHelix.h"
  18. #include <TMath.h>
  19. #ifdef __ROOT__
  20. ClassImpT(MpdHelix,double);
  21. #endif
  22. const double MpdHelix::NoSolution = 3.e+33;
  23. MpdHelix::MpdHelix():
  24. mSingularity(false),
  25. mOrigin(),
  26. mDipAngle(0),
  27. mCurvature(0),
  28. mPhase(0),
  29. mH(0),
  30. mCosDipAngle(0),
  31. mSinDipAngle(0),
  32. mCosPhase(0),
  33. mSinPhase(0)
  34. { /*noop*/ }
  35. MpdHelix::MpdHelix(double c, double d, double Phase,
  36. const TVector3 o, int hh)
  37. {
  38. setParameters(c, d, Phase, o, hh);
  39. }
  40. MpdHelix::MpdHelix(TVector3 mom, TVector3 o, Double_t charge,
  41. Double_t Bz) {
  42. Double_t temp_h = -TMath::Sign(1.,charge*Bz);
  43. Double_t dip_angle = TMath::ATan2(mom.Pz(),mom.Pt());
  44. Double_t curv = TMath::Abs(charge*Bz*0.299792458/mom.Pt())*0.01;
  45. Double_t Phase = mom.Phi()-temp_h*TMath::PiOver2();
  46. setParameters(curv,dip_angle,Phase,o,temp_h);
  47. }
  48. MpdHelix::~MpdHelix() { /* noop */ };
  49. void MpdHelix::setParameters(double c, double dip, double Phase,
  50. const TVector3 o, int hh)
  51. {
  52. //
  53. // The order in which the parameters are set is important
  54. // since setCurvature might have to adjust the others.
  55. //
  56. mH = (hh>=0) ? 1 : -1; // Default is: positive particle
  57. // positive field
  58. mOrigin = o;
  59. setDipAngle(dip);
  60. setPhase(Phase);
  61. //
  62. // Check for singularity and correct for negative curvature.
  63. // May change mH and mPhase. Must therefore be set last.
  64. //
  65. setCurvature(c);
  66. //
  67. // For the case B=0, h is ill defined. In the following we
  68. // always assume h = +1. Since phase = psi - h * pi/2
  69. // we have to correct the phase in case h = -1.
  70. // This assumes that the user uses the same h for phase
  71. // as the one he passed to the constructor.
  72. //
  73. if (mSingularity && mH == -1) {
  74. mH = +1;
  75. setPhase(mPhase-M_PI);
  76. }
  77. }
  78. void MpdHelix::setCurvature(double val)
  79. {
  80. if (val < 0) {
  81. mCurvature = -val;
  82. mH = -mH;
  83. setPhase(mPhase+M_PI);
  84. }
  85. else
  86. mCurvature = val;
  87. #ifndef NO_NUMERIC_LIMITS
  88. if (fabs(mCurvature) <= numeric_limits<double>::epsilon())
  89. #else
  90. if (fabs(mCurvature) <= static_cast<double>(0))
  91. #endif
  92. mSingularity = true; // straight line
  93. else
  94. mSingularity = false; // curved
  95. }
  96. void MpdHelix::setPhase(double val)
  97. {
  98. mPhase = val;
  99. mCosPhase = cos(mPhase);
  100. mSinPhase = sin(mPhase);
  101. if (fabs(mPhase) > M_PI)
  102. mPhase = atan2(mSinPhase, mCosPhase); // force range [-pi,pi]
  103. }
  104. void MpdHelix::setDipAngle(double val)
  105. {
  106. mDipAngle = val;
  107. mCosDipAngle = cos(mDipAngle);
  108. mSinDipAngle = sin(mDipAngle);
  109. }
  110. double MpdHelix::xcenter() const
  111. {
  112. if (mSingularity)
  113. return 0;
  114. else
  115. return mOrigin.X()-mCosPhase/mCurvature;
  116. }
  117. double MpdHelix::ycenter() const
  118. {
  119. if (mSingularity)
  120. return 0;
  121. else
  122. return mOrigin.Y()-mSinPhase/mCurvature;
  123. }
  124. double MpdHelix::fudgePathLength(const TVector3 p) const
  125. {
  126. double s;
  127. double dx = p.X()-mOrigin.X();
  128. double dy = p.Y()-mOrigin.Y();
  129. if (mSingularity) {
  130. s = (dy*mCosPhase - dx*mSinPhase)/mCosDipAngle;
  131. }
  132. else {
  133. s = atan2(dy*mCosPhase - dx*mSinPhase,
  134. 1/mCurvature + dx*mCosPhase+dy*mSinPhase)/
  135. (mH*mCurvature*mCosDipAngle);
  136. }
  137. return s;
  138. }
  139. double MpdHelix::distance(const TVector3 p, bool scanPeriods) const
  140. {
  141. return (this->at(pathLength(p,scanPeriods))-p).Mag();
  142. }
  143. double MpdHelix::pathLength(const TVector3 p, bool scanPeriods) const
  144. {
  145. //
  146. // Returns the path length at the distance of closest
  147. // approach between the helix and point p.
  148. // For the case of B=0 (straight line) the path length
  149. // can be calculated analytically. For B>0 there is
  150. // unfortunately no easy solution to the problem.
  151. // Here we use the Newton method to find the root of the
  152. // referring equation. The 'fudgePathLength' serves
  153. // as a starting value.
  154. //
  155. double s;
  156. double dx = p.X()-mOrigin.X();
  157. double dy = p.Y()-mOrigin.Y();
  158. double dz = p.Z()-mOrigin.Z();
  159. if (mSingularity) {
  160. s = mCosDipAngle*(mCosPhase*dy-mSinPhase*dx) +
  161. mSinDipAngle*dz;
  162. }
  163. else { //
  164. // #ifndef NO_NAMESPACES
  165. {
  166. // using namespace units;
  167. // #endif
  168. // const double MaxPrecisionNeeded = 0.000001; // micrometer;
  169. const double MaxPrecisionNeeded = micrometer;
  170. const int MaxIterations = 100;
  171. //
  172. // The math is taken from Maple with C(expr,optimized) and
  173. // some hand-editing. It is not very nice but efficient.
  174. //
  175. double t34 = mCurvature*mCosDipAngle*mCosDipAngle;
  176. double t41 = mSinDipAngle*mSinDipAngle;
  177. double t6, t7, t11, t12, t19;
  178. //
  179. // Get a first guess by using the dca in 2D. Since
  180. // in some extreme cases we might be off by n periods
  181. // we add (subtract) periods in case we get any closer.
  182. //
  183. s = fudgePathLength(p);
  184. if (scanPeriods) {
  185. double ds = period();
  186. int j, jmin = 0;
  187. double d, dmin = (at(s) - p).Mag();
  188. for(j=1; j<MaxIterations; j++) {
  189. if ((d = (at(s+j*ds) - p).Mag()) < dmin) {
  190. dmin = d;
  191. jmin = j;
  192. }
  193. else
  194. break;
  195. }
  196. for(j=-1; -j<MaxIterations; j--) {
  197. if ((d = (at(s+j*ds) - p).Mag()) < dmin) {
  198. dmin = d;
  199. jmin = j;
  200. }
  201. else
  202. break;
  203. }
  204. if (jmin) s += jmin*ds;
  205. }
  206. //
  207. // Newtons method:
  208. // Stops after MaxIterations iterations or if the required
  209. // precision is obtained. Whatever comes first.
  210. //
  211. double sOld = s;
  212. for (int i=0; i<MaxIterations; i++) {
  213. t6 = mPhase+s*mH*mCurvature*mCosDipAngle;
  214. t7 = cos(t6);
  215. t11 = dx-(1/mCurvature)*(t7-mCosPhase);
  216. t12 = sin(t6);
  217. t19 = dy-(1/mCurvature)*(t12-mSinPhase);
  218. s -= (t11*t12*mH*mCosDipAngle-t19*t7*mH*mCosDipAngle -
  219. (dz-s*mSinDipAngle)*mSinDipAngle)/
  220. (t12*t12*mCosDipAngle*mCosDipAngle+t11*t7*t34 +
  221. t7*t7*mCosDipAngle*mCosDipAngle +
  222. t19*t12*t34+t41);
  223. if (fabs(sOld-s) < MaxPrecisionNeeded) break;
  224. sOld = s;
  225. }
  226. //#ifndef NO_NAMESPACES
  227. }
  228. //#endif
  229. }
  230. return s;
  231. }
  232. double MpdHelix::period() const
  233. {
  234. if (mSingularity)
  235. #ifndef NO_NUMERIC_LIMITS
  236. return numeric_limits<double>::max();
  237. #else
  238. return DBL_MAX;
  239. #endif
  240. else
  241. return fabs(2*M_PI/(mH*mCurvature*mCosDipAngle));
  242. }
  243. pair<double, double> MpdHelix::pathLength(double r) const
  244. {
  245. pair<double,double> value;
  246. pair<double,double> VALUE(999999999.,999999999.);
  247. //
  248. // The math is taken from Maple with C(expr,optimized) and
  249. // some hand-editing. It is not very nice but efficient.
  250. // 'first' is the smallest of the two solutions (may be negative)
  251. // 'second' is the other.
  252. //
  253. if (mSingularity) {
  254. double t1 = mCosDipAngle*(mOrigin.X()*mSinPhase-mOrigin.Y()*mCosPhase);
  255. double t12 = mOrigin.Y()*mOrigin.Y();
  256. double t13 = mCosPhase*mCosPhase;
  257. double t15 = r*r;
  258. double t16 = mOrigin.X()*mOrigin.X();
  259. double t20 = -mCosDipAngle*mCosDipAngle*(2.0*mOrigin.X()*mSinPhase*mOrigin.Y()*mCosPhase +
  260. t12-t12*t13-t15+t13*t16);
  261. if (t20<0.) return VALUE;
  262. t20 = ::sqrt(t20);
  263. value.first = (t1-t20)/(mCosDipAngle*mCosDipAngle);
  264. value.second = (t1+t20)/(mCosDipAngle*mCosDipAngle);
  265. }
  266. else {
  267. double t1 = mOrigin.Y()*mCurvature;
  268. double t2 = mSinPhase;
  269. double t3 = mCurvature*mCurvature;
  270. double t4 = mOrigin.Y()*t2;
  271. double t5 = mCosPhase;
  272. double t6 = mOrigin.X()*t5;
  273. double t8 = mOrigin.X()*mOrigin.X();
  274. double t11 = mOrigin.Y()*mOrigin.Y();
  275. double t14 = r*r;
  276. double t15 = t14*mCurvature;
  277. double t17 = t8*t8;
  278. double t19 = t11*t11;
  279. double t21 = t11*t3;
  280. double t23 = t5*t5;
  281. double t32 = t14*t14;
  282. double t35 = t14*t3;
  283. double t38 = 8.0*t4*t6 - 4.0*t1*t2*t8 - 4.0*t11*mCurvature*t6 +
  284. 4.0*t15*t6 + t17*t3 + t19*t3 + 2.0*t21*t8 + 4.0*t8*t23 -
  285. 4.0*t8*mOrigin.X()*mCurvature*t5 - 4.0*t11*t23 -
  286. 4.0*t11*mOrigin.Y()*mCurvature*t2 + 4.0*t11 - 4.0*t14 +
  287. t32*t3 + 4.0*t15*t4 - 2.0*t35*t11 - 2.0*t35*t8;
  288. double t40 = (-t3*t38);
  289. if (t40<0.) return VALUE;
  290. t40 = ::sqrt(t40);
  291. double t43 = mOrigin.X()*mCurvature;
  292. double t45 = 2.0*t5 - t35 + t21 + 2.0 - 2.0*t1*t2 -2.0*t43 - 2.0*t43*t5 + t8*t3;
  293. double t46 = mH*mCosDipAngle*mCurvature;
  294. value.first = (-mPhase + 2.0*atan((-2.0*t1 + 2.0*t2 + t40)/t45))/t46;
  295. value.second = -(mPhase + 2.0*atan((2.0*t1 - 2.0*t2 + t40)/t45))/t46;
  296. //
  297. // Solution can be off by +/- one period, select smallest
  298. //
  299. double p = period();
  300. if (!std::isnan(value.first)) {
  301. if (fabs(value.first-p) < fabs(value.first)) value.first = value.first-p;
  302. else if (fabs(value.first+p) < fabs(value.first)) value.first = value.first+p;
  303. }
  304. if (!std::isnan(value.second)) {
  305. if (fabs(value.second-p) < fabs(value.second)) value.second = value.second-p;
  306. else if (fabs(value.second+p) < fabs(value.second)) value.second = value.second+p;
  307. }
  308. }
  309. if (value.first > value.second)
  310. swap(value.first,value.second);
  311. return(value);
  312. }
  313. pair<double, double> MpdHelix::pathLength(double r, double xx, double yy)
  314. {
  315. double x0 = mOrigin.X();
  316. double y0 = mOrigin.Y();
  317. mOrigin.SetX(x0-xx);
  318. mOrigin.SetY(y0-yy);
  319. pair<double, double> result = this->pathLength(r);
  320. mOrigin.SetX(x0);
  321. mOrigin.SetY(y0);
  322. return result;
  323. }
  324. double MpdHelix::pathLength(const TVector3 r,
  325. const TVector3 n) const
  326. {
  327. //
  328. // Vector 'r' defines the position of the center and
  329. // vector 'n' the normal vector of the plane.
  330. // For a straight line there is a simple analytical
  331. // solution. For curvatures > 0 the root is determined
  332. // by Newton method. In case no valid s can be found
  333. // the max. largest value for s is returned.
  334. //
  335. double s;
  336. if (mSingularity) {
  337. double t = n.Z()*mSinDipAngle +
  338. n.Y()*mCosDipAngle*mCosPhase -
  339. n.X()*mCosDipAngle*mSinPhase;
  340. if (t == 0)
  341. s = NoSolution;
  342. else
  343. s = ((r - mOrigin)*n)/t;
  344. }
  345. else {
  346. // const double MaxPrecisionNeeded = 0.000001; // micrometer;
  347. const double MaxPrecisionNeeded = micrometer;
  348. const int MaxIterations = 20;
  349. double A = mCurvature*((mOrigin - r)*n) -
  350. n.X()*mCosPhase -
  351. n.Y()*mSinPhase;
  352. double t = mH*mCurvature*mCosDipAngle;
  353. double u = n.Z()*mCurvature*mSinDipAngle;
  354. double a, f, fp;
  355. double sOld = s = 0;
  356. double shiftOld = 0;
  357. double shift;
  358. // (cos(angMax)-1)/angMax = 0.1
  359. const double angMax = 0.21;
  360. double deltas = fabs(angMax/(mCurvature*mCosDipAngle));
  361. // dampingFactor = exp(-0.5);
  362. double dampingFactor = 0.60653;
  363. int i;
  364. for (i=0; i<MaxIterations; i++) {
  365. a = t*s+mPhase;
  366. double sina = sin(a);
  367. double cosa = cos(a);
  368. f = A +
  369. n.X()*cosa +
  370. n.Y()*sina +
  371. u*s;
  372. fp = -n.X()*sina*t +
  373. n.Y()*cosa*t +
  374. u;
  375. if ( fabs(fp)*deltas <= fabs(f) ) { //too big step
  376. int sgn = 1;
  377. if (fp<0.) sgn = -sgn;
  378. if (f <0.) sgn = -sgn;
  379. shift = sgn*deltas;
  380. if (shift<0) shift*=0.9; // don't get stuck shifting +/-deltas
  381. } else {
  382. shift = f/fp;
  383. }
  384. s -= shift;
  385. shiftOld = shift;
  386. if (fabs(sOld-s) < MaxPrecisionNeeded) break;
  387. sOld = s;
  388. }
  389. if (i == MaxIterations) return NoSolution;
  390. }
  391. return s;
  392. }
  393. pair<double, double>
  394. MpdHelix::pathLengths(const MpdHelix& hh) const
  395. {
  396. //
  397. // Cannot handle case where one is a helix
  398. // and the other one is a straight line.
  399. //
  400. if (mSingularity != hh.mSingularity)
  401. return pair<double, double>(NoSolution, NoSolution);
  402. double s1, s2;
  403. if (mSingularity) {
  404. //
  405. // Analytic solution
  406. //
  407. TVector3 dv = hh.mOrigin - mOrigin;
  408. TVector3 a(-mCosDipAngle*mSinPhase,
  409. mCosDipAngle*mCosPhase,
  410. mSinDipAngle);
  411. TVector3 b(-hh.mCosDipAngle*hh.mSinPhase,
  412. hh.mCosDipAngle*hh.mCosPhase,
  413. hh.mSinDipAngle);
  414. double ab = a*b;
  415. double g = dv*a;
  416. double k = dv*b;
  417. s2 = (k-ab*g)/(ab*ab-1.);
  418. s1 = g+s2*ab;
  419. return pair<double, double>(s1, s2);
  420. }
  421. else {
  422. //
  423. // First step: get dca in the xy-plane as start value
  424. //
  425. double dx = hh.xcenter() - xcenter();
  426. double dy = hh.ycenter() - ycenter();
  427. double dd = ::sqrt(dx*dx + dy*dy);
  428. double r1 = 1/curvature();
  429. double r2 = 1/hh.curvature();
  430. double cosAlpha = (r1*r1 + dd*dd - r2*r2)/(2*r1*dd);
  431. double s;
  432. double xx, yy;
  433. if (fabs(cosAlpha) < 1) { // two solutions
  434. double sinAlpha = sin(acos(cosAlpha));
  435. xx = xcenter() + r1*(cosAlpha*dx - sinAlpha*dy)/dd;
  436. yy = ycenter() + r1*(sinAlpha*dx + cosAlpha*dy)/dd;
  437. s = pathLength(xx, yy);
  438. xx = xcenter() + r1*(cosAlpha*dx + sinAlpha*dy)/dd;
  439. yy = ycenter() + r1*(cosAlpha*dy - sinAlpha*dx)/dd;
  440. double a = pathLength(xx, yy);
  441. if (hh.distance(at(a)) < hh.distance(at(s))) s = a;
  442. }
  443. else { // no intersection (or exactly one)
  444. int rsign = ((r2-r1) > dd ? -1 : 1); // set -1 when *this* helix is
  445. // completely contained in the other
  446. xx = xcenter() + rsign*r1*dx/dd;
  447. yy = ycenter() + rsign*r1*dy/dd;
  448. s = pathLength(xx, yy);
  449. }
  450. //
  451. // Second step: scan in decreasing intervals around seed 's'
  452. //
  453. // const double MinStepSize = 0.001; // 10*micrometer;
  454. // const double MinRange = 10.; // 10*centimeter;
  455. const double MinStepSize = 10*micrometer;
  456. const double MinRange = 10*centimeter;
  457. double dmin = hh.distance(at(s));
  458. double range = max(2*dmin, MinRange);
  459. double ds = range/10;
  460. double slast=-999999, ss, d;
  461. s1 = s - range/2.;
  462. s2 = s + range/2.;
  463. double d1 = 0, d2 = 0, d3 = 0, s11 = 0, s12 = 0, s13 = 0;
  464. while (ds > MinStepSize) {
  465. int i = 0;
  466. for (ss=s1; ss<s2+ds; ss+=ds) {
  467. d = hh.distance(at(ss));
  468. i++;
  469. if (i > 10){
  470. if (i == 11) {d1 = d; s11 = s;}
  471. if (i == 12) {d2 = d; s12 = s;}
  472. if (i == 13) {d3 = d; s13 = s;}
  473. if (i == 14 && d1 == d2 && d2 == d3 && s11 == s12 && s12 == s13) {
  474. printf("REPEATING VALUES IN MpdHelix\n");
  475. return pair<double, double>(s, hh.pathLength(at(s)));
  476. }
  477. }
  478. if (d < dmin) {
  479. dmin = d;
  480. s = ss;
  481. }
  482. slast = ss;
  483. }
  484. //
  485. // In the rare cases where the minimum is at the
  486. // the border of the current range we shift the range
  487. // and start all over, i.e we do not decrease 'ds'.
  488. // Else we decrease the search intervall around the
  489. // current minimum and redo the scan in smaller steps.
  490. //
  491. if (s == s1) {
  492. d = 0.8*(s2-s1);
  493. s1 -= d;
  494. s2 -= d;
  495. }
  496. else if (s == slast) {
  497. d = 0.8*(s2-s1);
  498. s1 += d;
  499. s2 += d;
  500. }
  501. else {
  502. s1 = s-ds;
  503. s2 = s+ds;
  504. ds /= 10;
  505. }
  506. }
  507. return pair<double, double>(s, hh.pathLength(at(s)));
  508. }
  509. }
  510. void MpdHelix::moveOrigin(double s)
  511. {
  512. if (mSingularity)
  513. mOrigin = at(s);
  514. else {
  515. TVector3 newOrigin = at(s);
  516. double newPhase = atan2(newOrigin.Y() - ycenter(),
  517. newOrigin.X() - xcenter());
  518. mOrigin = newOrigin;
  519. setPhase(newPhase);
  520. }
  521. }
  522. int operator== (const MpdHelix& a, const MpdHelix& b)
  523. {
  524. //
  525. // Checks for numerical identity only !
  526. //
  527. return (a.origin() == b.origin() &&
  528. a.dipAngle() == b.dipAngle() &&
  529. a.curvature() == b.curvature() &&
  530. a.phase() == b.phase() &&
  531. a.h() == b.h());
  532. }
  533. int operator!= (const MpdHelix& a, const MpdHelix& b) {return !(a == b);}
  534. // ostream& operator<<(ostream& os, const MpdHelix& h)
  535. // {
  536. // return os << '('
  537. // << "curvature = " << h.curvature() << ", "
  538. // << "dip angle = " << h.dipAngle() << ", "
  539. // << "phase = " << h.phase() << ", "
  540. // << "h = " << h.h() << ", "
  541. // << "origin = " << h.origin() << ')';
  542. // }
  543. //