KinematicFit.cxx

Go to the documentation of this file.

#include "VertexFit/KinematicFit.h"
#include "VertexFit/KinematicConstraints.h"
#include "VertexFit/HTrackParameter.h"
#include "TStopwatch.h"
 
 
const int KinematicFit::NTRKPAR = 3;        //modify by yanl 2010.7.26  4---->3
 
const int KinematicFit::Resonance = 1;
const int KinematicFit::TotalEnergy = 2;
const int KinematicFit::TotalMomentum = 4;
const int KinematicFit::Position = 8;
const int KinematicFit::ThreeMomentum = 16;
const int KinematicFit::FourMomentum = 32;
const int KinematicFit::EqualMass = 64;
 
 
KinematicFit * KinematicFit::m_pointer = 0;
 
KinematicFit * KinematicFit::instance() {
    if(m_pointer) return m_pointer;
    m_pointer = new KinematicFit();
    return m_pointer;
}
 
KinematicFit::KinematicFit(){;}
 
KinematicFit::~KinematicFit() {
    //  if(m_pointer) delete m_pointer;
}
 
 
void KinematicFit::init() {
    clearWTrackOrigin();
    clearWTrackInfit();
    clearWTrackList();
    //For Virtual Particles
    //gamma shape
    clearGammaShape();
    clearGammaShapeList();  
    clearMapkinematic();
    clearMappositionA();
    clearMappositionB();
    clearone();
    cleartwo();
    setBeamPosition(HepPoint3D(0.0,0.0,0.0));
    setVBeamPosition(HepSymMatrix(3,0));
 
    //=============  
    m_kc.clear();
    m_chisq.clear();
    m_chi = 9999.;
    m_niter = 10;
    m_chicut = 200.;
    m_chiter = 0.005;
    m_espread = 0.0;
    m_kalman = 0;
    m_collideangle = 11e-3;
    m_flag = 0;
    m_dynamicerror = 0; 
    m_nc = 0;
    m_cpu = HepVector(10, 0);
    m_massvector = HepVector(12,0);
    m_virtual_wtrk.clear(); 
}
 
 
//
//  Add Resonance
//
 
void KinematicFit::AddResonance(int number, double mres, int n1) {
    std::vector<int> tlis = AddList(n1);
    AddResonance(number, mres, tlis);
}
 
void KinematicFit::AddResonance(int number, double mres, int n1, int n2) {
    std::vector<int> tlis = AddList(n1, n2);
    AddResonance(number, mres, tlis);
}
 
void KinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3) {
    std::vector<int> tlis = AddList(n1, n2, n3);
    AddResonance(number, mres, tlis);
}
 
void KinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4) {
    std::vector<int> tlis = AddList(n1, n2, n3, n4);
    AddResonance(number, mres, tlis);
}
 
void KinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4,
        int n5) {
    std::vector<int> tlis = AddList(n1, n2, n3, n4, n5);
    AddResonance(number, mres, tlis);
}
 
void KinematicFit::AddResonance(int number, double mres, int n1, int n2, int n3, int n4,
        int n5, int n6) {
    std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6);
    AddResonance(number, mres, tlis);
}
 
void KinematicFit::AddResonance(int number, double mres, 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);
    AddResonance(number, mres, tlis);
}
 
void KinematicFit::AddResonance(int number, double mres, 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);
    AddResonance(number, mres, tlis);
}
 
void KinematicFit::AddResonance(int number, double mres, 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);
    AddResonance(number, mres, tlis);
}
 
void KinematicFit::AddResonance(int number, double mres, 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);
    AddResonance(number, mres, tlis);
}
 
 
void KinematicFit::AddResonance(int number, double mres, 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);
    AddResonance(number, mres, tlis);
}
 
void KinematicFit::AddResonance(int number, double mres, 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);
    AddResonance(number, mres, tlis);
}
 
void KinematicFit::AddResonance(int number, double mres, std::vector<int> tlis) {
    KinematicConstraints kc;
    HepSymMatrix Vre = HepSymMatrix(1,0);
    kc.ResonanceConstraints(mres, tlis, Vre);
    m_kc.push_back(kc);
    if((unsigned int) number != m_kc.size()-1) 
        std::cout << "wrong kinematic constraints index" << std::endl;
}
 
 
//
//  Add TotalMomentum
//
 
void KinematicFit::AddTotalMomentum(int number, double ptot, int n1) {
    std::vector<int> tlis = AddList(n1);
    AddTotalMomentum(number, ptot, tlis);
}
void KinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2) {
    std::vector<int> tlis = AddList(n1, n2);
    AddTotalMomentum(number, ptot, tlis);
}
 
void KinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3) {
    std::vector<int> tlis = AddList(n1, n2, n3);
    AddTotalMomentum(number, ptot, tlis);
}
 
void KinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4) {
    std::vector<int> tlis = AddList(n1, n2, n3, n4);
    AddTotalMomentum(number, ptot, tlis);
}
 
void KinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4,
        int n5) {
    std::vector<int> tlis = AddList(n1, n2, n3, n4, n5);
    AddTotalMomentum(number, ptot, tlis);
}
 
void KinematicFit::AddTotalMomentum(int number, double ptot, int n1, int n2, int n3, int n4,
        int n5, int n6) {
    std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6);
    AddTotalMomentum(number, ptot, tlis);
}
 
void KinematicFit::AddTotalMomentum(int number, double ptot, 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);
    AddTotalMomentum(number, ptot, tlis);
}
 
void KinematicFit::AddTotalMomentum(int number, double ptot, 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);
    AddTotalMomentum(number, ptot, tlis);
}
 
void KinematicFit::AddTotalMomentum(int number, double ptot, 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);
    AddTotalMomentum(number, ptot, tlis);
}
 
void KinematicFit::AddTotalMomentum(int number, double ptot, 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);
    AddTotalMomentum(number, ptot, tlis);
}
 
 
void KinematicFit::AddTotalMomentum(int number, double ptot, 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);
    AddTotalMomentum(number, ptot, tlis);
}
 
void KinematicFit::AddTotalMomentum(int number, double ptot, 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);
    AddTotalMomentum(number, ptot, tlis);
}
 
void KinematicFit::AddTotalMomentum(int number, double ptot, std::vector<int> tlis) {
    KinematicConstraints kc;
    kc.TotalMomentumConstraints(ptot, tlis);
    m_kc.push_back(kc);
    if((unsigned int) number != m_kc.size()-1) 
        std::cout << "wrong kinematic constraints index" << std::endl;
}
 
//
//  Add TotalEnergy
//
 
