/home/bes3soft/bes3soft/Boss/7.0.2/dist/7.0.2/Analysis/VertexFit/VertexFit-00-02-78/src/VertexFit.cxx

Go to the documentation of this file.

#include <cassert>

#include "VertexFit/VertexFit.h"
#include "VertexFit/VertexConstraints.h"
#include "VertexFit/BField.h"
#include "VertexFit/HTrackParameter.h"
#include "TStopwatch.h"

const int VertexFit::NTRKPAR = 6;
const int VertexFit::NVTXPAR = 3;
const int VertexFit::NCONSTR = 2;

VertexFit* VertexFit::m_pointer = 0;

VertexFit* VertexFit::instance() 
{
        if (m_pointer) return m_pointer;
        m_pointer = new VertexFit();
        return m_pointer;
}

VertexFit::~VertexFit() 
{
        //if (m_pointer) delete m_pointer;
}

VertexFit::VertexFit() {;} 

void VertexFit::init() 
{
        //derived from TrackPool
        clearWTrackOrigin();
        clearWTrackInfit();
        clearWTrackList();
        clearGammaShape();
        clearGammaShapeList();
        clearMapkinematic();
        clearMappositionA();
        clearMappositionB();
        clearone();
        cleartwo();

        m_vpar_origin.clear();
        m_vpar_infit.clear();
        m_vc.clear();
        m_chisq.clear();
        m_chi = 9999.;
        m_virtual_wtrk.clear();
        m_niter = 10;
        m_chiter = 1.0e-3;
        m_chicut = 1000;

        m_TRA = HepMatrix(6, 7, 0);
        m_TRA[0][0] = 1.0;
        m_TRA[1][1] = 1.0;
        m_TRA[2][2] = 1.0;
        m_TRA[3][4] = 1.0;
        m_TRA[4][5] = 1.0;
        m_TRA[5][6] = 1.0;
        m_TRB = HepMatrix(7, 6, 0);
        m_TRB[0][0] = 1.0;
        m_TRB[1][1] = 1.0;
        m_TRB[2][2] = 1.0;
        m_TRB[4][3] = 1.0;
        m_TRB[5][4] = 1.0;
        m_TRB[6][5] = 1.0;

        m_factor = 1.000;
}

//
//  Add Beam Fit
//
void VertexFit::AddBeamFit(int number, VertexParameter vpar, int n) 
{
        std::vector<int> tlis = AddList(n);
        VertexConstraints vc;
        vc.FixedVertexConstraints(tlis);
        m_vc.push_back(vc);
        m_vpar_origin.push_back(vpar);
        m_vpar_infit.push_back(vpar);
        if ((unsigned int)number != m_vc.size() - 1)
                std::cout << "wrong kinematic constraints index" << std::endl;
}

//
//  Add Vertex
//
void VertexFit::AddVertex(int number, VertexParameter vpar, std::vector<int> tlis)
{
        VertexConstraints vc;
        vc.CommonVertexConstraints(tlis);
        m_vc.push_back(vc);
        HepVector vx(3, 0);
        for (unsigned int i = 0; i < tlis.size(); i++) 
                vx += wTrackOrigin(tlis[i]).X();
        vx = vx/tlis.size();
        VertexParameter n_vpar = vpar;
        n_vpar.setVx(vx);
        m_vpar_origin.push_back(n_vpar);
        m_vpar_infit.push_back(n_vpar);
        WTrackParameter vwtrk;
        m_virtual_wtrk.push_back(vwtrk);
        if ((unsigned int)number != m_vc.size() - 1)
                std::cout << "wrong kinematic constraints index" << std::endl;
}

void VertexFit::AddVertex(int number, VertexParameter vpar, int n1, int n2) 
{
        std::vector<int> tlis = AddList(n1, n2);
        AddVertex(number, vpar, tlis);
}


void VertexFit::AddVertex(int number, VertexParameter vpar, int n1, int n2, int n3) 
{
        std::vector<int> tlis = AddList(n1, n2, n3);
        AddVertex(number, vpar, tlis);
}

void VertexFit::AddVertex(int number, VertexParameter vpar, int n1, int n2, int n3, int n4) 
{
        std::vector<int> tlis = AddList(n1, n2, n3, n4);
        AddVertex(number, vpar, tlis);
}

void VertexFit::AddVertex(int number, VertexParameter vpar, int n1, int n2, int n3, int n4,     int n5) 
{
        std::vector<int> tlis = AddList(n1, n2, n3, n4, n5);
        AddVertex(number, vpar, tlis);
}

