--- java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/GblData.java	2014-09-17 17:45:13 UTC (rev 1035)
+++ java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/GblData.java	2014-09-17 18:01:56 UTC (rev 1036)
@@ -2,9 +2,9 @@

 
 import java.util.ArrayList;
 import java.util.List;

+import org.hps.recon.tracking.gbl.matrix.Matrix;

 import org.hps.recon.tracking.gbl.matrix.VVector;
 

-

 /**
  *
  * @author Norman A Graf

@@ -40,92 +40,109 @@

         thePrediction = 0.;
     }
 

-///// Add derivatives from measurement.
-///**
-// * Add (non-zero) derivatives to data block. Fill list of labels of used fit parameters.
-// * \param [in] iRow Row index (0-4) in up to 5D measurement
-// * \param [in] labDer Labels for derivatives
-// * \param [in] matDer Derivatives (matrix) 'measurement vs track fit parameters'
-// * \param [in] iOff Offset for row index for additional parameters
-// * \param [in] derLocal Derivatives (matrix) for additional local parameters
-// * \param [in] labGlobal Labels for additional global (MP-II) parameters
-// * \param [in] derGlobal Derivatives (matrix) for additional global (MP-II) parameters
-// * \param [in] extOff Offset for external parameters
-// * \param [in] extDer Derivatives for external Parameters
-// */
-//void addDerivatives(unsigned int iRow,
-//		const std::vector<unsigned int> &labDer, const SMatrix55 &matDer,
-//		unsigned int iOff, const TMatrixD &derLocal,
-//		const std::vector<int> &labGlobal, const TMatrixD &derGlobal,
-//		unsigned int extOff, const TMatrixD &extDer) {
-//
-//	unsigned int nParMax = 5 + derLocal.GetNcols() + extDer.GetNcols();

