44 inline constexpr
T square(
const T&
x ) {
return x*
x; }
62 void CircleFitByTaubin (std::vector<Acts::Vector3D>& clusters,
double &
R,
double &X0,
double &Y0)
66 double Mz,Mxy,Mxx,Myy,Mxz,Myz,Mzz,Cov_xy,Var_z;
67 double A0,A1,A2,
A22,A3,
A33;
69 double DET,Xcenter,Ycenter;
76 for(
auto& global : clusters)
87 Mxx=Myy=Mxy=Mxz=Myz=Mzz=0.;
89 for (
auto& global : clusters)
91 double Xi = global(0) - meanX;
92 double Yi = global(1) - meanY;
93 double Zi = Xi*Xi + Yi*Yi;
112 Cov_xy = Mxx*Myy - Mxy*Mxy;
116 A1 = Var_z*Mz + 4*Cov_xy*Mz - Mxz*Mxz - Myz*Myz;
117 A0 = Mxz*(Mxz*Myy - Myz*Mxy) + Myz*(Myz*Mxx - Mxz*Mxy) - Var_z*Cov_xy;
125 for (x=0.,y=A0,iter=0; iter<IterMAX; iter++)
127 double Dy = A1 + x*(A22 + A33*
x);
128 double xnew = x - y/
Dy;
129 if ((xnew == x)||(!
isfinite(xnew)))
break;
130 double ynew = A0 + xnew*(A1 + xnew*(A2 + xnew*A3));
137 DET = x*x - x*Mz + Cov_xy;
138 Xcenter = (Mxz*(Myy -
x) - Myz*Mxy)/DET/2;
139 Ycenter = (Myz*(Mxx -
x) - Mxz*Mxy)/DET/2;
143 X0 = Xcenter + meanX;
144 Y0 = Ycenter + meanY;
145 R = sqrt(Xcenter*Xcenter + Ycenter*Ycenter + Mz);
149 void line_fit(std::vector<Acts::Vector3D>& clusters,
double &
a,
double &
b)
155 double xsum=0,x2sum=0,ysum=0,xysum=0;
157 for (
auto& global : clusters)
159 const double z = global(2);
160 const double r =
get_r( global(0), global(1) );
168 a=(clusters.size()*xysum-xsum*ysum)/(clusters.size()*x2sum-xsum*xsum);
169 b=(x2sum*ysum-xsum*xysum)/(x2sum*clusters.size()-xsum*xsum);
187 bool circle_circle_intersection(
double r1,
double r2,
double x2,
double y2,
double &xplus,
double &yplus,
double &xminus,
double &yminus)
191 const double a = 1.0 +
square(x2/y2);
192 const double b = - D * x2/
square(y2);
195 if( delta < 0 )
return false;
197 const double sqdelta = std::sqrt( delta );
199 xplus = (-b + sqdelta ) / (2. * a);
200 xminus = (-b - sqdelta ) / (2. * a);
206 yplus = -(2*x2*xplus -
D) / (2.*y2);
207 yminus = -(2*x2*xminus -
D) / (2.*y2);
220 bool circle_line_intersection(
221 double r,
double xc,
double yc,
222 double x0,
double y0,
double nx,
double ny,
223 double &xplus,
double &yplus,
double &xminus,
double &yminus)
232 if( delta < 0 )
return false;
234 const double sqdelta = std::sqrt( delta );
235 yplus = yc + sqdelta;
236 yminus = yc - sqdelta;
241 const double b = -2.*(
square(ny)*xc +
square(nx)*x0 + nx*ny*(y0-yc) );
243 const double delta =
square(b) - 4.*a*
c;
244 if( delta < 0 )
return false;
246 const double sqdelta = std::sqrt( delta );
247 xplus = (-b + sqdelta)/(2.*a);
248 xminus = (-b - sqdelta)/(2.*a);
250 yplus = y0 -(nx/ny)*(xplus-x0);
251 yminus = y0 - (nx/ny)*(xminus-x0);
260 [[maybe_unused]]
inline std::ostream&
operator << (std::ostream& out,
const TVector3& vector )
262 out <<
"( " << vector.x() <<
"," << vector.y() <<
"," << vector.z() <<
")";
277 std::cout << std::endl <<
PHWHERE
288 std::cout <<
PHWHERE <<
"Could not find CYLINDERGEOM_MICROMEGAS_FULL." << std::endl;
295 std::cout <<
PHWHERE <<
"Inconsistent number of micromegas layers." << std::endl;
302 fdrphi =
new TF1(
"fdrphi",
"[0] + [1]*std::abs(x)");
320 _iteration_map = findNode::getClass<TrkrClusterIterationMapv1>(topNode,
"CLUSTER_ITERATION_MAP");
323 std::cerr <<
PHWHERE <<
"Cluster Iteration Map missing, aborting." << std::endl;
344 if(phtrk_iter->first > original_track_map_lastkey)
break;
346 auto tracklet_tpc = phtrk_iter->second;
350 std::cout << std::endl
352 <<
": Processing seed itrack: " << phtrk_iter->first
353 <<
": nhits: " << tracklet_tpc-> size_cluster_keys()
355 <<
": phi: " << tracklet_tpc->get_phi()
360 std::map<unsigned int, TrkrCluster*> outer_clusters;
361 std::vector<TrkrCluster*> clusters;
362 std::vector<Acts::Vector3D> clusGlobPos;
375 if(!tpc_clus)
continue;
377 outer_clusters.insert(std::make_pair(layer, tpc_clus));
378 clusters.push_back(tpc_clus);
384 <<
" TPC cluster in layer " << layer <<
" with position " << tpc_clus->
getLocalX()
385 <<
" " << tpc_clus->
getLocalY() <<
" " <<
" outer_clusters.size() " << outer_clusters.size() << std::endl;
390 if(outer_clusters.size() < 3)
392 if(
Verbosity() > 3) std::cout <<
PHWHERE <<
" -- skip this tpc tracklet, not enough outer clusters " << std::endl;
400 CircleFitByTaubin(clusGlobPos, R, X0, Y0);
401 if(
Verbosity() > 10) std::cout <<
" Fitted circle has R " << R <<
" X0 " << X0 <<
" Y0 " << Y0 << std::endl;
404 if(R < 40.0)
continue;
407 double A = 0;
double B = 0;
408 line_fit(clusGlobPos, A, B);
409 if(
Verbosity() > 10) std::cout <<
" Fitted line has A " << A <<
" B " << B << std::endl;
418 const auto layer_radius = layergeom->
get_radius();
427 if( !circle_circle_intersection( layer_radius, R, X0, Y0, xplus, yplus, xminus, yminus) )
431 std::cout <<
PHWHERE <<
" circle/circle intersection calculation failed, skip this case" << std::endl;
432 std::cout <<
PHWHERE <<
" mm_radius " << layer_radius <<
" fitted R " << R <<
" fitted X0 " << X0 <<
" fitted Y0 " << Y0 << std::endl;
439 const double last_clus_phi = std::atan2(clusGlobPos.back()(1), clusGlobPos.back()(0));
440 double phi_plus = std::atan2(yplus, xplus);
441 double phi_minus = std::atan2(yminus, xminus);
444 double r = layer_radius;
449 double phi =
std::abs(last_clus_phi - phi_plus) <
std::abs(last_clus_phi - phi_minus) ? phi_plus:phi_minus;
453 const TVector3 world_intersection_cylindrical( r*std::cos(phi), r*std::sin(phi), z );
456 int tileid = layergeom->find_tile_cylindrical( world_intersection_cylindrical );
457 if( tileid < 0 )
continue;
460 const auto tile_center_phi = layergeom->get_center_phi( tileid );
461 const double x0 = r*std::cos( tile_center_phi );
462 const double y0 = r*std::sin( tile_center_phi );
464 const double nx = x0;
465 const double ny = y0;
468 if( !circle_line_intersection( R, X0, Y0, x0, y0, nx, ny, xplus, yplus, xminus, yminus) )
471 { std::cout <<
PHWHERE <<
"circle_line_intersection - failed" << std::endl; }
476 phi_plus = std::atan2(yplus, xplus);
477 phi_minus = std::atan2(yminus, xminus);
478 const bool is_plus = (
std::abs(last_clus_phi - phi_plus) <
std::abs(last_clus_phi - phi_minus));
481 const double x = (is_plus ? xplus:xminus);
482 const double y = (is_plus ? yplus:yminus);
490 const TVector3 world_intersection_planar( x, y, z );
493 TVector3 local_intersection_planar = layergeom->get_local_from_world_coords( tileid, world_intersection_planar );
501 local_intersection_planar.SetX( local_intersection_planar.x() -
fdrphi->Eval(z) );
505 const auto segmentation_type = layergeom->get_segmentation_type();
512 for(
auto clusiter = mm_clusrange.first; clusiter != mm_clusrange.second; ++clusiter)
520 const auto& [key, cluster] = *clusiter;
522 const TVector3 world_cluster(glob(0), glob(1), glob(2));
523 const TVector3 local_cluster = layergeom->get_local_from_world_coords( tileid, world_cluster );
527 const double drphi = local_intersection_planar.x() - local_cluster.x();
528 const double dz = local_intersection_planar.z() - local_cluster.z();
533 tracklet_tpc->insert_cluster_key(key);
540 const double mm_clus_rphi =
get_r( glob(0), glob(1) ) * std::atan2( glob(1), glob(0) );
541 const double mm_clus_z = glob(2);
544 const double rphi_proj =
get_r( world_intersection_planar.x(), world_intersection_planar.y() ) * std::atan2( world_intersection_planar.y(), world_intersection_planar.x() );
545 const double z_proj = world_intersection_planar.z();
553 <<
" Try_mms: " << (
int) layer
554 <<
" drphi " << drphi
556 <<
" mm_clus_rphi " << mm_clus_rphi <<
" mm_clus_z " << mm_clus_z
557 <<
" rphi_proj " << rphi_proj <<
" z_proj " << z_proj
567 { tracklet_tpc->identify(); }
576 { std::cout <<
" Final track map size " <<
_track_map->
size() << std::endl; }
592 std::cout <<
" Found CORRECTED_TRKR_CLUSTER node " << std::endl;
597 _cluster_map = findNode::getClass<TrkrClusterContainer>(topNode,
"TRKR_CLUSTER_TRUTH");
600 _cluster_map = findNode::getClass<TrkrClusterContainer>(topNode,
"TRKR_CLUSTER");
605 std:: cerr <<
PHWHERE <<
" ERROR: Can't find node TRKR_CLUSTER" << std::endl;
610 _tGeometry = findNode::getClass<ActsTrackingGeometry>(topNode,
"ActsTrackingGeometry");
613 std::cerr <<
PHWHERE <<
"No acts tracking geometry, can't continue."
618 _surfmaps = findNode::getClass<ActsSurfaceMaps>(topNode,
"ActsSurfaceMaps");
621 std::cerr <<
PHWHERE <<
"No acts surface son node tree, exiting."
626 _track_map = findNode::getClass<SvtxTrackMap>(topNode,
"SvtxTrackMap");
629 std::cerr <<
PHWHERE <<
" ERROR: Can't find SvtxTrackMap" << std::endl;
633 _assoc_container = findNode::getClass<AssocInfoContainer>(topNode,
"AssocInfoContainer");
636 std::cerr <<
PHWHERE <<
" ERROR: Can't find AssocInfoContainer." << std::endl;
644 std::cout <<
PHWHERE <<
"Could not find CYLINDERGEOM_MICROMEGAS_FULL." << std::endl;
670 if( !cluster )
continue;