void VertexFit::AddVertex(int number, VertexParameter vpar, int n1, int n2, int n3, int n4,     int n5, int n6)
{
        std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6);
        AddVertex(number, vpar, tlis);
}

void VertexFit::AddVertex(int number, VertexParameter vpar, int n1, int n2, int n3, int n4, int n5, int n6, int n7) 
{
        std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7);
        AddVertex(number, vpar, tlis);
}

void VertexFit::AddVertex(int number, VertexParameter vpar, int n1, int n2, int n3, int n4, int n5, int n6, int n7, int n8) 
{
        std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8);
        AddVertex(number, vpar, tlis);
}

void VertexFit::AddVertex(int number, VertexParameter vpar, int n1, int n2, int n3, int n4, int n5, int n6, int n7, int n8, int n9) 
{
        std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9);
        AddVertex(number, vpar, tlis);
}

void VertexFit::AddVertex(int number, VertexParameter vpar, int n1, int n2, int n3, int n4, int n5, int n6, int n7, int n8, int n9, int n10) 
{
        std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
        AddVertex(number, vpar, tlis);
}

void VertexFit::AddVertex(int number, VertexParameter vpar, int n1, int n2, int n3, int n4, int n5, int n6, int n7, int n8, int n9, int n10, int n11) 
{
        std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11);
        AddVertex(number, vpar, tlis);
}

void VertexFit::AddVertex(int number, VertexParameter vpar, int n1, int n2, int n3, int n4, int n5, int n6, int n7, int n8, int n9, int n10, int n11, int n12) 
{
        std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11, n12);
        AddVertex(number, vpar, tlis);
}

bool VertexFit::Fit(int n) 
{
        bool okfit = false;
        TStopwatch timer;
        m_cpu = HepVector(10, 0);
        if (n < 0 || (unsigned int)n >= m_vc.size()) return okfit;

        timer.Start();
        int ifail;
        m_nvtrk = numberWTrack();

        m_pOrigin = HepVector(m_nvtrk * NTRKPAR, 0);
        m_pInfit  = HepVector(m_nvtrk * NTRKPAR, 0);
        m_pcovOrigin = HepSymMatrix(m_nvtrk * NTRKPAR, 0);
        m_pcovInfit  = HepSymMatrix(m_nvtrk * NTRKPAR, 0);
  
        int ntrk = numberWTrack();
        for(unsigned int itk = 0; itk < ntrk; itk++) 
        {
                setWTrackInfit(itk, wTrackOrigin(itk));
                setPOrigin(itk, Convert76(wTrackOrigin(itk).w()));
                setPCovOrigin(itk, wTrackOrigin(itk).Ew().similarity(m_TRA));
        }
        m_pInfit = m_pOrigin;

        m_xOrigin = HepVector(NVTXPAR, 0);
        m_xInfit  = HepVector(NVTXPAR, 0);
        m_xcovOrigin = HepSymMatrix(NVTXPAR, 0);
        m_xcovInfit  = HepSymMatrix(NVTXPAR, 0);
        m_xcovOriginInversed = HepSymMatrix(NVTXPAR, 0);
        m_xcovInfitInversed  = HepSymMatrix(NVTXPAR, 0);

        m_xOrigin = m_vpar_origin[n].Vx();
        m_xcovOrigin = m_vpar_origin[n].Evx();
        m_xcovOriginInversed = m_xcovOrigin.inverse(ifail);
        m_xInfit = m_xOrigin;

        m_vpar_infit[n] = m_vpar_origin[n];

        m_B = HepMatrix(NCONSTR*m_nvtrk, NVTXPAR, 0);
        m_A = HepMatrix(NCONSTR*m_nvtrk, NTRKPAR*m_nvtrk, 0);
        m_BT = HepMatrix(NVTXPAR,NCONSTR*m_nvtrk, 0);
        m_AT = HepMatrix(NTRKPAR*m_nvtrk, NCONSTR*m_nvtrk, 0);
        m_G = HepVector(NCONSTR*m_nvtrk, 0);
        m_W = HepSymMatrix(NCONSTR*m_nvtrk, 0);
        m_E = HepMatrix(NTRKPAR*m_nvtrk, NVTXPAR, 0);
 
        timer.Stop();
        m_cpu[0] += timer.CpuTime();

        // iteration loop
        std::vector<double> chisq;
        chisq.clear();
        for (int it = 0; it < m_niter; it++)
        {
                timer.Start();
                UpdateConstraints(m_vc[n]);
                timer.Stop();
                m_cpu[1] += timer.CpuTime();
                timer.Start();
                fitVertex(n);
                timer.Stop();
                m_cpu[2] += timer.CpuTime();
                chisq.push_back(m_chisq[n]);
                if (it > 0) 
                {
                        double delchi = chisq[it] - chisq[it-1];
                        if (fabs(delchi) < m_chiter) break;
                }
        }

        /*REVISED
        if(m_chisq[n] >= m_chicut || m_chisq[n] < 0) return okfit;
        REVISED*/
        if (m_chisq[n] >= m_chicut) return okfit;

        // update vertex and its covariance
        m_vpar_infit[n].setVx(m_xInfit);
        m_vpar_infit[n].setEvx(m_xcovInfit);

        okfit = true;
        return okfit;
}