+/// Add derivatives from measurement.
+    /**
+     * Add (non-zero) derivatives to data block. Fill list of labels of used fit
+     * parameters. \param [in] iRow Row index (0-4) in up to 5D measurement
+     * \param [in] labDer Labels for derivatives \param [in] matDer Derivatives
+     * (matrix) 'measurement vs track fit parameters' \param [in] iOff Offset
+     * for row index for additional parameters \param [in] derLocal Derivatives
+     * (matrix) for additional local parameters \param [in] labGlobal Labels for
+     * additional global (MP-II) parameters \param [in] derGlobal Derivatives
+     * (matrix) for additional global (MP-II) parameters \param [in] extOff
+     * Offset for external parameters \param [in] extDer Derivatives for
+     * external Parameters
+     */
+    void addDerivatives(int iRow,
+                        List<Integer> labDer, Matrix matDer,
+                        int iOff, Matrix derLocal,
+                        List<Integer> labGlobal, Matrix derGlobal,
+                        int extOff, Matrix extDer)
+    {
+        int nLocal = 0;
+        int nExt = 0;
+        if (derLocal != null) {
+            nLocal = derLocal.getColumnDimension();
+        }
+        if (extDer != null) {
+            nExt = extDer.getColumnDimension();
+        }
+        int nParMax = 5 + nLocal + nExt;

 //	theParameters.reserve(nParMax); // have to be sorted
 //	theDerivatives.reserve(nParMax);

-//
-//	for (int i = 0; i < derLocal.GetNcols(); ++i) // local derivatives
-//			{
-//		if (derLocal(iRow - iOff, i)) {
-//			theParameters.push_back(i + 1);
-//			theDerivatives.push_back(derLocal(iRow - iOff, i));
-//		}
-//	}
-//
-//	for (int i = 0; i < extDer.GetNcols(); ++i) // external derivatives
-//			{
-//		if (extDer(iRow - iOff, i)) {
-//			theParameters.push_back(extOff + i + 1);
-//			theDerivatives.push_back(extDer(iRow - iOff, i));
-//		}
-//	}
-//
-//	for (unsigned int i = 0; i < 5; ++i) // curvature, offset derivatives
-//			{
-//		if (labDer[i] && matDer(iRow, i)) {
-//			theParameters.push_back(labDer[i]);
-//			theDerivatives.push_back(matDer(iRow, i));
-//		}
-//	}
-//
-//	globalLabels = labGlobal;
-//	for (int i = 0; i < derGlobal.GetNcols(); ++i) // global derivatives
-//		globalDerivatives.push_back(derGlobal(iRow - iOff, i));
-//}
-//
-///// Add derivatives from kink.
-///**
-// * Add (non-zero) derivatives to data block. Fill list of labels of used fit parameters.
-// * \param [in] iRow Row index (0-1) in 2D kink
-// * \param [in] labDer Labels for derivatives
-// * \param [in] matDer Derivatives (matrix) 'kink vs track fit parameters'
-// * \param [in] extOff Offset for external parameters
-// * \param [in] extDer Derivatives for external Parameters
-// */
-//void addDerivatives(unsigned int iRow,
-//		const std::vector<unsigned int> &labDer, const SMatrix27 &matDer,
-//		unsigned int extOff, const TMatrixD &extDer) {
-//
-//	unsigned int nParMax = 7 + extDer.GetNcols();

+
+        if (derLocal != null) {
+            for (int i = 0; i < derLocal.getColumnDimension(); ++i) // local derivatives
+            {
+                if (derLocal.get(iRow - iOff, i) != 0) {
+                    theParameters.add(i + 1);
+                    theDerivatives.add(derLocal.get(iRow - iOff, i));
+                }
+            }
+        }
+
+        if (extDer != null) {
+            for (int i = 0; i < extDer.getColumnDimension(); ++i) // external derivatives
+            {
+                if (extDer.get(iRow - iOff, i) > 0) {
+                    theParameters.add(extOff + i + 1);
+                    theDerivatives.add(extDer.get(iRow - iOff, i));
+                }
+            }
+        }
+        for (int i = 0; i < 5; ++i) // curvature, offset derivatives
+        {
+            if (labDer.get(i) != 0 && matDer.get(iRow, i) != 0) {
+                theParameters.add(labDer.get(i));
+                theDerivatives.add(matDer.get(iRow, i));
+            }
+        }
+
+        globalLabels = labGlobal;
+        for (int i = 0; i < derGlobal.getColumnDimension(); ++i) // global derivatives
+        {
+            globalDerivatives.add(derGlobal.get(iRow - iOff, i));
+        }
+    }
+
+/// Add derivatives from kink.
+    /**
+     * Add (non-zero) derivatives to data block. Fill list of labels of used fit
+     * parameters. \param [in] iRow Row index (0-1) in 2D kink \param [in]
+     * labDer Labels for derivatives \param [in] matDer Derivatives (matrix)
+     * 'kink vs track fit parameters' \param [in] extOff Offset for external
+     * parameters \param [in] extDer Derivatives for external Parameters
+     */
+    void addDerivatives(int iRow,
+                        List<Integer> labDer, Matrix matDer,
+                        int extOff, Matrix extDer)
+    {
+        int nExtDer = 0;
+        if (extDer != null) {
+            nExtDer = extDer.getColumnDimension();
+        }
+        int nParMax = 7 + nExtDer;

 //	theParameters.reserve(nParMax); // have to be sorted
 //	theDerivatives.reserve(nParMax);

-//
-//	for (int i = 0; i < extDer.GetNcols(); ++i) // external derivatives
-//			{
-//		if (extDer(iRow, i)) {
-//			theParameters.push_back(extOff + i + 1);
-//			theDerivatives.push_back(extDer(iRow, i));
-//		}
-//	}
-//
-//	for (unsigned int i = 0; i < 7; ++i) // curvature, offset derivatives
-//			{
-//		if (labDer[i] && matDer(iRow, i)) {
-//			theParameters.push_back(labDer[i]);
-//			theDerivatives.push_back(matDer(iRow, i));
-//		}
-//	}
-//}
-//

+
+        if (extDer != null) {
+            for (int i = 0; i < extDer.getColumnDimension(); ++i) // external derivatives
+            {
+                if (extDer.get(iRow, i) != 0) {
+                    theParameters.add(extOff + i + 1);
+                    theDerivatives.add(extDer.get(iRow, i));
+                }
+            }
+        }
+        for (int i = 0; i < 7; ++i) // curvature, offset derivatives
+        {
+            if (labDer.get(i) != 0 && matDer.get(iRow, i) != 0) {
+                theParameters.add(labDer.get(i));
+                theDerivatives.add(matDer.get(iRow, i));
+            }
+        }
+    }
+

 ///// Add derivatives from external seed.
 ///**
 // * Add (non-zero) derivatives to data block. Fill list of labels of used fit parameters.

@@ -191,23 +208,23 @@

         return aDiff * aDiff * thePrecision * theDownWeight;
     }
 

-///// Print data block.
-//void printData() const {
-//
-//	std::cout << " measurement at label " << theLabel << ": " << theValue
-//			<< ", " << thePrecision << std::endl;
-//	std::cout << "  param " << theParameters.size() << ":";
-//	for (unsigned int i = 0; i < theParameters.size(); ++i)
-//		std::cout << " " << theParameters[i];
-//	}
-//	std::cout << std::endl;
-//	std::cout << "  deriv " << theDerivatives.size() << ":";
-//	for (unsigned int i = 0; i < theDerivatives.size(); ++i) {
-//		std::cout << " " << theDerivatives[i];
-//	}
-//	std::cout << std::endl;
-//}
-//

+/// Print data block.
+    void printData()
+    {
+        System.out.println(" measurement at label " + theLabel + ": " + theValue
+                + ", " + thePrecision);
+        System.out.print("  param " + theParameters.size() + ":");
+        for (int i = 0; i < theParameters.size(); ++i) {
+            System.out.print(" " + theParameters.get(i));
+        }
+        System.out.println("\n");
+        System.out.print("  deriv " + theDerivatives.size() + ":");
+        for (int i = 0; i < theDerivatives.size(); ++i) {
+            System.out.print(" " + theDerivatives.get(i));
+        }
+        System.out.println("");
+    }
+

     public String toString()
     {
         StringBuffer sb = new StringBuffer(" measurement at label " + theLabel + ": " + theValue + ", " + thePrecision + "\n");

@@ -216,6 +233,10 @@

             sb.append(" " + theParameters.get(i));
         }
         sb.append("\n");