void KinematicFit::AddTotalEnergy(int number, double etot, int n1) {
    std::vector<int> tlis = AddList(n1);
    AddTotalEnergy(number, etot, tlis);
}
void KinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2) {
    std::vector<int> tlis = AddList(n1, n2);
    AddTotalEnergy(number, etot, tlis);
}
 
void KinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3) {
    std::vector<int> tlis = AddList(n1, n2, n3);
    AddTotalEnergy(number, etot, tlis);
}
 
void KinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4) {
    std::vector<int> tlis = AddList(n1, n2, n3, n4);
    AddTotalEnergy(number, etot, tlis);
}
 
void KinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4,
        int n5) {
    std::vector<int> tlis = AddList(n1, n2, n3, n4, n5);
    AddTotalEnergy(number, etot, tlis);
}
 
void KinematicFit::AddTotalEnergy(int number, double etot, int n1, int n2, int n3, int n4,
        int n5, int n6) {
    std::vector<int> tlis = AddList(n1, n2, n3, n4, n5, n6);
    AddTotalEnergy(number, etot, tlis);
}
 
void KinematicFit::AddTotalEnergy(int number, double etot, 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);
    AddTotalEnergy(number, etot, tlis);
}
 
void KinematicFit::AddTotalEnergy(int number, double etot, 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);
    AddTotalEnergy(number, etot, tlis);
}
 
void KinematicFit::AddTotalEnergy(int number, double etot, 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);
    AddTotalEnergy(number, etot, tlis);
}
 
void KinematicFit::AddTotalEnergy(int number, double etot, 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);
    AddTotalEnergy(number, etot, tlis);
}
 
 
void KinematicFit::AddTotalEnergy(int number, double etot, 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);
    AddTotalEnergy(number, etot, tlis);
}
 
void KinematicFit::AddTotalEnergy(int number, double etot, 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);
    AddTotalEnergy(number, etot, tlis);
}
 
void KinematicFit::AddTotalEnergy(int number, double etot, std::vector<int> tlis) {
    KinematicConstraints kc;
    kc.TotalEnergyConstraints(etot, tlis);
    m_kc.push_back(kc);
    if((unsigned int) number != m_kc.size()-1) 
        std::cout << "wrong kinematic constraints index" << std::endl;
}
//
//  Equal Mass constraints
//
 
void KinematicFit::AddEqualMass(int number, std::vector<int> tlis1, std::vector<int> tlis2)  {
    KinematicConstraints kc;
    HepSymMatrix Vne = HepSymMatrix(1,0);
    kc.EqualMassConstraints(tlis1, tlis2, Vne);
    m_kc.push_back(kc);
    if((unsigned int) number != m_kc.size()-1) 
        std::cout << "wrong kinematic constraints index" << std::endl;
}
 
//
//  Total 3-momentum constraints
//
 
void KinematicFit::AddThreeMomentum(int number, Hep3Vector p3) {
    std::vector<int>  tlis;
    tlis.clear();
    WTrackParameter wtrk;
    KinematicConstraints kc;
 
    for(int i = 0; i < numberWTrack(); i++) {
        tlis.push_back(i);
    }
    kc.ThreeMomentumConstraints(p3, tlis);
    m_kc.push_back(kc);
    if((unsigned int) number != m_kc.size()-1) 
        std::cout << "wrong kinematic constraints index" << std::endl;
}
 
//
//  Total 4-momentum constraints
//
 
void KinematicFit::AddFourMomentum(int number, HepLorentzVector p4) {
 
    std::vector<int> tlis;
    tlis.clear();
    KinematicConstraints kc;
 
    for(int i = 0; i < numberWTrack(); i++) {
        tlis.push_back(i);
    }
    //  for(int i = 0; i < numberWTrack_V(); i++) {
    //    tlis_V.push_back(i);
    //  }
 
    HepSymMatrix Vme = HepSymMatrix(4,0);
    Vme[0][0] = 2*m_espread*m_espread*sin(m_collideangle)*sin(m_collideangle);
    Vme[0][3] = 2*m_espread*m_espread*sin(m_collideangle);
    Vme[3][3] = 2*m_espread*m_espread;
 
    //  kc.FourMomentumConstraints(p4, tlis, Vme);
    kc.FourMomentumConstraints(p4, tlis, Vme);
    m_kc.push_back(kc);
    if((unsigned int) number != m_kc.size()-1) 
        std::cout << "wrong kinematic constraints index" << std::endl;
}
 
void KinematicFit::AddFourMomentum(int number, double etot ){
 
    HepLorentzVector p4(0.0, 0.0, 0.0, etot);
    std::vector<int> tlis;
    tlis.clear();
    KinematicConstraints kc;
 
    for(int i = 0; i < numberWTrack(); i++) {
        tlis.push_back(i);
    }
    HepSymMatrix Vme = HepSymMatrix (4,0);
    Vme[3][3] = 2*m_espread*m_espread;
    // kc.FourMomentumConstraints(p4, tlis, Vme);
    kc.FourMomentumConstraints(p4, tlis, Vme);
    m_kc.push_back(kc);
    if((unsigned int) number != m_kc.size()-1) 
        std::cout << "wrong kinematic constraints index" << std::endl;
}
/*
   void KinematicFit::AddPosition(int number, HepPoint3D xorigin, std::vector<int> tlis_V){
   KinematicConstraints kc;
   HepSymMatrix Vpe = HepSymMatrix(2,0);
   HepSymMatrix Vclus = HepSymMatrix (3,0);
   Vclus[0][0] = wTrackOrigin_V(tlis_V[0]).Ew()[4][4];
   Vclus[0][1] = wTrackOrigin_V(tlis_V[0]).Ew()[4][5];
   Vclus[1][1] = wTrackOrigin_V(tlis_V[0]).Ew()[5][5];
   Vclus[2][2] = wTrackOrigin_V(tlis_V[0]).Ew()[6][6];
   HepMatrix p(2,3,0);
   p[0][0] = wTrackOrigin_V(tlis_V[0]).w()[1];
   p[0][1] = -wTrackOrigin_V(tlis_V[0]).w()[0];
   p[1][0] = wTrackOrigin_V(tlis_V[0]).w()[2];
   p[1][2] = -wTrackOrigin_V(tlis_V[0]).w()[0];
   Vpe = Vclus.similarity(p);   
 
   kc.PositionConstraints(xorigin, tlis_V, Vpe);
   m_kc.push_back(kc);
   if((unsigned int) number != m_kc.size()-1) 
   std::cout << "wrong kinematic constraints index" << std::endl;
   }     
   */
 
 
 