bool VertexFit::BeamFit(int n)
{
        bool okfit = false;
        if (n < 0 || (unsigned int)n >= m_vc.size()) 
                return okfit;
        for (unsigned int i = 0; i < (m_vc[n].Ltrk()).size(); i++) 
        {
                int itk = (m_vc[n].Ltrk())[i];
                setWTrackInfit(itk, wTrackOrigin(itk));
        }
        m_vpar_infit[n] = m_vpar_origin[n];

        // iteration loop
        std::vector<double> chisq;
        chisq.clear();
        for (int it = 0; it < m_niter; it++) 
        {
                std::vector<WTrackParameter> wlis;
                wlis.clear();
                for (unsigned int i = 0; i < (m_vc[n].Ltrk()).size(); i++) 
                {
                        int itk = (m_vc[n].Ltrk())[i];
                        wlis.push_back(wTrackInfit(itk));
                }
                VertexParameter vpar = m_vpar_infit[n];
                m_vc[n].UpdateConstraints(vpar, wlis);

                fitBeam(n);
                chisq.push_back(m_chisq[n]);
                if (it > 0) 
                {
                        double delchi = chisq[it] - chisq[it-1];
                        if(fabs(delchi) < m_chiter)
                        break;
                }
        }
        if(m_chisq[n]>=m_chicut) return okfit;
        swimBeam(n);
        okfit = true;
        return okfit;
}

bool VertexFit::Fit() 
{
        bool okfit = false;
        double mychi = 0;
        for (unsigned int n = 0; n<(int)(m_vc.size()); n++) 
        {
                Fit(n);
                if (m_chisq[n] >= m_chicut) return okfit;
                swimVertex(n);
                mychi = mychi + m_chisq[n];
        }
        m_chi = mychi;
        okfit = true;
        return okfit;
}

void VertexFit::fitVertex(int n) 
{
        if (m_chisq.size() == 0) 
        {
                for (unsigned int i = 0; i < m_vc.size(); i++)
                m_chisq.push_back(9999.0);
        }
        TStopwatch timer;
        VertexConstraints vc = m_vc[n];

        int ifail;
        int NSIZE = (vc.Ltrk()).size();

        // get new Vx
        timer.Start();
        m_xcovInfitInversed = m_xcovOriginInversed;

        for (unsigned int i = 0; i < NSIZE; i++) 
        {
                int itk = (vc.Ltrk())[i];
                m_xcovInfitInversed += vfW(itk).similarity(vfBT(itk));
        }
        m_xcovInfit = m_xcovInfitInversed.inverse(ifail);
  
        // calculate Kq and E
        m_KQ = HepMatrix(NVTXPAR, m_nvtrk * NCONSTR, 0);
        m_E  = HepMatrix(m_nvtrk*NTRKPAR, NVTXPAR, 0);
        for (unsigned int i = 0; i < NSIZE; i++) 
        {
                int itk = (vc.Ltrk())[i];
                setKQ(itk, (m_xcovInfit * vfBT(itk) * vfW(itk)));
                setE(itk, (-pcovOrigin(itk) * vfAT(itk) * vfKQ(itk).T()));
        }
        // update vertex position
        m_xInfit = m_xOrigin;
        for (unsigned int i = 0; i < NSIZE; i++) 
        {
                int itk = (vc.Ltrk())[i];
                m_xInfit -= vfKQ(itk) * vfG(itk);
        }
        // update Track parameter
        HepVector dq0q(NVTXPAR, 0);
        dq0q = m_xcovInfitInversed * (m_xOrigin - m_xInfit);
        for (unsigned int i = 0; i < NSIZE; i++) 
        {
                int itk = (vc.Ltrk())[i];
                HepVector alpha(NTRKPAR, 0);
                alpha = pOrigin(itk) - pcovOrigin(itk) * vfAT(itk) * (vfW(itk)*vfG(itk) - vfKQ(itk).T()*dq0q);
                setPInfit(itk, alpha);
        }
        // get chisquare value
        m_chisq[n] = (m_W.similarity(m_G.T())- m_xcovInfitInversed.similarity((m_xInfit-m_xOrigin).T()))[0][0];
}

