Fix CVT projectors for aligned SVT and BMT geometry

common-tools/clas-tracking/pom.xml

@@ -14,6 +14,10 @@
   </parent>
 
   <dependencies>
+    <dependency>
+      <groupId>junit</groupId>
+      <artifactId>junit</artifactId>
+    </dependency>
     <dependency>
       <groupId>org.apache.commons</groupId>
       <artifactId>commons-math3</artifactId>

common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/KFitter.java

@@ -223,6 +223,14 @@ public StateVec filter(int k, StateVec vec, AMeasVecs mv) {
     //            System.out.println(dh);
                 //get the projector Matrix
                 double[] H = mv.H(fVec, sv,  mv.measurements.get(k), this.getSwimmer());
+                if (H == null) {
+                    // A perturbed state could not be placed on the measurement surface. A
+                    // nominal-surface substitute would make the derivative geometrically
+                    // inconsistent, so exclude this measurement and retain the incoming state.
+                    this.NDF--;
+                    mv.measurements.get(k).skip = true;
+                    return fVec;
+                }
     //            System.out.println(k + " " + mv.measurements.get(k).layer + " " + H[0] + " " + H[1] + " " + H[2] + " " + H[3] + " " + H[4] + " " +dh );
 
                 double[][] CaInv =  this.getMatrixOps().filterCovMat(H, fVec.covMat, V);

common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/MeasVecs.java

@@ -74,8 +74,9 @@ public double[] H(AStateVecs.StateVec stateVec, AStateVecs sv, MeasVec mv, Swim
 
             //SVplus.updateFromHelix();
             //SVminus.updateFromHelix();
-            sv.setStateVecPosAtMeasSite(SVplus,  mv, null);
-            sv.setStateVecPosAtMeasSite(SVminus, mv, null);
+            boolean plusOK = sv.setStateVecPosAtMeasSite(SVplus, mv, null);
+            boolean minusOK = sv.setStateVecPosAtMeasSite(SVminus, mv, null);
+            if (!plusOK || !minusOK) return null;
 //            sv.printlnStateVec(SVplus);
 //            sv.printlnStateVec(SVminus);
             Hval[i] = (this.h(stateVec.k, SVplus) - this.h(stateVec.k, SVminus)) / getDelta_d_a()[i] ;

common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/StateVecs.java

@@ -18,7 +18,6 @@
  */
 public class StateVecs extends AStateVecs {
 
-
     @Override
     public boolean getStateVecPosAtMeasSite(StateVec sv, AMeasVecs.MeasVec mv, Swim swim) {
         double[] swimPars = new double[7];
@@ -73,30 +72,48 @@ else if(mv.surface.cylinder!=null) {
             if(swim==null) { // applicable only to planes parallel to the z -axis
                 Helix helix = sv.getHelix(xref, yref);
                 if(mv.surface.plane!=null) {
-                    Point3D pos = helix.getHelixPointAtPlane(mv.surface.finitePlaneCorner1.x(), mv.surface.finitePlaneCorner1.y(),
-                                                             mv.surface.finitePlaneCorner2.x(), mv.surface.finitePlaneCorner2.y(), 10);
-                    Vector3D mom = helix.getMomentumAtPlane(mv.surface.finitePlaneCorner1.x(), mv.surface.finitePlaneCorner1.y(),
-                                                            mv.surface.finitePlaneCorner2.x(), mv.surface.finitePlaneCorner2.y(), 10);
-                    sv.path = helix.getLAtPlane(mv.surface.finitePlaneCorner1.x(), mv.surface.finitePlaneCorner1.y(),
-                                                mv.surface.finitePlaneCorner2.x(), mv.surface.finitePlaneCorner2.y(), 10);
-                    sv.x = pos.x();
-                    sv.y = pos.y();
-                    sv.z = pos.z();
-                    sv.px = mom.x();
-                    sv.py = mom.y();
-                    sv.pz = mom.z();
+                    Vector3D normal = mv.surface.plane.normal();
+                    Point3D point = mv.surface.plane.point();
+                    double l = helix.getLAtPlane3D(point.x(), point.y(), point.z(),
+                                                  normal.x(), normal.y(), normal.z(),
+                                                  point.x(), point.y());
+                    if(!Double.isFinite(l)) return false;
+                    sv.path = l;
+                    sv.x = helix.getX(l);
+                    sv.y = helix.getY(l);
+                    sv.z = helix.getZ(l);
+                    sv.px = helix.getPx(helix.getB(), l);
+                    sv.py = helix.getPy(helix.getB(), l);
+                    sv.pz = helix.getPz(helix.getB());
                 }
                 else {
                     double r = mv.surface.cylinder.baseArc().radius();
-                    Point3D pos = helix.getHelixPointAtR(r);
-                    Vector3D mom = helix.getMomentumAtR(r);
-                    sv.path = helix.getLAtR(r);
-                    sv.x = pos.x();
-                    sv.y = pos.y();
-                    sv.z = pos.z();
-                    sv.px = mom.x();
-                    sv.py = mom.y();
-                    sv.pz = mom.z();
+                    double l = helix.getLAtR(r);
+                    Point3D axisStart = mv.surface.cylinder.getAxis().origin();
+                    Point3D axisEnd = mv.surface.cylinder.getAxis().end();
+
+                    // Preserve the exact closed-form result for an ideal beam-axis cylinder.
+                    // Alignment may displace or tilt the BMT axis; in that case intersect the
+                    // same cylinder used by the measurement surface.
+                    boolean beamAxis = Math.abs(axisStart.x()) < 1.0e-9
+                            && Math.abs(axisStart.y()) < 1.0e-9
+                            && Math.abs(axisEnd.x()) < 1.0e-9
+                            && Math.abs(axisEnd.y()) < 1.0e-9;
+                    if (!beamAxis) {
+                        l = helix.getLAtCylinder3D(
+                                axisStart.x(), axisStart.y(), axisStart.z(),
+                                axisEnd.x() - axisStart.x(),
+                                axisEnd.y() - axisStart.y(),
+                                axisEnd.z() - axisStart.z(), r, l);
+                        if (!Double.isFinite(l)) return false;
+                    }
+                    sv.path = l;
+                    sv.x = helix.getX(l);
+                    sv.y = helix.getY(l);
+                    sv.z = helix.getZ(l);
+                    sv.px = helix.getPx(helix.getB(), l);
+                    sv.py = helix.getPy(helix.getB(), l);
+                    sv.pz = helix.getPz(helix.getB());
                 }
             }
             else {

common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java

@@ -241,6 +241,161 @@ public double getPz() {
         return this.getPz(this.getB());
     }
 
+    /** Intersect the helix with the complete plane n.(r-p)=0, including nz. */
+    public double getLAtPlane3D(double px, double py, double pz,
+                                double nx, double ny, double nz,
+                                double xNear, double yNear) {
+        // Start at the point on the helix circle nearest the module. The projected trace of a
+        // nearly tangential plane can miss the circle even when the full 3-D plane intersects it.
+        double phi0 = Math.atan2(_yd - getYc(), _xd - getXc());
+        double phiNear = Math.atan2(yNear - getYc(), xNear - getXc());
+        double dphi = phiNear - phi0;
+        if (dphi >  Math.PI) dphi -= 2.0 * Math.PI;
+        if (dphi < -Math.PI) dphi += 2.0 * Math.PI;
+        double l = dphi / getOmega();
+        if (!Double.isFinite(l)) return Double.NaN;
+
+        final double dl = 1.0e-3;
+        for (int i = 0; i < 12; i++) {
+            double f = nx * (getX(l) - px) + ny * (getY(l) - py)
+                     + nz * (getZ(l) - pz);
+            if (Math.abs(f) < 1.0e-7) return l;
+            double derivative = nx * (getX(l + dl) - getX(l - dl)) / (2 * dl)
+                              + ny * (getY(l + dl) - getY(l - dl)) / (2 * dl)
+                              + nz * (getZ(l + dl) - getZ(l - dl)) / (2 * dl);
+            if (!Double.isFinite(derivative) || Math.abs(derivative) < 1.0e-10)
+                return Double.NaN;
+            double step = f / derivative;
+            if (Math.abs(step) > 100) step = Math.copySign(100, step);
+            l -= step;
+            if (!Double.isFinite(l)) return Double.NaN;
+        }
+        double residual = nx * (getX(l) - px) + ny * (getY(l) - py)
+                        + nz * (getZ(l) - pz);
+        return Math.abs(residual) < 1.0e-5 ? l : Double.NaN;
+    }
+
+    /**
+     * Intersect this helix with a cylinder of radius {@code radius} about an arbitrary axis.
+     * The supplied starting value normally comes from the closed-form beam-axis solution.
+     *
+     * @return the local root nearest {@code lStart}, or NaN when no root can be established
+     */
+    public double getLAtCylinder3D(double ax, double ay, double az,
+                                   double ux, double uy, double uz,
+                                   double radius, double lStart) {
+        double un = Math.sqrt(ux * ux + uy * uy + uz * uz);
+        if (!(un > 0) || !Double.isFinite(lStart)) return Double.NaN;
+        ux /= un;
+        uy /= un;
+        uz /= un;
+
+        double l = lStart;
+        for (int i = 0; i < 12; i++) {
+            double f = radialMiss(l, ax, ay, az, ux, uy, uz, radius);
+            if (!Double.isFinite(f)) break;
+            if (Math.abs(f) < 1.0e-7) return l;
+
+            double derivative = radialMissDerivative(l, ax, ay, az, ux, uy, uz);
+            if (!Double.isFinite(derivative) || Math.abs(derivative) < 1.0e-10) break;
+            double step = f / derivative;
+            if (Math.abs(step) > 100.0) step = Math.copySign(100.0, step);
+            l -= step;
+            if (!Double.isFinite(l)) break;
+        }
+        double residual = radialMiss(l, ax, ay, az, ux, uy, uz, radius);
+        if (Double.isFinite(residual) && Math.abs(residual) < 1.0e-5) return l;
+
+        // Safeguard Newton near tangencies by bracketing the first local root on each side.
+        // The quarter-turn limit prevents a failure from selecting the opposite crossing.
+        double maxSpan = Math.min(200.0, Math.PI / (2.0 * Math.abs(getOmega())));
+        double left = lStart;
+        double right = lStart;
+        double fLeft = radialMiss(left, ax, ay, az, ux, uy, uz, radius);
+        double fRight = fLeft;
+        double span = 0.5;
+        while (Double.isFinite(fLeft) && Double.isFinite(fRight)
+                && Math.abs(right - lStart) < maxSpan) {
+            double nextSpan = Math.min(span, maxSpan);
+            double nextLeft = lStart - nextSpan;
+            double nextRight = lStart + nextSpan;
+            double nextFLeft = radialMiss(nextLeft, ax, ay, az, ux, uy, uz, radius);
+            double nextFRight = radialMiss(nextRight, ax, ay, az, ux, uy, uz, radius);
+
+            double rootLeft = bracketedCylinderRoot(nextLeft, left, nextFLeft, fLeft,
+                    ax, ay, az, ux, uy, uz, radius);
+            double rootRight = bracketedCylinderRoot(right, nextRight, fRight, nextFRight,
+                    ax, ay, az, ux, uy, uz, radius);
+            if (Double.isFinite(rootLeft) || Double.isFinite(rootRight)) {
+                if (!Double.isFinite(rootLeft)) return rootRight;
+                if (!Double.isFinite(rootRight)) return rootLeft;
+                return Math.abs(rootLeft - lStart) <= Math.abs(rootRight - lStart)
+                        ? rootLeft : rootRight;
+            }
+            left = nextLeft;
+            right = nextRight;
+            fLeft = nextFLeft;
+            fRight = nextFRight;
+            span *= 2.0;
+        }
+        return Double.NaN;
+    }
+
+    private double radialMissDerivative(double l, double ax, double ay, double az,
+                                        double ux, double uy, double uz) {
+        double wx = getX(l) - ax;
+        double wy = getY(l) - ay;
+        double wz = getZ(l) - az;
+        double along = wx * ux + wy * uy + wz * uz;
+        double qx = wx - along * ux;
+        double qy = wy - along * uy;
+        double qz = wz - along * uz;
+        double distance = Math.sqrt(qx * qx + qy * qy + qz * qz);
+        if (!(distance > 0)) return Double.NaN;
+
+        double s = -KFitter.polarity;
+        double phi = getPhi(l);
+        double vx = s * getTurningSign() * getR() * getOmega() * Math.cos(phi);
+        double vy = s * getTurningSign() * getR() * getOmega() * Math.sin(phi);
+        double vz = -getTanL();
+        return (qx * vx + qy * vy + qz * vz) / distance;
+    }
+
+    private double bracketedCylinderRoot(double lo, double hi, double flo, double fhi,
+                                         double ax, double ay, double az,
+                                         double ux, double uy, double uz, double radius) {
+        if (!Double.isFinite(flo) || !Double.isFinite(fhi) || flo * fhi > 0) {
+            return Double.NaN;
+        }
+        if (Math.abs(flo) < 1.0e-7) return lo;
+        if (Math.abs(fhi) < 1.0e-7) return hi;
+        for (int i = 0; i < 80; i++) {
+            double mid = 0.5 * (lo + hi);
+            double fm = radialMiss(mid, ax, ay, az, ux, uy, uz, radius);
+            if (!Double.isFinite(fm)) return Double.NaN;
+            if (Math.abs(fm) < 1.0e-7 || Math.abs(hi - lo) < 1.0e-9) return mid;
+            if (flo * fm <= 0) {
+                hi = mid;
+            } else {
+                lo = mid;
+                flo = fm;
+            }
+        }
+        return Double.NaN;
+    }
+
+    private double radialMiss(double l, double ax, double ay, double az,
+                              double ux, double uy, double uz, double radius) {
+        double wx = getX(l) - ax;
+        double wy = getY(l) - ay;
+        double wz = getZ(l) - az;
+        double along = wx * ux + wy * uy + wz * uz;
+        double dx = wx - along * ux;
+        double dy = wy - along * uy;
+        double dz = wz - along * uz;
+        return Math.sqrt(dx * dx + dy * dy + dz * dz) - radius;
+    }
+
     public double getLAtPlane(double X1, double Y1, double X2, double Y2, 
             double tolerance) {
         // Find the intersection of the helix circle with the module plane projection in XY which is a line