PixelTrackFitting/src/CircleFromThreePoints.cc

0001 #include "RecoTracker/PixelTrackFitting/interface/CircleFromThreePoints.h"
0002
0003 CircleFromThreePoints::CircleFromThreePoints(const GlobalPoint& inner,
0004                                              const GlobalPoint& mid,
0005                                              const GlobalPoint& outer,
0006                                              double precision) {
0007   // move to frame where inner.x() == inner.y() == 0;
0008   Vector2D b(mid.x() - inner.x(), mid.y() - inner.y());
0009   Vector2D c(outer.x() - inner.x(), outer.y() - inner.y());
0010   init(b, c, Vector2D(inner.x(), inner.y()), precision);
0011 }
0012
0013 void CircleFromThreePoints::init(const Vector2D& b, const Vector2D& c, const Vector2D& offset, double precision) {
0014   double b2 = b.mag2();
0015   double c2 = c.mag2();
0016
0017   double oX(0), oY(0);
0018   bool solved = false;
0019   if (fabs(b.x()) > fabs(b.y())) {  // solve for y first
0020     double k = c.x() / b.x();
0021     double div = 2 * (k * b.y() - c.y());
0022     if (fabs(div) < precision)
0023       theCurvature = 0;  // if the 3 points lie on a line
0024     else {
0025       oY = (k * b2 - c2) / div;
0026       oX = b2 / (2 * b.x()) - b.y() / b.x() * oY;
0027       solved = true;
0028     }
0029   } else {  // solve for x first
0030     double k = c.y() / b.y();
0031     double div = 2 * (k * b.x() - c.x());
0032     if (fabs(div) < precision)
0033       theCurvature = 0;  // if the 3 points lie on a line
0034     else {
0035       oX = (k * b2 - c2) / div;
0036       oY = b2 / (2 * b.y()) - b.x() / b.y() * oX;
0037       solved = true;
0038     }
0039   }
0040   if (solved) {
0041     theCurvature = 1. / sqrt(oX * oX + oY * oY);
0042     double xC = oX + offset.x();
0043     double yC = oY + offset.y();
0044     theCenter = Vector2D(xC, yC);
0045     //    thePhi = acos(xC/sqrt(xC*xC + yC*yC));
0046
0047     //    if (xC<0.) thePhi = thePhi - PI;
0048     //    cout << setiosflags(ios::showpoint | ios::fixed);
0049     //
0050     //    cout << "CircleFromThreePoints::init curv = " << theCurvature << endl;
0051     //    cout << "CircleFromThreePoints::init center prime = " << oX << " " << oY << endl;
0052     //    cout << "CircleFromThreePoints::init offset = " << offset.x() << " " << offset.y() << endl;
0053     //    cout << "CircleFromThreePoints::init center = " << theCenter.x()<< " " << theCenter.y() << endl;
0054     //
0055     //    float d = sqrt(theCenter.x()*theCenter.x()+theCenter.y()*theCenter.y());
0056     //    cout << "CircleFromThreePoints::init dfloat = " << setw(10) << setprecision(5) << d << endl;
0057     //    cout << "CircleFromThreePoints::init radius = " << 1/theCurvature << endl;
0058   }
0059 }