void VertexFit::vertexCovMatrix(int n)
{
        TStopwatch timer;
        timer.Start();

        VertexConstraints vc = m_vc[n];

        unsigned int NSIZE = vc.Ltrk().size();
        int ifail;
        m_pcovInfit = HepSymMatrix(NTRKPAR*m_nvtrk, 0);
        for (unsigned int i = 0; i < NSIZE; i++) 
        {
                int itk = vc.Ltrk()[i];
                setPCovInfit(itk, pcovOrigin(itk) - vfW(itk).similarity(pcovOrigin(itk) * vfAT(itk)));
        }
        m_pcovInfit = m_pcovInfit + m_xcovInfitInversed.similarity(m_E);

        timer.Stop();
        m_cpu[3] += timer.CpuTime();
}

void VertexFit::swimVertex(int n)
{
        TStopwatch timer;
        timer.Start();
        // Track parameter can be expressed as:
        //
        // px = px0 + a*(y0 - yv)
        // py = py0 - a*(x0 - xv)
        // pz = pz0
        // e  = e
        // x  = xv
        // y  = yv
        // z  = zv
        //
        // thus p = A * a + B * vx
        // x = vx
        VertexConstraints vc = m_vc[n];
        unsigned int NSIZE = vc.Ltrk().size();

        HepMatrix A(6, 6, 0);
        A[0][0] = 1.0;
        A[1][1] = 1.0;
        A[2][2] = 1.0;
        HepMatrix B(6, 3, 0);
        B[3][0] = 1.0;
        B[4][1] = 1.0;
        B[5][2] = 1.0;
        HepVector w1(6, 0);
        HepVector w2(7, 0);
        HepSymMatrix Ew(7, 0);
        HepMatrix ew1(6, 6, 0);
        HepMatrix ew2(7, 7, 0);

        for (unsigned int i = 0; i < NSIZE; i++) 
        {
                int itk = vc.Ltrk()[i];
//              double afield = VertexFitBField::instance()->getCBz(m_xInfit, pInfit(itk).sub(4, 6));
                double afield = m_factor * VertexFitBField::instance()->getCBz(m_xInfit, pInfit(itk).sub(4, 6));
                double a = afield * wTrackInfit(itk).charge();

                A[0][4] = a;
                A[1][3] = -a;
                B[0][1] = -a;
                B[1][0] = a;
                w1 = A * pInfit(itk) + B * m_xInfit;
                ew1 = pcovInfit(itk).similarity(A) + m_xcovInfit.similarity(B) + A*vfE(itk)*B.T() + B*(vfE(itk).T())*A.T();

                WTrackParameter wtrk = wTrackOrigin(itk);
                w2 = Convert67(wtrk.mass(), w1);
                wtrk.setW(w2);

                m_TRB[3][0] = w2[0] / w2[3];
                m_TRB[3][1] = w2[1] / w2[3];
                m_TRB[3][2] = w2[2] / w2[3];

                ew2 = m_TRB * ew1 * m_TRB.T();
                Ew.assign(ew2);
                wtrk.setEw(Ew);
                //renew parameters of input track 
                setWTrackInfit(itk, wtrk);
        }
        timer.Stop();
        m_cpu[4] += timer.CpuTime();
}

bool VertexFit::pull(int n, int itk, HepVector& p) 
{
        assert(p.num_row() == 5);
        vertexCovMatrix(n);
        
        WTrackParameter wtrk0, wtrk1;
        HepVector w1(6, 0);
        HepVector w2(7, 0);
        HepSymMatrix ew1(6, 0);
        HepSymMatrix ew2(7, 0);
        wtrk0 = wTrackOrigin(itk);
        w1  = pInfit(itk);
        ew1 = pcovInfit(itk);
        w2  = Convert67(wtrk0.mass(), w1);
        m_TRB[3][0] = w2[0] / w2[3];
        m_TRB[3][1] = w2[1] / w2[3];
        m_TRB[3][2] = w2[2] / w2[3];
        ew2 = ew1.similarity(m_TRB);
        wtrk1.setW(w2);
        wtrk1.setEw(ew2);
        wtrk1.setCharge(wtrk0.charge());

        HTrackParameter htrk0(wtrk0);
        HTrackParameter htrk1(wtrk1);
        for (int i = 0; i < 5; i++) 
        {
                double del = htrk0.eHel()[i][i] - htrk1.eHel()[i][i];
                if (del == 0.0) 
                {
                        return false;
                }
                p[i] = (htrk0.helix()[i] - htrk1.helix()[i]) / sqrt(abs(del));
        } 
        return true;
}

