Upgrade FD tracking

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

@@ -14,6 +14,7 @@
 /**
  *
  * @author ziegler
+ * @author Tongtong Cao
  */
 
 public abstract class AMeasVecs {
@@ -22,7 +23,8 @@ public abstract class AMeasVecs {
 
     public void setMeasVecs(List<Surface> measSurfaces) {
         measurements = new ArrayList<>();
-        Collections.sort(measSurfaces);
+        if(measSurfaces.get(0).type != Type.LINEDOCA) // Measurements from DC has been sorted 
+        	Collections.sort(measSurfaces);
         for(int i = 0; i < measSurfaces.size(); i++) {
             MeasVec mvec = new MeasVec();
             mvec.k = i ;
@@ -33,9 +35,57 @@ public void setMeasVecs(List<Surface> measSurfaces) {
             mvec.skip = mvec.surface.passive;
             mvec.hemisphere = measSurfaces.get(i).hemisphere;
             measurements.add(mvec);
+            if(measSurfaces.get(i).type == Type.LINEDOCA) { //Todo: assign a value to the Surface index and use that as the identifier
+            	mvec.region = measSurfaces.get(i).region;
+            	mvec.sector = measSurfaces.get(i).getSector();
+            	mvec.superlayer = measSurfaces.get(i).getSuperLayer();
+            	mvec.layer = measSurfaces.get(i).getLayer();
+            }
         }
     }
 
+    // For DC, there could be two hits at a measurement
+    public double[] dhDoca(int k, StateVec stateVec) {  
+    	double value[] = {Double.NaN, Double.NaN};
+
+    	Surface surf = this.measurements.get(stateVec.k).surface;
+    	Point3D point = new Point3D(stateVec.x, stateVec.y, stateVec.z);
+    	double h = hDoca(point, surf.wireLine[0]);
+
+    	double signMeas = 1;
+    	double sign = 1;
+    	if(surf.doca[1]!=-99 || !(Math.abs(surf.doca[0])<0.5 && surf.doca[1]==-99 ) ) { //use LR only for double hits or large enough doca for one hit
+    		signMeas = Math.signum(surf.doca[0]);
+    		sign = Math.signum(h);
+    	} else {
+    		signMeas = Math.signum(h);
+    		sign = Math.signum(h);
+    	}
+
+    	value[0] = signMeas*Math.abs(surf.doca[0]) - sign*Math.abs(h);
+
+        //USE THE DOUBLE HIT
+        if(surf.doca[1]!=-99) { 
+            h = hDoca(point, surf.wireLine[1]);
+
+            signMeas = Math.signum(surf.doca[1]);
+            sign = Math.signum(h);
+
+            value[1]= signMeas*Math.abs(surf.doca[1]) - sign*Math.abs(h);
+        } 
+
+    	return value;
+    }
+
+    // Return a signed doca for DC
+    public double hDoca(Point3D point, Line3D wireLine) {              
+
+        Line3D WL = new Line3D();
+        WL.copy(wireLine);
+        WL.copy(WL.distance(point));
+
+        return WL.length()*Math.signum(WL.direction().x());
+    }
 
     public double dh(int k, StateVec stateVec) {
 
@@ -53,7 +103,7 @@ public double dh(int k, StateVec stateVec) {
         }
         if( this.measurements.get(stateVec.k).surface.type == Type.PLANEWITHPOINT || 
                 this.measurements.get(stateVec.k).surface.type == Type.CYLINDERWITHPOINT) {
-            Point3D p = new Point3D(this.measurements.get(stateVec.k).surface.refPoint);
+            Point3D p = new Point3D(this.measurements.get(stateVec.k).surface.measPoint);
             value = p.distance(stateVec.x, stateVec.y, stateVec.z);
         }
         if( this.measurements.get(stateVec.k).surface.type == Type.CYLINDERWITHARC) {
@@ -133,7 +183,7 @@ public double h(int k, StateVec stateVec) {
         }
         if( this.measurements.get(stateVec.k).surface.type == Type.PLANEWITHPOINT || 
                 this.measurements.get(stateVec.k).surface.type == Type.CYLINDERWITHPOINT) {
-            Point3D p = new Point3D(this.measurements.get(stateVec.k).surface.refPoint);
+            Point3D p = new Point3D(this.measurements.get(stateVec.k).surface.measPoint);
             p.setZ(0);
             value = p.distance(stateVec.x, stateVec.y, 0);
         }
@@ -209,6 +259,10 @@ public class MeasVec implements Comparable<MeasVec> {
         public boolean skip  = false;
         public double hemisphere = 1;
 
+        //For DC
+        public int region = -1;
+        public int sector = -1;
+        public int superlayer = -1;
 
         @Override
         public int compareTo(MeasVec arg) {

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

@@ -10,6 +10,14 @@
 import org.jlab.clas.tracking.kalmanfilter.helical.KFitter;
 import org.jlab.clas.tracking.trackrep.Helix;
 import org.jlab.geom.prim.Point3D;
+import org.jlab.jnp.matrix.Matrix;
+import org.jlab.jnp.matrix.Matrix5x5;
+
+/**
+ *
+ * @author ziegler
+ * @author Tongtong Cao
+ */
 
 public abstract class AStateVecs {
 
@@ -245,6 +253,81 @@ public class StateVec {
         public double energyLoss;
         public double dx;
         public double path;
+
+        /////////////////////// extra variables for forward tracking ///////////////////////
+        // tx = px/pz
+        public double ty; //=py/pz
+        public double Q; //q/p
+        public double B;
+        public double deltaPath;   
+        public Matrix CM = new Matrix(); // Todo: unify covMat and CM
+        private double _PathLength;
+
+        /////////////////////// For DAF ///////////////////////
+        private double weightDAF_single = 1;
+        private double[] weightDAF_double = {0.5, 0.5};
+
+        public double getWeightDAF_singleHit(){
+            return weightDAF_single;
+        }
+        public void setWeightDAF_singleHit(double weight){
+            this.weightDAF_single = weight;
+        }
+
+        public double[] getWeightDAF_doubleHits(){
+            return weightDAF_double;
+        }
+        public void setWeightDAF_doubleHits(double[] weight){
+            this.weightDAF_double = weight;
+        }
+
+        // Todo: move variables finalDAFWeight, _PathLength, hw, and h from AStateVecs to AMeasVecs
+        private double finalDAFWeight = -999;
+        public double getFinalDAFWeight(){
+            return finalDAFWeight;
+        }
+        public void setFinalDAFWeight(double weight){
+            this.finalDAFWeight = weight;
+        }
+
+        boolean isDoubleHit = false;
+
+        public boolean getIsDoubleHit(){
+            return isDoubleHit;
+        }
+        public void setIsDoubleHit(boolean isDoubleHit){
+            this.isDoubleHit = isDoubleHit;
+        }
+
+        public double getPathLength() {
+            return _PathLength;
+        }
+
+        public void setPathLength(double _PathLength) {
+            this._PathLength = _PathLength;
+        }
+
+        // KF projector --> get Wire midPoint match
+        private double hw;
+
+        public double getProjector() {
+            return hw;
+        }
+
+        public void setProjector(double h) {
+            this.hw = h;
+        }
+        // KF projector --> get fit doca
+        private double h;
+
+        public double getProjectorDoca() {
+            return h;
+        }
+
+        public void setProjectorDoca(double h) {
+            this.h = h;
+        }
+        /////////////////////// extra variables for forward tracking ///////////////////////
 
         public StateVec(int k) {
             this.k = k;
@@ -294,8 +377,18 @@ public final void copy(StateVec s) {
             this.energyLoss = s.energyLoss;
             this.dx = s.dx;
             this.path = s.path;
-            this.covMat = this.copyMatrix(s.covMat);
-            this.F = this.copyMatrix(s.F);
+            if(s.covMat != null)
+            	this.covMat = this.copyMatrix(s.covMat);
+            if(s.F != null)
+            	this.F = this.copyMatrix(s.F);            
+            this.ty = s.ty;
+            this.Q = s.Q;
+            this.B = s.B;
+            this.deltaPath = s.deltaPath;
+            if(s.CM != null)
+            	Matrix5x5.copy(s.CM, this.CM);
+            this.weightDAF_single = s.weightDAF_single;
+            this.weightDAF_double = s.weightDAF_double;
         }
 
         public void copyCovMat(double[][] c) {

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

@@ -14,12 +14,15 @@
 /**
  *
  * @author ziegler
+ * @author Tongtong Cao
  */
 public class Surface implements Comparable<Surface> {
 
     public Type type;
     public Plane3D plane;
-    public Point3D refPoint;
+    public Point3D measPoint;
+    public Point3D measPoint_err;
+    // Todo: make a line to replace two end points
     public Point3D lineEndPoint1;
     public Point3D lineEndPoint2;
     public Point3D finitePlaneCorner1;
@@ -40,10 +43,56 @@ public class Surface implements Comparable<Surface> {
     public boolean passive = false;
     public double hemisphere = 1;
 
+    public double[] unc = new double[2];
+    public double[] doca = new double[2];
+    public Line3D[] wireLine = new Line3D[2];
+    public int region;
+    public int superlayer;
+    public int nMeas = 1;
+
+
+    public void setNMeas(int n) {
+    	nMeas = n;
+    }
+
+    public int getNMeas() {
+    	return nMeas;
+    }
+
+    // For URWell
+    public Surface(int sector, double x, double y, double z, double x_err, double y_err) {
+        type = Type.PLANEWITHPOINT;
+        this.sector = sector;
+        Point3D point = new Point3D(x, y, z);
+        measPoint = point;
+        Point3D point_err = new Point3D(x_err, y_err, 0);
+        measPoint_err = point_err;
+        Material material_air = new Material("air", 0, 0, 0, 30400, 0, Units.CM);
+        Material material_argon = new Material("argon", 0, 0, 0, 14, 0, Units.CM);
+        materials.add(material_air);
+        materials.add(material_argon);
+    }
+
+    // For DC
+    public Surface(int region, double[] doca, double[] unc, double error, Line3D[] wireLine) {
+    	type = Type.LINEDOCA;
+    	this.region = region;
+        this.doca = doca;
+        this.unc = unc;
+        this.error = error;
+        this.wireLine = wireLine;
+        Point3D point = new Point3D(0, 0, wireLine[0].origin().z());
+        this.measPoint = point;
+        Material material_air = new Material("air", 0, 0, 0, 30400, 0, Units.CM);
+        Material material_argon = new Material("argon", 0, 0, 0, 14, 0, Units.CM);
+        materials.add(material_air);
+        materials.add(material_argon);
+    }
+
     public Surface(Plane3D plane3d, Point3D refrPoint, Point3D c1, Point3D c2, double accuracy) {
         type = Type.PLANEWITHPOINT;
         plane = plane3d;
-        refPoint = refrPoint;
+        measPoint = refrPoint;
         finitePlaneCorner1 = c1;
         finitePlaneCorner2 = c2;
         swimAccuracy = accuracy;
@@ -82,7 +131,7 @@ public Surface(Cylindrical3D cylinder3d, Strip strp, double accuracy) {
     public Surface(Cylindrical3D cylinder3d, Point3D refrPoint, double accuracy) {
         type = Type.CYLINDERWITHPOINT;
         cylinder = cylinder3d;
-        refPoint = refrPoint;
+        measPoint = refrPoint;
         swimAccuracy = accuracy;
     }
     public Surface(Cylindrical3D cylinder3d, Point3D endPoint1, Point3D endPoint2, double accuracy) {
@@ -184,6 +233,20 @@ public int getLayer() {
     public void setLayer(int layer) {
         this.layer = layer;
     }
+
+    /**
+     * @return the superlayer
+     */
+    public int getSuperLayer() {
+        return superlayer;
+    }
+
+    /**
+     * @param superlayer the superlayer to set
+     */
+    public void setSuperLayer(int superlayer) {
+        this.superlayer = superlayer;
+    }
 
     /**
      * @return the sector

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

@@ -3,11 +3,12 @@
 /**
  *
  * @author ziegler
+ * @author Tongtong Cao
  */
 public enum Type {
     UDF(-1), PLANEWITHPOINT(0), PLANEWITHLINE(1), PLANEWITHSTRIP(2),
     CYLINDERWITHPOINT(3), CYLINDERWITHLINE(4), CYLINDERWITHARC(5), 
-    CYLINDERWITHSTRIP(6), LINE(7);
+    CYLINDERWITHSTRIP(6), LINE(7), LINEDOCA(8);
     private final int value;
 
     Type(int value) {

common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/zReference/Constants.java

@@ -0,0 +1,32 @@
+package org.jlab.clas.tracking.kalmanfilter.zReference;
+
+import java.util.logging.Logger;
+
+/**
+ * Constants used in forward tracking
+ * 
+ * author: Tongtong
+ */
+public class Constants {
+
+    // private constructor for a singleton
+    private Constants() {
+    }
+
+
+    public static Logger LOGGER = Logger.getLogger(Constants.class.getName());
+
+    // CONSTATNS for TRANSFORMATION
+    public static final double ITERSTOPXHB = 1.2e-2; 
+    public static final double ITERSTOPYHB = 1.4e-1;  
+    public static final double ITERSTOPTXHB = 2.5e-4;  
+    public static final double ITERSTOPTYHB = 1.0e-3;  
+    public static final double ITERSTOPQHB = 1.6e-3;  
+
+    public static final double ITERSTOPXTB = 5.5e-5; 
+    public static final double ITERSTOPYTB = 8.0e-4;  
+    public static final double ITERSTOPTXTB = 2.1e-6;  
+    public static final double ITERSTOPTYTB = 3.5e-6;  
+    public static final double ITERSTOPQTB = 1.1e-5;       
+
+}