void KinematicFit::fit() {
 
    KinematicConstraints kc;
    int nc = m_nc;
    int ntrk = numberWTrack();
 
    TStopwatch timer;
    timer.Start();
 
    m_VD = HepSymMatrix(m_nc, 0);
    m_VD = m_covOrigin.similarity(m_D);
    timer.Stop();
    m_cpu[1] += timer.CpuTime();
    timer.Start();
    
    int ifail;
    m_VD.invert(ifail);
    
    timer.Stop();
    m_cpu[2] += timer.CpuTime();
    timer.Start();
 
    HepVector G(m_nc, 0);
    G = m_D * (m_pOrigin - m_pInfit) + m_d; 
 
    m_KP = HepMatrix(NTRKPAR*m_nktrk, m_nc, 0);
    m_KP = m_covOrigin * m_DT * m_VD;
    m_chi = (m_VD.similarity(G.T()))[0][0];
 
 
    timer.Stop();
    m_cpu[3] += timer.CpuTime();
    timer.Start();
 
    m_pInfit = m_pOrigin - m_KP * G;
 
    //  ===== gamma dynamic adjustment====
 
    timer.Stop();
    m_cpu[4] += timer.CpuTime();
 
    timer.Start();
    if(m_dynamicerror ==1) 
        gda(); 
 
    timer.Stop();
    m_cpu[6] += timer.CpuTime();
}
 
 
 
 
 
void KinematicFit::upCovmtx() {
 
    int ntrk = numberWTrack();
    HepSymMatrix I(NTRKPAR*m_nktrk, 1);
    m_covInfit = m_covOrigin.similarity(I - m_KP*m_D);
    for(int i = 0; i < m_nktrk; i++) {
        HepSymMatrix ew3 (3, 0);          
        HepSymMatrix ew6 (6, 0);
        HepSymMatrix Ew1 (7, 0);
        ew3 = m_covInfit.sub(i*NTRKPAR + 1, (i+1)*NTRKPAR);
        for(int j = 0; j < NTRKPAR; j++) {
            for(int k = 0; k < NTRKPAR; k++) {
                ew6[j][k] = ew3[j][k];
            }
        }
        for(int m = NTRKPAR; m < 6; m++) {
            for(int n = NTRKPAR; n < 6; n++) {
                ew6[m][n] = 0;//(wTrackOrigin(i).Ew())[m+1][n+1];
            }
        }
        double px = p4Infit(i).px();
        double py = p4Infit(i).py();
        double pz = p4Infit(i).pz();
        double m =  p4Infit(i).m();
        double e =  p4Infit(i).e();
 
        HepMatrix J(7, 6, 0);
        J[0][0] = 1;
        J[1][1] = 1;
        J[2][2] = 1;
        J[3][0] = px/e;
        J[3][1] = py/e;
        J[3][2] = pz/e;
        J[4][3] = 1;
        J[5][4] = 1;
        J[6][5] = 1;
        Ew1 = ew6.similarity(J);
 
        HepVector W(7, 0);
        for(int j=0; j<4; j++) {
            W[j] = p4Infit(i)[j];
        }
        W[4] = wTrackOrigin(i).w()[4];
        W[5] = wTrackOrigin(i).w()[5];
        W[6] = wTrackOrigin(i).w()[6];
        
 
        WTrackParameter wtrk = wTrackInfit(i);
        wtrk.setEw(Ew1);
        wtrk.setW(W);
        setWTrackInfit(i, wtrk);
    }
 
 
}    
 
   
 
void KinematicFit::fit(int n) {
 
    if(m_chisq.size() == 0) {
        for(unsigned int i = 0; i < m_kc.size(); i++)
            m_chisq.push_back(9999.0);
    }
    KinematicConstraints kc;
    int nc = m_nc;
    int ntrk = numberWTrack();
 
    m_VD = HepSymMatrix(m_nc, 0);
    m_VD = m_covOrigin.similarity(m_D);
    int ifail;
    m_VD.invert(ifail);
    HepVector G(m_nc, 0);
    G = m_D * (m_pOrigin - m_pInfit) + m_d; 
    m_KP = HepMatrix(NTRKPAR*m_nktrk, m_nc, 0);
    m_KP = m_covOrigin * m_DT * m_VD;
    m_chisq[n]  = (m_VD.similarity(G.T()))[0][0];
    m_pInfit = m_pOrigin - m_KP * G;
 
}
 
 
 