void VertexFit::fitBeam(int n) 
{
/*
        if(m_chisq.size() == 0) {
    for(unsigned int i = 0; i < m_vc.size(); i++)
      m_chisq.push_back(0.0);
  }

  VertexConstraints vc = m_vc[n];
  VertexParameter   vpar = m_vpar_origin[n];

  unsigned int NSIZE = vc.Ltrk().size();
  int ifail;

  // get VD, lambda
  for(unsigned int i = 0; i < NSIZE; i++) {
    int itk = vc.Ltrk()[i];
    HepVector dela0(7, 0);
    dela0 = wTrackOrigin(itk).w()-wTrackInfit(itk).w();
    HepSymMatrix vd(2, 0);
    vd = ((wTrackOrigin(itk).Ew()).similarity(vc.Dc()[i])).inverse(ifail);
    HepVector lambda(2, 0);
    lambda = vd*(vc.Dc()[i]*dela0+vc.dc()[i]);
    vc.setVD(i, vd);
    vc.setLambda(i, lambda);
  }
  // get new dela = dela0 - Va0 Dt lambda
  // get new Va
  // get chisq
  HepMatrix covax(7, 3, 0);
  m_chisq[n] = 0;
  for(unsigned int i = 0; i < NSIZE; i++) {
    HepVector DtL(7, 0);
    DtL = (vc.Dc()[i]).T() * vc.lambda()[i];
    int itk = vc.Ltrk()[i];
    HepVector dela(7, 0);
    HepVector dela0(7, 0);
    dela0 = wTrackOrigin(itk).w()-wTrackInfit(itk).w();
    WTrackParameter wtrk = wTrackOrigin(itk);
    dela = -wTrackOrigin(itk).Ew() * DtL; 
    wtrk.setW(wtrk.w()+dela);
    HepSymMatrix Va(7, 0);
    HepSymMatrix Va0(7, 0);
    Va0 = wTrackOrigin(itk).Ew();
    HepMatrix DcT(7, 2, 0);
    DcT = (vc.Dc()[i]).T();
    HepSymMatrix DVdD(7, 0);
    DVdD = (vc.VD()[i]).similarity(DcT);
    Va = Va0 - DVdD.similarity(Va0);
    wtrk.setEw(Va);
    setWTrackInfit(itk, wtrk);
    HepVector dc(2, 0);
    dc = vc.Dc()[i]*dela0 +vc.dc()[i];
    m_chisq[n] = m_chisq[n] + dot(vc.lambda()[i], dc);
  }
  m_vpar_infit[n] = vpar;
  m_vc[n] = vc;
*/
//No needed
}


void VertexFit::swimBeam(int n)
{
/*
  // const double alpha = -0.00299792458;
  //
  //  Track parameter can be expressed as:
  // 
  //  px = px0 + a*(y0 - yv)
  //  py = py0 - a*(x0 - xv)
  //  pz = pz0
  //  e  = e
  //  x  = xv
  //  y  = yv
  //  z  = zv
  //
  //  thus p = A * a + B * vx
  //      x = vx

  
  VertexConstraints vc = m_vc[n];
  VertexParameter  vpar = m_vpar_infit[n];
  unsigned int NSIZE = vc.Ltrk().size();

  for(unsigned int i = 0; i < NSIZE; i++) {
    int itk = vc.Ltrk()[i];
    HepMatrix A(4, 7, 0);
    HepMatrix B(4, 3, 0);
    
    double afield = VertexFitBField::instance()->getCBz(vpar.Vx(), pInfit(itk).sub(4, 6));
    double a = afield * wTrackInfit(itk).charge();
    A[0][0] = 1.0;
    A[0][5] = a;
    A[1][1] = 1.0;
    A[1][4] = -a;
    A[2][2] = 1.0;
    A[3][3] = 1.0;
    B[0][1] = -a;
    B[1][0] = a;
    HepVector p(4, 0);
    p = A * wTrackInfit(itk).w() + B * vpar.Vx();
    HepVector x(3, 0);
    x = vpar.Vx();
    HepVector w(7, 0);
    HepSymMatrix Ew(7, 0);
    for(int j = 0; j < 4; j++)
      w[j] = p[j];
    for(int j = 0; j < 3; j++)
      w[j+4] = x[j];

    WTrackParameter wtrk;
    wtrk.setCharge(wTrackInfit(itk).charge());
    wtrk.setW(w);
    HepSymMatrix Vpv(4, 0);
    Vpv = (wTrackInfit(itk).Ew()).similarity(A);
    for(int j = 0; j < 4; j++) 
      for(int k= 0; k < 4; k++) 
        Ew[j][k] = Vpv[j][k];
    wtrk.setEw(Ew);
    setWTrackInfit(itk, wtrk);
  }
*/
//No needed
}