+        for (int i = 0; i < theDerivatives.size(); ++i) {
+            sb.append(" " + theDerivatives.get(i));
+        }
+        sb.append("\n");

         return sb.toString();
     }
 

@@ -236,16 +257,17 @@

 
     void getLocalData(double[] retVal, int[] indLocal, double[] derLocal)
     {

-

         retVal[0] = theValue;
         retVal[1] = thePrecision * theDownWeight;

-        indLocal = new int[theParameters.size()];
-        derLocal = new double[theParameters.size()];

         for (int i = 0; i < theParameters.size(); ++i) {
             indLocal[i] = theParameters.get(i);
             derLocal[i] = theDerivatives.get(i);
         }

+    }

+    int getNumParameters()
+    {
+        return theParameters.size();

     }
 
 //

--- java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/GblPoint.java	2014-09-17 17:45:13 UTC (rev 1035)
+++ java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/GblPoint.java	2014-09-17 18:01:56 UTC (rev 1036)
@@ -14,9 +14,11 @@

  *
  * @version $Id:
  */

-public class GblPoint {

+public class GblPoint
+{

-    public GblPoint(hep.physics.matrix.BasicMatrix jacPointToPoint) {

+    public GblPoint(hep.physics.matrix.BasicMatrix jacPointToPoint)
+    {

         theLabel = 0;
         theOffset = 0;
         measDim = 0;

@@ -34,70 +36,65 @@

         }
     }
 

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

+    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");

+        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));

+        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");
-        

+        System.out.println("meas has " + measnrows + " rows and " + measncols + " columns");
+

         Vector measvec = new Vector(measncols);