void KinematicFit::covMatrix(int n) {
    KinematicConstraints kc = m_kc[n];
    int nc = kc.nc();
    int ntrk = (kc.Ltrk()).size();
    HepSymMatrix Va0(7*ntrk, 0);
    for (int i = 0; i < ntrk; i++) {
        int itk = (kc.Ltrk())[i];
        for(int j = 0; j < 7; j++) 
            for (int k = 0; k < 7; k++)
                Va0[7*i+j][7*i+k] = (wTrackOrigin(itk).Ew())[j][k];
 
    }
    HepMatrix D(nc, 7*ntrk, 0);
    int ncc = 0;
    for(int j = 0; j < kc.nc(); j++) {
        for(int l = 0; l < ntrk; l++) { 
            for(int k = 0; k < 7; k++)
                D[ncc][7*l+k] = (kc.Dc()[l])[j][k];
        }
        ncc++;
    }
 
 
    HepSymMatrix VD(nc, 0);
    VD = kc.VD()[0];
    HepSymMatrix Va(7*ntrk, 0);
    Va = Va0 - (VD.similarity(D.T())).similarity(Va0);
    for(int i = 0; i < ntrk; i++) {
        int itk = (kc.Ltrk())[i];
        HepVector w(7, 0);
        HepSymMatrix Ew(7, 0);
        for(int j = 0; j < 7; j++) {
            for(int k = 0; k < 7; k++) 
                Ew[j][k] = Va[7*i + j] [7*i + k];
        }
        WTrackParameter wtrk = wTrackInfit(itk);
        wtrk.setEw(Ew);
        setWTrackInfit(itk, wtrk);
    }
    m_kc[n] = kc;
 
}
 
 
 
 
bool KinematicFit::Fit() {
    bool okfit = false;
    TStopwatch timer;
    m_nktrk = numberWTrack();
    m_pOrigin = HepVector(m_nktrk * NTRKPAR, 0);
    m_pInfit = HepVector(m_nktrk * NTRKPAR, 0);
    m_covOrigin = HepSymMatrix(m_nktrk * NTRKPAR, 0);
    m_covInfit = HepSymMatrix(m_nktrk * NTRKPAR, 0);
    m_massvector = HepVector(m_nktrk, 0);
    for(int i = 0; i < numberWTrack(); i++) {
        setWTrackInfit(i, wTrackOrigin(i));
        setPOrigin(i, (wTrackOrigin(i).w()).sub(1, NTRKPAR));
        setPInfit(i, (wTrackOrigin(i).w()).sub(1, NTRKPAR));
        setCovOrigin(i, (wTrackOrigin(i).Ew()).sub(1, NTRKPAR));
        setMassvector(i, wTrackOrigin(i).mass());
    }
    
    //
    // iteration
    //
    //    cout<<"m_pInfit ="<<m_pInfit<<endl;
    //    cout<<"m_covOrigin="<<m_covOrigin<<endl;
    //    cout<<"m_massvector ="<<m_massvector<<endl;
 
    std::vector<double> chisq;
    chisq.clear();
    int nc = 0;
    for(int i = 0; i < m_kc.size(); i++)
        nc += m_kc[i].nc();
 
    m_D = HepMatrix(nc, m_nktrk * NTRKPAR, 0);
    m_DT = HepMatrix(m_nktrk * NTRKPAR, nc, 0);
    m_d = HepVector(nc, 0);
 
    for(int it = 0; it < m_niter; it++) {
 
        timer.Start();
        m_nc = 0;
        for(unsigned int i = 0; i < m_kc.size(); i++) {
            KinematicConstraints kc = m_kc[i];
            //      std::vector<WTrackParameter> wlis;
            //      std::vector<WTrackParameter> wlis_V;
            //      wlis.clear();
            //      wlis_V.clear();
            //      for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++) {
            //  int n = (kc.Ltrk())[j];
            //  WTrackParameter wtrk = wTrackInfit(n);
            //       if(m_espread!=0)    wtrk = wTrackOrigin(n);
            //  wlis.push_back(wtrk);
            //      } 
            //      for(unsigned int j = 0; j < (kc.Ltrk_V()).size(); j++) {
            //  int n = (kc.Ltrk_V())[j];
            //  WTrackParameter wtrk = wTrackInfit_V(n);
            //  wlis_V.push_back(wtrk);
            //      } 
            //      kc.UpdateConstraints(wlis, wlis_V);
            //      m_kc[i] = kc;
            //cout<<"wlis_V ="<<(wlis_V[0].w())[0]<<endl;     
            updateConstraints(kc);
            //      std::cout << "updata OK " << m_d << std::endl;
        }
        timer.Stop();
        m_cpu[0] += timer.CpuTime();
 
        fit();  
        chisq.push_back(m_chi);
        if(it > 0) {
            double delchi = chisq[it]- chisq[it-1];
            if(fabs(delchi) < m_chiter) 
                break;
        }
    }
    if(m_chi >= m_chicut) {
 
        return okfit;
    }
    // update track parameter and its covariance matrix
    //    upTrkpar();
    //    covMatrix();
    timer.Start();
    //upCovmtx();
    timer.Stop();
    m_cpu[5] += timer.CpuTime();
 
    okfit = true;
 
    /*
       for (int i = 0; i<numberWTrack(); i++){
       if (wTrackOrigin(i).charge()==0) continue ;
       HTrackParameter horigin = HTrackParameter(wTrackOrigin(i));
       HTrackParameter hinfit = HTrackParameter(wTrackInfit(i));
 
       HepVector a0 = horigin.hel();
       HepVector a1 = hinfit.hel();
       HepSymMatrix v0 = horigin.eHel();
       HepSymMatrix v1 = hinfit.eHel(); 
       HepVector pull(5,0);
       for (int k=0; k<5; k++) {
       pull[k] = (a0[k]-a1[k])/sqrt(abs(v0[k][k]-v1[k][k]));
       }
 
       WTrackParameter wtrk2 = wTrackInfit(i);
       wtrk2.setPull(pull);
    //    for (int l=0;l<5; l++) {
    //(wTrackInfit(i).pull())[l]=(wtrk2.pull())[l];
    //   }
    setWTrackInfit(i, wtrk2);
    }
    */
    /*/
 
      for (int i = 0; i<numberWTrack_V(); i++){ 
    //if (wTrackOrigin(i).charge()==0) continue ;
    HTrackParameter horigin_V = HTrackParameter(wTrackOrigin_V(i));
    HTrackParameter hinfit_V = HTrackParameter(wTrackInfit_V(i));
 
    HepVector a0 = horigin.hel();
    HepVector a1 = hinfit.hel();
    HepSymMatrix v0 = horigin.eHel();
    HepSymMatrix v1 = hinfit.eHel();
    HepVector pull(5,0);
    for (int k=0; k<5; k++) {
    pull[k] = (a0[k]-a1[k])/sqrt(abs(v0[k][k]-v1[k][k]));
    }
 
    WTrackParameter wtrk2 = wTrackInfit(i);
    wtrk2.setPull(pull);
    //    for (int l=0;l<5; l++) {
    //(wTrackInfit(i).pull())[l]=(wtrk2.pull())[l];
    //   }
    setWTrackInfit(i, wtrk2);
    }  
    */
 
    return okfit;
}
 