void VertexFit::BuildVirtualParticle(int n) 
{

        vertexCovMatrix(n);
        TStopwatch timer;
        timer.Start();

        HepMatrix A(NTRKPAR, NTRKPAR * m_nvtrk, 0);
        HepMatrix B(NTRKPAR, NVTXPAR, 0);
        VertexConstraints vc = m_vc[n];
        unsigned int NSIZE = vc.Ltrk().size();
        int charge = 0;

        HepMatrix Ai(6, 6, 0);
        Ai[0][0] = 1.0;
        Ai[1][1] = 1.0;
        Ai[2][2] = 1.0;
        HepMatrix Bi(6, 3, 0);
        Bi[3][0] = 1.0;
        Bi[4][1] = 1.0;
        Bi[5][2] = 1.0;
        HepVector w1(6, 0);
        HepVector w2(7, 0);
        HepSymMatrix Ew(7, 0);
        HepMatrix ew1(6, 6, 0);
        HepMatrix ew2(7, 7, 0);
        double totalE = 0;

        for(unsigned int i = 0; i < NSIZE; i++) 
        {
                int itk = vc.Ltrk()[i];
                charge += wTrackInfit(itk).charge();
//              double afield = VertexFitBField::instance()->getCBz(m_xInfit, pInfit(itk).sub(4, 6));
                double afield = m_factor * VertexFitBField::instance()->getCBz(m_xInfit, pInfit(itk).sub(4, 6));
                double a = afield * wTrackInfit(itk).charge();
                
                totalE += wTrackOrigin(itk).w()[3];
                Ai[0][4] = a;
                Ai[1][3] = -a;
                Bi[0][1] = -a;
                Bi[1][0] = a;
                A.sub(1, NTRKPAR*itk + 1, Ai);
                B += Bi;
        }
        B[3][0] = 1.0;
        B[4][1] = 1.0;
        B[5][2] = 1.0;

        w1  = A * m_pInfit + B * m_xInfit;
        ew1 = m_pcovInfit.similarity(A) + m_xcovInfit.similarity(B) + A*m_E*B.T()+B*(m_E.T())*A.T();

        //convert w1(6x1) to w2(7x1)
        w2[0] = w1[0];
        w2[1] = w1[1];
        w2[2] = w1[2];
        w2[3] = totalE;
        w2[4] = w1[3];
        w2[5] = w1[4];
        w2[6] = w1[5];
        //convert ew1(6x6) to ew2(7x7)
        m_TRB[3][0] = w1[0] / totalE;
        m_TRB[3][1] = w1[1] / totalE;
        m_TRB[3][2] = w1[2] / totalE;
        ew2 = m_TRB * ew1 * m_TRB.T();
        Ew.assign(ew2);
        WTrackParameter vwtrk;
        vwtrk.setCharge(charge);
        vwtrk.setW(w2);
        vwtrk.setEw(Ew);

        m_virtual_wtrk[n] = vwtrk;
        timer.Stop();
        m_cpu[5] += timer.CpuTime();
}    