-        for(int i=0; i<measnrows; ++i)
-        {

+        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");

+
+        System.out.println("measPrec has " + measPrecnrows + " rows and " + measPrecncols + " columns");

         Vector measPrecvec = new Vector(measPrecncols);

-        for(int i=0; i<measPrecnrows; ++i)
-        {

+        for (int i = 0; i < measPrecnrows; ++i) {

             measPrecvec.set(i, measPrec.e(0, i));
         }
         System.out.println("GblPoint add measPrec: ");

-        measPrecvec.print(10, 6); 
-        

+        measPrecvec.print(10, 6);
+

         addMeasurement(a, measvec, measPrecvec, 0.);
     }
 

-    public void addScatterer(hep.physics.matrix.BasicMatrix scat, hep.physics.matrix.BasicMatrix scatPrec) {

+    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 Matrix p2pJacobian = new Matrix(5, 5); ///< Point-to-point jacobian from previous point
+    private Matrix prevJacobian = new Matrix(5, 5); ///< Jacobian to previous scatterer (or first measurement)
+    private Matrix nextJacobian = new Matrix(5, 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 Matrix measProjection = new Matrix(5, 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 Matrix scatTransformation = new Matrix(2, 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

@@ -110,7 +107,8 @@

      * previous point. \param [in] aJacobian Transformation jacobian from
      * previous point
      */

-    public GblPoint(Matrix aJacobian) {

+    public GblPoint(Matrix aJacobian)
+    {

 
         theLabel = 0;
         theOffset = 0;

@@ -129,7 +127,8 @@

         }
     }
 

-    public GblPoint(SymMatrix aJacobian) {

+    public GblPoint(SymMatrix aJacobian)
+    {

         theLabel = 0;
         theOffset = 0;
         p2pJacobian = new SymMatrix(aJacobian);

@@ -153,8 +152,9 @@

      * Minimal precision to accept measurement
      */
     public void addMeasurement(Matrix aProjection,

-            Vector aResiduals, Vector aPrecision,
-            double minPrecision) {

+                               Vector aResiduals, Vector aPrecision,
+                               double minPrecision)
+    {

         measDim = aResiduals.getRowDimension();
         int iOff = 5 - measDim;
         for (int i = 0; i < measDim; ++i) {

@@ -176,8 +176,9 @@

      * (matrix) \param [in] minPrecision Minimal precision to accept measurement
      */
     public void addMeasurement(Matrix aProjection,

-            Vector aResiduals, SymMatrix aPrecision,
-            double minPrecision) {

+                               Vector aResiduals, SymMatrix aPrecision,
+                               double minPrecision)
+    {

         measDim = aResiduals.getRowDimension();
         EigenvalueDecomposition measEigen = new EigenvalueDecomposition(aPrecision);
         measTransformation.ResizeTo(measDim, measDim);

@@ -206,7 +207,8 @@

      * minPrecision Minimal precision to accept measurement
      */
     public void addMeasurement(Vector aResiduals,

-            Vector aPrecision, double minPrecision) {

+                               Vector aPrecision, double minPrecision)
+    {

         measDim = aResiduals.getRowDimension();
         int iOff = 5 - measDim;
         for (int i = 0; i < measDim; ++i) {

@@ -214,7 +216,7 @@

             measPrecision.set(iOff + i,
                     aPrecision.get(i) >= minPrecision ? aPrecision.get(i) : 0.);
         }

-        measProjection.setToIdentity();

+        measProjection.UnitMatrix();// setToIdentity();

     }
 
 /// Add a measurement to a point.

@@ -226,7 +228,8 @@

      * (matrix) \param [in] minPrecision Minimal precision to accept measurement
      */
     public void addMeasurement(Vector aResiduals,

-            SymMatrix aPrecision, double minPrecision) {

+                               SymMatrix aPrecision, double minPrecision)
+    {

         measDim = aResiduals.getRowDimension();
         EigenvalueDecomposition measEigen = new EigenvalueDecomposition(aPrecision);
         measTransformation.ResizeTo(measDim, measDim);

@@ -249,7 +252,8 @@

     /**
      * Get dimension of measurement (0 = none). \return measurement dimension
      */

-    int hasMeasurement() {

+    int hasMeasurement()
+    {

         return measDim;
     }
 

@@ -259,18 +263,20 @@

      * 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;

+    public void getMeasurement(Matrix aProjection, Vector aResiduals,
+                               Vector aPrecision)
+    {
+        aProjection.placeAt(measProjection, 0, 0);
+        aResiduals.placeInCol(measResiduals, 0, 0);
+        aPrecision.placeInCol(measPrecision, 0, 0);

     }
 
 /// Get measurement transformation (from diagonalization).
     /**
      * \param [out] aTransformation Transformation matrix
      */

-    public void getMeasTransformation(Matrix aTransformation) {

+    public void getMeasTransformation(Matrix aTransformation)
+    {

         aTransformation.ResizeTo(measDim, measDim);
         if (transFlag) {
             aTransformation = measTransformation;

@@ -288,13 +294,14 @@

      * Scatterer precision (diagonal of inverse covariance matrix)
      */
     public void addScatterer(Vector aResiduals,

-            Vector aPrecision) {

+                             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();

+        scatTransformation.UnitMatrix();// setToIdentity();

     }
 
 /// Add a (thin) scatterer to a point.

@@ -313,7 +320,8 @@

      * Scatterer precision (matrix)
      */
     public void addScatterer(Vector aResiduals,

-            SymMatrix aPrecision) {

+                             SymMatrix aPrecision)
+    {

         scatFlag = true;
         EigenvalueDecomposition scatEigen = new EigenvalueDecomposition(aPrecision);
         Matrix aTransformation = scatEigen.getEigenVectors();

@@ -330,7 +338,8 @@

     }
 
 /// Check for scatterer at a point.

-    boolean hasScatterer() {

+    boolean hasScatterer()
+    {

         return scatFlag;
     }
 

@@ -340,18 +349,20 @@

      * 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;

+    public void getScatterer(Matrix aTransformation, Vector aResiduals,
+                             Vector aPrecision)
+    {
+        aTransformation.placeAt(scatTransformation, 0, 0);
+        aResiduals.placeAt(scatResiduals, 0, 0);
+        aPrecision.placeAt(scatPrecision, 0, 0);

     }
 
 /// Get scatterer transformation (from diagonalization).
     /**
      * \param [out] aTransformation Transformation matrix
      */

-    public void getScatTransformation(Matrix aTransformation) {

+    public void getScatTransformation(Matrix aTransformation)
+    {

         aTransformation.ResizeTo(2, 2);
         if (scatFlag) {
             for (int i = 0; i < 2; ++i) {

@@ -369,7 +380,8 @@

      * Point needs to have a measurement. \param [in] aDerivatives Local
      * derivatives (matrix)
      */

-    public void addLocals(Matrix aDerivatives) {

+    public void addLocals(Matrix aDerivatives)
+    {

         if (measDim != 0) {
             localDerivatives.ResizeTo(aDerivatives.getRowDimension(), aDerivatives.getColumnDimension());
             if (transFlag) {

@@ -381,12 +393,14 @@

     }
 
 /// Retrieve number of local derivatives from a point.

-    int getNumLocals() {

+    int getNumLocals()
+    {

         return localDerivatives.getColumnDimension();
     }
 
 /// Retrieve local derivatives from a point.

-    Matrix getLocalDerivatives() {

+    Matrix getLocalDerivatives()
+    {

         return localDerivatives;
     }
 

@@ -396,7 +410,8 @@

      * labels \param [in] aDerivatives Global derivatives (matrix)
      */
     public void addGlobals(List<Integer> aLabels,

-            Matrix aDerivatives) {

+                           Matrix aDerivatives)
+    {

         if (measDim != 0) {
             globalLabels = aLabels;
             globalDerivatives.ResizeTo(aDerivatives.getRowDimension(), aDerivatives.getColumnDimension());

@@ -410,17 +425,20 @@

     }
 
 /// Retrieve number of global derivatives from a point.

-    int getNumGlobals() {

+    int getNumGlobals()
+    {

         return globalDerivatives.getColumnDimension();
     }
 
 /// Retrieve global derivatives labels from a point.

-    List<Integer> getGlobalLabels() {

+    List<Integer> getGlobalLabels()
+    {

         return globalLabels;
     }
 
 /// Retrieve global derivatives from a point.

-    Matrix getGlobalDerivatives() {

+    Matrix getGlobalDerivatives()
+    {

         return globalDerivatives;
     }
 

@@ -428,12 +446,14 @@

     /**
      * \param [in] aLabel Label identifying point
      */

-    public void setLabel(int aLabel) {

+    public void setLabel(int aLabel)
+    {

         theLabel = aLabel;
     }
 
 /// Retrieve label of point

-    int getLabel() {

+    int getLabel()
+    {

         return theLabel;
     }
 

@@ -441,17 +461,20 @@

     /**
      * \param [in] anOffset Offset number
      */

-    public void setOffset(int anOffset) {

+    public void setOffset(int anOffset)
+    {

         theOffset = anOffset;
     }
 
 /// Retrieve offset for point

-    int getOffset() {

+    int getOffset()
+    {

         return theOffset;
     }
 
 /// Retrieve point-to-(previous)point jacobian

-    Matrix getP2pJacobian() {

+    Matrix getP2pJacobian()
+    {

         return p2pJacobian;
     }
 

@@ -459,7 +482,8 @@

     /**
      * \param [in] aJac Jacobian
      */

-    public void addPrevJacobian(Matrix aJac) {

+    public void addPrevJacobian(Matrix aJac)
+    {

         int ifail = 0;
 // to optimize: need only two last rows of inverse
 //	prevJacobian = aJac.InverseFast(ifail);

@@ -476,7 +500,8 @@

     /**
      * \param [in] aJac Jacobian
      */

-    public void addNextJacobian(Matrix aJac) {

+    public void addNextJacobian(Matrix aJac)
+    {

         nextJacobian = aJac;
     }
 

@@ -489,21 +514,23 @@

      * std::overflow_error : matrix S is singular.
      */
     public void getDerivatives(int aDirection, Matrix matW, Matrix matWJ,

-            Vector vecWd) {

+                               Vector vecWd)
+    {

 
         Matrix matJ;

+        Matrix matWt;

         Vector vecd;
         if (aDirection < 1) {
             matJ = prevJacobian.sub(2, 3, 3);

-            matW = prevJacobian.sub(2, 3, 1).uminus();

+            matWt = 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);

+            matWt = nextJacobian.sub(2, 3, 1);

             vecd = nextJacobian.subCol(2, 0, 3);
         }
 

-        matW = matW.inverse();

+        matW.placeAt(matWt.inverse(), 0, 0);

 //	if (!matW.InvertFast()) {
 //		std::cout << " getDerivatives failed to invert matrix: "
 //				<< matW << "\n";

@@ -512,8 +539,8 @@

 //				<< "\n";
 //		throw std::overflow_error("Singular matrix inversion exception");
 //	}

-        matWJ = matW.times(matJ);
-        vecWd = matW.times(vecd);

+        matWJ.placeAt(matW.times(matJ), 0, 0);
+        vecWd.placeAt(matW.times(vecd), 0, 0);

 
     }
 //

@@ -522,7 +549,8 @@

     /**
      * \param [in] level print level (0: minimum, >0: more)
      */

-    public void printPoint(int level) {

+    public void printPoint(int level)
+    {

         StringBuffer sb = new StringBuffer("GblPoint");
         if (theLabel != 0) {
             sb.append(", label " + theLabel);

@@ -586,5 +614,4 @@

         }
         System.out.println(sb.toString());
     }

-

 }

--- java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/HpsGblRefitter.java	                        (rev 0)
+++ java/trunk/tracking/src/main/java/org/hps/recon/tracking/gbl/HpsGblRefitter.java	2014-09-17 18:01:56 UTC (rev 1036)
@@ -0,0 +1,418 @@

+package org.hps.recon.tracking.gbl;
+
+import hep.physics.vec.BasicHep3Vector;
+import hep.physics.vec.Hep3Vector;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.Set;
+import org.lcsim.event.EventHeader;
+import org.lcsim.event.GenericObject;
+import org.lcsim.event.LCRelation;
+import org.lcsim.event.Track;
+import org.lcsim.geometry.Detector;
+import org.lcsim.util.Driver;
+
+import static java.lang.Math.sin;
+import static java.lang.Math.sqrt;
+import static java.lang.Math.abs;
+
+import org.hps.recon.tracking.gbl.matrix.Matrix;
+import org.hps.recon.tracking.gbl.matrix.SymMatrix;
+import org.hps.recon.tracking.gbl.matrix.Vector;
+
+/**
+ * A Driver which refits tracks using GBL 
+ * Modeled on the hps-dst code written by Per Hansson and Omar Moreno
+ * Requires the GBL Collections and Relations to be present in the event.
+ * 
+ * @author Norman A Graf
+ *
+ * @version $Id:
+ */
+public class HpsGblRefitter extends Driver
+{
+    private boolean _debug = false;
+    private final String trackCollectionName = "MatchedTracks";
+    private final String track2GblTrackRelationName = "TrackToGBLTrack";
+    private final String gblTrack2StripRelationName = "GBLTrackToStripData";
+
+    public void setDebug(boolean debug)
+    {
+        _debug = debug;
+    }
+
+    @Override
+    protected void process(EventHeader event)
+    {
+        Hep3Vector bfield = event.getDetector().getFieldMap().getField(new BasicHep3Vector(0., 0., 1.));
+        double By = bfield.y();
+        double bfac = 0.0002998 * By;
+        // get the tracks
+//        List<Track> tracks = null;
+        if (event.hasCollection(Track.class, trackCollectionName)) {
+//            tracks = event.get(Track.class, trackCollectionName);
+            if (_debug) {
+//                System.out.printf("%s: Event %d has %d tracks\n", this.getClass().getSimpleName(), event.getEventNumber(), tracks.size());
+            }
+        } else {
+            if (_debug) {
+                System.out.printf("%s: No tracks in Event %d \n", this.getClass().getSimpleName(), event.getEventNumber());
+            }
+            return;
+        }
+//        System.out.println("Tracks from event " + event.getRunNumber());
+//        for (Track t : tracks) {
+//            System.out.println(t);
+//            System.out.println(t.getTrackStates().get(0));
+//        }
+        //get the relations to the GBLtracks
+        if(!event.hasItem(track2GblTrackRelationName))
+        {
+            System.out.println("Need Relations "+track2GblTrackRelationName);
+            return;
+        }
+        // and strips
+        if(!event.hasItem(gblTrack2StripRelationName))
+        {
+            System.out.println("Need Relations "+gblTrack2StripRelationName);
+            return;
+        }
+        
+        
+        List<LCRelation> track2GblTrackRelations = event.get(LCRelation.class, track2GblTrackRelationName);
+        //need a map of GBLTrackData keyed on the Generic object from which it created
+        Map<GenericObject, GBLTrackData> gblObjMap = new HashMap<GenericObject, GBLTrackData>();
+
+        // loop over the relations
+        for (LCRelation relation : track2GblTrackRelations) {
+            Track t = (Track) relation.getFrom();
+            GenericObject gblTrackObject = (GenericObject) relation.getTo();
+            GBLTrackData gblT = new GBLTrackData(gblTrackObject);
+            gblObjMap.put(gblTrackObject, gblT);
+        } //end of loop over tracks
+
+        //get the strip hit relations
+        List<LCRelation> gblTrack2StripRelations = event.get(LCRelation.class, gblTrack2StripRelationName);
+
+        // need a map of lists of strip data keyed by the gblTrack to which they correspond
+        Map<GBLTrackData, List<GBLStripClusterData>> stripsGblMap = new HashMap<GBLTrackData, List<GBLStripClusterData>>();
+        for (LCRelation relation : gblTrack2StripRelations) {
+            //from GBLTrackData to GBLStripClusterData
+            GenericObject gblTrackObject = (GenericObject) relation.getFrom();
+            //Let's get the GBLTrackData that corresponds to this object...
+            GBLTrackData gblT = gblObjMap.get(gblTrackObject);
+            GBLStripClusterData sd = new GBLStripClusterData((GenericObject) relation.getTo());
+            if (stripsGblMap.containsKey(gblT)) {
+                stripsGblMap.get(gblT).add(sd);
+            } else {
+                stripsGblMap.put(gblT, new ArrayList<GBLStripClusterData>());
+                stripsGblMap.get(gblT).add(sd);
+            }
+        }
+
+        Set<GBLTrackData> keys = stripsGblMap.keySet();
+        int trackNum = 0;
+        for (GBLTrackData t : keys) {
+            int stat = fit(t, stripsGblMap.get(t), bfac);
+            ++trackNum;
+        }
+
+    }
+
+    private int fit(GBLTrackData track, List<GBLStripClusterData> hits, double bfac)
+    {
+        // path length along trajectory
+        double s = 0.;
+
+        // jacobian to transport errors between points along the path
+        Matrix jacPointToPoint = new Matrix(5, 5);
+        jacPointToPoint.UnitMatrix();
+        // 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;
+        Matrix msCov = new Matrix(5, 5);
+        Matrix measMsCov = new Matrix(2, 2);
+        // 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
+        Map< Integer, Matrix> proL2m_list = new HashMap<Integer, Matrix>();
+        // Save the association between strip cluster and label
+
+        //start trajectory at refence point (s=0) - this point has no measurement
+        GblPoint ref_point = new GblPoint(jacPointToPoint);
+        listOfPoints.add(ref_point);
+
+        // Loop over strips
+        int n_strips = hits.size();
+        for (int istrip = 0; istrip != n_strips; ++istrip) {
+            GBLStripClusterData strip = hits.get(istrip);
+            if (_debug) {
+                System.out.println("HpsGblFitter: Processing strip " + istrip + " with id/layer " + strip.getId());
+            }
+            // Path length step for this cluster
+            double step = strip.getPath3D() - s;
+            if (_debug) {
+                System.out.println("HpsGblFitter: " + "Path length step " + step + " from " + s + " to " + strip.getPath());
+            }
+            // Measurement direction (perpendicular and parallel to strip direction)
+            Matrix mDir = new Matrix(2, 3);
+            mDir.set(0, 0, strip.getUx());
+            mDir.set(0, 1, strip.getUy());
+            mDir.set(0, 2, strip.getUz());
+            mDir.set(1, 0, strip.getVx());
+            mDir.set(1, 1, strip.getVy());
+            mDir.set(1, 2, strip.getVz());
+            if (_debug) {
+                System.out.println("HpsGblFitter: " + "mDir");
+                mDir.print(4, 6);
+            }
+            Matrix mDirT = mDir.copy().transpose();
+
+            if (_debug) {
+                System.out.println("HpsGblFitter: " + "mDirT");
+                mDirT.print(4, 6);
+            }
+
+            // Track direction 
+            double sinLambda = sin(strip.getTrackLambda());//->GetLambda());
+            double cosLambda = sqrt(1.0 - sinLambda * sinLambda);
+            double sinPhi = sin(strip.getTrackPhi());//->GetPhi());
+            double cosPhi = sqrt(1.0 - sinPhi * sinPhi);
+
+            if (_debug) {
+                System.out.println("HpsGblFitter: " + "Track direction sinLambda=" + sinLambda + " sinPhi=" + sinPhi);
+            }
+
+            // Track direction in curvilinear frame (U,V,T)
+            // U = Z x T / |Z x T|, V = T x U
+            Matrix uvDir = new Matrix(2, 3);
+            uvDir.set(0, 0, -sinPhi);
+            uvDir.set(0, 1, cosPhi);
+            uvDir.set(0, 2, 0.);
+            uvDir.set(1, 0, -sinLambda * cosPhi);
+            uvDir.set(1, 1, -sinLambda * sinPhi);
+            uvDir.set(1, 2, cosLambda);
+
+            if (_debug) {
+                System.out.println("HpsGblFitter: " + "uvDir");
+                uvDir.print(6, 4);
+            }
+
+            // projection from  measurement to local (curvilinear uv) directions (duv/dm)
+            Matrix proM2l = uvDir.times(mDirT);
+
+            //projection from local (uv) to measurement directions (dm/duv)
+            Matrix proL2m = proM2l.copy();
+            proL2m = proL2m.inverse();
+            proL2m_list.put(strip.getId(), proL2m.copy()); // is a copy needed or is that just a C++/root thing?
+
+            if (_debug) {
+                System.out.println("HpsGblFitter: " + "proM2l:");
+                proM2l.print(4, 6);
+                System.out.println("HpsGblFitter: " + "proL2m:");
+                proL2m.print(4, 6);
+                System.out.println("HpsGblFitter: " + "proM2l*proL2m (should be unit matrix):");
+                (proM2l.times(proL2m)).print(4, 6);
+            }
+
+            // measurement/residual in the measurement system
+            // only 1D measurement in u-direction, set strip measurement direction to zero
+            Vector meas = new Vector(2);
+//		double uRes = strip->GetUmeas() - strip->GetTrackPos().x(); // how can this be correct?
+            double uRes = strip.getMeas() - strip.getTrackPos().x();
+            meas.set(0, uRes);
+            meas.set(1, 0.);
+//		//meas[0][0] += deltaU[iLayer] # misalignment
+            Vector measErr = new Vector(2);
+            measErr.set(0, strip.getMeasErr());
+            measErr.set(1, 0.);
+            Vector measPrec = new Vector(2);
+            measPrec.set(0, 1.0 / (measErr.get(0) * measErr.get(0)));
+            measPrec.set(1, 0.);
+
+            if (_debug) {
+                System.out.println("HpsGblFitter: " + "meas: ");
+                meas.print(4, 6);
+                System.out.println("HpsGblFitter: " + "measErr:");
+                measErr.print(4, 6);
+                System.out.println("HpsGblFitter: " + "measPrec:");
+                measPrec.print(4, 6);
+            }
+
+            //Find the Jacobian to be able to propagate the covariance matrix to this strip position
+            jacPointToPoint = gblSimpleJacobianLambdaPhi(step, cosLambda, abs(bfac));
+
+            if (_debug) {
+                System.out.println("HpsGblFitter: " + "jacPointToPoint to extrapolate to this point:");
+                jacPointToPoint.print(4, 6);
+            }
+
+            // Get the transpose of the Jacobian
+            Matrix jacPointToPointTransposed = jacPointToPoint.copy().transpose();
+
+            // Propagate the MS covariance matrix (in the curvilinear frame) to this strip position
+            msCov = msCov.times(jacPointToPointTransposed);
+            msCov = jacPointToPoint.times(msCov);
+
+            // Get the MS covariance for the measurements in the measurement frame
+            Matrix proL2mTransposed = proL2m.copy().transpose();
+
+            measMsCov = proL2m.times((msCov.getMatrix(3, 4, 3, 4)).times(proL2mTransposed));
+
+            if (_debug) {
+                System.out.println("HpsGblFitter: " + " msCov at this point:");
+                msCov.print(4, 6);
+                System.out.println("HpsGblFitter: " + "measMsCov at this point:");
+                measMsCov.print(4, 6);
+            }
+
+            GblPoint point = new GblPoint(jacPointToPoint);
+            //Add measurement to the point
+            point.addMeasurement(proL2m, meas, measPrec, 0.);
+            //Add scatterer in curvilinear frame to the point
+            // no direction in this frame
+            Vector scat = new Vector(2);
+
+            // Scattering angle in the curvilinear frame
+            //Note the cosLambda to correct for the projection in the phi direction
+            Vector scatErr = new Vector(2);
+            scatErr.set(0, strip.getScatterAngle());
+            scatErr.set(1, strip.getScatterAngle() / cosLambda);
+            Vector scatPrec = new Vector(2);
+            scatPrec.set(0, 1.0 / (scatErr.get(0) * scatErr.get(0)));
+            scatPrec.set(1, 1.0 / (scatErr.get(1) * scatErr.get(1)));
+
+            // add scatterer if not using the uncorrelated MS covariances for testing
+            if (!useUncorrMS) {
+                point.addScatterer(scat, scatPrec);
+                if (_debug) {
+                    System.out.println("HpsGblFitter: " + "adding scatError to this point:");
+                    scatErr.print(4, 6);
+                }
+            }
+
+            // Add this GBL point to list that will be used in fit
+            listOfPoints.add(point);
+            int iLabel = listOfPoints.size();
+
+            // Update MS covariance matrix 
+            msCov.set(1, 1, msCov.get(1, 1) + scatErr.get(0) * scatErr.get(0));
+            msCov.set(2, 2, msCov.get(2, 2) + scatErr.get(1) * scatErr.get(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)
+             ##### 
+
+             */
+            if (_debug) {
+                System.out.println("HpsGblFitter: " + "uRes " + strip.getId() + " uRes " + uRes + " pred (" + strip.getTrackPos().x() + "," + strip.getTrackPos().y() + "," + strip.getTrackPos().z() + ") s(3D) " + strip.getPath3D());
+            }
+
+            //go to next point
+            s += step;
+
+        } //strips
+
+        //create the trajectory
+        GblTrajectory traj = new GblTrajectory(listOfPoints); //,seedLabel, clSeed);
+
+        if (!traj.isValid()) {
+            System.out.println("HpsGblFitter: " + " Invalid GblTrajectory -> skip");
+            return 1;//INVALIDTRAJ;
+        }
+
+        // print the trajectory
+        if (_debug) {
+            System.out.println("%%%% Gbl Trajectory ");
+            traj.printTrajectory(1);
+            traj.printData();
+            traj.printPoints(4);
+        }
+        // fit trajectory
+        double[] dVals = new double[2];
+        int[] iVals = new int[1];
+        traj.fit(dVals, iVals, "");
+        if (_debug) {
+            System.out.println("HpsGblFitter: Chi2 " + " Fit: " + dVals[0] + ", " + iVals[0] + ", " + dVals[1]);
+        }
+
+        Vector aCorrection = new Vector(5);
+        SymMatrix aCovariance = new SymMatrix(5);
+        traj.getResults(1, aCorrection, aCovariance);
+        System.out.println(" cor ");
+        aCorrection.print(6, 4);
+        System.out.println(" cov ");
+        aCovariance.print(6, 4);
+
+//	// write to MP binary file
+//	traj.milleOut(mille);
+//
+        return 0;
+    }
+
+    @Override
+    protected void detectorChanged(Detector detector)
+    {
+    }
+
+    private Matrix gblSimpleJacobianLambdaPhi(double ds, double cosl, double bfac)
+    {
+        /**
+         * Simple jacobian: quadratic in arc length difference. using lambda phi
+         * as directions
+         *
+         * @param ds: arc length difference
+         * @type ds: float
+         * @param cosl: cos(lambda)
+         * @type cosl: float
+         * @param bfac: Bz*c
+         * @type bfac: float
+         * @return: jacobian to move by 'ds' on trajectory
+         * @rtype: matrix(float) ajac(1,1)= 1.0D0 ajac(2,2)= 1.0D0
+         * ajac(3,1)=-DBLE(bfac*ds) ajac(3,3)= 1.0D0
+         * ajac(4,1)=-DBLE(0.5*bfac*ds*ds*cosl) ajac(4,3)= DBLE(ds*cosl)
+         * ajac(4,4)= 1.0D0 ajac(5,2)= DBLE(ds) ajac(5,5)= 1.0D0 ''' jac =
+         * np.eye(5) jac[2, 0] = -bfac * ds jac[3, 0] = -0.5 * bfac * ds * ds *
+         * cosl jac[3, 2] = ds * cosl jac[4, 1] = ds return jac
+         */
+        Matrix jac = new Matrix(5, 5);
+        jac.UnitMatrix();
+        jac.set(2, 0, -bfac * ds);
+        jac.set(3, 0, -0.5 * bfac * ds * ds * cosl);
+        jac.set(3, 2, ds * cosl);
+        jac.set(4, 1, ds);
+        return jac;
+    }
+
+}