LISTSERV 16.5 - HPS-SVN Archives

java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl

--- java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/GblPoint.java	2014-08-28 08:14:53 UTC (rev 918)
+++ java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/GblPoint.java	2014-08-28 15:31:06 UTC (rev 919)
@@ -1,22 +1,590 @@

 package org.hps.recon.tracking.gbl;

-import hep.physics.matrix.BasicMatrix;
-import hep.physics.matrix.Matrix;

+import java.util.ArrayList;
+import java.util.List;
+import org.hps.recon.tracking.gbl.matrix.EigenvalueDecomposition;
+import org.hps.recon.tracking.gbl.matrix.Matrix;
+import org.hps.recon.tracking.gbl.matrix.SymMatrix;
+import org.hps.recon.tracking.gbl.matrix.Vector;

+/**
+ *
+ * @author phansson
+ * @author Norman A Graf
+ *
+ * @version $Id:
+ */

 public class GblPoint {

-	public GblPoint(BasicMatrix jacPointToPoint) {
-		// TODO Auto-generated constructor stub
-	}

+    public GblPoint(hep.physics.matrix.BasicMatrix jacPointToPoint) {
+        theLabel = 0;
+        theOffset = 0;
+        measDim = 0;
+        transFlag = false;
+        //measTransformation() 
+        scatFlag = false;
+        //localDerivatives()
+        //globalLabels()
+        //globalDerivatives()

-	public void addMeasurement(Matrix proL2m, BasicMatrix meas, BasicMatrix measPrec) {
-		// TODO Auto-generated method stub
-		
-	}

+        for (int i = 0; i < 5; ++i) {
+            for (int j = 0; j < 5; ++j) {
+                p2pJacobian.set(i, j, jacPointToPoint.e(i, j));
+            }
+        }
+    }

-	public void addScatterer(BasicMatrix scat, BasicMatrix scatPrec) {
-		// TODO Auto-generated method stub
-		
-	}

+    public void addMeasurement(hep.physics.matrix.Matrix proL2m, hep.physics.matrix.BasicMatrix meas, hep.physics.matrix.BasicMatrix measPrec) {

+        int ncols = proL2m.getNColumns();
+        int nrows = proL2m.getNRows();
+        System.out.println("proL2m has "+nrows+" rows and "+ ncols+ "columns");
+        Matrix a = new Matrix(nrows, ncols);
+        for(int i=0; i<nrows; ++i)
+        {
+            for(int j=0; j<ncols; ++j)
+            {
+                a.set(i,j,proL2m.e(i, j));
+            }
+        }
+        System.out.println("GblPoint add matrix: ");
+        a.print(10, 6);
+        
+        
+        
+        int measnrows = meas.getNRows();
+        int measncols = meas.getNColumns();
+        System.out.println("meas has "+measnrows+" rows and "+measncols+" columns");
+        
+        Vector measvec = new Vector(measncols);
+        for(int i=0; i<measnrows; ++i)
+        {
+            measvec.set(i, meas.e(0, i));
+        }
+        System.out.println("GblPoint add meas: ");
+        measvec.print(10, 6);
+        
+        
+        int measPrecnrows = measPrec.getNRows();
+        int measPrecncols = measPrec.getNColumns();
+        
+        System.out.println("measPrec has "+measPrecnrows+" rows and "+measPrecncols+" columns");
+        Vector measPrecvec = new Vector(measPrecncols);
+        for(int i=0; i<measPrecnrows; ++i)
+        {
+            measPrecvec.set(i, measPrec.e(0, i));
+        }
+        System.out.println("GblPoint add measPrec: ");
+        measPrecvec.print(10, 6); 
+        
+        addMeasurement(a, measvec, measPrecvec, 0.);
+    }
+
+    public void addScatterer(hep.physics.matrix.BasicMatrix scat, hep.physics.matrix.BasicMatrix scatPrec) {
+        // TODO Auto-generated method stub
+
+    }
+
+    private int theLabel; ///< Label identifying point
+    private int theOffset; ///< Offset number at point if not negative (else interpolation needed)
+    private Matrix p2pJacobian = new SymMatrix(5); ///< Point-to-point jacobian from previous point
+    private Matrix prevJacobian = new SymMatrix(5); ///< Jacobian to previous scatterer (or first measurement)
+    private Matrix nextJacobian = new SymMatrix(5); ///< Jacobian to next scatterer (or last measurement)
+    private int measDim; ///< Dimension of measurement (1-5), 0 indicates absence of measurement
+    private SymMatrix measProjection = new SymMatrix(5); ///< Projection from measurement to local system
+    private Vector measResiduals = new Vector(5); ///< Measurement residuals
+    private Vector measPrecision = new Vector(5); ///< Measurement precision (diagonal of inverse covariance matrix)
+    private boolean transFlag; ///< Transformation exists?
+    private Matrix measTransformation; ///< Transformation of diagonalization (of meas. precision matrix)
+    private boolean scatFlag; ///< Scatterer present?
+    private SymMatrix scatTransformation = new SymMatrix(2); ///< Transformation of diagonalization (of scat. precision matrix)
+    private Vector scatResiduals = new Vector(2); ///< Scattering residuals (initial kinks if iterating)
+    private Vector scatPrecision = new Vector(2); ///< Scattering precision (diagonal of inverse covariance matrix)
+    private Matrix localDerivatives = new Matrix(0, 0); ///< Derivatives of measurement vs additional local (fit) parameters
+    private List<Integer> globalLabels = new ArrayList<Integer>(); ///< Labels of global (MP-II) derivatives
+    private Matrix globalDerivatives = new Matrix(0, 0); ///< Derivatives of measurement vs additional global (MP-II) parameters
+
+/// Create a point.
+    /**
+     * Create point on (initial) trajectory. Needs transformation jacobian from
+     * previous point. \param [in] aJacobian Transformation jacobian from
+     * previous point
+     */
+    public GblPoint(Matrix aJacobian) {
+
+        theLabel = 0;
+        theOffset = 0;
+        measDim = 0;
+        transFlag = false;
+        //measTransformation() 
+        scatFlag = false;
+        //localDerivatives()
+        //globalLabels()
+        //globalDerivatives()
+
+        for (int i = 0; i < 5; ++i) {
+            for (int j = 0; j < 5; ++j) {
+                p2pJacobian.set(i, j, aJacobian.get(i, j));
+            }
+        }
+    }
+
+    public GblPoint(SymMatrix aJacobian) {
+        theLabel = 0;
+        theOffset = 0;
+        p2pJacobian = new SymMatrix(aJacobian);
+        measDim = 0;
+        transFlag = false;
+        //measTransformation() 
+        scatFlag = false;
+        //localDerivatives() 
+        //globalLabels() 
+        //globalDerivatives()
+
+    }
+
+/// Add a measurement to a point.
+    /**
+     * Add measurement (in meas. system) with diagonal precision (inverse
+     * covariance) matrix. ((up to) 2D: position, 4D: slope+position, 5D:
+     * curvature+slope+position) \param [in] aProjection Projection from local
+     * to measurement system \param [in] aResiduals Measurement residuals \param
+     * [in] aPrecision Measurement precision (diagonal) \param [in] minPrecision
+     * Minimal precision to accept measurement
+     */
+    public void addMeasurement(Matrix aProjection,
+            Vector aResiduals, Vector aPrecision,
+            double minPrecision) {
+        measDim = aResiduals.getRowDimension();
+        int iOff = 5 - measDim;
+        for (int i = 0; i < measDim; ++i) {
+            measResiduals.set(iOff + i, aResiduals.get(i));
+            measPrecision.set(iOff + i, (aPrecision.get(i) >= minPrecision ? aPrecision.get(i) : 0.));
+            for (int j = 0; j < measDim; ++j) {
+                measProjection.set(iOff + i, iOff + j, aProjection.get(i, j));
+            }
+        }
+    }
+/// Add a measurement to a point.
+
+    /**
+     * Add measurement (in meas. system) with arbitrary precision (inverse
+     * covariance) matrix. Will be diagonalized. ((up to) 2D: position, 4D:
+     * slope+position, 5D: curvature+slope+position) \param [in] aProjection
+     * Projection from local to measurement system \param [in] aResiduals
+     * Measurement residuals \param [in] aPrecision Measurement precision
+     * (matrix) \param [in] minPrecision Minimal precision to accept measurement
+     */
+    public void addMeasurement(Matrix aProjection,
+            Vector aResiduals, SymMatrix aPrecision,
+            double minPrecision) {
+        measDim = aResiduals.getRowDimension();
+        EigenvalueDecomposition measEigen = new EigenvalueDecomposition(aPrecision);
+        measTransformation.ResizeTo(measDim, measDim);
+        measTransformation = measEigen.getV();
+        measTransformation.transposeInPlace();
+        transFlag = true;
+        Vector transResiduals = measTransformation.times(aResiduals);
+        Vector transPrecision = new Vector(measEigen.getRealEigenvalues());
+        Matrix transProjection = measTransformation.times(aProjection);
+        int iOff = 5 - measDim;
+        for (int i = 0; i < measDim; ++i) {
+            measResiduals.set(iOff + i, transResiduals.get(i));
+            measPrecision.set(iOff + i, (transPrecision.get(i) >= minPrecision ? transPrecision.get(i) : 0.));
+            for (int j = 0; j < measDim; ++j) {
+                measProjection.set(iOff + i, iOff + j, transProjection.get(i, j));
+            }
+        }
+    }
+
+/// Add a measurement to a point.
+    /**
+     * Add measurement in local system with diagonal precision (inverse
+     * covariance) matrix. ((up to) 2D: position, 4D: slope+position, 5D:
+     * curvature+slope+position) \param [in] aResiduals Measurement residuals
+     * \param [in] aPrecision Measurement precision (diagonal) \param [in]
+     * minPrecision Minimal precision to accept measurement
+     */
+    public void addMeasurement(Vector aResiduals,
+            Vector aPrecision, double minPrecision) {
+        measDim = aResiduals.getRowDimension();
+        int iOff = 5 - measDim;
+        for (int i = 0; i < measDim; ++i) {
+            measResiduals.set(iOff + i, aResiduals.get(i));
+            measPrecision.set(iOff + i,
+                    aPrecision.get(i) >= minPrecision ? aPrecision.get(i) : 0.);
+        }
+        measProjection.setToIdentity();
+    }
+
+/// Add a measurement to a point.
+    /**
+     * Add measurement in local system with arbitrary precision (inverse
+     * covariance) matrix. Will be diagonalized. ((up to) 2D: position, 4D:
+     * slope+position, 5D: curvature+slope+position) \param [in] aResiduals
+     * Measurement residuals \param [in] aPrecision Measurement precision
+     * (matrix) \param [in] minPrecision Minimal precision to accept measurement
+     */
+    public void addMeasurement(Vector aResiduals,
+            SymMatrix aPrecision, double minPrecision) {
+        measDim = aResiduals.getRowDimension();
+        EigenvalueDecomposition measEigen = new EigenvalueDecomposition(aPrecision);
+        measTransformation.ResizeTo(measDim, measDim);
+        measTransformation = measEigen.getV();
+        measTransformation.transposeInPlace();
+        transFlag = true;
+        Vector transResiduals = measTransformation.times(aResiduals);
+        Vector transPrecision = new Vector(measEigen.getRealEigenvalues());
+        int iOff = 5 - measDim;
+        for (int i = 0; i < measDim; ++i) {
+            measResiduals.set(iOff + i, transResiduals.get(i));
+            measPrecision.set(iOff + i, (transPrecision.get(i)) >= minPrecision ? transPrecision.get(i) : 0.);
+            for (int j = 0; j < measDim; ++j) {
+                measProjection.set(iOff + i, iOff + j, measTransformation.get(i, j));
+            }
+        }
+    }
+
+/// Check for measurement at a point.
+    /**
+     * Get dimension of measurement (0 = none). \return measurement dimension
+     */
+    int hasMeasurement() {
+        return measDim;
+    }
+
+/// Retrieve measurement of a point.
+    /**
+     * \param [out] aProjection Projection from (diagonalized) measurement to
+     * local system \param [out] aResiduals Measurement residuals \param [out]
+     * aPrecision Measurement precision (diagonal)
+     */
+    public void getMeasurement(SymMatrix aProjection, Vector aResiduals,
+            Vector aPrecision) {
+        aProjection = measProjection;
+        aResiduals = measResiduals;
+        aPrecision = measPrecision;
+    }
+
+/// Get measurement transformation (from diagonalization).
+    /**
+     * \param [out] aTransformation Transformation matrix
+     */
+    public void getMeasTransformation(Matrix aTransformation) {
+        aTransformation.ResizeTo(measDim, measDim);
+        if (transFlag) {
+            aTransformation = measTransformation;
+        } else {
+            aTransformation.UnitMatrix();
+        }
+    }
+
+/// Add a (thin) scatterer to a point.
+    /**
+     * Add scatterer with diagonal precision (inverse covariance) matrix.
+     * Changes local track direction.
+     *
+     * \param [in] aResiduals Scatterer residuals \param [in] aPrecision
+     * Scatterer precision (diagonal of inverse covariance matrix)
+     */
+    public void addScatterer(Vector aResiduals,
+            Vector aPrecision) {
+        scatFlag = true;
+        scatResiduals.set(0, aResiduals.get(0));
+        scatResiduals.set(1, aResiduals.get(1));
+        scatPrecision.set(0, aPrecision.get(0));
+        scatPrecision.set(1, aPrecision.get(1));
+        scatTransformation.setToIdentity();
+    }
+
+/// Add a (thin) scatterer to a point.
+    /**
+     * Add scatterer with arbitrary precision (inverse covariance) matrix. Will
+     * be diagonalized. Changes local track direction.
+     *
+     * The precision matrix for the local slopes is defined by the angular
+     * scattering error theta_0 and the scalar products c_1, c_2 of the offset
+     * directions in the local frame with the track direction:
+     *
+     * (1 - c_1*c_1 - c_2*c_2) | 1 - c_1*c_1 - c_1*c_2 | P =
+     * ~~~~~~~~~~~~~~~~~~~~~~~ * | | theta_0*theta_0 | - c_1*c_2 1 - c_2*c_2 |
+     *
+     * \param [in] aResiduals Scatterer residuals \param [in] aPrecision
+     * Scatterer precision (matrix)
+     */
+    public void addScatterer(Vector aResiduals,
+            SymMatrix aPrecision) {
+        scatFlag = true;
+        EigenvalueDecomposition scatEigen = new EigenvalueDecomposition(aPrecision);
+        Matrix aTransformation = scatEigen.getEigenVectors();
+        aTransformation.transposeInPlace();
+        Vector transResiduals = aTransformation.times(aResiduals);
+        Vector transPrecision = new Vector(scatEigen.getRealEigenvalues());
+        for (int i = 0; i < 2; ++i) {
+            scatResiduals.set(i, transResiduals.get(i));
+            scatPrecision.set(i, transPrecision.get(i));
+            for (int j = 0; j < 2; ++j) {
+                scatTransformation.set(i, j, aTransformation.get(i, j));
+            }
+        }
+    }
+
+/// Check for scatterer at a point.
+    boolean hasScatterer() {
+        return scatFlag;
+    }
+
+/// Retrieve scatterer of a point.
+    /**
+     * \param [out] aTransformation Scatterer transformation from
+     * diagonalization \param [out] aResiduals Scatterer residuals \param [out]
+     * aPrecision Scatterer precision (diagonal)
+     */
+    public void getScatterer(SymMatrix aTransformation, Vector aResiduals,
+            Vector aPrecision) {
+        aTransformation = scatTransformation;
+        aResiduals = scatResiduals;
+        aPrecision = scatPrecision;
+    }
+
+/// Get scatterer transformation (from diagonalization).
+    /**
+     * \param [out] aTransformation Transformation matrix
+     */
+    public void getScatTransformation(Matrix aTransformation) {
+        aTransformation.ResizeTo(2, 2);
+        if (scatFlag) {
+            for (int i = 0; i < 2; ++i) {
+                for (int j = 0; j < 2; ++j) {
+                    aTransformation.set(i, j, scatTransformation.get(i, j));
+                }
+            }
+        } else {
+            aTransformation.UnitMatrix();
+        }
+    }
+
+/// Add local derivatives to a point.
+    /**
+     * Point needs to have a measurement. \param [in] aDerivatives Local
+     * derivatives (matrix)
+     */
+    public void addLocals(Matrix aDerivatives) {
+        if (measDim != 0) {
+            localDerivatives.ResizeTo(aDerivatives.getRowDimension(), aDerivatives.getColumnDimension());
+            if (transFlag) {
+                localDerivatives = measTransformation.times(aDerivatives);
+            } else {
+                localDerivatives = aDerivatives;
+            }
+        }
+    }
+
+/// Retrieve number of local derivatives from a point.
+    int getNumLocals() {
+        return localDerivatives.getColumnDimension();
+    }
+
+/// Retrieve local derivatives from a point.
+    Matrix getLocalDerivatives() {
+        return localDerivatives;
+    }
+
+/// Add global derivatives to a point.
+    /**
+     * Point needs to have a measurement. \param [in] aLabels Global derivatives
+     * labels \param [in] aDerivatives Global derivatives (matrix)
+     */
+    public void addGlobals(List<Integer> aLabels,
+            Matrix aDerivatives) {
+        if (measDim != 0) {
+            globalLabels = aLabels;
+            globalDerivatives.ResizeTo(aDerivatives.getRowDimension(), aDerivatives.getColumnDimension());
+            if (transFlag) {
+                globalDerivatives = measTransformation.times(aDerivatives);
+            } else {
+                globalDerivatives = aDerivatives;
+            }
+
+        }
+    }
+
+/// Retrieve number of global derivatives from a point.
+    int getNumGlobals() {
+        return globalDerivatives.getColumnDimension();
+    }
+
+/// Retrieve global derivatives labels from a point.
+    List<Integer> getGlobalLabels() {
+        return globalLabels;
+    }
+
+/// Retrieve global derivatives from a point.
+    Matrix getGlobalDerivatives() {
+        return globalDerivatives;
+    }
+
+/// Define label of point (by GBLTrajectory constructor)
+    /**
+     * \param [in] aLabel Label identifying point
+     */
+    public void setLabel(int aLabel) {
+        theLabel = aLabel;
+    }
+
+/// Retrieve label of point
+    int getLabel() {
+        return theLabel;
+    }
+
+/// Define offset for point (by GBLTrajectory constructor)
+    /**
+     * \param [in] anOffset Offset number
+     */
+    public void setOffset(int anOffset) {
+        theOffset = anOffset;
+    }
+
+/// Retrieve offset for point
+    int getOffset() {
+        return theOffset;
+    }
+
+/// Retrieve point-to-(previous)point jacobian
+    Matrix getP2pJacobian() {
+        return p2pJacobian;
+    }
+
+/// Define jacobian to previous scatterer (by GBLTrajectory constructor)
+    /**
+     * \param [in] aJac Jacobian
+     */
+    public void addPrevJacobian(Matrix aJac) {
+        int ifail = 0;
+// to optimize: need only two last rows of inverse
+//	prevJacobian = aJac.InverseFast(ifail);
+//  block matrix algebra
+        Matrix CA = aJac.sub(2, 3, 3, 0).times(aJac.sub(3, 0, 0).inverse()); // C*A^-1
+        Matrix DCAB = aJac.sub(2, 3, 3).minus(CA.times(aJac.sub(3, 2, 0, 3))); // D - C*A^-1 *B
+        DCAB = DCAB.inverse();
+        prevJacobian.placeAt(DCAB, 3, 3);
+        prevJacobian.placeAt(DCAB.times(CA).uminus(), 3, 0);
+    }
+//
+/// Define jacobian to next scatterer (by GBLTrajectory constructor)
+
+    /**
+     * \param [in] aJac Jacobian
+     */
+    public void addNextJacobian(Matrix aJac) {
+        nextJacobian = aJac;
+    }
+
+/// Retrieve derivatives of local track model
+    /**
+     * Linearized track model: F_u(q/p,u',u) = J*u + S*u' + d*q/p, W is inverse
+     * of S, negated for backward propagation. \param [in] aDirection
+     * Propagation direction (>0 forward, else backward) \param [out] matW W
+     * \param [out] matWJ W*J \param [out] vecWd W*d \exception
+     * std::overflow_error : matrix S is singular.
+     */
+    public void getDerivatives(int aDirection, Matrix matW, Matrix matWJ,
+            Vector vecWd) {
+
+        Matrix matJ;
+        Vector vecd;
+        if (aDirection < 1) {
+            matJ = prevJacobian.sub(2, 3, 3);
+            matW = prevJacobian.sub(2, 3, 1).uminus();
+            vecd = prevJacobian.subCol(2, 0, 3);
+        } else {
+            matJ = nextJacobian.sub(2, 3, 3);
+            matW = nextJacobian.sub(2, 3, 1);
+            vecd = nextJacobian.subCol(2, 0, 3);
+        }
+
+        matW = matW.inverse();
+//	if (!matW.InvertFast()) {
+//		std::cout << " getDerivatives failed to invert matrix: "
+//				<< matW << "\n";
+//		std::cout
+//				<< " Possible reason for singular matrix: multiple GblPoints at same arc-length"
+//				<< "\n";
+//		throw std::overflow_error("Singular matrix inversion exception");
+//	}
+        matWJ = matW.times(matJ);
+        vecWd = matW.times(vecd);
+
+    }
+//
+/// Print GblPoint
+
+    /**
+     * \param [in] level print level (0: minimum, >0: more)
+     */
+    public void printPoint(int level) {
+        StringBuffer sb = new StringBuffer("GblPoint");
+        if (theLabel != 0) {
+            sb.append(", label " + theLabel);
+            if (theOffset >= 0) {
+                sb.append(", offset " + theOffset);
+            }
+        }
+        if (measDim != 0) {
+            sb.append(", " + measDim + " measurements");
+        }
+        if (scatFlag) {
+            sb.append(", scatterer");
+        }
+        if (transFlag) {
+            sb.append(", diagonalized");
+        }
+        if (localDerivatives != null) {
+            sb.append(", " + localDerivatives.getColumnDimension()
+                    + " local derivatives");
+        }
+        if (globalDerivatives != null) {
+            sb.append(", " + globalDerivatives.getColumnDimension()
+                    + " global derivatives");
+        }
+        sb.append("\n");
+        if (level > 0) {
+            if (measDim != 0) {
+                sb.append("  Measurement" + "\n");
+                sb.append("   Projection: " + "\n" + measProjection
+                        + "\n");
+                sb.append("   Residuals: " + measResiduals + "\n");
+                sb.append("   Precision: " + measPrecision + "\n");
+            }
+            if (scatFlag) {
+                sb.append("  Scatterer" + "\n");
+                sb.append("   Residuals: " + scatResiduals + "\n");
+                sb.append("   Precision: " + scatPrecision + "\n");
+            }
+            if (localDerivatives.getColumnDimension() != 0) {
+                sb.append("  Local Derivatives:" + "\n");
+                localDerivatives.print(4, 6);
+            }
+            if (globalDerivatives.getColumnDimension() != 0) {
+                sb.append("  Global Labels:");
+                for (int i = 0; i < globalLabels.size(); ++i) {
+                    sb.append(" " + globalLabels.get(i));
+                }
+                sb.append("\n");
+                sb.append("  Global Derivatives:" + "\n");
+                globalDerivatives.print(4, 6);
+            }
+            sb.append("  Jacobian " + "\n");
+            sb.append("   Point-to-point " + "\n" + p2pJacobian
+                    + "\n");
+            if (theLabel != 0) {
+                sb.append("   To previous offset " + "\n" + prevJacobian
+                        + "\n");
+                sb.append("   To next offset " + "\n" + nextJacobian
+                        + "\n");
+            }
+        }
+        System.out.println(sb.toString());
+    }
+

java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl

GblTrajectory.java 918 -> 919

--- java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/GblTrajectory.java	2014-08-28 08:14:53 UTC (rev 918)
+++ java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/GblTrajectory.java	2014-08-28 15:31:06 UTC (rev 919)
@@ -5,20 +5,28 @@

  * 
  * @author Per Hansson Adrian <[log in to unmask]>
  *

+ * @author Norman A Graf
+ *
+ * @version $Id:
+ *

*/

+import static java.lang.Math.max;
+import java.util.ArrayList;

 import java.util.List;

+import org.hps.recon.tracking.gbl.matrix.BorderedBandMatrix;
+import org.hps.recon.tracking.gbl.matrix.Matrix;
+import org.hps.recon.tracking.gbl.matrix.SymMatrix;
+import org.hps.recon.tracking.gbl.matrix.VVector;
+import org.hps.recon.tracking.gbl.matrix.Vector;

 public class GblTrajectory {
 
 	public GblTrajectory(List<GblPoint> listOfPoints) {

-		// TODO Auto-generated constructor stub

+		this(listOfPoints, 0, new SymMatrix(0), false, false, false);

-	public boolean isValid() {
-		// TODO Auto-generated method stub
-		return false;
-	}

 
 	public void fit(double m_chi2, int m_ndf, int m_lost_weight) {
 		// TODO Auto-generated method stub

@@ -30,4 +38,1283 @@

+    /**
+     * \mainpage General information
+     *
+     * \section intro_sec Introduction
+     *
+     * For a track with an initial trajectory from a prefit of the measurements
+     * (internal seed) or an external prediction (external seed) the description
+     * of multiple scattering is added by offsets in a local system. Along the
+     * initial trajectory points are defined with can describe a measurement or
+     * a (thin) scatterer or both. Measurements are arbitrary functions of the
+     * local track parameters at a point (e.g. 2D: position, 4D:
+     * direction+position). The refit provides corrections to the local track
+     * parameters (in the local system) and the corresponding covariance matrix
+     * at any of those points. Non-diagonal covariance matrices will be
+     * diagonalized internally. Outliers can be down-weighted by use of
+     * M-estimators.
+     *
+     * The broken lines trajectory is defined by (2D) offsets at the first and
+     * last point and all points with a scatterer. The prediction for a
+     * measurement is obtained by interpolation of the enclosing offsets and for
+     * triplets of adjacent offsets kink angles are determined. This requires
+     * for all points the jacobians for propagation to the previous and next
+     * offset. These are calculated from the point-to-point jacobians along the
+     * initial trajectory. The sequence of points has to be strictly monotonic
+     * in arc-length.
+     *
+     * Additional local or global parameters can be added and the trajectories
+     * can be written to special binary files for calibration and alignment with
+     * Millepede-II. (V. Blobel, NIM A, 566 (2006), pp. 5-13).
+     *
+     * Besides simple trajectories describing the path of a single particle
+     * composed trajectories are supported. These are constructed from the
+     * trajectories of multiple particles and some external parameters (like
+     * those describing a decay) and transformations at the first points from
+     * the external to the local track parameters.
+     *
+     * The conventions for the coordinate systems follow: Derivation of
+     * Jacobians for the propagation of covariance matrices of track parameters
+     * in homogeneous magnetic fields A. Strandlie, W. Wittek, NIM A, 566 (2006)
+     * 687-698.
+     *
+     * \section call_sec Calling sequence
+     *
+     * -# Create list of points on initial trajectory:\n
+     * <tt>List<GblPoint> list</tt>
+     * -# For all points on initial trajectory: - Create points and add
+     * appropriate attributes:\n - <tt>point = gbl::GblPoint(..)</tt>
+     * - <tt>point.addMeasurement(..)</tt>
+     * - Add additional local or global parameters to measurement:\n -
+     * <tt>point.addLocals(..)</tt>
+     * - <tt>point.addGlobals(..)</tt>
+     * - <tt>point.addScatterer(..)</tt>
+     * - Add point (ordered by arc length) to list:\n
+     * <tt>list.add(point)</tt>
+     * -# Create (simple) trajectory from list of points:\n
+     * <tt>traj = gbl::GblTrajectory (list)</tt>
+     * -# Optionally with external seed:\n
+     * <tt>traj = gbl::GblTrajectory (list,seed)</tt>
+     * -# Optionally check validity of trajectory:\n
+     * <tt>if (not traj.isValid()) .. //abort</tt>
+     * -# Fit trajectory, return error code, get Chi2, Ndf (and weight lost by
+     * M-estimators):\n
+     * <tt>ierr = traj.fit(..)</tt>
+     * -# For any point on initial trajectory: - Get corrections and covariance
+     * matrix for track parameters:\n
+     * <tt>[..] = traj.getResults(label)</tt>
+     * -# Optionally write trajectory to MP binary file:\n
+     * <tt>traj.milleOut(..)</tt>
+     *
+     * \section loc_sec Local system and local parameters At each point on the
+     * trajectory a local coordinate system with local track parameters has to
+     * be defined. The first of the five parameters describes the bending, the
+     * next two the direction and the last two the position (offsets). The
+     * curvilinear system (T,U,V) with parameters (q/p, lambda, phi, x_t, y_t)
+     * is well suited.
+     *
+     * \section impl_sec Implementation
+     *
+     * Matrices are implemented with ROOT (root.cern.ch). User input or output
+     * is in the form of TMatrices. Internally SMatrices are used for fixes
+     * sized and simple matrices based on List<> for variable sized matrices.
+     *
+     * \section ref_sec References - V. Blobel, C. Kleinwort, F. Meier, Fast
+     * alignment of a complex tracking detector using advanced track models,
+     * Computer Phys. Communications (2011), doi:10.1016/j.cpc.2011.03.017 - C.
+     * Kleinwort, General Broken Lines as advanced track fitting method, NIM A,
+     * 673 (2012), 107-110, doi:10.1016/j.nima.2012.01.024
+     */
+    int numAllPoints; ///< Number of all points on trajectory
+    List< Integer> numPoints = new ArrayList<Integer>(); ///< Number of points on (sub)trajectory
+    int numTrajectories; ///< Number of trajectories (in composed trajectory)
+    int numOffsets; ///< Number of (points with) offsets on trajectory
+    int numInnerTrans; ///< Number of inner transformations to external parameters
+    int numCurvature; ///< Number of curvature parameters (0 or 1) or external parameters
+    int numParameters; ///< Number of fit parameters
+    int numLocals; ///< Total number of (additional) local parameters
+    int numMeasurements; ///< Total number of measurements
+    int externalPoint; ///< Label of external point (or 0)
+    boolean constructOK; ///< Trajectory has been successfully constructed (ready for fit/output)
+    boolean fitOK; ///< Trajectory has been successfully fitted (results are valid)
+    List< Integer> theDimension = new ArrayList<Integer>(); ///< List of active dimensions (0=u1, 1=u2) in fit
+    List<List<GblPoint>> thePoints = new ArrayList<List<GblPoint>>(); ///< (list of) List of points on trajectory
+    List<GblData> theData = new ArrayList<GblData>(); ///< List of data blocks
+    List<Integer> measDataIndex = new ArrayList<Integer>(); ///< mapping points to data blocks from measurements
+    List<Integer> scatDataIndex; ///< mapping points to data blocks from scatterers
+    List<Integer> externalIndex; ///< List of fit parameters used by external seed
+    SymMatrix externalSeed; ///< Precision (inverse covariance matrix) of external seed
+    List<Matrix> innerTransformations; ///< Transformations at innermost points of
+    // composed trajectory (from common external parameters)
+    Matrix externalDerivatives; // Derivatives for external measurements of composed trajectory
+    Vector externalMeasurements; // Residuals for external measurements of composed trajectory
+    Vector externalPrecisions; // Precisions for external measurements of composed trajectory
+    VVector theVector; ///< Vector of linear equation system
+    BorderedBandMatrix theMatrix = new BorderedBandMatrix(); ///< (Bordered band) matrix of linear equation system
+
+///// Create new (simple) trajectory from list of points.
+//    /**
+//     * Curved trajectory in space (default) or without curvature (q/p) or in one
+//     * plane (u-direction) only. \param [in] aPointList List of points \param
+//     * [in] flagCurv Use q/p \param [in] flagU1dir Use in u1 direction \param
+//     * [in] flagU2dir Use in u2 direction
+//     */
+//    GblTrajectory(List<GblPoint> aPointList,
+//            boolean flagCurv, boolean flagU1dir, boolean flagU2dir)
+//    {
+//        numAllPoints = aPointList.size();
+//        //numPoints()
+//        numOffsets = 0;
+//        numInnerTrans = 0;
+//        numCurvature = (flagCurv ? 1 : 0);
+//        numParameters = 0;
+//        numLocals = 0;
+//        numMeasurements = 0;
+//        externalPoint = 0;
+//                //externalSeed()
+//        //innerTransformations()
+//        //externalDerivatives()
+//        //externalMeasurements()
+//        // externalPrecisions() 
+//
+//        if (flagU1dir)
+//        {
+//            theDimension.add(0);
+//        }
+//        if (flagU2dir)
+//        {
+//            theDimension.add(1);
+//        }
+//        // simple (single) trajectory
+//        thePoints.add(aPointList);
+//        numPoints.add(numAllPoints);
+////	construct(); // construct trajectory
+//    }
+/// Create new (simple) trajectory from list of points with external seed.
+    /**
+     * Curved trajectory in space (default) or without curvature (q/p) or in one
+     * plane (u-direction) only. \param [in] aPointList List of points \param
+     * [in] aLabel (Signed) label of point for external seed (<0: in front, >0:
+     * after point, slope changes at scatterer!) \param [in] aSeed Precision
+     * matrix of external seed \param [in] flagCurv Use q/p \param [in]
+     * flagU1dir Use in u1 direction \param [in] flagU2dir Use in u2 direction
+     */
+    GblTrajectory(List<GblPoint> aPointList,
+            int aLabel, SymMatrix aSeed, boolean flagCurv,
+            boolean flagU1dir, boolean flagU2dir)
+    {
+        numAllPoints = aPointList.size();
+        //numPoints(), 
+        numOffsets = 0;
+        numInnerTrans = 0;
+        numCurvature = (flagCurv ? 1 : 0);
+        numParameters = 0;
+        numLocals = 0;
+        numMeasurements = 0;
+        //externalPoint(aLabel) 
+        //theDimension(),
+        //thePoints(), 
+        //theData(), 
+        //measDataIndex(), 
+        //scatDataIndex(), 
+        //externalIndex(), 
+        //externalSeed(aSeed), 
+        //innerTransformations(), 
+        //externalDerivatives(), 
+        //externalMeasurements(), 
+        //externalPrecisions() {
+
+        if (flagU1dir)
+        {
+            theDimension.add(0);
+        }
+        if (flagU2dir)
+        {
+            theDimension.add(1);
+        }
+        // simple (single) trajectory
+        thePoints.add(aPointList);
+        numPoints.add(numAllPoints);
+        construct(); // construct trajectory
+    }
+
+///// Create new composed trajectory from list of points and transformations.
+///**
+// * Composed of curved trajectory in space.
+// * \param [in] aPointsAndTransList List containing pairs with list of points and transformation (at inner (first) point)
+// */
+//GblTrajectory(
+//		const List<std::pair<List<GblPoint>, TMatrixD> > &aPointsAndTransList) :
+//		numAllPoints(), numPoints(), numOffsets(0), numInnerTrans(
+//				aPointsAndTransList.size()), numParameters(0), numLocals(0), numMeasurements(
+//				0), externalPoint(0), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalIndex(), externalSeed(), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
+//
+//	for ( int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
+//		thePoints.add(aPointsAndTransList[iTraj].first);
+//		numPoints.add(thePoints.back().size());
+//		numAllPoints += numPoints.back();
+//		innerTransformations.add(aPointsAndTransList[iTraj].second);
+//	}
+//	theDimension.add(0);
+//	theDimension.add(1);
+//	numCurvature = innerTransformations[0].GetNcols();
+//	construct(); // construct (composed) trajectory
+//}
+//
+///// Create new composed trajectory from list of points and transformations with (independent) external measurements.
+///**
+// * Composed of curved trajectory in space.
+// * \param [in] aPointsAndTransList List containing pairs with list of points and transformation (at inner (first) point)
+// * \param [in] extDerivatives Derivatives of external measurements vs external parameters
+// * \param [in] extMeasurements External measurements (residuals)
+// * \param [in] extPrecisions Precision of external measurements
+// */
+//GblTrajectory(
+//		const List<std::pair<List<GblPoint>, TMatrixD> > &aPointsAndTransList,
+//		const TMatrixD &extDerivatives, const TVectorD &extMeasurements,
+//		const TVectorD &extPrecisions) :
+//		numAllPoints(), numPoints(), numOffsets(0), numInnerTrans(
+//				aPointsAndTransList.size()), numParameters(0), numLocals(0), numMeasurements(
+//				0), externalPoint(0), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalIndex(), externalSeed(), innerTransformations(), externalDerivatives(
+//				extDerivatives), externalMeasurements(extMeasurements), externalPrecisions(
+//				extPrecisions) {
+//
+//	for ( int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
+//		thePoints.add(aPointsAndTransList[iTraj].first);
+//		numPoints.add(thePoints.back().size());
+//		numAllPoints += numPoints.back();
+//		innerTransformations.add(aPointsAndTransList[iTraj].second);
+//	}
+//	theDimension.add(0);
+//	theDimension.add(1);
+//	numCurvature = innerTransformations[0].GetNcols();
+//	construct(); // construct (composed) trajectory
+//}
+//
+//~GblTrajectory() {
+//}
+//
+/// Retrieve validity of trajectory
+    boolean isValid()
+    {
+        return constructOK;
+    }
+
+/// Retrieve number of point from trajectory
+    int getNumPoints()
+    {
+        return numAllPoints;
+    }
+
+/// Construct trajectory from list of points.
+    /**
+     * Trajectory is prepared for fit or output to binary file, may consists of
+     * sub-trajectories.
+     */
+    void construct()
+    {
+
+        constructOK = false;
+        fitOK = false;
+        int aLabel = 0;
+        // loop over trajectories
+        numTrajectories = thePoints.size();
+        //std::cout << " numTrajectories: " << numTrajectories << ", "<< innerTransformations.size()  << std::endl;
+//	for ( int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
+//		List<GblPoint>::iterator itPoint;
+//		for (itPoint = thePoints[iTraj].begin();
+//				itPoint < thePoints[iTraj].end(); ++itPoint) {
+//			numLocals = std::max(numLocals, itPoint->getNumLocals());
+//			numMeasurements += itPoint->hasMeasurement();
+//			itPoint->setLabel(++aLabel);
+//		}
+//	}
+        for (List<GblPoint> list : thePoints)
+        {
+            for (GblPoint p : list)
+            {
+                numLocals = max(numLocals, p.getNumLocals());
+                numMeasurements += p.hasMeasurement();
+                p.setLabel(++aLabel);
+            }
+        }
+        defineOffsets();
+        calcJacobians();
+//	try {
+		prepare();
+//	} catch (std::overflow_error &e) {
+//		std::cout << " GblTrajectory construction failed: " << e.what()
+//				<< std::endl;
+//		return;
+//	}
+        constructOK = true;
+        // number of fit parameters
+        numParameters = (numOffsets - 2 * numInnerTrans) * theDimension.size()
+                + numCurvature + numLocals;
+    }
+//
+/// Define offsets from list of points.
+
+    /**
+     * Define offsets at points with scatterers and first and last point. All
+     * other points need interpolation from adjacent points with offsets.
+     */
+    void defineOffsets()
+    {
+
+        // loop over trajectories
+        for (int iTraj = 0; iTraj < numTrajectories; ++iTraj)
+        {
+            List<GblPoint> list = thePoints.get(iTraj);
+            int size = list.size();
+            // first point is offset
+//		thePoints[iTraj].front().setOffset(numOffsets++);
+            list.get(0).setOffset(numOffsets++);		// intermediate scatterers are offsets
+//		List<GblPoint>::iterator itPoint;
+//		for (itPoint = thePoints[iTraj].begin() + 1;
+//				itPoint < thePoints[iTraj].end() - 1; ++itPoint) {
+//			if (itPoint->hasScatterer()) {
+//				itPoint->setOffset(numOffsets++);
+//			} else {
+//				itPoint->setOffset(-numOffsets);
+//			}
+//		}
+            for (int i = 1; i < size - 1; ++i)
+            {
+                GblPoint p = list.get(i);
+                if (p.hasScatterer())
+                {
+                    p.setOffset(numOffsets++);
+                } else
+                {
+                    p.setOffset(-numOffsets);
+                }
+
+            }
+            // last point is offset
+//		thePoints[iTraj].back().setOffset(numOffsets++);
+            list.get(size - 1).setOffset(numOffsets++);
+        }
+    }
+
+/// Calculate Jacobians to previous/next scatterer from point to point ones.
+    void calcJacobians()
+    {
+
+        Matrix scatJacobian = new Matrix(5, 5);
+        // loop over trajectories
+        for (int iTraj = 0; iTraj < numTrajectories; ++iTraj)
+        {
+            List<GblPoint> list = thePoints.get(iTraj);
+            int size = list.size();
+            // forward propagation (all)
+            GblPoint previousPoint = list.get(0);
+            int numStep = 0;
+            for (int i = 1; i < size; ++i)
+            {
+                GblPoint p = list.get(i);
+                if (numStep == 0)
+                {
+                    scatJacobian = p.getP2pJacobian();
+                } else
+                {
+                    scatJacobian = p.getP2pJacobian().times(scatJacobian);
+                }
+                numStep++;
+                p.addPrevJacobian(scatJacobian);// iPoint -> previous scatterer
+                if (p.getOffset() >= 0)
+                {
+                    previousPoint.addNextJacobian(scatJacobian); // lastPoint -> next scatterer
+                    numStep = 0;
+                    previousPoint = p;
+                }
+            }
+//            List<GblPoint>::iterator itPoint;
+//            for (itPoint = thePoints[iTraj].begin() + 1;
+//                    itPoint < thePoints[iTraj].end(); ++itPoint)
+//            {
+//                if (numStep == 0)
+//                {
+//                    scatJacobian = itPoint -> getP2pJacobian();
+//                } else
+//                {
+//                    scatJacobian = itPoint -> getP2pJacobian() * scatJacobian;
+//                }
+//                numStep++;
+//                itPoint -> addPrevJacobian(scatJacobian); // iPoint -> previous scatterer
+//                if (itPoint -> getOffset() >= 0)
+//                {
+//                    previousPoint -> addNextJacobian(scatJacobian); // lastPoint -> next scatterer
+//                    numStep = 0;
+//                    previousPoint =  & ( * itPoint);
+//                }
+//            }
+//            // backward propagation (without scatterers)
+//            for (itPoint = thePoints[iTraj].end() - 1;
+//                    itPoint > thePoints[iTraj].begin(); --itPoint)
+//            {
+//                if (itPoint -> getOffset() >= 0)
+//                {
+//                    scatJacobian = itPoint -> getP2pJacobian();
+//                    continue; // skip offsets
+//                }
+//                itPoint -> addNextJacobian(scatJacobian); // iPoint -> next scatterer
+//                scatJacobian = scatJacobian * itPoint -> getP2pJacobian();
+//            }
+            // backward propagation (without scatterers)            
+            for (int i = size - 1; i > 0; --i)
+            {
+                GblPoint p = list.get(i);
+                if (p.getOffset() >= 0)
+                {
+                    scatJacobian = p.getP2pJacobian();
+                    continue; // skip offsets
+                }
+                p.addNextJacobian(scatJacobian); // iPoint -> next scatterer
+                scatJacobian = scatJacobian.times(p.getP2pJacobian());
+            }
+        }
+    }
+//
+///// Get jacobian for transformation from fit to track parameters at point.
+///**
+// * Jacobian broken lines (q/p,..,u_i,u_i+1..) to track (q/p,u',u) parameters
+// * including additional local parameters.
+// * \param [in] aSignedLabel (Signed) label of point for external seed
+// * (<0: in front, >0: after point, slope changes at scatterer!)
+// * \return List of fit parameters with non zero derivatives and
+// * corresponding transformation matrix
+// */
+//std::pair<List< int>, TMatrixD> getJacobian(
+//		int aSignedLabel) const {
+//
+//	 int nDim = theDimension.size();
+//	 int nCurv = numCurvature;
+//	 int nLocals = numLocals;
+//	 int nBorder = nCurv + nLocals;
+//	 int nParBRL = nBorder + 2 * nDim;
+//	 int nParLoc = nLocals + 5;
+//	List< int> anIndex;
+//	anIndex.reserve(nParBRL);
+//	TMatrixD aJacobian(nParLoc, nParBRL);
+//	aJacobian.Zero();
+//
+//	 int aLabel = abs(aSignedLabel);
+//	 int firstLabel = 1;
+//	 int lastLabel = 0;
+//	 int aTrajectory = 0;
+//	// loop over trajectories
+//	for ( int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
+//		aTrajectory = iTraj;
+//		lastLabel += numPoints[iTraj];
+//		if (aLabel <= lastLabel)
+//			break;
+//		if (iTraj < numTrajectories - 1)
+//			firstLabel += numPoints[iTraj];
+//	}
+//	int nJacobian; // 0: prev, 1: next
+//	// check consistency of (index, direction)
+//	if (aSignedLabel > 0) {
+//		nJacobian = 1;
+//		if (aLabel >= lastLabel) {
+//			aLabel = lastLabel;
+//			nJacobian = 0;
+//		}
+//	} else {
+//		nJacobian = 0;
+//		if (aLabel <= firstLabel) {
+//			aLabel = firstLabel;
+//			nJacobian = 1;
+//		}
+//	}
+//	const GblPoint aPoint = thePoints[aTrajectory][aLabel - firstLabel];
+//	List< int> labDer(5);
+//	SMatrix55 matDer;
+//	getFitToLocalJacobian(labDer, matDer, aPoint, 5, nJacobian);
+//
+//	// from local parameters
+//	for ( int i = 0; i < nLocals; ++i) {
+//		aJacobian(i + 5, i) = 1.0;
+//		anIndex.add(i + 1);
+//	}
+//	// from trajectory parameters
+//	 int iCol = nLocals;
+//	for ( int i = 0; i < 5; ++i) {
+//		if (labDer[i] > 0) {
+//			anIndex.add(labDer[i]);
+//			for ( int j = 0; j < 5; ++j) {
+//				aJacobian(j, iCol) = matDer(j, i);
+//			}
+//			++iCol;
+//		}
+//	}
+//	return std::make_pair(anIndex, aJacobian);
+//}
+
+/// Get (part of) jacobian for transformation from (trajectory) fit to track parameters at point.
+    /**
+     * Jacobian broken lines (q/p,..,u_i,u_i+1..) to local (q/p,u',u)
+     * parameters. \param [out] anIndex List of fit parameters with non zero
+     * derivatives \param [out] aJacobian Corresponding transformation matrix
+     * \param [in] aPoint Point to use \param [in] measDim Dimension of
+     * 'measurement' (<=2: calculate only offset part, >2: complete matrix)
+     * \param [in] nJacobian Direction (0: to previous offset, 1: to next
+     * offset)
+     */
+    void getFitToLocalJacobian(List<Integer> anIndex,
+            Matrix aJacobian, GblPoint aPoint, int measDim,
+            int nJacobian)
+    {
+
+        int nDim = theDimension.size();
+        int nCurv = numCurvature;
+        int nLocals = numLocals;
+
+        int nOffset = aPoint.getOffset();
+
+        if (nOffset < 0) // need interpolation
+        {
+            Matrix prevW = new Matrix(2, 2);
+            Matrix prevWJ = new Matrix(2, 2);
+            Matrix nextW = new Matrix(2, 2);
+            Matrix nextWJ = new Matrix(2, 2);
+            Matrix matN = new Matrix(2, 2);
+            Vector prevWd = new Vector(2);
+            Vector nextWd = new Vector(2);
+            int ierr;
+            aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
+            aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W-, W- * J-, W- * d-
+            Matrix sumWJ = prevWJ.plus(nextWJ);
+//?		matN = sumWJ.inverse(ierr); // N = (W- * J- + W+ * J+)^-1
+            // derivatives for u_int
+            Matrix prevNW = matN.times(prevW); // N * W-
+            Matrix nextNW = matN.times(nextW); // N * W+
+            Vector prevNd = matN.times(prevWd); // N * W- * d-
+            Vector nextNd = matN.times(nextWd); // N * W+ * d+
+
+            int iOff = nDim * (-nOffset - 1) + nLocals + nCurv + 1; // first offset ('i' in u_i)
+
+            // local offset
+            if (nCurv > 0)
+            {
+                Vector negDiff = prevNd.minus(nextNd).uminus();
+                aJacobian.placeInCol(negDiff, 3, 0); // from curvature
+                anIndex.set(0, nLocals + 1);
+            }
+            aJacobian.placeAt(prevNW, 3, 1); // from 1st Offset
+            aJacobian.placeAt(nextNW, 3, 3); // from 2nd Offset
+            for (int i = 0; i < nDim; ++i)
+            {
+                anIndex.set(1 + theDimension.get(i), iOff + i);
+                anIndex.set(3 + theDimension.get(i), iOff + nDim + i);
+            }
+//
+////		// local slope and curvature
+////		if (measDim > 2) {
+////			// derivatives for u'_int
+////			const SMatrix22 prevWPN(nextWJ * prevNW); // W+ * J+ * N * W-
+////			const SMatrix22 nextWPN(prevWJ * nextNW); // W- * J- * N * W+
+////			const SVector2 prevWNd(nextWJ * prevNd); // W+ * J+ * N * W- * d-
+////			const SVector2 nextWNd(prevWJ * nextNd); // W- * J- * N * W+ * d+
+////			if (nCurv > 0) {
+////				aJacobian(0, 0) = 1.0;
+////				aJacobian.Place_in_col(prevWNd - nextWNd, 1, 0); // from curvature
+////			}
+////			aJacobian.Place_at(-prevWPN, 1, 1); // from 1st Offset
+////			aJacobian.Place_at(nextWPN, 1, 3); // from 2nd Offset
+////		}
+//	} else { // at point
+            // anIndex must be sorted
+            // forward : iOff2 = iOff1 + nDim, index1 = 1, index2 = 3
+            // backward: iOff2 = iOff1 - nDim, index1 = 3, index2 = 1
+            int iOff1 = nDim * nOffset + nCurv + nLocals + 1; // first offset ('i' in u_i)
+            int index1 = 3 - 2 * nJacobian; // index of first offset
+            int iOff2 = iOff1 + nDim * (nJacobian * 2 - 1); // second offset ('i' in u_i)
+            int index2 = 1 + 2 * nJacobian; // index of second offset
+            // local offset
+            aJacobian.set(3, index1, 1.0); // from 1st Offset
+            aJacobian.set(4, index1 + 1, 1.0);
+            for (int i = 0; i < nDim; ++i)
+            {
+                anIndex.set(index1 + theDimension.get(i), iOff1 + i);
+            }
+
+            // local slope and curvature
+            if (measDim > 2)
+            {
+                Matrix matW = new Matrix(2, 2);
+                Matrix matWJ = new Matrix(2, 2);
+                Vector vecWd = new Vector(2);
+                aPoint.getDerivatives(nJacobian, matW, matWJ, vecWd); // W, W * J, W * d
+                double sign = (nJacobian > 0) ? 1. : -1.;
+                if (nCurv > 0)
+                {
+                    aJacobian.set(0, 0, 1.0);
+                    aJacobian.placeInCol(vecWd.timesScalar(-sign), 1, 0); // from curvature
+                    anIndex.set(0, nLocals + 1);
+                }
+                aJacobian.placeAt(matWJ.times(-sign), 1, index1); // from 1st Offset
+                aJacobian.placeAt(matW.times(sign), 1, index2); // from 2nd Offset
+                for (int i = 0; i < nDim; ++i)
+                {
+                    anIndex.set(index2 + theDimension.get(i), iOff2 + i);
+                }
+            }
+        }
+    }
+
+/// Get jacobian for transformation from (trajectory) fit to kink parameters at point.
+    /**
+     * Jacobian broken lines (q/p,..,u_i-1,u_i,u_i+1..) to kink (du')
+     * parameters. \param [out] anIndex List of fit parameters with non zero
+     * derivatives \param [out] aJacobian Corresponding transformation matrix
+     * \param [in] aPoint Point to use
+     */
+    void getFitToKinkJacobian(List< Integer> anIndex,
+            Matrix aJacobian, GblPoint aPoint)
+    {
+
+        //nb aJacobian has dimension 2,7
+        int nDim = theDimension.size();
+        int nCurv = numCurvature;
+        int nLocals = numLocals;
+
+        int nOffset = aPoint.getOffset();
+
+        Matrix prevW = new Matrix(2, 2);
+        Matrix prevWJ = new Matrix(2, 2);
+        Matrix nextW = new Matrix(2, 2);
+        Matrix nextWJ = new Matrix(2, 2);
+        Vector prevWd = new Vector(2);
+        Vector nextWd = new Vector(2);
+        aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
+        aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W-, W- * J-, W- * d-
+        Matrix sumWJ = prevWJ.plus(nextWJ); // W- * J- + W+ * J+
+        Vector sumWd = prevWd.plus(nextWd); // W+ * d+ + W- * d-
+
+        int iOff = (nOffset - 1) * nDim + nCurv + nLocals + 1; // first offset ('i' in u_i)
+
+        // local offset
+        if (nCurv > 0)
+        {
+            aJacobian.placeInCol(sumWd.uminus(), 0, 0); // from curvature
+            anIndex.set(0, nLocals + 1);
+        }
+        aJacobian.placeAt(prevW, 0, 1); // from 1st Offset
+        aJacobian.placeAt(sumWJ.uminus(), 0, 3); // from 2nd Offset
+        aJacobian.placeAt(nextW, 0, 5); // from 1st Offset
+        for (int i = 0; i < nDim; ++i)
+        {
+            anIndex.set(1 + theDimension.get(i), iOff + i);
+            anIndex.set(3 + theDimension.get(i), iOff + nDim + i);
+            anIndex.set(5 + theDimension.get(i), iOff + nDim * 2 + i);
+        }
+    }
+
+///// Get fit results at point.
+///**
+// * Get corrections and covariance matrix for local track and additional parameters
+// * in forward or backward direction.
+// * \param [in] aSignedLabel (Signed) label of point on trajectory
+// * (<0: in front, >0: after point, slope changes at scatterer!)
+// * \param [out] localPar Corrections for local parameters
+// * \param [out] localCov Covariance for local parameters
+// * \return error code (non-zero if trajectory not fitted successfully)
+// */
+// int getResults(int aSignedLabel, TVectorD &localPar,
+//		SymMatrix &localCov) const {
+//	if (! fitOK)
+//		return 1;
+//	std::pair<List< int>, TMatrixD> indexAndJacobian =
+//			getJacobian(aSignedLabel);
+//	 int nParBrl = indexAndJacobian.first.size();
+//	TVectorD aVec(nParBrl); // compressed vector
+//	for ( int i = 0; i < nParBrl; ++i) {
+//		aVec[i] = theVector(indexAndJacobian.first[i] - 1);
+//	}
+//	SymMatrix aMat = theMatrix.getBlockMatrix(indexAndJacobian.first); // compressed matrix
+//	localPar = indexAndJacobian.second * aVec;
+//	localCov = aMat.Similarity(indexAndJacobian.second);
+//	return 0;
+//}
+//
+///// Get residuals at point from measurement.
+///**
+// * Get (diagonalized) residual, error of measurement and residual and down-weighting
+// * factor for measurement at point
+// *
+// * \param [in]  aLabel Label of point on trajectory
+// * \param [out] numData Number of data blocks from measurement at point
+// * \param [out] aResiduals Measurements-Predictions
+// * \param [out] aMeasErrors Errors of Measurements
+// * \param [out] aResErrors Errors of Residuals (including correlations from track fit)
+// * \param [out] aDownWeights Down-Weighting factors
+// * \return error code (non-zero if trajectory not fitted successfully)
+// */
+// int getMeasResults( int aLabel,
+//		 int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
+//		TVectorD &aResErrors, TVectorD &aDownWeights) {
+//	numData = 0;
+//	if (! fitOK)
+//		return 1;
+//
+//	 int firstData = measDataIndex[aLabel - 1]; // first data block with measurement
+//	numData = measDataIndex[aLabel] - firstData; // number of data blocks
+//	for ( int i = 0; i < numData; ++i) {
+//		getResAndErr(firstData + i, aResiduals[i], aMeasErrors[i],
+//				aResErrors[i], aDownWeights[i]);
+//	}
+//	return 0;
+//}
+//
+///// Get (kink) residuals at point from scatterer.
+///**
+// * Get (diagonalized) residual, error of measurement and residual and down-weighting
+// * factor for scatterering kinks at point
+// *
+// * \param [in]  aLabel Label of point on trajectory
+// * \param [out] numData Number of data blocks from scatterer at point
+// * \param [out] aResiduals (kink)Measurements-(kink)Predictions
+// * \param [out] aMeasErrors Errors of (kink)Measurements
+// * \param [out] aResErrors Errors of Residuals (including correlations from track fit)
+// * \param [out] aDownWeights Down-Weighting factors
+// * \return error code (non-zero if trajectory not fitted successfully)
+// */
+// int getScatResults( int aLabel,
+//		 int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
+//		TVectorD &aResErrors, TVectorD &aDownWeights) {
+//	numData = 0;
+//	if (! fitOK)
+//		return 1;
+//
+//	 int firstData = scatDataIndex[aLabel - 1]; // first data block with scatterer
+//	numData = scatDataIndex[aLabel] - firstData; // number of data blocks
+//	for ( int i = 0; i < numData; ++i) {
+//		getResAndErr(firstData + i, aResiduals[i], aMeasErrors[i],
+//				aResErrors[i], aDownWeights[i]);
+//	}
+//	return 0;
+//}
+//
+///// Get (list of) labels of points on (simple) trajectory
+///**
+// * \param [out] aLabelList List of labels (aLabelList[i] = i+1)
+// */
+//void getLabels(List< int> &aLabelList) {
+//	 int aLabel = 0;
+//	 int nPoint = thePoints[0].size();
+//	aLabelList.resize(nPoint);
+//	for ( i = 0; i < nPoint; ++i) {
+//		aLabelList[i] = ++aLabel;
+//	}
+//}
+//
+///// Get (list of lists of) labels of points on (composed) trajectory
+///**
+// * \param [out] aLabelList List of of lists of labels
+// */
+//void getLabels(
+//		List<List< int> > &aLabelList) {
+//	 int aLabel = 0;
+//	aLabelList.resize(numTrajectories);
+//	for ( int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
+//		 int nPoint = thePoints[iTraj].size();
+//		aLabelList[iTraj].resize(nPoint);
+//		for ( i = 0; i < nPoint; ++i) {
+//			aLabelList[iTraj][i] = ++aLabel;
+//		}
+//	}
+//}
+//
+///// Get residual and errors from data block.
+///**
+// * Get residual, error of measurement and residual and down-weighting
+// * factor for (single) data block
+// * \param [in]  aData Label of data block
+// * \param [out] aResidual Measurement-Prediction
+// * \param [out] aMeasError Error of Measurement
+// * \param [out] aResError Error of Residual (including correlations from track fit)
+// * \param [out] aDownWeight Down-Weighting factor
+// */
+//void getResAndErr( int aData, double &aResidual,
+//		double &aMeasError, double &aResError, double &aDownWeight) {
+//
+//	double aMeasVar;
+//	List< int>* indLocal;
+//	List<double>* derLocal;
+//	theData[aData].getResidual(aResidual, aMeasVar, aDownWeight, indLocal,
+//			derLocal);
+//	 int nParBrl = (*indLocal).size();
+//	TVectorD aVec(nParBrl); // compressed vector of derivatives
+//	for ( int j = 0; j < nParBrl; ++j) {
+//		aVec[j] = (*derLocal)[j];
+//	}
+//	SymMatrix aMat = theMatrix.getBlockMatrix(*indLocal); // compressed (covariance) matrix
+//	double aFitVar = aMat.Similarity(aVec); // variance from track fit
+//	aMeasError = sqrt(aMeasVar); // error of measurement
+//	aResError = (aFitVar < aMeasVar ? sqrt(aMeasVar - aFitVar) : 0.); // error of residual
+//}
+/// Build linear equation system from data (blocks).
+    void buildLinearEquationSystem()
+    {
+        int nBorder = numCurvature + numLocals;
+//	theVector. resize(numParameters);
+        theMatrix.resize(numParameters, nBorder, 5);
+        double[] retVals = new double[2];
+        double aValue, aWeight;
+        int[] indLocal = null;
+        double[] derLocal = null;
+        for (GblData d : theData)
+        {
+            d.getLocalData(retVals, indLocal, derLocal);
+            aValue = retVals[0];
+            aWeight = retVals[1];
+            for (int j = 0; j < indLocal.length; ++j)
+            {
+                theVector.addTo(indLocal[j] - 1, derLocal[j] * aWeight * aValue);
+            }
+            theMatrix.addBlockMatrix(aWeight, indLocal, derLocal);
+        }
+    }
+//}
+
+/// Prepare fit for simple or composed trajectory
+/**
+ * Generate data (blocks) from measurements, kinks, external seed and measurements.
+ */
+void prepare() {
+	 int nDim = theDimension.size();
+	// upper limit
+	 int maxData = numMeasurements + nDim * (numOffsets - 2);
+//cng			+ externalSeed.getRowDimension();
+//	theData.reserve(maxData);
+//	measDataIndex.resize(numAllPoints + 3); // include external seed and measurements
+         //cng
+         for(int i = 0; i<numAllPoints + 3; ++i) measDataIndex.add(i,0);
+         //cng
+//	scatDataIndex.resize(numAllPoints + 1);
+	 int nData = 0;
+	List<Matrix> innerTransDer = new ArrayList<Matrix>();
+	List<List<Integer> > innerTransLab = new ArrayList<List<Integer>>();
+//	// composed trajectory ?
+//	if (numInnerTrans > 0) {
+//		//std::cout << "composed trajectory" << std::endl;
+//		for ( int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
+//			// innermost point
+//			GblPoint* innerPoint = &thePoints[iTraj].front();
+//			// transformation fit to local track parameters
+//			List< int> firstLabels(5);
+//			SMatrix55 matFitToLocal, matLocalToFit;
+//			getFitToLocalJacobian(firstLabels, matFitToLocal, *innerPoint, 5);
+//			// transformation local track to fit parameters
+//			int ierr;
+//			matLocalToFit = matFitToLocal.Inverse(ierr);
+//			TMatrixD localToFit(5, 5);
+//			for ( int i = 0; i < 5; ++i) {
+//				for ( int j = 0; j < 5; ++j) {
+//					localToFit(i, j) = matLocalToFit(i, j);
+//				}
+//			}
+//			// transformation external to fit parameters at inner (first) point
+//			innerTransDer.add(localToFit * innerTransformations[iTraj]);
+//			innerTransLab.add(firstLabels);
+//		}
+//	}
+	// measurements
+	Matrix matP = new Matrix(5,5);
+//	// loop over trajectories
+//	List<GblPoint>::iterator itPoint;
+//	for ( int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
+//		for (itPoint = thePoints[iTraj].begin();
+//				itPoint < thePoints[iTraj].end(); ++itPoint) {
+//			SVector5 aMeas, aPrec;
+//			 int nLabel = itPoint->getLabel();
+//			 int measDim = itPoint->hasMeasurement();
+//			if (measDim) {
+//				const TMatrixD localDer = itPoint->getLocalDerivatives();
+//				const List<int> globalLab = itPoint->getGlobalLabels();
+//				const TMatrixD globalDer = itPoint->getGlobalDerivatives();
+//				TMatrixD transDer;
+//				itPoint->getMeasurement(matP, aMeas, aPrec);
+//				 int iOff = 5 - measDim; // first active component
+//				List< int> labDer(5);
+//				SMatrix55 matDer, matPDer;
+//				 int nJacobian =
+//						(itPoint < thePoints[iTraj].end() - 1) ? 1 : 0; // last point needs backward propagation
+//				getFitToLocalJacobian(labDer, matDer, *itPoint, measDim,
+//						nJacobian);
+//				if (measDim > 2) {
+//					matPDer = matP * matDer;
+//				} else { // 'shortcut' for position measurements
+//					matPDer.Place_at(
+//							matP.Sub<SMatrix22>(3, 3)
+//									* matDer.Sub<SMatrix25>(3, 0), 3, 0);
+//				}
+//
+////				if (numInnerTrans > 0) {
+////					// transform for external parameters
+////					TMatrixD proDer(measDim, 5);
+////					// match parameters
+////					 int ifirst = 0;
+////					 int ilabel = 0;
+////					while (ilabel < 5) {
+////						if (labDer[ilabel] > 0) {
+////							while (innerTransLab[iTraj][ifirst]
+////									!= labDer[ilabel] && ifirst < 5) {
+////								++ifirst;
+////							}
+////							if (ifirst >= 5) {
+////								labDer[ilabel] -= 2 * nDim * (iTraj + 1); // adjust label
+////							} else {
+////								// match
+////								labDer[ilabel] = 0; // mark as related to external parameters
+////								for ( int k = iOff; k < 5; ++k) {
+////									proDer(k - iOff, ifirst) = matPDer(k,
+////											ilabel);
+////								}
+////							}
+////						}
+////						++ilabel;
+////					}
+////					transDer.ResizeTo(measDim, numCurvature);
+////					transDer = proDer * innerTransDer[iTraj];
+////				}
+//				for ( int i = iOff; i < 5; ++i) {
+//					if (aPrec(i) > 0.) {
+//						GblData aData(nLabel, aMeas(i), aPrec(i));
+//						aData.addDerivatives(i, labDer, matPDer, iOff, localDer,
+//								globalLab, globalDer, numLocals, transDer);
+//						theData.add(aData);
+//						nData++;
+//					}
+//				}
+//
+//			}
+//			measDataIndex[nLabel] = nData;
+//		}
+//	} // end loop over trajectories
+
+//	// pseudo measurements from kinks
+//	SMatrix22 matT;
+//	scatDataIndex[0] = nData;

[truncated at 1000 lines; 320 more skipped]

java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl

HpsGblFitter.java 918 -> 919

--- java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/HpsGblFitter.java	2014-08-28 08:14:53 UTC (rev 918)
+++ java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/HpsGblFitter.java	2014-08-28 15:31:06 UTC (rev 919)
@@ -35,249 +35,245 @@

 import org.lcsim.recon.tracking.seedtracker.SeedCandidate;
 import org.lcsim.recon.tracking.seedtracker.SeedTrack;

 /**
  * Class to running GBL on HPS track.

- *

+ *

  * @author phansson

+ * 
+ * @version $Id:

  *
  */
 public class HpsGblFitter {

-	
-	private boolean _debug = false;
-	private double _B = 0.;
-	private double _bfac = 0.;
-	private boolean isMC = false;
-	TrackerHitUtils _trackHitUtils = null;
-	MultipleScattering _scattering = null;
-	private double m_chi2 = -1.;
-	private int m_ndf = -1;
-	private int m_lost_weight = -1;
-	GblTrajectory _traj = null;
-	MilleBinary _mille = null;
-

+    private boolean _debug = true;
+    private double _B = 0.;
+    private double _bfac = 0.;
+    private boolean isMC = false;
+    TrackerHitUtils _trackHitUtils = null;
+    MultipleScattering _scattering = null;
+    private double m_chi2 = -1.;
+    private int m_ndf = -1;
+    private int m_lost_weight = -1;
+    GblTrajectory _traj = null;
+    MilleBinary _mille = null;

-	
-	public HpsGblFitter(double Bz, MultipleScattering scattering, boolean isMCFlag) {
-		isMC = isMCFlag;
-		_B = Bz;
-		_bfac = Bz * Constants.fieldConversion;
-		_trackHitUtils = new TrackerHitUtils();
-		_scattering = scattering;
-	}
-	
-	public void setMilleBinary(MilleBinary mille) {
-		_mille = mille;
-	}
-	
-	public void clear() {
-		m_chi2 = -1.;
-		m_ndf = -1;
-		m_lost_weight = -1;
-		_traj = null;
-	}

+    public HpsGblFitter() {
+        _B = -0.5;
+        _bfac = _B * Constants.fieldConversion;
+        _trackHitUtils = new TrackerHitUtils();
+        _scattering = new MultipleScattering(new MaterialSupervisor());
+        System.out.println("Default constructor");
+    }

-	public int Fit(Track track) {
-

+    public HpsGblFitter(double Bz, MultipleScattering scattering, boolean isMCFlag) {
+        isMC = isMCFlag;
+        _B = Bz;
+        _bfac = Bz * Constants.fieldConversion;
+        _trackHitUtils = new TrackerHitUtils();
+        _scattering = scattering;
+        System.out.println("Constructor");
+    }

-		// Check that things are setup
-		if(_B == 0.) {
-			System.out.printf("%s: B-field not set!\n", this.getClass().getSimpleName());
-			return -1;
-		}
-		if(_scattering == null) {
-			System.out.printf("%s: Multiple scattering calculator not set!\n", this.getClass().getSimpleName());
-			return -2;
-		}
-		
-		// Time the fits
-		//clock_t startTime = clock();

+    public void setMilleBinary(MilleBinary mille) {
+        _mille = mille;
+    }

-		
-		
-		// path length along trajectory
-		double s = 0.;
-		// jacobian to transport errors between points along the path
-		BasicMatrix jacPointToPoint = GblUtils.getInstance().unitMatrix(5, 5);
-		// Option to use uncorrelated  MS errors
-		// This is similar to what is done in lcsim seedtracker
-		// The msCov below holds the MS errors
-		// This is for testing purposes only.
-		boolean useUncorrMS = false;
-		BasicMatrix msCov = GblUtils.getInstance().zeroMatrix(5, 5);
-		
-		// Vector of the strip clusters used for the GBL fit
-		List<GblPoint> listOfPoints = new ArrayList<GblPoint>();
-		
-		// Store the projection from local to measurement frame for each strip cluster
-		// need to use pointer for TMatrix here?
-		Map<Integer, Matrix> proL2m_list = new HashMap<Integer, Matrix>(); 
-		// Save the association between strip cluster and label	
-		Map<HelicalTrackStrip, Integer> stripLabelMap = new HashMap<HelicalTrackStrip, Integer>();
-		  
-		//start trajectory at refence point (s=0) - this point has no measurement
-		GblPoint ref_point = new GblPoint(jacPointToPoint);
-		listOfPoints.add(ref_point);
-		
-		//Create a list of all the strip clusters making up the track 
-		List<HelicalTrackStrip> stripClusters = new ArrayList<HelicalTrackStrip>();
-		SeedCandidate seed = ((SeedTrack) track).getSeedCandidate();
-		HelicalTrackFit htf = seed.getHelix();
-		List<HelicalTrackHit> stereoHits = seed.getHits();
-		for(int ihit = 0; ihit < stereoHits.size(); ++ihit) {
-			HelicalTrackCross cross = (HelicalTrackCross) stereoHits.get(ihit);
-			stripClusters.add(cross.getStrips().get(0));
-			stripClusters.add(cross.getStrips().get(1));
-		}
-		
-		// sort the clusters along path
-		// TODO use actual path length and not layer id!
-		//Collections.sort(stripClusters, new HelicalTrackStripComparer());
-		Collections.sort(stripClusters, new Comparator<HelicalTrackStrip>() {
-			public int compare(HelicalTrackStrip o1, HelicalTrackStrip o2) {
-				return o1.layer() < o2.layer() ? -1 : o1.layer() > o2.layer() ? 1 : 0;
-			}
-		});

+    public void clear() {
+        m_chi2 = -1.;
+        m_ndf = -1;
+        m_lost_weight = -1;
+        _traj = null;
+    }

-		if(_debug ) {
-			System.out.printf("%s: %d strip clusters:\n", stripClusters.size());
-			for(int istrip=0;istrip<stripClusters.size();++istrip) {
-				System.out.printf("%s: layer %d origin %s\n", stripClusters.get(istrip).layer(),stripClusters.get(istrip).origin().toString());
-			}
-		}

+    public int Fit(Track track) {

-		
-		// Find scatter points along the path of the track

+        // Check that things are setup
+        if (_B == 0.) {
+            System.out.printf("%s: B-field not set!\n", this.getClass().getSimpleName());
+            return -1;
+        }
+        if (_scattering == null) {
+            System.out.printf("%s: Multiple scattering calculator not set!\n", this.getClass().getSimpleName());
+            return -2;
+        }
+
+        // Time the fits
+        //clock_t startTime = clock();
+        // path length along trajectory
+        double s = 0.;
+        // jacobian to transport errors between points along the path
+        BasicMatrix jacPointToPoint = GblUtils.getInstance().unitMatrix(5, 5);
+        // Option to use uncorrelated  MS errors
+        // This is similar to what is done in lcsim seedtracker
+        // The msCov below holds the MS errors
+        // This is for testing purposes only.
+        boolean useUncorrMS = false;
+        BasicMatrix msCov = GblUtils.getInstance().zeroMatrix(5, 5);
+
+        // Vector of the strip clusters used for the GBL fit
+        List<GblPoint> listOfPoints = new ArrayList<GblPoint>();
+
+        // Store the projection from local to measurement frame for each strip cluster
+        // need to use pointer for TMatrix here?
+        Map<Integer, Matrix> proL2m_list = new HashMap<Integer, Matrix>();
+        // Save the association between strip cluster and label	
+        Map<HelicalTrackStrip, Integer> stripLabelMap = new HashMap<HelicalTrackStrip, Integer>();
+
+        //start trajectory at refence point (s=0) - this point has no measurement
+        GblPoint ref_point = new GblPoint(jacPointToPoint);
+        listOfPoints.add(ref_point);
+
+        //Create a list of all the strip clusters making up the track 
+        List<HelicalTrackStrip> stripClusters = new ArrayList<HelicalTrackStrip>();
+        SeedCandidate seed = ((SeedTrack) track).getSeedCandidate();
+        HelicalTrackFit htf = seed.getHelix();
+        List<HelicalTrackHit> stereoHits = seed.getHits();
+        for (int ihit = 0; ihit < stereoHits.size(); ++ihit) {
+            HelicalTrackCross cross = (HelicalTrackCross) stereoHits.get(ihit);
+            stripClusters.add(cross.getStrips().get(0));
+            stripClusters.add(cross.getStrips().get(1));
+        }
+
+        // sort the clusters along path
+        // TODO use actual path length and not layer id!
+        //Collections.sort(stripClusters, new HelicalTrackStripComparer());
+        Collections.sort(stripClusters, new Comparator<HelicalTrackStrip>() {
+            public int compare(HelicalTrackStrip o1, HelicalTrackStrip o2) {
+                return o1.layer() < o2.layer() ? -1 : o1.layer() > o2.layer() ? 1 : 0;
+            }
+        });
+
+        if (_debug) {
+            System.out.printf(" %d strip clusters:\n", stripClusters.size());
+            for (int istrip = 0; istrip < stripClusters.size(); ++istrip) {
+                System.out.printf("layer %d origin %s\n", stripClusters.get(istrip).layer(), stripClusters.get(istrip).origin().toString());
+            }
+        }
+
+        // Find scatter points along the path of the track

         ScatterPoints scatters = _scattering.FindHPSScatterPoints(htf);

-		
-		
-		if(_debug ) {
-			System.out.printf("%s: Process %d strip clusters\n", stripClusters.size());
-		}
-		for(int istrip=0;istrip<stripClusters.size();++istrip) {
-			
-			HelicalTrackStrip strip = stripClusters.get(istrip);
-			
-			if(_debug) {
-				System.out.printf("%s: layer %d origin %s\n", strip.layer(),strip.origin().toString());
-			}
-			
-			 //Find intercept point with sensor in tracking frame

+
+        if (_debug) {
+            System.out.printf(" Process %d strip clusters\n", stripClusters.size());
+        }
+        for (int istrip = 0; istrip < stripClusters.size(); ++istrip) {
+
+            HelicalTrackStrip strip = stripClusters.get(istrip);
+
+            if (_debug) {
+                System.out.printf(" layer %d origin %s\n", strip.layer(), strip.origin().toString());
+            }
+
+            //Find intercept point with sensor in tracking frame

             Hep3Vector trkpos = TrackUtils.getHelixPlaneIntercept(htf, strip, Math.abs(_B));

-            if(_debug) {
-            	System.out.printf("trkpos at intercept [%.10f %.10f %.10f]\n",trkpos.x(),trkpos.y(),trkpos.z());

+            if (_debug) {
+                System.out.printf("trkpos at intercept [%.10f %.10f %.10f]\n", trkpos.x(), trkpos.y(), trkpos.z());

-            
-          //path length to intercept
-            double path = HelixUtils.PathToXPlane(htf,trkpos.x(),0,0).get(0);

+
+            //path length to intercept
+            double path = HelixUtils.PathToXPlane(htf, trkpos.x(), 0, 0).get(0);

             double path3D = path / Math.cos(Math.atan(htf.slope()));

-            
-		    
-		    // Path length step for this cluster
-		    double step = path3D - s;
-		    
-		    if( _debug ) {
-		      System.out.printf("%s: Path length step %f from %f to %f\n",this.getClass().getSimpleName(),step, s, path3D);
-		    }
-		    
-		    // Measurement direction (perpendicular and parallel to strip direction)
-		    BasicMatrix mDir = new BasicMatrix(2,3);
-		    mDir.setElement(0, 0, strip.u().x());
-		    mDir.setElement(0, 1, strip.u().y());
-		    mDir.setElement(0, 2, strip.u().z());
-		    mDir.setElement(1, 0, strip.v().x());
-		    mDir.setElement(1, 1, strip.v().y());
-		    mDir.setElement(1, 2, strip.v().z());
-		    
-		    Matrix mDirT = MatrixOp.transposed(mDir); //new BasicMatrix(MatrixOp.transposed(mDir));
-		    if(_debug) {
-		    	System.out.printf("%s: mDir \n%s\n",this.getClass().getSimpleName(),mDir.toString());
-		    	System.out.printf("%s: mDirT \n%s\n",this.getClass().getSimpleName(),mDirT.toString());
-		    }
-

-		    // Track direction 
-		    double sinLambda = Math.sin(Math.atan(htf.slope())); 
-		    double cosLambda = Math.sqrt(1.0 - sinLambda*sinLambda);
-		    double sinPhi = Math.sin(htf.phi0());
-		    double cosPhi = Math.sqrt(1.0 - sinPhi*sinPhi);
-		    
-		    // Track direction in curvilinear frame (U,V,T)
-		    // U = Z x T / |Z x T|, V = T x U
-		    BasicMatrix uvDir = new BasicMatrix(2,3);
-		    uvDir.setElement(0, 0, -sinPhi);
-		    uvDir.setElement(0, 1, cosPhi);
-		    uvDir.setElement(0, 2, 0.);
-		    uvDir.setElement(1, 0, -sinLambda * cosPhi);
-		    uvDir.setElement(1, 1, -sinLambda * sinPhi);
-		    uvDir.setElement(1, 2, cosLambda);
-		    
-		    if(_debug) {
-		    	System.out.printf("%s: uvDir \n%s\n",this.getClass().getSimpleName(),uvDir.toString());
-		    }

+            // Path length step for this cluster
+            double step = path3D - s;

-		    // projection from  measurement to local (curvilinear uv) directions (duv/dm)
-		    Matrix proM2l = MatrixOp.mult(uvDir, mDirT); //uvDir * mDirT;
-		    
-		    //projection from local (curvilinear uv) to measurement directions (dm/duv)
-		    Matrix proL2m = MatrixOp.inverse(proM2l);
-		    
-		    //proL2m_list[strip->GetId()] = new TMatrixD(proL2m);

+            if (_debug) {
+                System.out.printf("%s Path length step %f from %f to %f\n", this.getClass().getSimpleName(), step, s, path3D);
+            }

-			if(_debug) {
-				System.out.printf("%s: proM2l \n%s\n",this.getClass().getSimpleName(),proM2l.toString());
-				System.out.printf("%s: proL2m \n%s\n",this.getClass().getSimpleName(),proL2m.toString());
-			}
-			
-			// measurement/residual in the measurement system
-

+            // Measurement direction (perpendicular and parallel to strip direction)
+            BasicMatrix mDir = new BasicMatrix(2, 3);
+            mDir.setElement(0, 0, strip.u().x());
+            mDir.setElement(0, 1, strip.u().y());
+            mDir.setElement(0, 2, strip.u().z());
+            mDir.setElement(1, 0, strip.v().x());
+            mDir.setElement(1, 1, strip.v().y());
+            mDir.setElement(1, 2, strip.v().z());
+
+            Matrix mDirT = MatrixOp.transposed(mDir); //new BasicMatrix(MatrixOp.transposed(mDir));
+            if (_debug) {
+                System.out.printf(" mDir \n%s\n", this.getClass().getSimpleName(), mDir.toString());
+                System.out.printf(" mDirT \n%s\n", this.getClass().getSimpleName(), mDirT.toString());
+            }
+
+            // Track direction 
+            double sinLambda = Math.sin(Math.atan(htf.slope()));
+            double cosLambda = Math.sqrt(1.0 - sinLambda * sinLambda);
+            double sinPhi = Math.sin(htf.phi0());
+            double cosPhi = Math.sqrt(1.0 - sinPhi * sinPhi);
+
+            // Track direction in curvilinear frame (U,V,T)
+            // U = Z x T / |Z x T|, V = T x U
+            BasicMatrix uvDir = new BasicMatrix(2, 3);
+            uvDir.setElement(0, 0, -sinPhi);
+            uvDir.setElement(0, 1, cosPhi);
+            uvDir.setElement(0, 2, 0.);
+            uvDir.setElement(1, 0, -sinLambda * cosPhi);
+            uvDir.setElement(1, 1, -sinLambda * sinPhi);
+            uvDir.setElement(1, 2, cosLambda);
+
+            if (_debug) {
+                System.out.printf(" uvDir \n%s\n", this.getClass().getSimpleName(), uvDir.toString());
+            }
+
+            // projection from  measurement to local (curvilinear uv) directions (duv/dm)
+            Matrix proM2l = MatrixOp.mult(uvDir, mDirT); //uvDir * mDirT;
+
+            //projection from local (curvilinear uv) to measurement directions (dm/duv)
+            Matrix proL2m = MatrixOp.inverse(proM2l);
+
+            //proL2m_list[strip->GetId()] = new TMatrixD(proL2m);
+            if (_debug) {
+                System.out.printf(" proM2l \n%s\n", this.getClass().getSimpleName(), proM2l.toString());
+                System.out.printf(" proL2m \n%s\n", this.getClass().getSimpleName(), proL2m.toString());
+            }
+
+            // measurement/residual in the measurement system

             // start by find the distance vector between the center and the track position
             Hep3Vector vdiffTrk = VecOp.sub(trkpos, strip.origin());

             // then find the rotation from tracking to measurement frame
             Hep3Matrix trkToStripRot = _trackHitUtils.getTrackToStripRotation(strip);

             // then rotate that vector into the measurement frame to get the predicted measurement position
             Hep3Vector trkpos_meas = VecOp.mult(trkToStripRot, vdiffTrk);

             // hit measurement and uncertainty in measurement frame

-            Hep3Vector m_meas = new BasicHep3Vector(strip.umeas(),0.,0.);
-

+            Hep3Vector m_meas = new BasicHep3Vector(strip.umeas(), 0., 0.);
+

             // finally the residual
             Hep3Vector res_meas = VecOp.sub(m_meas, trkpos_meas);

-            Hep3Vector res_err_meas = new BasicHep3Vector(strip.du(),(strip.vmax() - strip.vmin()) / Math.sqrt(12),10.0/Math.sqrt(12));

+            Hep3Vector res_err_meas = new BasicHep3Vector(strip.du(), (strip.vmax() - strip.vmin()) / Math.sqrt(12), 10.0 / Math.sqrt(12));

 
             // Move to matrix objects instead of 3D vectors
             // TODO use only one type

-            
-			// only 1D measurement in u-direction, set strip measurement direction to zero
-		    BasicMatrix meas = new BasicMatrix(0,2);
-		    meas.setElement(0, 0, res_meas.x());
-		    meas.setElement(0, 1, 0.);

+            // only 1D measurement in u-direction, set strip measurement direction to zero
+            BasicMatrix meas = new BasicMatrix(1, 2);
+            meas.setElement(0, 0, res_meas.x());
+            meas.setElement(0, 1, 0.);

 //			    //meas[0][0] += deltaU[iLayer] # misalignment

-		    BasicMatrix measErr = new BasicMatrix(0,2);
-		    measErr.setElement(0, 0, res_err_meas.x());
-		    measErr.setElement(0, 1, 0.);

+            BasicMatrix measErr = new BasicMatrix(1, 2);
+            measErr.setElement(0, 0, res_err_meas.x());
+            measErr.setElement(0, 1, 0.);

-		    BasicMatrix measPrec = new BasicMatrix(0,2);
-		    measPrec.setElement(0, 0, 1.0/( measErr.e(0, 0) * measErr.e(0, 0)));
-		    measPrec.setElement(0, 1, 0.);
-		    if (_debug) {
-		    	System.out.printf("%s: meas \n%s\n",this.getClass().getSimpleName(),meas.toString());
-		    	System.out.printf("%s: measErr \n%s\n",this.getClass().getSimpleName(),measErr.toString());
-		    	System.out.printf("%s: measPrec \n%s\n",this.getClass().getSimpleName(),measPrec.toString());

+            BasicMatrix measPrec = new BasicMatrix(1, 2);
+            measPrec.setElement(0, 0, 1.0 / (measErr.e(0, 0) * measErr.e(0, 0)));
+            measPrec.setElement(0, 1, 0.);
+            if (_debug) {
+                System.out.printf("%s: meas \n%s\n", this.getClass().getSimpleName(), meas.toString());
+                System.out.printf("%s: measErr \n%s\n", this.getClass().getSimpleName(), measErr.toString());
+                System.out.printf("%s: measPrec \n%s\n", this.getClass().getSimpleName(), measPrec.toString());

             }
 
             //Find the Jacobian to be able to propagate the covariance matrix to this strip position
             jacPointToPoint = GblUtils.getInstance().gblSimpleJacobianLambdaPhi(step, cosLambda, Math.abs(_bfac));

-            
-            if(_debug) {
-				System.out.printf("%s: jacPointToPoint \n%s\n",this.getClass().getSimpleName(),jacPointToPoint.toString());

+
+            if (_debug) {
+                System.out.printf("%s: jacPointToPoint \n%s\n", this.getClass().getSimpleName(), jacPointToPoint.toString());

-            
-          //propagate MS covariance matrix (in the curvilinear frame) to this strip position

+
+            //propagate MS covariance matrix (in the curvilinear frame) to this strip position

             //msCov = np.dot(jacPointToPoint, np.dot(msCov, jacPointToPoint.T))
             //measMsCov = np.dot(proL2m, np.dot(msCov[3:, 3:], proL2m.T))
 //                if (m_debug) {

@@ -286,187 +282,172 @@

 //                  //cout << "HpsGblFitter: " << "measMsCov at this point:" << endl;
 //                  //measMsCov.Print();
 //                }

             //Option to blow up measurement error according to multiple scattering
             //if useUncorrMS:
             //measPrec[0] = 1.0 / (measErr[0] ** 2 + measMsCov[0, 0])
             //  if debug:
             //print 'Adding measMsCov ', measMsCov[0,0]

             // point with independent measurement
             GblPoint point = new GblPoint(jacPointToPoint);
 
             //Add measurement to the point

-            point.addMeasurement(proL2m,meas,measPrec);

+            point.addMeasurement(proL2m, meas, measPrec);

             //Add scatterer in curvilinear frame to the point
             // no direction in this frame as it moves along the track

-            BasicMatrix scat = GblUtils.getInstance().zeroMatrix(0,2);

+            BasicMatrix scat = GblUtils.getInstance().zeroMatrix(0, 2);

 
             // find scattering angle

-            ScatterPoint scatter = scatters.getScatterPoint(((RawTrackerHit)strip.rawhits().get(0)).getDetectorElement());

+            ScatterPoint scatter = scatters.getScatterPoint(((RawTrackerHit) strip.rawhits().get(0)).getDetectorElement());

             double scatAngle;

-         
-            if(scatter != null) {
-            	scatAngle = scatter.getScatterAngle().Angle();
-            }	
-            else {
-                System.out.printf("%s: WARNING cannot find scatter for detector %s with strip cluster at %s\n",this.getClass(),((RawTrackerHit)strip.rawhits().get(0)).getDetectorElement().getName(),strip.origin().toString());

+
+            if (scatter != null) {
+                scatAngle = scatter.getScatterAngle().Angle();
+            } else {
+                System.out.printf("%s: WARNING cannot find scatter for detector %s with strip cluster at %s\n", this.getClass(), ((RawTrackerHit) strip.rawhits().get(0)).getDetectorElement().getName(), strip.origin().toString());

                 //can be edge case where helix is outside, but close to sensor, so use hack with the sensor origin closest to hit
                 //TODO check if this really makes sense to do
                 DetectorPlane closest = null;
                 double dx = 999999.9;

-                if(MaterialSupervisor.class.isInstance(_scattering.getMaterialManager())) {
-                    MaterialSupervisor matSup = (MaterialSupervisor)_scattering.getMaterialManager();
-                    for(ScatteringDetectorVolume vol : matSup.getMaterialVolumes()) {
-                        DetectorPlane plane = (DetectorPlane)vol;

+                if (MaterialSupervisor.class.isInstance(_scattering.getMaterialManager())) {
+                    MaterialSupervisor matSup = (MaterialSupervisor) _scattering.getMaterialManager();
+                    for (ScatteringDetectorVolume vol : matSup.getMaterialVolumes()) {
+                        DetectorPlane plane = (DetectorPlane) vol;

                         double dx_loop = Math.abs(strip.origin().x() - plane.origin().x());

-                        if(dx_loop<dx) {

+                        if (dx_loop < dx) {

                             dx = dx_loop;
                             closest = plane;
                         }
                     }

-                    if(closest==null) {

+                    if (closest == null) {

                         throw new RuntimeException("cannot find any plane that is close to strip!");
                     } else {
                         // find scatterlength

-                        double s_closest = HelixUtils.PathToXPlane(htf,closest.origin().x(), 0., 0).get(0);

+                        double s_closest = HelixUtils.PathToXPlane(htf, closest.origin().x(), 0., 0).get(0);

                         double X0 = closest.getMaterialTraversedInRL(HelixUtils.Direction(htf, s_closest));

-                        ScatterAngle scatterAngle = new ScatterAngle(s_closest, _scattering.msangle(htf.p(Math.abs(_B)),X0));

+                        ScatterAngle scatterAngle = new ScatterAngle(s_closest, _scattering.msangle(htf.p(Math.abs(_B)), X0));

                         scatAngle = scatterAngle.Angle();
                     }

-                } 
-                else {

+                } else {

                     throw new UnsupportedOperationException("Should not happen. This problem is only solved with the MaterialSupervisor.");
                 }
             }

-            
-

             // Scattering angle in the curvilinear frame
             //Note the cosLambda to correct for the projection in the phi direction

-            BasicMatrix scatErr = new BasicMatrix(0,2);

+            BasicMatrix scatErr = new BasicMatrix(1, 2);

             scatErr.setElement(0, 0, scatAngle);
             scatErr.setElement(0, 1, scatAngle / cosLambda);

-            BasicMatrix scatPrec = new BasicMatrix(0,2);

+            BasicMatrix scatPrec = new BasicMatrix(1, 2);

             scatPrec.setElement(0, 0, 1.0 / (scatErr.e(0, 0) * scatErr.e(0, 0)));
             scatPrec.setElement(0, 1, 1.0 / (scatErr.e(0, 1) * scatErr.e(0, 1)));

             // add scatterer if not using the uncorrelated MS covariances for testing

-            if (! useUncorrMS) {
-              point.addScatterer(scat, scatPrec);
-              if (_debug) {
-            	  System.out.printf("%s: scatError to this point \n%s\n",this.getClass().getSimpleName(),scatErr.toString());
-              }

+            if (!useUncorrMS) {
+                point.addScatterer(scat, scatPrec);
+                if (_debug) {
+                    System.out.printf("%s: scatError to this point \n%s\n", this.getClass().getSimpleName(), scatErr.toString());
+                }

             // Add this GBL point to list that will be used in fit
             listOfPoints.add(point);
             int iLabel = listOfPoints.size();

             // Update MS covariance matrix

-            msCov.setElement(1, 1, msCov.e(1, 1) + scatErr.e(0, 0)*scatErr.e(0, 0));
-            msCov.setElement(2, 2, msCov.e(2, 2) + scatErr.e(0, 1)*scatErr.e(0, 1));
-            
-            
-

+            msCov.setElement(1, 1, msCov.e(1, 1) + scatErr.e(0, 0) * scatErr.e(0, 0));
+            msCov.setElement(2, 2, msCov.e(2, 2) + scatErr.e(0, 1) * scatErr.e(0, 1));
+

/*

-            ##### 
-            ## Calculate global derivatives for this point
-            # track direction in tracking/global frame
-            tDirGlobal = np.array( [ [cosPhi * cosLambda, sinPhi * cosLambda, sinLambda] ] )        
-            # Cross-check that the input is consistent
-            if( np.linalg.norm( tDirGlobal - strip.tDir) > 0.00001):
-              print 'ERROR: tDirs are not consistent!'
-              sys.exit(1)
-            # rotate track direction to measurement frame          
-            tDirMeas = np.dot( tDirGlobal, np.array([strip.u, strip.v, strip.w]) )
-            #tDirMeas = utils.rotateGlToMeas(strip,tDirGlobal)
-            normalMeas = np.dot( strip.w , np.array([strip.u, strip.v, strip.w]) ) 
-            #normalMeas = utils.rotateGlToMeas(strip,strip.w) 
-            # non-measured directions 
-            vmeas = 0.
-            wmeas = 0.
-            # calculate and add derivatives to point
-            glDers = utils.globalDers(strip.layer,strip.meas,vmeas,wmeas,tDirMeas,normalMeas)
-            ders = glDers.getDers(track.isTop())
-            labGlobal = ders['labels']
-            addDer = ders['ders']
-            if debug:
-              print 'global derivatives:'
-              print labGlobal
-              print addDer
-            point.addGlobals(labGlobal, addDer)
-            #####

+             ##### 
+             ## Calculate global derivatives for this point
+             # track direction in tracking/global frame
+             tDirGlobal = np.array( [ [cosPhi * cosLambda, sinPhi * cosLambda, sinLambda] ] )        
+             # Cross-check that the input is consistent
+             if( np.linalg.norm( tDirGlobal - strip.tDir) > 0.00001):
+             print 'ERROR: tDirs are not consistent!'
+             sys.exit(1)
+             # rotate track direction to measurement frame          
+             tDirMeas = np.dot( tDirGlobal, np.array([strip.u, strip.v, strip.w]) )
+             #tDirMeas = utils.rotateGlToMeas(strip,tDirGlobal)
+             normalMeas = np.dot( strip.w , np.array([strip.u, strip.v, strip.w]) ) 
+             #normalMeas = utils.rotateGlToMeas(strip,strip.w) 
+             # non-measured directions 
+             vmeas = 0.
+             wmeas = 0.
+             # calculate and add derivatives to point
+             glDers = utils.globalDers(strip.layer,strip.meas,vmeas,wmeas,tDirMeas,normalMeas)
+             ders = glDers.getDers(track.isTop())
+             labGlobal = ders['labels']
+             addDer = ders['ders']
+             if debug:
+             print 'global derivatives:'
+             print labGlobal
+             print addDer
+             point.addGlobals(labGlobal, addDer)
+             #####

*/

-            
-        
-

             //move on to next point
             s += step;

             // save strip and label map
             //stripLabelMap[strip] = iLabel;

+        }
+
+        //create the trajectory
+        _traj = new GblTrajectory(listOfPoints); //,seedLabel, clSeed);
+
+        if (!_traj.isValid()) {
+            System.out.printf("%s:  Invalid GblTrajectory -> skip \n", this.getClass().getSimpleName());
+            return -3;
+        }
+
+        // fit trajectory
+        _traj.fit(m_chi2, m_ndf, m_lost_weight);

-        
-

+        //cng
+//        System.out.println("fitting the traectory...");
+//        double[] retDVals = new double[2];
+//        int[] retIVals = new int[1];
+//        int success = _traj.fit(retDVals, retIVals, "");
+        //cng

+        if (_debug) {
+            System.out.printf("%s:  Chi2  Fit: %f , %d , %d\n", this.getClass().getSimpleName(), m_chi2, m_ndf, m_lost_weight);
+        }

-		//create the trajectory
-		_traj = new GblTrajectory(listOfPoints); //,seedLabel, clSeed);
-			  
-		if (! _traj.isValid()) {
-			System.out.printf("%s:  Invalid GblTrajectory -> skip \n",this.getClass().getSimpleName());
-			return -3;
-		}
-		
-		// fit trajectory
-		_traj.fit(m_chi2, m_ndf, m_lost_weight);
-		
-		if( _debug ) {
-			System.out.printf("%s:  Chi2  Fit: %f , %d , %d\n",this.getClass().getSimpleName(), m_chi2, m_ndf,m_lost_weight);
-		}
-		
-		// write to MP binary file
-		if(_mille != null) {
-			_traj.milleOut(_mille);
-		}
-		
-		//stop the clock
-		//clock_t endTime = clock();
-		//double diff = endTime - startTime;
-		//double cps = CLOCKS_PER_SEC;
-		//if( m_debug ) {
-		//	std::cout << "HpsGblFitter: " << " Time elapsed " << diff / cps << " s" << std::endl;
-		//}

+        // write to MP binary file
+        if (_mille != null) {
+            _traj.milleOut(_mille);
+        }

-		if(_debug) {
-			System.out.printf("%s:  Fit() done successfully.\n",this.getClass().getSimpleName());
-		}
-		
-		return 0;
-	}
-

+        //stop the clock
+        //clock_t endTime = clock();
+        //double diff = endTime - startTime;
+        //double cps = CLOCKS_PER_SEC;
+        //if( m_debug ) {
+        //	std::cout << "HpsGblFitter: " << " Time elapsed " << diff / cps << " s" << std::endl;
+        //}
+        if (_debug) {
+            System.out.printf("%s:  Fit() done successfully.\n", this.getClass().getSimpleName());
+        }

-	public static class HelicalTrackStripComparer implements Comparator<HelicalTrackStrip> {
-		public int compare(HelicalTrackStrip o1, HelicalTrackStrip o2) {
-			// TODO Change this to path length!?
-			return compare(o1.layer(),o2.layer());
-		}
-	
-		private static int compare(int s1, int s2) {
-			return s1 < s2 ? -1 : s2 > s1 ? 1 : 0;
-		}
-	
-	
-	}

+        return 0;
+    }

+    public static class HelicalTrackStripComparer implements Comparator<HelicalTrackStrip> {

+        public int compare(HelicalTrackStrip o1, HelicalTrackStrip o2) {
+            // TODO Change this to path length!?
+            return compare(o1.layer(), o2.layer());
+        }
+
+        private static int compare(int s1, int s2) {
+            return s1 < s2 ? -1 : s2 > s1 ? 1 : 0;
+        }
+
+    }
+

java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix

Matrix.java added at 919

--- java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix/Matrix.java	                        (rev 0)
+++ java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix/Matrix.java	2014-08-28 15:31:06 UTC (rev 919)
@@ -0,0 +1,1553 @@

+package org.hps.recon.tracking.gbl.matrix;
+
+import java.io.BufferedReader;
+import java.io.PrintWriter;
+import java.io.StreamTokenizer;
+import java.text.DecimalFormat;
+import java.text.DecimalFormatSymbols;
+import java.text.NumberFormat;
+import java.util.Locale;
+
+///**
+// * Jama = Java Matrix class.
+// * <P>
+// * The Java Matrix Class provides the fundamental operations of numerical linear
+// * algebra. Various constructors create Matrices from two dimensional arrays of
+// * double precision floating point numbers. Various "gets" and "sets" provide
+// * access to submatrices and matrix elements. Several methods implement basic
+// * matrix arithmetic, including matrix addition and multiplication, matrix
+// * norms, and element-by-element array operations. Methods for reading and
+// * printing matrices are also included. All the operations in this version of
+// * the Matrix Class involve real matrices. Complex matrices may be handled in a
+// * future version.
+// * <P>
+// * Five fundamental matrix decompositions, which consist of pairs or triples of
+// * matrices, permutation vectors, and the like, produce results in five
+// * decomposition classes. These decompositions are accessed by the Matrix class
+// * to compute solutions of simultaneous linear equations, determinants, inverses
+// * and other matrix functions. The five decompositions are:
+// * <P>
+// * <UL>
+// * <LI>Cholesky Decomposition of symmetric, positive definite matrices.
+// * <LI>LU Decomposition of rectangular matrices.
+// * <LI>QR Decomposition of rectangular matrices.
+// * <LI>Singular Value Decomposition of rectangular matrices.
+// * <LI>Eigenvalue Decomposition of both symmetric and nonsymmetric square
+// * matrices.
+// * </UL>
+// * <DL>
+// * <DT><B>Example of use:</B></DT>
+// * <P>
+// * <DD>Solve a linear system A x = b and compute the residual norm, ||b - A x||.
+// * <P>
+// * <
+// * PRE>
+// * double[][] vals = {{1.,2.,3},{4.,5.,6.},{7.,8.,10.}};
+// * Matrix A = new Matrix(vals);
+// * Matrix b = Matrix.random(3,1);
+// * Matrix x = A.solve(b);
+// * Matrix r = A.times(x).minus(b);
+// * double rnorm = r.normInf();
+// * </PRE></DD>
+// * </DL>
+// *
+// * @author The MathWorks, Inc. and the National Institute of Standards and
+// * Technology.
+// * @author Norman A. Graf (modifications for the gbl package)
+// * @version 5 August 1998 $Id: Matrix.java,v 1.1.1.1 2010/11/30 21:31:59 jeremy
+// * Exp $
+// */
+public class Matrix implements Cloneable, java.io.Serializable
+{
+
+    /* ------------------------
+     Class variables
+     * ------------------------ */
+    /**
+     * Array for internal storage of elements.
+     *
+     * @serial internal array storage.
+     */
+    protected double[][] A;
+
+    /**
+     * Row and column dimensions.
+     *
+     * @serial row dimension.
+     * @serial column dimension.
+     */
+    protected int m, n;
+
+    /* ------------------------
+     Constructors
+     * ------------------------ */
+    /**
+     * Construct an m-by-n matrix of zeros.
+     *
+     * @param m Number of rows.
+     * @param n Number of columns.
+     */
+    public Matrix(int m, int n)
+    {
+        this.m = m;
+        this.n = n;
+        A = new double[m][n];
+    }
+//
+//    /**
+//     * Construct an m-by-n constant matrix.
+//     *
+//     * @param m Number of rows.
+//     * @param n Number of colums.
+//     * @param s Fill the matrix with this scalar value.
+//     */
+//    public Matrix(int m, int n, double s)
+//    {
+//        this.m = m;
+//        this.n = n;
+//        A = new double[m][n];
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                A[i][j] = s;
+//            }
+//        }
+//    }
+//
+//    /**
+//     * Construct a matrix from a 2-D array.
+//     *
+//     * @param A Two-dimensional array of doubles.
+//     * @exception IllegalArgumentException All rows must have the same length
+//     * @see #constructWithCopy
+//     */
+//    public Matrix(double[][] A)
+//    {
+//        m = A.length;
+//        n = A[0].length;
+//        for (int i = 0; i < m; i++)
+//        {
+//            if (A[i].length != n)
+//            {
+//                throw new IllegalArgumentException("All rows must have the same length.");
+//            }
+//        }
+//        this.A = A;
+//    }
+//
+    /**
+     * Construct a matrix quickly without checking arguments.
+     *
+     * @param A Two-dimensional array of doubles.
+     * @param m Number of rows.
+     * @param n Number of colums.
+     */
+    public Matrix(double[][] A, int m, int n)
+    {
+        this.A = A;
+        this.m = m;
+        this.n = n;
+    }
+
+//    /**
+//     * Construct a matrix from a one-dimensional packed array
+//     *
+//     * @param vals One-dimensional array of doubles, packed by columns (ala
+//     * Fortran).
+//     * @param m Number of rows.
+//     * @exception IllegalArgumentException Array length must be a multiple of m.
+//     */
+//    public Matrix(double vals[], int m)
+//    {
+//        this.m = m;
+//        n = (m != 0 ? vals.length / m : 0);
+//        if (m * n != vals.length)
+//        {
+//            throw new IllegalArgumentException("Array length must be a multiple of m.");
+//        }
+//        A = new double[m][n];
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                A[i][j] = vals[i + j * m];
+//            }
+//        }
+//    }
+//
+//    /* ------------------------
+//     Public Methods
+//     * ------------------------ */
+//    /**
+//     * Construct a matrix from a copy of a 2-D array.
+//     *
+//     * @param A Two-dimensional array of doubles.
+//     * @exception IllegalArgumentException All rows must have the same length
+//     */
+//    public static Matrix constructWithCopy(double[][] A)
+//    {
+//        int m = A.length;
+//        int n = A[0].length;
+//        Matrix X = new Matrix(m, n);
+//        double[][] C = X.getArray();
+//        for (int i = 0; i < m; i++)
+//        {
+//            if (A[i].length != n)
+//            {
+//                throw new IllegalArgumentException("All rows must have the same length.");
+//            }
+//            for (int j = 0; j < n; j++)
+//            {
+//                C[i][j] = A[i][j];
+//            }
+//        }
+//        return X;
+//    }
+//
+//    /**
+//     * Make a deep copy of a matrix
+//     */
+//    public Matrix copy()
+//    {
+//        Matrix X = new Matrix(m, n);
+//        double[][] C = X.getArray();
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                C[i][j] = A[i][j];
+//            }
+//        }
+//        return X;
+//    }
+//
+//    /**
+//     * Clone the Matrix object.
+//     */
+//    public Object clone()
+//    {
+//        return this.copy();
+//    }
+//
+    /**
+     * Access the internal two-dimensional array.
+     *
+     * @return Pointer to the two-dimensional array of matrix elements.
+     * TODO should this return a copy?
+     */
+    public double[][] getArray()
+    {
+        return A;
+    }
+
+    /**
+     * Copy the internal two-dimensional array.
+     *
+     * @return Two-dimensional array copy of matrix elements.
+     */
+    public double[][] getArrayCopy()
+    {
+        double[][] C = new double[m][n];
+        for (int i = 0; i < m; i++)
+        {
+            for (int j = 0; j < n; j++)
+            {
+                C[i][j] = A[i][j];
+            }
+        }
+        return C;
+    }
+
+//    /**
+//     * Make a one-dimensional column packed copy of the internal array.
+//     *
+//     * @return Matrix elements packed in a one-dimensional array by columns.
+//     */
+//    public double[] getColumnPackedCopy()
+//    {
+//        double[] vals = new double[m * n];
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                vals[i + j * m] = A[i][j];
+//            }
+//        }
+//        return vals;
+//    }
+//
+//    /**
+//     * Make a one-dimensional row packed copy of the internal array.
+//     *
+//     * @return Matrix elements packed in a one-dimensional array by rows.
+//     */
+//    public double[] getRowPackedCopy()
+//    {
+//        double[] vals = new double[m * n];
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                vals[i * n + j] = A[i][j];
+//            }
+//        }
+//        return vals;
+//    }
+//
+    /**
+     * @return the number of rows.
+     */
+    public int getRowDimension()
+    {
+        return m;
+    }
+
+    /**
+     * @return the number of columns.
+     */
+    public int getColumnDimension()
+    {
+        return n;
+    }
+
+    /**
+     * @param i Row index.
+     * @param j Column index.
+     * @return A(i,j)
+     * @exception ArrayIndexOutOfBoundsException
+     */
+    public double get(int i, int j)
+    {
+        return A[i][j];
+    }
+
+    //for symmetric matrices
+public Matrix sub(int size, int rIndex, int cIndex)
+{
+    return getMatrix(rIndex, rIndex+size-1,cIndex, cIndex+size-1);
+}
+
+//non-symmetric Matrix
+public Matrix sub(int rSize, int cSize, int rIndex, int cIndex)
+{
+    return getMatrix(rIndex, rIndex+rSize-1,cIndex, cIndex+cSize-1);
+}
+
+public Vector subCol(int size, int colIndex, int rStart)
+{
+  return new Vector(getMatrix(rStart, rStart+size-1, colIndex, colIndex));  
+}
+
+public void placeAt(Matrix a, int rIndex, int cIndex)
+{
+    for(int i=0; i<a.getRowDimension(); ++i)
+    {
+        for(int j=0; j<a.getColumnDimension(); ++j)
+        {
+            A[rIndex+i][cIndex+j] = a.get(i,j);
+        }
+    }
+}
+
+public void placeInCol(Vector v, int rIndex, int cIndex)
+{
+    for(int i=0; i < v.size(); ++i)
+    {
+        A[rIndex+i][cIndex] = v.get(i);
+    }
+}
+    
+    
+    /**
+     * Returns a submatrix.
+     *
+     * @param i0 Initial row index
+     * @param i1 Final row index
+     * @param j0 Initial column index
+     * @param j1 Final column index
+     * @return A(i0:i1,j0:j1)
+     * @exception ArrayIndexOutOfBoundsException Submatrix indices
+     */
+    public Matrix getMatrix(int i0, int i1, int j0, int j1)
+    {
+        Matrix X = new Matrix(i1 - i0 + 1, j1 - j0 + 1);
+        double[][] B = X.getArray();
+        try
+        {
+            for (int i = i0; i <= i1; i++)
+            {
+                for (int j = j0; j <= j1; j++)
+                {
+                    B[i - i0][j - j0] = A[i][j];
+                }
+            }
+        } catch (ArrayIndexOutOfBoundsException e)
+        {
+            throw new ArrayIndexOutOfBoundsException("Submatrix indices");
+        }
+        return X;
+    }
+
+//    /**
+//     * Returns a submatrix.
+//     *
+//     * @param r Array of row indices.
+//     * @param c Array of column indices.
+//     * @return A(r(:),c(:))
+//     * @exception ArrayIndexOutOfBoundsException Submatrix indices
+//     */
+//    public Matrix getMatrix(int[] r, int[] c)
+//    {
+//        Matrix X = new Matrix(r.length, c.length);
+//        double[][] B = X.getArray();
+//        try
+//        {
+//            for (int i = 0; i < r.length; i++)
+//            {
+//                for (int j = 0; j < c.length; j++)
+//                {
+//                    B[i][j] = A[r[i]][c[j]];
+//                }
+//            }
+//        } catch (ArrayIndexOutOfBoundsException e)
+//        {
+//            throw new ArrayIndexOutOfBoundsException("Submatrix indices");
+//        }
+//        return X;
+//    }
+//
+//    /**
+//     * Returns a submatrix.
+//     *
+//     * @param i0 Initial row index
+//     * @param i1 Final row index
+//     * @param c Array of column indices.
+//     * @return A(i0:i1,c(:))
+//     * @exception ArrayIndexOutOfBoundsException Submatrix indices
+//     */
+//    public Matrix getMatrix(int i0, int i1, int[] c)
+//    {
+//        Matrix X = new Matrix(i1 - i0 + 1, c.length);
+//        double[][] B = X.getArray();
+//        try
+//        {
+//            for (int i = i0; i <= i1; i++)
+//            {
+//                for (int j = 0; j < c.length; j++)
+//                {
+//                    B[i - i0][j] = A[i][c[j]];
+//                }
+//            }
+//        } catch (ArrayIndexOutOfBoundsException e)
+//        {
+//            throw new ArrayIndexOutOfBoundsException("Submatrix indices");
+//        }
+//        return X;
+//    }
+//
+    /**
+     * Returns a submatrix.
+     *
+     * @param r Array of row indices.
+     * @param j0 Initial column index
+     * @param j1 Final column index
+     * @return A(r(:),j0:j1)
+     * @exception ArrayIndexOutOfBoundsException Submatrix indices
+     */
+    public Matrix getMatrix(int[] r, int j0, int j1)
+    {
+        Matrix X = new Matrix(r.length, j1 - j0 + 1);
+        double[][] B = X.getArray();
+        try
+        {
+            for (int i = 0; i < r.length; i++)
+            {
+                for (int j = j0; j <= j1; j++)
+                {
+                    B[i][j - j0] = A[r[i]][j];
+                }
+            }
+        } catch (ArrayIndexOutOfBoundsException e)
+        {
+            throw new ArrayIndexOutOfBoundsException("Submatrix indices");
+        }
+        return X;
+    }
+
+    /**
+     * Sets a single element.
+     *
+     * @param i Row index.
+     * @param j Column index.
+     * @param s A(i,j).
+     * @exception ArrayIndexOutOfBoundsException
+     */
+    public void set(int i, int j, double s)
+    {
+        A[i][j] = s;
+    }
+
+//    /**
+//     * Sets a submatrix.
+//     *
+//     * @param i0 Initial row index
+//     * @param i1 Final row index
+//     * @param j0 Initial column index
+//     * @param j1 Final column index
+//     * @param X A(i0:i1,j0:j1)
+//     * @exception ArrayIndexOutOfBoundsException Submatrix indices
+//     */
+//    public void setMatrix(int i0, int i1, int j0, int j1, Matrix X)
+//    {
+//        try
+//        {
+//            for (int i = i0; i <= i1; i++)
+//            {
+//                for (int j = j0; j <= j1; j++)
+//                {
+//                    A[i][j] = X.get(i - i0, j - j0);
+//                }
+//            }
+//        } catch (ArrayIndexOutOfBoundsException e)
+//        {
+//            throw new ArrayIndexOutOfBoundsException("Submatrix indices");
+//        }
+//    }
+//
+//    /**
+//     * Sets a submatrix.
+//     *
+//     * @param r Array of row indices.
+//     * @param c Array of column indices.
+//     * @param X A(r(:),c(:))
+//     * @exception ArrayIndexOutOfBoundsException Submatrix indices
+//     */
+//    public void setMatrix(int[] r, int[] c, Matrix X)
+//    {
+//        try
+//        {
+//            for (int i = 0; i < r.length; i++)
+//            {
+//                for (int j = 0; j < c.length; j++)
+//                {
+//                    A[r[i]][c[j]] = X.get(i, j);
+//                }
+//            }
+//        } catch (ArrayIndexOutOfBoundsException e)
+//        {
+//            throw new ArrayIndexOutOfBoundsException("Submatrix indices");
+//        }
+//    }
+//
+//    /**
+//     * Sets a submatrix.
+//     *
+//     * @param r Array of row indices.
+//     * @param j0 Initial column index
+//     * @param j1 Final column index
+//     * @param X A(r(:),j0:j1)
+//     * @exception ArrayIndexOutOfBoundsException Submatrix indices
+//     */
+//    public void setMatrix(int[] r, int j0, int j1, Matrix X)
+//    {
+//        try
+//        {
+//            for (int i = 0; i < r.length; i++)
+//            {
+//                for (int j = j0; j <= j1; j++)
+//                {
+//                    A[r[i]][j] = X.get(i, j - j0);
+//                }
+//            }
+//        } catch (ArrayIndexOutOfBoundsException e)
+//        {
+//            throw new ArrayIndexOutOfBoundsException("Submatrix indices");
+//        }
+//    }
+//
+//    /**
+//     * Sets a submatrix.
+//     *
+//     * @param i0 Initial row index
+//     * @param i1 Final row index
+//     * @param c Array of column indices.
+//     * @param X A(i0:i1,c(:))
+//     * @exception ArrayIndexOutOfBoundsException Submatrix indices
+//     */
+//    public void setMatrix(int i0, int i1, int[] c, Matrix X)
+//    {
+//        try
+//        {
+//            for (int i = i0; i <= i1; i++)
+//            {
+//                for (int j = 0; j < c.length; j++)
+//                {
+//                    A[i][c[j]] = X.get(i - i0, j);
+//                }
+//            }
+//        } catch (ArrayIndexOutOfBoundsException e)
+//        {
+//            throw new ArrayIndexOutOfBoundsException("Submatrix indices");
+//        }
+//    }
+//
+    /**
+     * Returns the transpose of the matrix. The elements of this matrix remain
+     * unchanged.
+     *
+     * @return a new matrix A<sup>T</sup>
+     */
+    public Matrix transpose()
+    {
+        Matrix X = new Matrix(n, m);
+        double[][] C = X.getArray();
+        for (int i = 0; i < m; i++)
+        {
+            for (int j = 0; j < n; j++)
+            {
+                C[j][i] = A[i][j];
+            }
+        }
+        return X;
+    }
+    
+    /**
+     * Transpose the matrix. The elements of this matrix 
+     * are changed in place.
+     *
+     */
+    public void transposeInPlace()
+    {
+        double[][] C = getArrayCopy();
+        for (int i = 0; i < m; i++)
+        {
+            for (int j = 0; j < n; j++)
+            {
+                A[j][i] = C[i][j];
+            }
+        }
+    }    
+
+//    /**
+//     * One norm
+//     *
+//     * @return maximum column sum.
+//     */
+//    public double norm1()
+//    {
+//        double f = 0;
+//        for (int j = 0; j < n; j++)
+//        {
+//            double s = 0;
+//            for (int i = 0; i < m; i++)
+//            {
+//                s += Math.abs(A[i][j]);
+//            }
+//            f = Math.max(f, s);
+//        }
+//        return f;
+//    }
+//
+//    /**
+//     * Two norm
+//     *
+//     * @return maximum singular value.
+//     */
+//    public double norm2()
+//    {
+//        return (new SingularValueDecomposition(this).norm2());
+//    }
+//
+//    /**
+//     * Infinity norm
+//     *
+//     * @return maximum row sum.
+//     */
+//    public double normInf()
+//    {
+//        double f = 0;
+//        for (int i = 0; i < m; i++)
+//        {
+//            double s = 0;
+//            for (int j = 0; j < n; j++)
+//            {
+//                s += Math.abs(A[i][j]);
+//            }
+//            f = Math.max(f, s);
+//        }
+//        return f;
+//    }
+//
+//    /**
+//     * Frobenius norm
+//     *
+//     * @return sqrt of sum of squares of all elements.
+//     */
+//    public double normF()
+//    {
+//        double f = 0;
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                f = hypot(f, A[i][j]);
+//            }
+//        }
+//        return f;
+//    }
+//
+    /**
+     * Unary minus
+     *
+     * @return -A
+     */
+    public Matrix uminus()
+    {
+        Matrix X = new Matrix(m, n);
+        double[][] C = X.getArray();
+        for (int i = 0; i < m; i++)
+        {
+            for (int j = 0; j < n; j++)
+            {
+                C[i][j] = -A[i][j];
+            }
+        }
+        return X;
+    }
+
+    /**
+     * C = A + B
+     *
+     * @param B another matrix
+     * @return A + B
+     */
+    public Matrix plus(Matrix B)
+    {
+        checkMatrixDimensions(B);
+        Matrix X = new Matrix(m, n);
+        double[][] C = X.getArray();
+        for (int i = 0; i < m; i++)
+        {
+            for (int j = 0; j < n; j++)
+            {
+                C[i][j] = A[i][j] + B.A[i][j];
+            }
+        }
+        return X;
+    }
+
+//    /**
+//     * A = A + B
+//     *
+//     * @param B another matrix
+//     * @return A + B
+//     */
+//    public Matrix plusEquals(Matrix B)
+//    {
+//        checkMatrixDimensions(B);
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                A[i][j] = A[i][j] + B.A[i][j];
+//            }
+//        }
+//        return this;
+//    }
+//
+    /**
+     * C = A - B
+     *
+     * @param B another matrix
+     * @return A - B
+     */
+    public Matrix minus(Matrix B)
+    {
+        checkMatrixDimensions(B);
+        Matrix X = new Matrix(m, n);
+        double[][] C = X.getArray();
+        for (int i = 0; i < m; i++)
+        {
+            for (int j = 0; j < n; j++)
+            {
+                C[i][j] = A[i][j] - B.A[i][j];
+            }
+        }
+        return X;
+    }
+
+//    /**
+//     * A = A - B
+//     *
+//     * @param B another matrix
+//     * @return A - B
+//     */
+//    public Matrix minusEquals(Matrix B)
+//    {
+//        checkMatrixDimensions(B);
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                A[i][j] = A[i][j] - B.A[i][j];
+//            }
+//        }
+//        return this;
+//    }
+//
+//    /**
+//     * Element-by-element multiplication, C = A.*B
+//     *
+//     * @param B another matrix
+//     * @return A.*B
+//     */
+//    public Matrix arrayTimes(Matrix B)
+//    {
+//        checkMatrixDimensions(B);
+//        Matrix X = new Matrix(m, n);
+//        double[][] C = X.getArray();
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                C[i][j] = A[i][j] * B.A[i][j];
+//            }
+//        }
+//        return X;
+//    }
+//
+//    /**
+//     * Element-by-element multiplication in place, A = A.*B
+//     *
+//     * @param B another matrix
+//     * @return A.*B
+//     */
+//    public Matrix arrayTimesEquals(Matrix B)
+//    {
+//        checkMatrixDimensions(B);
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                A[i][j] = A[i][j] * B.A[i][j];
+//            }
+//        }
+//        return this;
+//    }
+//
+//    /**
+//     * Element-by-element right division, C = A./B
+//     *
+//     * @param B another matrix
+//     * @return A./B
+//     */
+//    public Matrix arrayRightDivide(Matrix B)
+//    {
+//        checkMatrixDimensions(B);
+//        Matrix X = new Matrix(m, n);
+//        double[][] C = X.getArray();
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                C[i][j] = A[i][j] / B.A[i][j];
+//            }
+//        }
+//        return X;
+//    }
+//
+//    /**
+//     * Element-by-element right division in place, A = A./B
+//     *
+//     * @param B another matrix
+//     * @return A./B
+//     */
+//    public Matrix arrayRightDivideEquals(Matrix B)
+//    {
+//        checkMatrixDimensions(B);
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                A[i][j] = A[i][j] / B.A[i][j];
+//            }
+//        }
+//        return this;
+//    }
+//
+//    /**
+//     * Element-by-element left division, C = A.\B
+//     *
+//     * @param B another matrix
+//     * @return A.\B
+//     */
+//    public Matrix arrayLeftDivide(Matrix B)
+//    {
+//        checkMatrixDimensions(B);
+//        Matrix X = new Matrix(m, n);
+//        double[][] C = X.getArray();
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                C[i][j] = B.A[i][j] / A[i][j];
+//            }
+//        }
+//        return X;
+//    }
+//
+//    /**
+//     * Element-by-element left division in place, A = A.\B
+//     *
+//     * @param B another matrix
+//     * @return A.\B
+//     */
+//    public Matrix arrayLeftDivideEquals(Matrix B)
+//    {
+//        checkMatrixDimensions(B);
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                A[i][j] = B.A[i][j] / A[i][j];
+//            }
+//        }
+//        return this;
+//    }
+//
+    /**
+     * Multiply a matrix by a scalar, C = s*A
+     *
+     * @param s scalar
+     * @return s*A
+     */
+    public Matrix times(double s)
+    {
+        Matrix X = new Matrix(m, n);
+        double[][] C = X.getArray();
+        for (int i = 0; i < m; i++)
+        {
+            for (int j = 0; j < n; j++)
+            {
+                C[i][j] = s * A[i][j];
+            }
+        }
+        return X;
+    }
+
+//    /**
+//     * Multiply a matrix by a scalar in place, A = s*A
+//     *
+//     * @param s scalar
+//     * @return replace A by s*A
+//     */
+//    public Matrix timesEquals(double s)
+//    {
+//        for (int i = 0; i < m; i++)
+//        {
+//            for (int j = 0; j < n; j++)
+//            {
+//                A[i][j] = s * A[i][j];
+//            }
+//        }
+//        return this;
+//    }
+//
+    /**
+     * Linear algebraic matrix multiplication, A * B
+     *
+     * @param B another matrix
+     * @return Matrix product, A * B
+     * @exception IllegalArgumentException Matrix inner dimensions must agree.
+     */
+    public Matrix times(Matrix B)
+    {
+        if (B.m != n)
+        {
+            throw new IllegalArgumentException("Matrix inner dimensions must agree.");
+        }
+        Matrix X = new Matrix(m, B.n);
+        double[][] C = X.getArray();
+        double[] Bcolj = new double[n];
+        for (int j = 0; j < B.n; j++)
+        {
+            for (int k = 0; k < n; k++)
+            {
+                Bcolj[k] = B.A[k][j];
+            }
+            for (int i = 0; i < m; i++)
+            {
+                double[] Arowi = A[i];
+                double s = 0;
+                for (int k = 0; k < n; k++)
+                {
+                    s += Arowi[k] * Bcolj[k];
+                }
+                C[i][j] = s;
+            }
+        }
+        return X;
+    }
+
+//    /**
+//     * Calculates the vector y = A.x
+//     *
+//     * @param x
+//     * @return a new vector y

[truncated at 1000 lines; 556 more skipped]

java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix

SymMatrix.java added at 919

--- java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix/SymMatrix.java	                        (rev 0)
+++ java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix/SymMatrix.java	2014-08-28 15:31:06 UTC (rev 919)
@@ -0,0 +1,66 @@

+package org.hps.recon.tracking.gbl.matrix;
+
+/**
+ *
+ * @author Norman A Graf
+ * 
+ *  @version $Id:
+ */
+public class SymMatrix extends Matrix implements java.io.Serializable
+{
+
+    public SymMatrix(int n)
+    {
+        super(n, n);
+    }
+
+    public SymMatrix(SymMatrix smat)
+    {
+        super(smat.getRowDimension(), smat.getColumnDimension());
+        for (int i = 0; i < m; i++)
+        {
+            for (int j = 0; j < n; j++)
+            {
+                A[i][j] = smat.get(i, j);
+            }
+        }
+    }
+    
+    public SymMatrix(Matrix mat)
+    {
+        super(mat.getRowDimension(), mat.getColumnDimension());
+        for (int i = 0; i < m; i++)
+        {
+            for (int j = 0; j < n; j++)
+            {
+                A[i][j] = mat.get(i, j);
+            }
+        }
+    }
+
+    @Override
+    public void set(int i, int j, double s)
+    {
+        super.set(i, j, s);
+        super.set(j, i, s);
+    }
+
+    public void setToIdentity()
+    {
+        for (int i = 0; i < m; i++)
+        {
+            for (int j = 0; j < n; j++)
+            {
+                A[i][j] = (i == j ? 1.0 : 0.0);
+            }
+        }
+    } 
+    
+    //Calculate B * (this) * B^T , final matrix will be (nrowsb x nrowsb)
+    //TODO see if this needs to be made more efficient
+    public SymMatrix Similarity(Matrix b)
+    {
+        SymMatrix X =  new SymMatrix(b.times(this.times(b.transpose())));
+        return X;
+    }
+}

java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix

VMatrix.java added at 919

--- java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix/VMatrix.java	                        (rev 0)
+++ java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix/VMatrix.java	2014-08-28 15:31:06 UTC (rev 919)
@@ -0,0 +1,201 @@

+package org.hps.recon.tracking.gbl.matrix;
+
+import static java.lang.Math.min;
+import java.util.ArrayList;
+
+/**
+ * Simple Vector based on ArrayList
+ *
+ * @author Norman A. Graf
+ *
+ * @version $Id$
+ */
+public class VMatrix
+{
+
+    private int numRows;
+    private int numCols;
+    //TODO replace with a simple double[]
+    private ArrayList theVec;
+
+    public VMatrix(int nRows, int nCols)
+    {
+        numRows = nRows;
+        numCols = nCols;
+        theVec = new ArrayList(nRows * nCols);
+        for (int i = 0; i < numRows * nCols; ++i)
+        {
+            theVec.add(0.);
+        }
+    }
+    
+public VMatrix(VMatrix m)
+    {
+        numRows = m.numRows;
+        numCols = m.numCols;
+        theVec = new ArrayList(numRows * numCols);
+        for (int i = 0; i < numRows; ++i)
+        {
+            for(int j=0; j<numCols; ++j)
+            {
+            theVec.set(numCols * i + j, m.get(i, j));
+            }
+        }
+    }    
+
+    public VMatrix copy()
+    {
+        VMatrix aResult = new VMatrix(numRows, numCols);
+        for (int i = 0; i < numRows; ++i)
+        {
+            for (int j = 0; j < numCols; ++j)
+            {
+                {
+                    aResult.set(i, j, get(i, j));
+                }
+            }
+        }
+        return aResult;
+    }
+
+    /// Resize Matrix.
+    /**
+     * \param [in] nRows Number of rows. \param [in] nCols Number of columns.
+     */
+    // TODO understand what this method is supposed to do.
+    // may not need it as ArrayList already automatically resizes...
+    public void resize(int nRows, int nCols)
+    {
+        numRows = nRows;
+        numCols = nCols;
+        theVec = new ArrayList(numRows * nCols);
+        for (int i = 0; i < numRows * nCols; ++i)
+        {
+            theVec.add(0.);
+        }
+    }
+
+/// Get transposed matrix.
+    /**
+     * \return Transposed matrix.
+     */
+    public VMatrix transpose()
+    {
+        VMatrix aResult = new VMatrix(numCols, numRows);
+        for (int i = 0; i < numRows; ++i)
+        {
+            for (int j = 0; j < numCols; ++j)
+            {
+                //System.out.println("row: "+i+" col: "+j+" val: "+theVec.get(numCols * i + j));
+                aResult.set(j, i, (double) theVec.get(numCols * i + j));
+            }
+        }
+        return aResult;
+    }
+
+    public void set(int row, int col, double val)
+    {
+        theVec.set(numCols * row + col, val);
+    }
+    
+     public void addTo(int row, int col, double val)
+    {
+        theVec.set(numCols * row + col, (double) theVec.get(numCols * row + col)+val);
+    }
+     public void subFrom(int row, int col, double val)
+    {
+        theVec.set(numCols * row + col, (double) theVec.get(numCols * row + col)-val);
+    }
+
+    public double get(int row, int col)
+    {
+        return (double) theVec.get(numCols * row + col);
+    }
+
+/// Get number of rows.
+    /**
+     * \return Number of rows.
+     */
+    int getNumRows()
+    {
+        return numRows;
+    }
+
+/// Get number of columns.
+    /**
+     * \return Number of columns.
+     */
+    int getNumCols()
+    {
+        return numCols;
+    }
+
+/// Multiplication Matrix*Vector.
+    VVector times(VVector aVector)
+    {
+        VVector aResult = new VVector(numRows);
+        for (int i = 0; i < numRows; ++i)
+        {
+            double sum = 0.0;
+            for (int j = 0; j < numCols; ++j)
+            {
+                sum += (double) theVec.get(numCols * i + j) * aVector.get(j);
+            }
+            aResult.set(i, sum);
+        }
+        return aResult;
+    }
+
+// Multiplication Matrix*Matrix.
+    VMatrix times(VMatrix aMatrix)
+    {
+
+        VMatrix aResult = new VMatrix(numRows, aMatrix.numCols);
+        for (int i = 0; i < numRows; ++i)
+        {
+            for (int j = 0; j < aMatrix.numCols; ++j)
+            {
+                double sum = 0.0;
+                for (int k = 0; k < numCols; ++k)
+                {
+                    sum += (double) theVec.get(numCols * i + k) * aMatrix.get(k, j);
+                }
+                aResult.set(i, j, sum);
+            }
+        }
+        return aResult;
+    }
+
+/// Addition Matrix+Matrix.
+    VMatrix plus(VMatrix aMatrix)
+    {
+        VMatrix aResult = new VMatrix(numRows, numCols);
+        for (int i = 0; i < numRows; ++i)
+        {
+            for (int j = 0; j < numCols; ++j)
+            {
+                aResult.set(i, j, (double) theVec.get(numCols * i + j) + (double) aMatrix.get(i, j));
+            }
+        }
+        return aResult;
+    }
+
+/// Print matrix.
+    public void print()
+    {
+        System.out.println(" VMatrix: " + numRows + "*" + numCols);
+        for (int i = 0; i < numRows; ++i)
+        {
+            for (int j = 0; j < numCols; ++j)
+            {
+                if (j % 5 == 0)
+                {
+                    System.out.format("%n%4d " + "," + "%4d" + " - " + "%4d" + " : ", i, j, min(j + 4, numCols));
+                }
+                System.out.format("%13f", theVec.get(numCols * i + j));
+            }
+        }
+        System.out.print("\n\n\n");
+    }
+
+}

java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix

VSymMatrix.java added at 919

--- java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix/VSymMatrix.java	                        (rev 0)
+++ java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/matrix/VSymMatrix.java	2014-08-28 15:31:06 UTC (rev 919)
@@ -0,0 +1,303 @@

+package org.hps.recon.tracking.gbl.matrix;
+
+import static java.lang.Math.min;
+import static java.lang.Math.max;
+import static java.lang.Math.abs;
+import java.util.ArrayList;
+
+/**
+ * Simple symmetric Matrix based on ArrayList
+ *
+ * @author Norman A. Graf
+ *
+ * @version $Id$
+ */
+public class VSymMatrix
+{
+
+    private int numRows;
+    //TODO replace with a simple double[]
+    private ArrayList theVec;
+
+    public VSymMatrix(int nRows)
+    {
+        numRows = nRows;
+        theVec = new ArrayList((nRows * nRows + nRows) / 2);
+        for (int i = 0; i < (nRows * nRows + nRows) / 2; ++i)
+        {
+            theVec.add(i, 0.);
+        }
+    }
+
+    public VSymMatrix copy()
+    {
+        VSymMatrix aResult = new VSymMatrix(numRows);
+        for (int i = 0; i < numRows; ++i)
+        {
+            for (int j = 0; j < numRows; ++j)
+            {
+                {
+                    aResult.set(i, j, get(i, j));
+                }
+            }
+        }
+        return aResult;
+    }
+
+/// Resize symmetric matrix.
+    /**
+     * \param [in] nRows Number of rows.
+     */
+    // TODO understand what this method is supposed to do.
+    // may not need it as ArrayList already automatically resizes...
+    void resize(int nRows)
+    {
+        numRows = nRows;
+        theVec = new ArrayList(nRows * nRows + nRows);
+        for (int i = 0; i < nRows * nRows + nRows; ++i)
+        {
+            theVec.add(0.);
+        }
+    }
+
+/// Get number of rows (= number of colums).
+    /**
+     * \return Number of rows.
+     */
+    int getNumRows()
+    {
+        return numRows;
+    }
+
+    public void set(int row, int col, double val)
+    {
+        theVec.set((row * row + row) / 2 + col, val);
+    }
+
+    public void addTo(int row, int col, double val)
+    {
+        theVec.set((row * row + row) / 2 + col, (double) theVec.get((row * row + row) / 2 + col) + val);
+    }
+    
+    public void subFrom(int row, int col, double val)
+    {
+        theVec.set((row * row + row) / 2 + col, (double) theVec.get((row * row + row) / 2 + col) - val);
+    }
+
+    public double get(int row, int col)
+    {
+        return (double) theVec.get((row * row + row) / 2 + col);
+    }
+
+/// Print matrix.
+    void print()
+    {
+        System.out.println(" VSymMatrix: " + numRows + "*" + numRows);
+        for (int i = 0; i < numRows; ++i)
+        {
+            for (int j = 0; j <= i; ++j)
+            {
+                if (j % 5 == 0)
+                {
+                    System.out.format("%n%4d " + "," + "%4d" + " - " + "%4d" + " : ", i, j, min(j + 4, i));
+                }
+                System.out.format("%13f", theVec.get((i * i + i) / 2 + j));
+            }
+        }
+        System.out.print("\n\n\n");
+    }
+
+/// Subtraction SymMatrix-(sym)Matrix.
+    VSymMatrix minus(VMatrix aMatrix)
+    {
+        VSymMatrix aResult = new VSymMatrix(numRows);
+        for (int i = 0; i < numRows; ++i)
+        {
+            for (int j = 0; j <= i; ++j)
+            {
+                aResult.set(i, j, (double) theVec.get((i * i + i) / 2 + j) - (double) aMatrix.get(i, j));
+            }
+        }
+        return aResult;
+    }
+
+/// Multiplication SymMatrix*Vector.
+    VVector times(VVector aVector)
+    {
+        VVector aResult = new VVector(numRows);
+        for (int i = 0; i < numRows; ++i)
+        {
+            aResult.set(i, (double) theVec.get((i * i + i) / 2 + i) * (double) aVector.get(i));
+            for (int j = 0; j < i; ++j)
+            {
+                aResult.set(j, aResult.get(j) + (double) theVec.get((i * i + i) / 2 + j) * aVector.get(i));
+                aResult.set(i, aResult.get(i) + (double) theVec.get((i * i + i) / 2 + j) * aVector.get(j));
+            }
+        }
+        return aResult;
+    }
+
+/// Multiplication SymMatrix*Matrix.
+    VMatrix times(VMatrix aMatrix)
+    {
+        int nCol = aMatrix.getNumCols();
+        VMatrix aResult = new VMatrix(numRows, nCol);
+        for (int l = 0; l < nCol; ++l)
+        {
+            for (int i = 0; i < numRows; ++i)
+            {
+                aResult.set(i, l, (double) theVec.get((i * i + i) / 2 + i) * (double) aMatrix.get(i, l));
+                for (int j = 0; j < i; ++j)
+                {
+                    aResult.set(j, l, aResult.get(j, l) + (double) theVec.get((i * i + i) / 2 + j) * aMatrix.get(i, l));
+                    aResult.set(i, l, aResult.get(i, l) + (double) theVec.get((i * i + i) / 2 + j) * aMatrix.get(j, l));
+                }
+            }
+        }
+        return aResult;
+    }
+
+    /*============================================================================
+     from mpnum.F (MillePede-II by V. Blobel, Univ. Hamburg)
+     ============================================================================*/
+/// Matrix inversion.
+    /**
+     * Invert symmetric N-by-N matrix V in symmetric storage mode V(1) = V11,
+     * V(2) = V12, V(3) = V22, V(4) = V13, . . . replaced by inverse matrix
+     *
+     * Method of solution is by elimination selecting the pivot on the diagonal
+     * each stage. The rank of the matrix is returned in NRANK. For NRANK ne N,
+     * all remaining rows and cols of the resulting matrix V are set to zero.
+     * \exception 1 : matrix is singular. \return Rank of matrix.
+     */
+    public int invert()
+    {
+
+        final double eps = 1.0E-10;
+        int aSize = numRows;
+        int[] next = new int[aSize];
+        double[] diag = new double[aSize];
+        int nSize = aSize;
+
+        int first = 1;
+        for (int i = 1; i <= nSize; ++i)
+        {
+            next[i - 1] = i + 1; // set "next" pointer
+            diag[i - 1] = abs((double) theVec.get((i * i + i) / 2 - 1)); // save abs of diagonal elements
+        }
+        next[aSize - 1] = -1; // end flag
+
+        int nrank = 0;
+        for (int i = 1; i <= nSize; ++i)
+        { // start of loop
+            int k1 = 0;
+            double vkk = 0.0;
+
+            int j1 = first;
+            int previous = 0;
+            int last = previous;
+            // look for pivot
+            while (j1 > 0)
+            {
+                int jj = (j1 * j1 + j1) / 2 - 1;
+                if (abs((double) theVec.get(jj)) > max(abs(vkk), eps * diag[j1 - 1]))
+                {
+                    vkk = (double) theVec.get(jj);
+                    k1 = j1;
+                    last = previous;
+                }
+                previous = j1;
+                j1 = next[j1 - 1];
+            }
+            // pivot found
+            if (k1 > 0)
+            {
+                int kk = (k1 * k1 + k1) / 2 - 1;
+                if (last <= 0)
+                {
+                    first = next[k1 - 1];
+                } else
+                {
+                    next[last - 1] = next[k1 - 1];
+                }
+                next[k1 - 1] = 0; // index is used, reset
+                nrank++; // increase rank and ...
+
+                vkk = 1.0 / vkk;
+                theVec.set(kk, -vkk);
+                int jk = kk - k1;
+                int jl = -1;
+                for (int j = 1; j <= nSize; ++j)
+                { // elimination
+                    if (j == k1)
+                    {
+                        jk = kk;
+                        jl += j;
+                    } else
+                    {
+                        if (j < k1)
+                        {
+                            ++jk;
+                        } else
+                        {
+                            jk += j - 1;
+                        }
+
+                        double vjk = (double) theVec.get(jk);
+                        theVec.set(jk, vkk * vjk);
+                        int lk = kk - k1;
+                        if (j >= k1)
+                        {
+                            for (int l = 1; l <= k1 - 1; ++l)
+                            {
+                                ++jl;
+                                ++lk;
+                                theVec.set(jl, (double) theVec.get(jl) - (double) theVec.get(lk) * vjk);
+                            }
+                            ++jl;
+                            lk = kk;
+                            for (int l = k1 + 1; l <= j; ++l)
+                            {
+                                ++jl;
+                                lk += l - 1;
+                                theVec.set(jl, (double) theVec.get(jl) - (double) theVec.get(lk) * vjk);
+                            }
+                        } else
+                        {
+                            for (int l = 1; l <= j; ++l)
+                            {
+                                ++jl;
+                                ++lk;
+                                theVec.set(jl, (double) theVec.get(jl) - (double) theVec.get(lk) * vjk);
+                            }
+                        }
+                    }
+                }
+
+            } else
+            {
+                for (int k = 1; k <= nSize; ++k)
+                {
+                    if (next[k - 1] >= 0)
+                    {
+                        int kk = (k * k - k) / 2 - 1;
+                        for (int j = 1; j <= nSize; ++j)
+                        {
+                            if (next[j - 1] >= 0)
+                            {
+                                theVec.set(kk + j, 0.0); // clear matrix row/col
+                            }
+                        }
+                    }
+                }
+                throw new RuntimeException("Symmetric matrix inversion is singular");//1; // singular
+            }
+        }
+        for (int ij = 0; ij < (nSize * nSize + nSize) / 2; ++ij)
+        {
+            theVec.set(ij, -(double) theVec.get(ij)); // finally reverse sign of all matrix elements
+        }
+        return nrank;
+    }
+
+}

Commit in `java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl` on MAIN
`GblData.java`	+289		added 919
`GblPoint.java`	+581	-13	918 -> 919
`GblTrajectory.java`	+1292	-5	918 -> 919
`HpsGblFitter.java`	+307	-326	918 -> 919
`matrix/BorderedBandMatrix.java`	+360		added 919
`/EigenvalueDecomposition.java`	+959		added 919
`/LUDecomposition.java`	+310		added 919
`/Matrix.java`	+1553		added 919
`/QRDecomposition.java`	+218		added 919
`/SymMatrix.java`	+66		added 919
`/VMatrix.java`	+201		added 919
`/VSymMatrix.java`	+303		added 919
`/VVector.java`	+129		added 919
`/Vector.java`	+337		added 919
	+6905	-344