void VertexFit::UpdateConstraints(const VertexConstraints &vc) 
{
        int ntrk = (vc.Ltrk()).size();
        int type = vc.type();
        switch(type) 
        {
                case 1:
                {
                        for (unsigned int i = 0; i < ntrk; i++) 
                        {
                                int itk = (vc.Ltrk())[i];
                                HepVector alpha(6, 0);
                                double mass, e;
                                HepLorentzVector p;
                                HepPoint3D x, vx;
                                alpha = pInfit(itk);
                                
                                mass = wTrackOrigin(itk).mass();
                                e    = sqrt(mass*mass + alpha[0]*alpha[0] + alpha[1]*alpha[1] + alpha[2]*alpha[2]);
                                p    = HepLorentzVector(alpha[0], alpha[1], alpha[2], e);
                                x    = HepPoint3D(alpha[3], alpha[4], alpha[5]);

                                vx = HepPoint3D(m_xInfit[0], m_xInfit[1], m_xInfit[2]);
                                HepPoint3D delx = vx - x;

//                              double afield = VertexFitBField::instance()->getCBz(m_xInfit, wTrackOrigin(itk).X());
                                double afield = m_factor * VertexFitBField::instance()->getCBz(m_xInfit, wTrackOrigin(itk).X());
                                double a = afield * (0.0+wTrackOrigin(itk).charge());

                                double J = a * (delx.x()*p.px() + delx.y()*p.py()) / p.perp2();
                                J = std::min(J, 1-1e-4);
                                J = std::max(J, -1+1e-4);
                                double Rx = delx.x() - 2 * p.px() * (delx.x()*p.px() + delx.y()*p.py()) / p.perp2();
                                double Ry = delx.y() - 2 * p.py() *     (delx.x()*p.px() + delx.y()*p.py()) / p.perp2();
                                double S  = 1.0 / sqrt(1-J*J) / p.perp2();
                                // dc
                                HepVector dc(2, 0);
                                dc[0] = delx.y()*p.px() - delx.x()*p.py() -     0.5*a*(delx.x()*delx.x() + delx.y()*delx.y());
                                dc[1] = delx.z() - p.pz()/a*asin(J);
                                //setd(itk, dc);
                                // Ec
                                HepMatrix Ec(2, 3, 0);
                                // m_Ec.push_back(Ec);
                                // Dc
                                HepMatrix Dc(2, 6, 0);
                                Dc[0][0] = delx.y();
                                Dc[0][1] = -delx.x();
                                Dc[0][2] = 0;
                                Dc[0][3] = p.py() + a * delx.x();
                                Dc[0][4] = -p.px() + a * delx.y();
                                Dc[0][5] = 0;
                                Dc[1][0] = -p.pz() * S * Rx;
                                Dc[1][1] = -p.pz() * S * Ry;
                                Dc[1][2] = - asin(J) / a;
                                Dc[1][3] = p.px() * p.pz() * S;
                                Dc[1][4] = p.py() * p.pz() * S;
                                Dc[1][5] = -1.0;
                                // setD(itk, Dc);
                                // setDT(itk, Dc.T());
                                // VD
                                HepSymMatrix vd(2, 0);
                                int ifail;
                                vd = pcovOrigin(itk).similarity(Dc);
                                vd.invert(ifail);
                                // setVD(itk, vd);
                        }
                        break;
                }
                case 2:
                default: 
                {
                        for (unsigned int i = 0; i < ntrk; i++) 
                        {
                                int itk = (vc.Ltrk())[i];
                                HepVector alpha(6, 0);
                                double mass, e;
                                HepLorentzVector p;
                                HepPoint3D x, vx;
                                alpha = pInfit(itk);
                                //p  = HepLorentzVector(alpha[0], alpha[1], alpha[2], alpha[3]);
                                //x  = HepPoint3D(alpha[4], alpha[5], alpha[6]);

                                mass = wTrackOrigin(itk).mass();
                                e    = sqrt(mass*mass + alpha[0]*alpha[0] + alpha[1]*alpha[1] + alpha[2]*alpha[2]);
                                p    = HepLorentzVector(alpha[0], alpha[1], alpha[2], e);
                                x    = HepPoint3D(alpha[3], alpha[4], alpha[5]);
                                
                                vx = HepPoint3D(m_xInfit[0], m_xInfit[1], m_xInfit[2]);
                                HepPoint3D delx = vx - x;

                                if (wTrackOrigin(itk).charge() == 0) 
                                {
                                        // dc -->g
                                        HepVector dc(2, 0);
                                        dc[0] = p.pz()*delx.x() - p.px()*delx.z();
                                        dc[1] = p.pz()*delx.y() - p.py()*delx.z();
                                        // Ec --> B
                                        HepMatrix Ec(2, 3, 0);
                                        Ec[0][0] = p.pz();
                                        Ec[0][1] = 0;
                                        Ec[0][2] = -p.px();
                                        Ec[1][0] = 0;
                                        Ec[1][1] = p.pz();
                                        Ec[1][2] = -p.py();
                                        setB(itk, Ec);
                                        setBT(itk, Ec.T());
                                        // Dc
                                        HepMatrix Dc(2, 6, 0);
                                        Dc[0][0] = -delx.z();
                                        Dc[0][1] = 0;
                                        Dc[0][2] = delx.x();
                                        Dc[0][3] = -p.pz();
                                        Dc[0][4] = 0;
                                        Dc[0][5] = p.px();
                                        Dc[1][0] = 0;
                                        Dc[1][1] = -delx.z();
                                        Dc[1][2] = delx.y();
                                        Dc[1][3] = 0;
                                        Dc[1][4] = -p.pz();
                                        Dc[1][5] = p.py();
                                        setA(itk, Dc);
                                        setAT(itk, Dc.T());
                                        
                                        // VD --> W
                                        HepSymMatrix vd(2, 0);
                                        int ifail;
        
                                        vd = pcovOrigin(itk).similarity(Dc);
                                        vd.invert(ifail);
                                        setW(itk,vd);

                                        //G=A(p0-pe)+B(x0-xe)+d
                                        HepVector gc(2,0);
                                        gc = Dc*(pOrigin(itk)-pInfit(itk)) + Ec*(m_xOrigin-m_xInfit) + dc;
                                        setG(itk,gc);

                                } 
                                else 
                                {
        //                              double afield = VertexFitBField::instance()->getCBz(m_xInfit,wTrackOrigin(itk).X());
                                        double afield = m_factor * VertexFitBField::instance()->getCBz(m_xInfit,wTrackOrigin(itk).X());
                                        double a = afield * (0.0+wTrackOrigin(itk).charge());
                                        double J = a * (delx.x()*p.px() + delx.y()*p.py())/p.perp2();
                                        J = std::min(J, 1-1e-4);
                                        J = std::max(J, -1+1e-4);
                                        double Rx = delx.x() - 2*p.px()*(delx.x()*p.px() + delx.y()*p.py())/p.perp2();
                                        double Ry = delx.y() - 2*p.py()*(delx.x()*p.px() + delx.y()*p.py())/p.perp2();
                                        double S  = 1.0 / sqrt(1-J*J) / p.perp2();

                                        // dc
                                        HepVector dc(2, 0);
                                        dc[0] = delx.y() * p.px() - delx.x() * p.py() - 0.5 * a * (delx.x() * delx.x() + delx.y() * delx.y());
                                        dc[1] = delx.z() - p.pz() / a*asin(J);
                                        // Ec
                                        HepMatrix Ec(2, 3, 0);
                                        Ec[0][0] = -p.py()- a * delx.x();
                                        Ec[0][1] = p.px() - a * delx.y();
                                        Ec[0][2] = 0;
                                        Ec[1][0] = -p.px() * p.pz() * S ;
                                        Ec[1][1] = -p.py() * p.pz() * S ;
                                        Ec[1][2] = 1.0;
                                        setB(itk, Ec);
                                        setBT(itk, Ec.T());
 
                                        //Dc
                                        HepMatrix Dc(2,6,0);
                                        Dc[0][0] = delx.y();
                                        Dc[0][1] = -delx.x();
                                        Dc[0][2] = 0;
                                        Dc[0][3] = p.py() + a*delx.x();
                                        Dc[0][4] = -p.px() + a*delx.y();
                                        Dc[0][5] = 0;

                                        Dc[1][0] = -p.pz()*S*Rx;
                                        Dc[1][1] = -p.pz()*S*Ry;
                                        Dc[1][2] = -asin(J)/a;
                                        Dc[1][3] = p.px()*p.pz()*S;
                                        Dc[1][4] = p.py()*p.pz()*S;
                                        Dc[1][5] = -1.0;
                                        setA(itk,Dc);
                                        setAT(itk,Dc.T());
                                        // VD
                                        HepSymMatrix vd(2, 0);
                                        int ifail;
                                        vd = pcovOrigin(itk).similarity(Dc);
                                        vd.invert(ifail);
                                        setW(itk, vd);
                                        // G = A(p0-pe)+B(x0-xe)+d
                                        HepVector gc(2, 0);
                                        gc = Dc * (pOrigin(itk) - pInfit(itk)) + Ec *(m_xOrigin - m_xInfit) + dc;
                                        setG(itk, gc);
                                }
                        }
                        break;
                }
        }
}

HepVector VertexFit::Convert67(const double &mass, const HepVector &p)
{
//      assert(p.num_row() == 6);
        HepVector m(7, 0);
        m.sub(1, p.sub(1, 3));
        m.sub(5, p.sub(4, 6));
        m[3] = sqrt(mass*mass + p[0]*p[0] + p[1]*p[1] + p[2]*p[2]);
        return m;
}

HepVector VertexFit::Convert76(const HepVector &p)
{
//      assert(p.num_row() == 7);
        HepVector m(6, 0);
        m.sub(1, p.sub(1, 3));
        m.sub(4, p.sub(5, 7));
        return m;
}

---