diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AKFitter.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AKFitter.java index a940a6f9fe..ca09927de3 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AKFitter.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AKFitter.java @@ -24,7 +24,7 @@ public abstract class AKFitter { // return variables public boolean setFitFailed = false; - public double chi2; + public double chi2 = Double.POSITIVE_INFINITY; public int NDF; public int NDF0; public int numIter; diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Surface.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Surface.java index 59348bd03d..d17b281cfa 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Surface.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/Surface.java @@ -42,6 +42,7 @@ public class Surface implements Comparable { public double swimAccuracy; public boolean passive = false; public double hemisphere = 1; + public Point3D rayInterc = new Point3D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); public double[] unc = new double[2]; public double[] doca = new double[2]; @@ -421,11 +422,18 @@ public void setTransformation(Transformation3D transform) { @Override public int compareTo(Surface o) { - if (this.index > o.index) { - return 1; + if(this.rayInterc.y()==Double.POSITIVE_INFINITY) { + if (this.index > o.index) { + return 1; + } else { + return -1; + } } else { - return -1; - } + if (this.rayInterc.y() < o.rayInterc.y()) { + return 1; + } else { + return -1; + } + } } - } diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/StateVecs.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/StateVecs.java index 067bcf5c5a..c9ea7aa7eb 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/StateVecs.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/StateVecs.java @@ -1,5 +1,7 @@ package org.jlab.clas.tracking.kalmanfilter.helical; +import java.util.ArrayList; +import java.util.List; import org.jlab.clas.swimtools.Swim; import org.jlab.clas.tracking.kalmanfilter.AMeasVecs; import org.jlab.clas.tracking.kalmanfilter.AMeasVecs.MeasVec; @@ -7,7 +9,6 @@ import org.jlab.clas.tracking.kalmanfilter.Surface; import org.jlab.clas.tracking.kalmanfilter.Units; import org.jlab.clas.tracking.trackrep.Helix; -import org.jlab.clas.tracking.utilities.MatrixOps; import org.jlab.geom.prim.Line3D; import org.jlab.geom.prim.Point3D; import org.jlab.geom.prim.Vector3D; @@ -42,9 +43,8 @@ public boolean getStateVecPosAtMeasSite(StateVec sv, AMeasVecs.MeasVec mv, Swim Point3D st = new Point3D(sv.x, sv.y, sv.z); Vector3D stu = new Vector3D(sv.px,sv.py,sv.pz).asUnit(); - + Line3D toPln = new Line3D(st, stu); if(mv.surface.plane!=null) { - Line3D toPln = new Line3D(st, stu); Point3D inters = new Point3D(); int ints = mv.surface.plane.intersection(toPln, inters); sv.x = inters.x(); @@ -53,22 +53,16 @@ public boolean getStateVecPosAtMeasSite(StateVec sv, AMeasVecs.MeasVec mv, Swim sv.path = inters.distance(st); } else if(mv.surface.cylinder!=null) { - mv.surface.toLocal().apply(st); - mv.surface.toLocal().apply(stu); - double r = mv.surface.cylinder.baseArc().radius(); - double delta = Math.sqrt((st.x()*stu.x()+st.y()*stu.y())*(st.x()*stu.x()+st.y()*stu.y())-(-r*r+st.x()*st.x()+st.y()*st.y())*(stu.x()*stu.x()+stu.y()*stu.y())); - double l = (-(st.x()*stu.x()+st.y()*stu.y())+delta)/(stu.x()*stu.x()+stu.y()*stu.y()); - if(Math.signum(st.y()+l*stu.y())!=mv.hemisphere) { - l = (-(st.x()*stu.x()+st.y()*stu.y())-delta)/(stu.x()*stu.x()+stu.y()*stu.y()); - } - Point3D inters = new Point3D(st.x()+l*stu.x(),st.y()+l*stu.y(),st.z()+l*stu.z()); - mv.surface.toGlobal().apply(inters); - // RDV: should switch to use clas-geometry intersection method, not done now to alwys return a value - sv.x = inters.x(); - sv.y = inters.y(); - sv.z = inters.z(); - sv.path = inters.distance(st); - } + List inters = new ArrayList(); + int ints = mv.surface.cylinder.intersectionRay(toPln, inters); + if(ints<1) return false; + + sv.x = inters.get(0).x() ; + sv.y = inters.get(0).y() ; + sv.z = inters.get(0).z() ; + sv.path = inters.get(0).distance(st); + + } } else { if(swim==null) { // applicable only to planes parallel to the z -axis Helix helix = sv.getHelix(xref, yref); diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/straight/KFitter.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/straight/KFitter.java index 4c212a492d..5a2b76ab18 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/straight/KFitter.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/straight/KFitter.java @@ -54,22 +54,25 @@ public void runFitter(AStateVecs sv, AMeasVecs mv) { double newchisq = this.calc_chi2(sv, mv); // if curvature is 0, fit failed if(Double.isNaN(newchisq) || sv.smoothed().get(0)==null) { - this.setFitFailed = true; + this.setFitFailed = true; break; } else if(newchisq < this.chi2) { this.chi2 = newchisq; + finalStateVec = sv.new StateVec(sv.smoothed().get(0)); this.setTrajectory(sv, mv); - setFitFailed = false; + //System.out.println(finalStateVec.toString()); + //setFitFailed = false; //allow for new iteration to be worse but save the track from the best iteration. + // There are instances where the next iteration is worse at the 1 per 10000 level. This would loose the track... } // stop if chi2 got worse else { break; } } - if(!this.setFitFailed) { - finalStateVec = sv.new StateVec(sv.smoothed().get(0)); - } + //if(!this.setFitFailed) { + // finalStateVec = sv.new StateVec(sv.smoothed().get(0)); + //} } @Override diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/straight/StateVecs.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/straight/StateVecs.java index d811b23f9b..5cf50fae3b 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/straight/StateVecs.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/straight/StateVecs.java @@ -1,5 +1,7 @@ package org.jlab.clas.tracking.kalmanfilter.straight; +import java.util.ArrayList; +import java.util.List; import org.jlab.clas.swimtools.Swim; import org.jlab.clas.tracking.kalmanfilter.AMeasVecs; import org.jlab.clas.tracking.kalmanfilter.AMeasVecs.MeasVec; @@ -25,7 +27,7 @@ public boolean getStateVecPosAtMeasSite(StateVec vec, MeasVec mv, Swim swim) { Point3D ref = new Point3D(vec.x0,vec.y0,vec.z0); Vector3D u = new Vector3D(vec.tx, 1, vec.tz).asUnit(); - + Line3D toPln = new Line3D(ref,u); if(mv.k==0) { vec.x = vec.x0-vec.y0*vec.tx; vec.y = 0; @@ -34,7 +36,6 @@ public boolean getStateVecPosAtMeasSite(StateVec vec, MeasVec mv, Swim swim) { } else if(mv.hemisphere!=0) { if(mv.surface.plane!=null) { - Line3D toPln = new Line3D(ref,u); Point3D inters = new Point3D(); int ints = mv.surface.plane.intersection(toPln, inters); vec.x = inters.x() ; @@ -43,22 +44,29 @@ else if(mv.hemisphere!=0) { vec.path = inters.distance(ref); } if(mv.surface.cylinder!=null) { - mv.surface.toLocal().apply(ref); - mv.surface.toLocal().apply(u); - double r = 0.5*(mv.surface.cylinder.baseArc().radius()+mv.surface.cylinder.highArc().radius()); - double delta = Math.sqrt((ref.x()*u.x()+ref.y()*u.y())*(ref.x()*u.x()+ref.y()*u.y()) - -(-r*r+ref.x()*ref.x()+ref.y()*ref.y())*(u.x()*u.x()+u.y()*u.y())); - double l = (-(ref.x()*u.x()+ref.y()*u.y())+delta)/(u.x()*u.x()+u.y()*u.y()); - if(Math.signum(ref.y()+l*u.y())!=mv.hemisphere) { - l = (-(ref.x()*u.x()+ref.y()*u.y())-delta)/(u.x()*u.x()+u.y()*u.y()); + List inters = new ArrayList(); + int ints = mv.surface.cylinder.intersection(toPln, inters); + if(ints<1) { + if(!mv.surface.passive) { + return false; + } else { + mv.skip=true; + return true; } - - Point3D cylInt = new Point3D(ref.x()+l*u.x(),ref.y()+l*u.y(),ref.z()+l*u.z()); - mv.surface.toGlobal().apply(cylInt); - vec.x = cylInt.x(); - vec.y = cylInt.y(); - vec.z = cylInt.z(); - vec.path = cylInt.distance(ref); + } else { + int index =0; + if(ints>1) {//pick the intersection that is closest to the global fit ray intersection + double dh0 = Math.abs(inters.get(0).y()-mv.surface.rayInterc.y()); + double dh1 = Math.abs(inters.get(1).y()-mv.surface.rayInterc.y()); + if(dh1 getMeasurements(Seed seed) { List surfaces = new ArrayList<>(); for(Surface surf : cvtSurfaces) { if(surf!=null) { - if(debug) System.out.println(surf.toString()); + if(debug) System.out.println("USING SURFACE "+surf.toString()); surfaces.add(surf); } } @@ -205,7 +208,7 @@ public List getActiveMeasurements(Seed seed) { for(Surface surf : surfaces) { if(surf.passive && surf.getIndex()!=0) continue; active.add(surf); - if(debug) System.out.println(surf.toString()); + if(debug) System.out.println("ACTIVE SURFACE "+surf.toString()); } return active; } @@ -229,18 +232,30 @@ public List getMeasurements(StraightTrack cosmic) { this.reset(); this.addClusters(cosmic); this.addMissing(cosmic); - List surfaces = new ArrayList<>(); + List surfaces ; + Map sMap = new HashMap<>(); for(Surface surf : cvtSurfaces) { if(surf!=null) { - if(debug) System.out.println(surf.toString()); - if(surf.passive && surf.getIndex()!=0 && !this.isCrossed(cosmic.getRay(), surf)) { - if(debug) System.out.println("Removing surface " + surf.passive + " " + this.isCrossed(cosmic.getRay(), surf)); - continue; + + if(surf.passive && surf.getIndex()!=0) { + surf.hemisphere = this.getHemisphere((int)surf.hemisphere, cosmic.getRay(), surf); + + if(!this.isCrossed(surf)) { + if(debug) System.out.println("Removing surface "+surf.toString()+" " + surf.passive ); + continue; + } } - surfaces.add(surf); + + sMap.put(surf.getIndex(), surf); + //surfaces.add(surf); } } - return surfaces; + surfaces = new ArrayList<>(sMap.values()); + if(debug) + for(Surface surf : surfaces) { + System.out.println("USED "+surf.toString()); + } + return surfaces; } public List getActiveMeasurements(StraightTrack cosmic) { @@ -249,7 +264,6 @@ public List getActiveMeasurements(StraightTrack cosmic) { for(Surface surf : surfaces) { if(surf.passive && surf.getIndex()!=0) continue; active.add(surf); - if(debug) System.out.println(surf.toString()); } return active; } @@ -283,11 +297,14 @@ private void addClusters(DetectorType type, List clusters) { // } private List getClusterSurfaces(DetectorType type, List clusters, Ray ray) { - - List surfaces = this.getClusterSurfaces(type, clusters, ray); - for(Surface surf : surfaces) { - surf.hemisphere = this.getHemisphere(ray, surf); - } + List surfaces = new ArrayList<>(); + for(Cluster cluster : clusters) { + Surface surf = this.getClusterSurface(type, cluster); + if(surf!=null) { + surf.hemisphere = this.getHemisphere(cluster, ray, surf); + surfaces.add(surf); + } + } return surfaces; } @@ -339,9 +356,10 @@ private void addMissing(StraightTrack ray) { DetectorType type = MLayer.getDetectorType(id); Surface surface = this.getDetectorSurface(ray, type, layer, hemisphere); if(surface == null) continue; - surface.hemisphere=hemisphere; + surface.hemisphere = this.getHemisphere(hemisphere, ray.getRay(), surface); //resets hemisphere for shallow angle tracks surface.passive=true; - if(debug) System.out.println("Generating surface for missing index " + i + " detector " + type.getName() + " layer " + layer + " sector " + surface.getSector()); + if(debug) System.out.println("Generating surface for missing index " + i +" id "+id+ " detector " + type.getName() + " layer " + layer + " sector " + surface.getSector()+" hemisphere "+ + surface.hemisphere+" y "+surface.rayInterc); this.add(i, surface); } } @@ -425,24 +443,145 @@ else if(type==DetectorType.BMT) { } - private boolean isCrossed(Ray ray, Surface surface){ - if(surface.cylinder==null) - return true; - List trajs = new ArrayList<>(); - Line3D line = ray.toLine(); - return surface.cylinder.intersection(line, trajs) > 1; + + + private Point3D getRayInters(Cluster cluster, Ray ray, Surface surface) { + if (ray == null || surface == null) { + return new Point3D(Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY); + } + + Line3D line = ray.toLine(); // Shared line for both cylinder and plane + if (surface.cylinder != null) { + return handleCylinderIntersection(cluster, surface.cylinder, line); + } + + if (surface.plane != null) { + return handlePlaneIntersection(line, surface.plane); + } + + return new Point3D(Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY); // Default if no surface type matched + } + + private Point3D handleCylinderIntersection(Cluster cluster, Cylindrical3D cylinder, Line3D line) { + if (cluster.getType() != BMTType.C) { + return cluster.center(); // For non-C type clusters, return the center + } + + List intersections = new ArrayList<>(); + int intersectionCount = cylinder.intersection(line, intersections); + + switch (intersectionCount) { + case 0: + return new Point3D(Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY); + case 1: + return intersections.get(0); + case 2: + // Choose the intersection closest to the cluster center on the z-axis + Point3D closest = getClosestIntersectionToClusterZ(cluster, intersections); + return closest; + default: + return new Point3D(Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY); + } + } + + private Point3D getRayInters(int h, Ray ray, Surface surface) { + if (ray == null || surface == null) { + return new Point3D(Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY); + } + + Line3D line = ray.toLine(); // Shared line for both cylinder and plane + if (surface.cylinder != null) { + return handleCylinderIntersection(h, surface.cylinder, line); + } + + if (surface.plane != null) { + return handlePlaneIntersection(line, surface.plane); + } + + return new Point3D(Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY); // Default if no surface type matched } - private int getHemisphere(Ray ray, Surface surface){ - if(surface.cylinder==null) - return 0; - List trajs = new ArrayList<>(); - Line3D line = ray.toLine(); - if(surface.cylinder.intersection(line, trajs)>= 1) { - return (int) Math.signum(trajs.get(0).y()); + private Point3D handleCylinderIntersection(int h, Cylindrical3D cylinder, Line3D line) { + + List intersections = new ArrayList<>(); + int intersectionCount = cylinder.intersection(line, intersections); + + switch (intersectionCount) { + case 0: + return new Point3D(Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY); + case 1: + return intersections.get(0); + case 2: + // Choose + Point3D yFirst = intersections.get(0); + Point3D ySecond =intersections.get(1); + + if(Math.signum(yFirst.y())==h) { + return yFirst ; + } else { + return ySecond; + } + + default: + return new Point3D(Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY); + } + } + + private Point3D handlePlaneIntersection(Line3D line, Plane3D plane) { + Point3D intersection = new Point3D(); + if (plane.intersection(line, intersection) != 0) { + return intersection; + } + return new Point3D(Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY); + } + + private Point3D getClosestIntersectionToClusterZ(Cluster cluster, List intersections) { + Point3D first = intersections.get(0); + Point3D second = intersections.get(1); + + double distanceToFirst = Math.abs(cluster.center().z() - first.z()); + double distanceToSecond = Math.abs(cluster.center().z() - second.z()); + + return distanceToFirst < distanceToSecond ? first : second; + } + + private int getHemisphere(Cluster cluster, Ray ray, Surface surface){ + Point3D Y = this.getRayInters(cluster, ray, surface); + if(Y.y() == Double.POSITIVE_INFINITY) return 0; + surface.rayInterc=Y; + return (int) Math.signum(Y.y()); + + } + + private int getHemisphere(int h, Ray ray, Surface surface){ + Point3D Y = this.getRayInters(h, ray, surface); + if(Y.y() == Double.POSITIVE_INFINITY) return 0; + surface.rayInterc=Y; + return (int) Math.signum(Y.y()); + + } + + private boolean isCrossed(Surface surface) { + if (surface.cylinder == null) { + return false; + } + //is Crossed is called after getHemisphere which computes the intersection of the ray with the cylinder + // so there is no need to recompute it and it can just use the hemisphere to determine if the intersection was valid + if(surface.hemisphere==0) { + return false; + } else { + return true; + } + + } + //functionality to find out if a surface is crossed for surfaces where the hemisphere has not been set yet (not used at present + private boolean isCrossed(Cluster cluster, Ray ray, Surface surface) { + int h = this.getHemisphere(cluster, ray, surface); + if(h==0) { + return false; + } else { + return true; } - else - return 0; } private int getHemisphere(Cluster cluster, Seed seed, Surface surface) { if(surface.cylinder==null) diff --git a/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/services/CVTEngine.java b/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/services/CVTEngine.java index e278b0c9c7..038b736911 100644 --- a/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/services/CVTEngine.java +++ b/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/services/CVTEngine.java @@ -186,6 +186,18 @@ public int getRun(DataEvent event) { } return run; } + + public void getEventNum(DataEvent event) { + + if (event.hasBank("RUN::config") == false) { + System.err.println("RUN CONDITIONS NOT READ!"); + return ; + } + + DataBank bank = event.getBank("RUN::config"); + System.out.println("EVENT NUMBER "+bank.getInt("event", 0)); + + } public int getPid() { return pid; @@ -314,7 +326,8 @@ public boolean processDataEvent(DataEvent event) { banks.add(RecoBankWriter.fillStraightTracksBank(event, tracks, "CVTRec::Cosmics")); banks.add(RecoBankWriter.fillStraightTracksTrajectoryBank(event, tracks, "CVTRec::Trajectory")); banks.add(RecoBankWriter.fillStraightTrackKFTrajectoryBank(event, tracks, "CVTRec::KFTrajectory")); - } + //if(tracks.isEmpty() && seeds!=null && !seeds.isEmpty()) this.getEventNum(event); + } } else { double[] xyBeam = CVTReconstruction.getBeamSpot(event, beamPos);