bool KinematicFit::Fit(int n) {
    bool okfit = false;
    if(n < 0 || (unsigned int)n >= m_kc.size()) return okfit;
 
    m_nktrk = numberWTrack();
    m_pOrigin = HepVector(m_nktrk * NTRKPAR, 0);
    m_pInfit = HepVector(m_nktrk * NTRKPAR, 0);
    m_covOrigin = HepSymMatrix(m_nktrk * NTRKPAR, 0);
    m_covInfit = HepSymMatrix(m_nktrk * NTRKPAR, 0);
    m_massvector = HepVector(m_nktrk * NTRKPAR, 0);
    for(int i = 0; i < numberWTrack(); i++) {
        setWTrackInfit(i, wTrackOrigin(i));
        setPOrigin(i, (wTrackOrigin(i).w()).sub(1, NTRKPAR));
        setPInfit(i, (wTrackOrigin(i).w()).sub(1, NTRKPAR));
        setCovOrigin(i, (wTrackOrigin(i).Ew()).sub(1, NTRKPAR));
        setMassvector(i, wTrackOrigin(i).mass());
    }
 
    //
    // iteration loop
    //
 
    std::vector<double> chisq;
    chisq.clear();
 
    m_D = HepMatrix(m_kc[n].nc(), m_nktrk * NTRKPAR, 0);
    m_DT = HepMatrix(m_nktrk * NTRKPAR, m_kc[n].nc(), 0);
    m_d = HepVector(m_kc[n].nc(), 0);
 
    for(int it = 0; it < m_niter; it++) {
        m_nc = 0;
        KinematicConstraints kc = m_kc[n];
        updateConstraints(kc);
        //     m_kc[n] = kc;
        fit(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;
    //    ====update cov====
    //   upCovmtx();
    okfit = true;
    return okfit;
}
 
 
void KinematicFit::BuildVirtualParticle( int n) {
//
//      q = p1 + p2 + ... + pn
//
    upCovmtx();
    KinematicConstraints kc = m_kc[n];
    int ntrk = (kc.Ltrk()).size();
    int charge = 0;
    HepVector w(7, 0);
    HepSymMatrix ew(7, 0);
    HepMatrix dwdp(7, 7, 0);
    dwdp[0][0] = 1;
    dwdp[1][1] = 1;
    dwdp[2][2] = 1;
    dwdp[3][3] = 1;
    dwdp[4][4] = 1;
    dwdp[5][5] = 1;
    dwdp[6][6] = 1;
    for (int i = 0; i < ntrk; i++) {
        int itk = (kc.Ltrk())[i];
        charge += wTrackInfit(itk).charge();
        w[0] = w[0] + wTrackInfit(itk).w()[0];
        w[1] = w[1] + wTrackInfit(itk).w()[1];
        w[2] = w[2] + wTrackInfit(itk).w()[2];
        w[3] = w[3] + wTrackInfit(itk).w()[3];
        w[4] = 0.0;//
        w[5] = 0.0;// set virtual particle's vertex at (0,0,0)
        w[6] = 0.0;//
        ew = ew + (wTrackInfit(itk).Ew()).similarity(dwdp);  // the  vertex matrix  of this particles is not correct, because we do not use vertex information in kinematicfit, so ...
    }
    double m = sqrt(w[3]*w[3] - w[0]*w[0] - w[1]*w[1] - w[2]*w[2]);
    WTrackParameter vwtrk;
    vwtrk.setCharge(charge);
    vwtrk.setW(w);
    vwtrk.setEw(ew);
    vwtrk.setMass(m);
    m_virtual_wtrk.push_back(vwtrk);
}
 
 
 
void KinematicFit::gda(){
    for(int i = 0; i < numberWTrack(); i++) {
 
        if ((wTrackOrigin(i)).type() == 2 ){
            int n ;
            for(int j = 0; j < numberGammaShape(); j++) {
                if(i==GammaShapeList(j))  n = j;
            }
            HepSymMatrix Ew(NTRKPAR, 0);
            HepLorentzVector p1 = p4Infit(i);
            double eorigin = pOrigin(i)[3];
            //cout<<"p1 ="<<p1<<endl;
            HepMatrix dwda1(NTRKPAR, 3, 0);
            dwda1[0][0] = -p1.py();
            dwda1[1][0] = p1.px();
            dwda1[0][1] = p1.px()*p1.pz()/p1.perp();
            dwda1[1][1] = p1.py()*p1.pz()/p1.perp();
            dwda1[2][1] = -p1.perp();
            dwda1[0][2] = p1.px()/p1.rho();
            dwda1[1][2] = p1.py()/p1.rho();
            dwda1[2][2] = p1.pz()/p1.rho();
            dwda1[3][2] = p1.rho()/p1.e();
            HepSymMatrix emcmea1(3, 0);
            double pk = p1[3];
            emcmea1[0][0] = GammaShapeValue(n).getdphi() * GammaShapeValue(n).getdphi();
            emcmea1[1][1] = GammaShapeValue(n).getdthe() * GammaShapeValue(n).getdthe();
            emcmea1[2][2] = GammaShapeValue(n).de(eorigin,pk) * GammaShapeValue(n).de(eorigin,pk);
            Ew = emcmea1.similarity(dwda1);
            //cout<<"emcmea1 ="<<emcmea1<<endl;
            //cout<<"Ew ="<<Ew<<endl;
            setCovOrigin(i, Ew); 
        } 
    }
}
 
 
HepVector KinematicFit::pull(int n){
    upCovmtx();
 
    if (wTrackOrigin(n).charge()!=0){
        HepVector W(6,0);
        HepSymMatrix Ew(6,0);
        HepVector W1(7,0);
        HepSymMatrix Ew1(7,0);
        WTrackParameter wtrk = wTrackOrigin(n);
        //                W = wTrackOrigin(n).w();
        //                Ew = wTrackOrigin(n).Ew();
        //cout<<"===Origin status==="<<endl;
        //cout<<"W = "<<W<<endl;
        //cout<<"Ew ="<<Ew<<endl; 
        for(int i=0; i<3; i++) {
            W[i] = pInfit(n)[i];
        }
        W[3] = wTrackOrigin(n).w()[4];
        W[4] = wTrackOrigin(n).w()[5];
        W[5] = wTrackOrigin(n).w()[6];
        for(int j=0; j<3; j++) {
            for(int k=0; k<3; k++) {
                Ew[j][k] = covInfit(n)[j][k];
            }
        }
 
        for(int j=3; j<6; j++) {
            for(int k=3; k<6; k++) {
                Ew[j][k] = wTrackOrigin(n).Ew()[j+1][k+1];
            }
        }
        //
        // define J matrix to transfer 3 parameters to 4 parameters
        // 
        double px = p4Infit(n).px();
        double py = p4Infit(n).py();
        double pz = p4Infit(n).pz();
        double e  = p4Infit(n).e(); 
        HepMatrix J(7, 6, 0);
        J[0][0] = 1;
        J[1][1] = 1;
        J[2][2] = 1;
        J[3][0] = px/e;
        J[3][1] = py/e;
        J[3][2] = pz/e;
        J[4][3] = 1;
        J[5][4] = 1;
        J[6][5] = 1;
        W1 = J * W;
        Ew1 = Ew.similarity(J) ;
 
 
 
        // cout<<"===Infiddt status==="<<endl;
        // cout<<"p4 ="<<p4Infit(n)<<endl;
        // cout<<"W ="<<wTrackOrigin(n).w()<<endl;
        // cout<<"W1 ="<<W1<<endl;
        // cout<<"Ew ="<<wTrackOrigin(n).Ew()<<endl;
        // cout<<"Ew1 ="<<Ew1<<endl;
 
        wtrk.setW(W1);
        wtrk.setEw(Ew1);
        setWTrackInfit(n, wtrk); 
        HTrackParameter horigin = HTrackParameter(wTrackOrigin(n));
        HTrackParameter hinfit = HTrackParameter(wTrackInfit(n));
 
        HepVector a0 = horigin.hel();
        HepVector a1 = hinfit.hel();
        HepSymMatrix v0 = horigin.eHel();
        HepSymMatrix v1 = hinfit.eHel(); 
        HepVector pull(11,0);
        for (int k=0; k<5; k++) {
            pull[k] = (a0[k]-a1[k])/sqrt(abs(v0[k][k]-v1[k][k]));
        //  cout<<"pull ["<<k<<"] ="<<pull[k]<<endl;
        }
        for (int l=5; l<9; l++) {
            pull[l] = (wTrackOrigin(n).w()[l-5] - wTrackInfit(n).w()[l-5])/sqrt(abs(wTrackOrigin(n).Ew()[l-5][l-5] - wTrackInfit(n).Ew()[l-5][l-5]));
            //        cout<<"pull ["<<l<<"] ="<<pull[l]<<endl;
        }
            
        //                pull[9] = wTrackOrigin(n).w()[3] - wTrackInfit(n).w()[3];
        //                pull[10] =1/sqrt(abs(wTrackOrigin(n).Ew()[3][3] - wTrackInfit(n).Ew()[3][3]));  
        return pull;  
    }else {
        HepVector pull(3,0);
        for (int m=0; m<3; m++) {
                pull[m] = (wTrackOrigin(n).w()[m] - wTrackInfit(n).w()[m])/sqrt(abs(wTrackOrigin(n).Ew()[m][m] - wTrackInfit(n).Ew()[m][m]));
        }
        return pull;
    }
}
 
 
void KinematicFit::updateConstraints(KinematicConstraints k ) {
    KinematicConstraints kc = k;
 
    //   std::vector<HepLorentzVector> wlis;
    //   std::vector<WTrackParameter> wlis_V;
    //   wlis.clear();
    //   wlis_V.clear();
    //   for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++) {
    //      int n = (kc.Ltrk())[j];
    //        HepVec wtrk = wTrackInfit(n);
    //       if(m_espread!=0)        wtrk = wTrackOrigin(n);
 
    //       wlis.push_back(p4Infit(n));
    //   }
    //    for(unsigned int j = 0; j < (kc.Ltrk_V()).size(); j++) {
    //      int n = (kc.Ltrk_V())[j];
    //      WTrackParameter wtrk = wTrackInfit_V(n);
    //       wlis_V.push_back(wtrk);
    //  }
 
 
    int type = kc.Type();
    switch(type) {
        case  Resonance: {
                     //
                     //  E^2 - px^2 - py^2 - pz^2 = mres^2
                     //
                     double mres = kc.mres();
                     HepLorentzVector pmis;
                     for(unsigned int j = 0; j< (kc.Ltrk()).size(); j++){
                         int n = (kc.Ltrk())[j];
 
                         pmis = pmis + p4Infit(n);
                     }
                     //    for(unsigned int i = 0; i < wlis_V.size(); i++)
                     //      pmis = pmis + wlis_V[i].p();
                     for(unsigned int j = 0; j< (kc.Ltrk()).size(); j++){
                         int n = (kc.Ltrk())[j];
                         HepMatrix Dc(1, NTRKPAR, 0);
                         Dc[0][0] = -2 * pmis.px() + 2 * pmis.e() * p4Infit(n).px() / p4Infit(n).e();
                         Dc[0][1] = -2 * pmis.py() + 2 * pmis.e() * p4Infit(n).py() / p4Infit(n).e();
                         Dc[0][2] = -2 * pmis.pz() + 2 * pmis.e() * p4Infit(n).pz() / p4Infit(n).e();
                         //                      Dc[0][3] = 2 * pmis.e();
                         setD(m_nc,n,Dc);
                         setDT(n, m_nc, Dc.T());
                     }  
 
                     // for(unsigned int i = 0; i < wlis_V.size(); i++) {
                     //      HepMatrix Dc_V(1, 7, 0);
                     //      Dc_V[0][0] = -2 * pmis.px();
                     //      Dc_V[0][1] = -2 * pmis.py();
                     //      Dc_V[0][2] = -2 * pmis.pz();
                     //     Dc_V[0][3] = 2 * pmis.e();
                     //      m_Dc_V.push_back(Dc_V);
                     //    }
 
 
                     HepVector dc(1, 0);
                     dc[0] = pmis.m2() - mres * mres;
                     m_d[m_nc] = dc[0];
                     m_nc+=1;
                     //    std::cout << "etot = " << dc[0] <<" , " << mres<< std::endl;
                     //    m_dc.push_back(dc);
                     //    HepVector lambda(1, 0);
                     //    m_lambda.push_back(lambda);
                     //    HepSymMatrix vd(1, 0);
                     //    m_VD.push_back(vd);
                     //    HepSymMatrix V6 = m_Vre;
                     //    m_Vm.push_back(V6);
 
                     break;
                 }
        case TotalEnergy: {
                      //
                      //  E - Etot = 0
                      //
                      double etot = kc.etot();
                      HepLorentzVector pmis;
                      for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++){
                          int n = (kc.Ltrk())[j];
                          pmis = pmis + p4Infit(n);
                      }
                      //    for(unsigned int i = 0; i < wlis.size(); i++)
                      //      pmis = pmis + wlis[i].p();
                      for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++) {
                          int n = (kc.Ltrk())[j];
                          HepMatrix Dc(1, NTRKPAR, 0);
                          Dc[0][0] = p4Infit(n).px()/p4Infit(n).e();
                          Dc[0][1] = p4Infit(n).py()/p4Infit(n).e();
                          Dc[0][2] = p4Infit(n).pz()/p4Infit(n).e();
                          //      Dc[0][3] = 1.0;
                          setD(m_nc,n,Dc);
                          setDT(n, m_nc, Dc.T());
                      }
                      HepVector dc(1, 0);
                      dc[0] = pmis.e() - etot;
                      m_d[m_nc] = dc[0];
                      m_nc+=1;
                      //    m_dc.push_back(dc);
                      //    HepVector lambda(1, 0);
                      //    m_lambda.push_back(lambda);
                      //    HepSymMatrix vd(1, 0);
                      //    m_VD.push_back(vd);
                      break;
                  }
        case TotalMomentum: {
                        //
                        //  sqrt(px^2+py^2+pz^2) - ptot = 0
                        //
                        double ptot = kc.ptot();
                        HepLorentzVector pmis;
                        for(unsigned int j = 0; j< (kc.Ltrk()).size(); j++){
                            int n = (kc.Ltrk())[j];
                            pmis = pmis + p4Infit(n);
                        }
 
                        //    for(unsigned int i = 0; i < wlis.size(); i++)
                        //      pmis = pmis + wlis[i].p();
                        for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++) {
                            int n = (kc.Ltrk())[j];
                            HepMatrix Dc(1, NTRKPAR, 0);
                            Dc[0][0] = pmis.px()/pmis.rho();
                            Dc[0][1] = pmis.py()/pmis.rho();
                            Dc[0][2] = pmis.pz()/pmis.rho();
                            setD(m_nc,n,Dc);
                            setDT(n, m_nc, Dc.T());
                            //      m_Dc.push_back(Dc);
                        }
                        HepVector dc(1, 0);
                        dc[0] = pmis.rho() - ptot;
                        m_d[m_nc] = dc[0];
                        m_nc+=1;     
                        //    m_dc.push_back(dc);
                        //    HepVector lambda(1, 0);
                        //    m_lambda.push_back(lambda);
                        //    HepSymMatrix vd(1, 0);
                        //    m_VD.push_back(vd);
                        break;
                    }
                    /*
                       case kc.typePoint(): {
                       HepPoint3D point = kc.point();
                       double flightpath;
                       HepMatrix p(2,3,0);
 
                       HepSymMatrix Vp(2,0);
                       for(unsigned int i = 0; i < wlis.size(); i++) {
                       HepMatrix Dc(2, 7, 0);
                       m_Dc.push_back(Dc);
                       }
                    // wlis_V.size() should be 1
                    for(unsigned int i = 0; i < wlis_V.size(); i++) {
                    //      HepMatrix Dc(3, 7, 0);
                    //      m_Dc.push_back(Dc);
                    HepMatrix Dc_V(2, 7, 0);
                    HepSymMatrix Vclus(3, 0); 
                    for(unsigned int j = 0; j < 3; j++){
                    for(unsigned int k = j; k < 3; k++){
                    Vclus[j][k] = wlis_V[i].Ew()[j+4][k+4];
                    }
                    }
                    cout<<"Vclus ="<<Vclus<<endl;   
                    p[0][0] = wlis_V[i].w()[1];
                    p[0][1] = -wlis_V[i].w()[0];
                    p[1][0] = wlis_V[i].w()[2];
                    p[1][2] = -wlis_V[i].w()[0];
                    Vp = Vclus.similarity(p);   
                    cout<<"Vp ="<<Vp<<endl;
                    Dc_V[0][0] = -(wlis_V[i].w()[5] - point[1]);
                    Dc_V[0][1] = wlis_V[i].w()[4] - point[0];
                    Dc_V[0][4] = wlis_V[i].w()[1];
                    Dc_V[0][5] = -wlis_V[i].w()[0];
 
                    Dc_V[1][0] = -(wlis_V[i].w()[6] - point[2]);
                    Dc_V[1][2] = wlis_V[i].w()[4] - point[0];
                    Dc_V[1][4] = wlis_V[i].w()[2];
                    Dc_V[1][6] = -wlis_V[i].w()[0];
 
                    //      HepMatrix q_x(3,1,0) ;
                    //      q_x[0][0] = 1;
                    //      HepMatrix deltaX_x(3,1,0) ;
                    //      deltaX_x[0][0] = 1;
                    //      HepMatrix p1_x = -(q_x.T()*Vclus.inverse(iver)*deltaX)*p2 + p1*(q_x.T()*Vclus.inverse(iver)*q + q.T()*Vclus.inverse(iver)*q_x);
                    //      HepMatrix p2_x = p2*p2;
 
                    //      HepMatrix q_y(3,1,0) ;
                    //      q_y[1][0] = 1;
                    //      HepMatrix deltaX_y(3,1,0);
                    //      deltaX_y[1][0] = 1;
                    //      HepMatrix p1_y = -(q_y.T()*Vclus.inverse(iver)*deltaX)*p2 + p1*(q_y.T()*Vclus.inverse(iver)*q + q.T()*Vclus.inverse(iver)*q_y);
                    //      HepMatrix p2_y = p2*p2;
 
                    //      HepMatrix q_z(3,1,0);
                    //      q_z[2][0] = 1;
                    //      HepMatrix deltaX_z(3,1,0);
                    //      deltaX_z[2][0] = 1;
                    //      Hw()[0]<<endl;
                    //cout<<"dc[0] ="<<dc[0]<<endl; 
                    dc[1] = (wlis_V[i].w()[4] - point[0])*wlis_V[i].w()[2] - (wlis_V[i].w()[6] - point[2])*wlis_V[i].w()[0];
                    m_dc.push_back(dc);
                    }
                    HepSymMatrix V3 = Vp;
                    m_Vm.push_back(V3);
                    break;
                    } 
                    */
 
        case ThreeMomentum: {
                        //
                        //  px - p3x = 0
                        //  py - p3y = 0
                        //  pz - p3z = 0
                        //
                        Hep3Vector p3 = kc.p3();
                        HepLorentzVector pmis;
                        for(unsigned int j = 0; j< (kc.Ltrk()).size(); j++){
                            int n = (kc.Ltrk())[j];
                            pmis = pmis + p4Infit(n);
                        }
                        //    for(unsigned int i = 0; i < wlis.size(); i++)
                        //      pmis = pmis + wlis[i].p();
                        for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++) {
                            int n = (kc.Ltrk())[j];
 
                            HepMatrix Dc(kc.nc(), NTRKPAR, 0);
                            Dc[0][0] = 1.0;
                            Dc[1][1] = 1.0;
                            Dc[2][2] = 1.0;
                            HepVector dc(kc.nc(), 0);
                            dc[0] = pmis.px() - p3.x();
                            dc[1] = pmis.py() - p3.y();
                            dc[2] = pmis.pz() - p3.z();
                            for(int i = 0; i < kc.nc(); i++) {
                                setD(m_nc+i, n, Dc.sub(i+1, i+1, 1, NTRKPAR));
                                setDT(n, m_nc+i, (Dc.sub(i+1, i+1, 1, NTRKPAR)).T());
                                m_d[m_nc+i] = dc[i];
                            }
                            //    m_Dc.push_back(Dc);
                        }
                        m_nc += 3; 
 
                        //    HepVector dc(3, 0);
                        //    dc[0] = pmis.px() - p3.x();
                        //    dc[1] = pmis.py() - p3.y();
                        //    dc[2] = pmis.pz() - p3.z();
                        //    m_dc.push_back(dc);
                        //    HepVector lambda(3, 0);
                        //    m_lambda.push_back(lambda);
                        //    HepSymMatrix vd(3, 0);
                        //    m_VD.push_back(vd);
                        break;
                    }
        case EqualMass: {
                    //
                    //  (E_1 ^2 - Px_1 ^2 - Py_1 ^2 - Pz_1 ^2) - (E_2 ^2 - Px_2 ^2 - Py_2 ^2 - Pz_2 ^2) = 0
                    //
 
                    int isiz = (kc.numEqual())[0];
                    HepLorentzVector pmis1, pmis2;
                    for(int n = 0; n < isiz; n++) {
                        int n1 = (kc.Ltrk())[n];
                        pmis1 = pmis1 + p4Infit(n1);
                    }
                    int jsiz = (kc.numEqual())[1];
                    for(int n = 0; n < jsiz; n++){
                        int n2 = (kc.Ltrk())[n+isiz];
                        pmis2 = pmis2 + p4Infit(n2);
                    }
                    for(int i = 0; i < isiz; i++) {
                        int n1 = (kc.Ltrk())[i];
                        HepMatrix Dc(1, NTRKPAR, 0);
                        Dc[0][0] = -2 * pmis1.px() + 2 * pmis1.e() * p4Infit(n1).px() / p4Infit(n1).e();
                        Dc[0][1] = -2 * pmis1.py() + 2 * pmis1.e() * p4Infit(n1).py() / p4Infit(n1).e();
                        Dc[0][2] = -2 * pmis1.pz() + 2 * pmis1.e() * p4Infit(n1).pz() / p4Infit(n1).e();
                        //  Dc[0][3] = 2 * pmis1.e();
                        setD(m_nc,n1,Dc);
                        setDT(n1,m_nc,Dc.T());
                    }
                    for(int i = 0; i < jsiz; i++) {
                        int n2 = (kc.Ltrk())[i+isiz];
                        HepMatrix Dc(1, NTRKPAR, 0);
                        Dc[0][0] = 2 * pmis2.px() - 2 * pmis2.e() * p4Infit(n2).px() / p4Infit(n2).e();
                        Dc[0][1] = 2 * pmis2.py() - 2 * pmis2.e() * p4Infit(n2).py() / p4Infit(n2).e();
                        Dc[0][2] = 2 * pmis2.pz() - 2 * pmis2.e() * p4Infit(n2).pz() / p4Infit(n2).e();
                        Dc[0][3] = -2 * pmis2.e();
                        setD(m_nc,n2,Dc);
                        setDT(n2,m_nc,Dc.T());
                    }
                    //    int isiz_V = m_nequal_V[0];
                    //    HepLorentzVector pmis1_V, pmis2_V;
                    //    if(isiz_V > 0){
                    //    for(int n = 0; n < isiz_V; n++) {
                    //      pmis1_V = pmis1_V + wlis_V[n].p();
                    //    } 
                    //    } 
                    //    int jsiz_V = m_nequal_V[1];
                    //    if(jsiz_V > 0) {
                    //    for(int n = 0; n < jsiz_V; n++) 
                    //      pmis2_V = pmis2_V + wlis_V[isiz_V+n].p();
                    //    }
 
                    //    for(int i = 0; i < isiz_V; i++) {
                    //      HepMatrix Dc_V(1, 7, 0);
                    //      Dc_V[0][0] = -2 * pmis1_V.px();
                    //      Dc_V[0][1] = -2 * pmis1_V.py();
                    //      Dc_V[0][2] = -2 * pmis1_V.pz();
                    //      Dc_V[0][3] = 2 * pmis1_V.e();
                    //      m_Dc_V.push_back(Dc_V);
                    //    }
                    //    for(int i = isiz_V; i < isiz_V+jsiz_V; i++) {
                    //      HepMatrix Dc_V(1, 7, 0);
                    //      Dc_V[0][0] = 2 * pmis2_V.px();
                    //      Dc_V[0][1] = 2 * pmis2_V.py();
                    //      Dc_V[0][2] = 2 * pmis2_V.pz();
                    //      Dc_V[0][3] = -2 * pmis2_V.e();
                    //      m_Dc_V.push_back(Dc_V);
                    //    }
                    HepVector dc(1, 0);
                    dc[0] = pmis1.m2() - pmis2.m2();// + pmis1_V.m2() - pmis2_V.m2();
                    m_d[m_nc] = dc[0];
 
                    m_nc+=1;
                    //    m_dc.push_back(dc);
                    //    HepVector lambda(1, 0);
                    //    m_lambda.push_back(lambda);
                    //    HepSymMatrix vd(1, 0);
                    //    m_VD.push_back(vd);
                    //    HepSymMatrix V2 = m_Vne;
                    //    m_Vm.push_back(V2);
                    //    std::cout <<"m_Vm[0] ="<<m_Vm[0]<<std::endl;
                    break;
                }
        case FourMomentum: 
        default: {
                 //
                 //  px - p4x = 0
                 //  py - p4y = 0
                 //  pz - p4z = 0
                 //  e  - p4e = 0
                 //
                 HepLorentzVector p4 = kc.p4();
                 HepLorentzVector pmis;
                 for(unsigned int j = 0; j< (kc.Ltrk()).size(); j++){
                     int n = (kc.Ltrk())[j];
                     pmis = pmis + p4Infit(n);
                 }
                 //    HepLorentzVector pmis_V;
                 for(unsigned int j = 0; j < (kc.Ltrk()).size(); j++) {
                     int n = (kc.Ltrk())[j];
                     HepMatrix Dc(kc.nc(), NTRKPAR, 0);
                     Dc[0][0] = 1.0;
                     Dc[1][1] = 1.0;
                     Dc[2][2] = 1.0;
                     Dc[3][0] = p4Infit(n).px() / p4Infit(n).e();
                     Dc[3][1] = p4Infit(n).py() / p4Infit(n).e();
                     Dc[3][2] = p4Infit(n).pz() / p4Infit(n).e();
                     //                  Dc[3][3] = 1.0;
 
                     //      m_Dc.push_back(Dc);
                     HepVector dc(kc.nc(), 0);
                     dc[0] = pmis.px()  - p4.px();
                     dc[1] = pmis.py()  - p4.py();
                     dc[2] = pmis.pz()  - p4.pz();
                     dc[3] = pmis.e()  - p4.e();
                     for(int i = 0; i < kc.nc(); i++) {
                         setD(m_nc+i, n, Dc.sub(i+1, i+1, 1, NTRKPAR));
                         setDT(n, m_nc+i, (Dc.sub(i+1, i+1, 1, NTRKPAR)).T());
                         m_d[m_nc+i] = dc[i];
                     }
                 }
                 m_nc += 4;
                 //    for(unsigned int i = 0; i < wlis_V.size(); i++)
                 //     pmis_V = pmis_V + wlis_V[i].p();
                 //    for(unsigned int i = 0; i < wlis_V.size(); i++) {
                 //      HepMatrix Dc_V(4, 7, 0);
                 //      Dc_V[0][0] = 1.0;
                 //      Dc_V[1][1] = 1.0;
                 //      Dc_V[2][2] = 1.0;
                 //      Dc_V[3][3] = 1.0;
                 //      m_Dc_V.push_back(Dc_V);
                 //    }
 
                 //    HepVector dc(4, 0);
                 //   dc[0] = pmis.px() + pmis_V.px() - p4.px();
                 //    dc[1] = pmis.py() + pmis_V.py() - p4.py();
                 //    dc[2] = pmis.pz() + pmis_V.pz() - p4.pz();
                 //    dc[3] = pmis.e() + pmis_V.e() - p4.e();
                 //    m_dc.push_back(dc);
 
                 //    HepSymMatrix V1 = m_Vme;
                 //    m_Vm.push_back(V1);
                 //    HepVector lambda(4, 0);
                 //    m_lambda.push_back(lambda);
                 //   HepSymMatrix vd(4, 0);
                 //    m_VD.push_back(vd);
 
                 break;
             }
    }
}