From f178a758df9447583362bc40c88cde14090b1928 Mon Sep 17 00:00:00 2001 From: yugotra Date: Mon, 28 Sep 2026 10:26:12 -0400 Subject: [PATCH 1/2] Fix SVT projector for aligned sensor planes --- common-tools/clas-tracking/pom.xml | 4 + .../kalmanfilter/helical/StateVecs.java | 26 ++--- .../jlab/clas/tracking/trackrep/Helix.java | 34 +++++++ .../clas/tracking/trackrep/HelixTest.java | 96 +++++++++++++++++++ 4 files changed, 147 insertions(+), 13 deletions(-) create mode 100644 common-tools/clas-tracking/src/test/java/org/jlab/clas/tracking/trackrep/HelixTest.java diff --git a/common-tools/clas-tracking/pom.xml b/common-tools/clas-tracking/pom.xml index 13cbe809aa..7e4ee4399b 100644 --- a/common-tools/clas-tracking/pom.xml +++ b/common-tools/clas-tracking/pom.xml @@ -14,6 +14,10 @@ + + junit + junit + org.apache.commons commons-math3 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..240e74cecf 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 @@ -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,18 +72,19 @@ 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(); diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java index 39582caf7b..6ca5845081 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java @@ -241,6 +241,40 @@ 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; + } + 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 diff --git a/common-tools/clas-tracking/src/test/java/org/jlab/clas/tracking/trackrep/HelixTest.java b/common-tools/clas-tracking/src/test/java/org/jlab/clas/tracking/trackrep/HelixTest.java new file mode 100644 index 0000000000..fc27470e23 --- /dev/null +++ b/common-tools/clas-tracking/src/test/java/org/jlab/clas/tracking/trackrep/HelixTest.java @@ -0,0 +1,96 @@ +package org.jlab.clas.tracking.trackrep; + +import org.jlab.clas.tracking.kalmanfilter.Units; +import org.junit.Test; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertTrue; + +public class HelixTest { + + private static Helix helix() { + return new Helix(0.2, 0.4, 0.004, 20.0, 0.6, + 1, 5.0, 0.0, 0.0, Units.MM); + } + + @Test + public void full3DPlaneIntersectionIncludesLongitudinalTilt() { + Helix helix = helix(); + double expectedL = 80.0; + double x = helix.getX(expectedL); + double y = helix.getY(expectedL); + double z = helix.getZ(expectedL); + double nx = 0.8; + double ny = 0.6; + double nz = 0.02; + + double offset = 50.0; + double px = x + nz * offset; + double py = y; + double pz = z - nx * offset; + + double actualL = helix.getLAtPlane3D(px, py, pz, nx, ny, nz, px, py); + double projectedL = helix.getLAtPlane3D(px, py, pz, nx, ny, 0.0, px, py); + double residual = nx * (helix.getX(actualL) - px) + + ny * (helix.getY(actualL) - py) + + nz * (helix.getZ(actualL) - pz); + + assertTrue(Double.isFinite(actualL)); + assertEquals(expectedL, actualL, 1.0e-5); + assertEquals(0.0, residual, 1.0e-7); + assertTrue("2-D approximation must differ for a tilted plane", + Math.abs(projectedL - actualL) > 0.1); + } + + @Test + public void full3DPlaneIntersectionHandlesBothSignsAndRotations() { + for (int turn : new int[] {-1, 1}) { + Helix helix = new Helix(0.2, 0.4, turn * 0.004, 20.0, 0.6, + turn, 5.0, 0.0, 0.0, Units.MM); + for (double nz : new double[] {-0.02, 0.02}) { + for (double angle : new double[] {0.3, 1.7}) { + double expectedL = 70.0; + double x = helix.getX(expectedL); + double y = helix.getY(expectedL); + double z = helix.getZ(expectedL); + double nx = Math.cos(angle); + double ny = Math.sin(angle); + + double offset = 5.0; + double px = x + nz * offset; + double py = y; + double pz = z - nx * offset; + double actualL = helix.getLAtPlane3D( + px, py, pz, nx, ny, nz, px, py); + double residual = nx * (helix.getX(actualL) - px) + + ny * (helix.getY(actualL) - py) + + nz * (helix.getZ(actualL) - pz); + + assertTrue("turn=" + turn + " nz=" + nz + " angle=" + angle, + Double.isFinite(actualL)); + assertEquals(expectedL, actualL, 1.0e-4); + assertEquals(0.0, residual, 1.0e-7); + } + } + } + } + + @Test + public void full3DMatchesLegacyForIdealBarrelPlane() { + Helix helix = helix(); + double expectedL = 60.0; + double px = helix.getX(expectedL); + double py = helix.getY(expectedL); + double pz = helix.getZ(expectedL); + double nx = 0.7; + double ny = Math.sqrt(1.0 - nx * nx); + + double full = helix.getLAtPlane3D(px, py, pz, nx, ny, 0.0, px, py); + double projected = helix.getLAtPlane3D( + px, py, pz, nx, ny, 0.0, px, py); + + assertTrue(Double.isFinite(full)); + assertEquals(expectedL, full, 1.0e-7); + assertEquals(projected, full, 0.0); + } +} From 3a83bde0fbbc8f7a40e0b5b8ccbef012d38f3800 Mon Sep 17 00:00:00 2001 From: yugotra Date: Mon, 28 Sep 2026 10:28:14 -0400 Subject: [PATCH 2/2] Fix BMT projector for aligned cylinder axes --- .../kalmanfilter/helical/KFitter.java | 8 ++ .../kalmanfilter/helical/MeasVecs.java | 5 +- .../kalmanfilter/helical/StateVecs.java | 35 +++-- .../jlab/clas/tracking/trackrep/Helix.java | 121 ++++++++++++++++++ .../clas/tracking/trackrep/HelixTest.java | 106 +++++++++++++++ 5 files changed, 264 insertions(+), 11 deletions(-) diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/KFitter.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/KFitter.java index f50f453521..2e29504ec4 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/KFitter.java +++ b/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); diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/MeasVecs.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/MeasVecs.java index fc15c2c77e..b7786b9614 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/MeasVecs.java +++ b/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] ; 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 240e74cecf..fec3d1486b 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 @@ -88,15 +88,32 @@ else if(mv.surface.cylinder!=null) { } 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 { diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java index 6ca5845081..e9000263db 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java @@ -275,6 +275,127 @@ public double getLAtPlane3D(double px, double py, double 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 diff --git a/common-tools/clas-tracking/src/test/java/org/jlab/clas/tracking/trackrep/HelixTest.java b/common-tools/clas-tracking/src/test/java/org/jlab/clas/tracking/trackrep/HelixTest.java index fc27470e23..25e6173354 100644 --- a/common-tools/clas-tracking/src/test/java/org/jlab/clas/tracking/trackrep/HelixTest.java +++ b/common-tools/clas-tracking/src/test/java/org/jlab/clas/tracking/trackrep/HelixTest.java @@ -93,4 +93,110 @@ public void full3DMatchesLegacyForIdealBarrelPlane() { assertEquals(expectedL, full, 1.0e-7); assertEquals(projected, full, 0.0); } + + private static double distanceToAxis(Helix helix, double l, + double ax, double ay, double az, + double ux, double uy, double uz) { + double un = Math.sqrt(ux * ux + uy * uy + uz * uz); + ux /= un; + uy /= un; + uz /= un; + double wx = helix.getX(l) - ax; + double wy = helix.getY(l) - ay; + double wz = helix.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); + } + + @Test + public void full3DCylinderIntersectionUsesDisplacedTiltedAxis() { + double ax = 2.0; + double ay = -1.5; + double az = -300.0; + double ux = 0.003; + double uy = -0.004; + double uz = 1.0; + + for (int turn : new int[] {-1, 1}) { + Helix helix = new Helix(0.2, 0.4, turn * 0.004, 20.0, 0.6, + turn, 5.0, 0.0, 0.0, Units.MM); + double expectedL = 70.0; + double radius = distanceToAxis(helix, expectedL, ax, ay, az, ux, uy, uz); + double legacyL = helix.getLAtR(radius); + double actualL = helix.getLAtCylinder3D( + ax, ay, az, ux, uy, uz, radius, legacyL); + + assertTrue("turn=" + turn, Double.isFinite(actualL)); + assertEquals(expectedL, actualL, 1.0e-5); + assertEquals(radius, + distanceToAxis(helix, actualL, ax, ay, az, ux, uy, uz), 1.0e-7); + assertTrue("the beam-axis crossing must miss the aligned cylinder", + Math.abs(distanceToAxis( + helix, legacyL, ax, ay, az, ux, uy, uz) - radius) > 0.1); + } + } + + @Test + public void full3DCylinderIntersectionIsIndependentOfAxisScaleAndOrigin() { + Helix helix = helix(); + double expectedL = 80.0; + double ax = -1.2; + double ay = 2.3; + double az = -250.0; + double ux = -0.002; + double uy = 0.005; + double uz = 1.0; + double radius = distanceToAxis(helix, expectedL, ax, ay, az, ux, uy, uz); + double seed = helix.getLAtR(radius); + + double nominal = helix.getLAtCylinder3D( + ax, ay, az, ux, uy, uz, radius, seed); + double scaledDirection = helix.getLAtCylinder3D( + ax, ay, az, 100.0 * ux, 100.0 * uy, 100.0 * uz, radius, seed); + double shiftedOrigin = helix.getLAtCylinder3D( + ax + 37.0 * ux, ay + 37.0 * uy, az + 37.0 * uz, + ux, uy, uz, radius, seed); + + assertEquals(expectedL, nominal, 1.0e-5); + assertEquals(nominal, scaledDirection, 1.0e-10); + assertEquals(nominal, shiftedOrigin, 1.0e-10); + } + + @Test + public void full3DCylinderMatchesClosedFormForBeamAxis() { + Helix helix = helix(); + double radius = Math.hypot(helix.getX(60.0), helix.getY(60.0)); + double legacy = helix.getLAtR(radius); + double full = helix.getLAtCylinder3D( + 0.0, 0.0, -500.0, 0.0, 0.0, 1000.0, radius, legacy); + + assertTrue(Double.isFinite(full)); + assertEquals(legacy, full, 0.0); + } + + @Test + public void full3DCylinderFallsBackToBracketAtStationarySeed() { + Helix helix = helix(); + double expectedAbsL = 60.0; + double radius = Math.hypot(helix.getX(expectedAbsL), helix.getY(expectedAbsL)); + + double actualL = helix.getLAtCylinder3D( + 0.0, 0.0, -500.0, 0.0, 0.0, 1000.0, radius, 0.0); + + assertTrue(Double.isFinite(actualL)); + assertEquals(expectedAbsL, Math.abs(actualL), 1.0e-5); + assertEquals(radius, Math.hypot( + helix.getX(actualL), helix.getY(actualL)), 1.0e-7); + } + + @Test + public void full3DCylinderRejectsDegenerateAxis() { + Helix helix = helix(); + double result = helix.getLAtCylinder3D( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 150.0, 10.0); + assertTrue(Double.isNaN(result)); + } }