// ------------------------------------------------------------------------- // ----- CbmLitExtrapolator source file ----- // ----- Created 14/08/06 by A. Lebedev ----- // ------------------------------------------------------------------------- #include "CbmRunAna.h" #include "CbmRootManager.h" #include "CbmField.h" #include "CbmFieldConst.h" #include "CbmFieldMap.h" #include "CbmFieldMapSym2.h" #include "CbmFieldMapSym3.h" #include "CbmFieldPar.h" #include "CbmLitExtrapolator.h" CbmLitExtrapolator::CbmLitExtrapolator() { fStepMin = 20.; //in cm fStepMinRK5 = 2.; //in cm fQpCurls = 0.02; fMethod = 3; fError = 0.0005; //in cm c_light = 0.000299792458; InitMagneticField(); } CbmLitExtrapolator::~CbmLitExtrapolator() { } void CbmLitExtrapolator::InitMagneticField() { CbmRunAna *Run = CbmRunAna::Instance(); if(NULL == Run) { cout << "-E- CbmLitExtrapolator::Init : " << "Run Ana is not instantiated" << endl; return; } cout << "-I- CbmLitExtrapolator::Init : " << "Reading Magnetic Field " << std::endl; fMagneticField = (CbmField*)Run->GetField(); CbmRuntimeDb *RunDB = Run->GetRuntimeDb(); /* CbmFieldPar* fFieldPar = (CbmFieldPar*) RunDB->findContainer("CbmFieldPar"); if ( ! fFieldPar ) { cerr << "-E- No field parameters available!" << endl; } // Instantiate correct field type Int_t fType = fFieldPar->GetType(); if ( fType == 0 ) fMagneticField = new CbmFieldConst(fFieldPar); else if ( fType == 1 ) fMagneticField = new CbmFieldMap(fFieldPar); else if ( fType == 2 ) fMagneticField = new CbmFieldMapSym2(fFieldPar); else if ( fType == 3 ) fMagneticField = new CbmFieldMapSym3(fFieldPar); else cerr << "-W- CbmRunAna::GetField: Unknown field type " << fType << endl; cout << "New field at " << fMagneticField << ", type " << fType << endl; // Initialise field if ( fMagneticField ) { fMagneticField->Init(); fMagneticField->Print(); } else { cout << "-W- CbmLitExtrapolator::Init : " << "No Magnetic Field Found" << std::endl; } */ return ; } void CbmLitExtrapolator::Propagate(CbmLitState &State, Double_t z_out, Bool_t IsCovMatrix) { Double_t z_in = State.GetZ(); fIsCovMatrix = IsCovMatrix; // check current z-position Double_t dz = fabs(z_out - z_in); if (dz < 0.01) { return ; } if (fMethod == 0) { // line extrapolation // F[0][0] = 1.; F[0][2] = dz; // F[1][1] = 1.; F[1][3] = dz; // F[2][2] = 1.; // F[3][3] = 1.; // F[4][4] = 1.; line(State , z_out); State.SetZ(z_out); } else { Double_t *X_in = State.GetStateVector(); Double_t fQp[25]; for (Int_t i = 0; i < 25; ++i) fQp[i] = 0.; Int_t istat = 0; Double_t X_out[5] = {0., 0., 0., 0., 0.}; //Double_t *X_out = State.GetStateVector(); //transport Extrapolate(z_in, X_in, z_out, X_out, fQp, istat); State.SetStateVector(X_out); // check for sucess /* if (istat != 0){ cout << "-E- CbmLitExtrapolator::Propagate : " << "Runga kutta: transport impossible " << endl; return; } */ TMatrixD F(5,5); //update the transport matrix F.SetMatrixArray(fQp, "F"); // TMatrixD F; //F.Use(5, 5, fQp); TMatrixDSym C; C.Use(5, State.GetCovMatrix()); //transport the covariance matrix F*C*F.T() C.Similarity(F); State.SetZ(z_out); } return; } void CbmLitExtrapolator::Extrapolate(double& zIn, double pIn[5], double& zNew, double pOut[5], double fQp[25], int& istat) { switch ( fMethod ) { case 1: rk4fast(zIn, pIn, zNew, pOut, fQp, istat); break; case 2: rk4order(zIn, pIn, zNew, pOut, fQp, istat); break; case 3: rk5fast(zIn, pIn, fError, zNew, pOut, fQp, istat); break; case 4: rk5order(zIn, pIn, fError, zNew, pOut, fQp, istat); break; case 5: rk5numde(zIn, pIn, fError, zNew, pOut, fQp, istat); break; default: cout << "-E- CbmLitExtrapolator::Init : " << "Incorrect Extrapolator name - taking rk5order !!!!" << endl; rk5order(zIn, pIn, fError, zNew, pOut, fQp, istat); break; } } //=========================================================================== // linear extrapolation //=========================================================================== void CbmLitExtrapolator::line( CbmLitState &State, Double_t z_out) { Double_t *X = State.GetStateVector(); Double_t dz = z_out - State.GetZ(); //transport state vector F*X*F.T() X[0] = X[0] + dz * X[2]; X[1] = X[1] + dz * X[3]; // X[2] = X[2]; // X[3] = X[3]; // X[4] = X[4]; if (fIsCovMatrix) { Double_t *C = State.GetCovMatrix(); //transport covariance matrix F*C*F.T() Double_t t3 = C[2] + dz * C[12]; Double_t t7 = dz * C[13]; Double_t t8 = C[3] + t7; Double_t t19 = C[8] + dz * C[18]; C[0] = C[0] + dz * C[2] + t3 * dz; C[1] = C[1] + dz * C[7] + t8 * dz; C[2] = t3; C[3] = t8; C[4] = C[4] + dz * C[14]; C[5] = C[1]; C[6] = C[6] + dz * C[8] + t19 * dz; C[7] = C[7] + t7; C[8] = t19; C[9] = C[9] + dz * C[19]; C[10] = C[2]; C[11] = C[7]; // C[12] = C[12]; // C[13] = C[13]; // C[14] = C[14]; C[15] = C[3]; C[16] = C[8]; C[17] = C[13]; // C[18] = C[18]; // C[19] = C[19]; C[20] = C[4]; C[21] = C[9]; C[22] = C[14]; C[23] = C[19]; // C[24] = C[24]; } } //=========================================================================== // parabolic extrapolation //=========================================================================== void CbmLitExtrapolator::parabolic( CbmLitState &State, Double_t z_out) { //TODO } //=========================================================================== // Runge-Kutta computation 5th order //=========================================================================== //-- Author : A.Spiridonov 26.03.98 void CbmLitExtrapolator::rk5order( double& z_in , // z value for input parameters double* p_in, // input track parameters (x,y,tx,ty,Q/p) double& error, // acceptable errors in mm double& z_out, // z value for output parameters double* p_out, // output track parameters double* rkd, // derivatives d p_out[0-4] / d p_in[0-4] int& ierror) // = 0 ok, = 1 track curls // // Fifth-order Runge-Kutta method with adaptive stepsize control // for solution of the equation of motion of a particle with // parameter qp = Q /P in the magnetic field B() // // | x | tx // | y | ty // d/dz = | tx| = ft * ( ty * ( B(3)+tx*b(1) ) - (1+tx**2)*B(2) ) // | ty| ft * (-tx * ( B(3)+ty+b(2) - (1+ty**2)*B(1) ) , // // where ft = C*qp*sqrt ( 1 + tx**2 + ty**2 ) . // // In the following for RK step : // x=x[0], y=x[1], tx=x[3], ty=x[4]. // { static double a[6] = {0.0, 0.2, 0.3, 0.6, 1.0, 7.0/8.0}; static double c[6] = {37.0/378.0, 0.0, 250.0/621.0, 125.0/594.0, 0., 512.0/1771.0}; static double c1[6] = {2825.0/27648.0, 0., 18575.0/48384.0, 13525.0/55296.0, 277.0/14336.0,1.0/4.0}; static double b[16] = {0.0, 1.0/5.0, 3.0/40.0, 9.0/40.0, 3.0/10.0, -9.0/10.0, 6.0/5.0, -11.0/54.0, 5.0/2.0, -70.0/27.0, 35.0f/27.0, 1631.0/55296.0, 175.0/512.0, 575.0/13824.0, 44275.0/110592.0, 253.0/4096.0}; double qp,hC,h; double k[24],x0[4],x[4],k1[24]; double tx,ty,tx2,ty2,txty,tx2ty2,tx2ty2qp,tx2ty21,I_tx2ty2,I_tx2ty21; double Ax[6],Ay[6],Ax_tx[6],Ay_tx[6],Ax_ty[6],Ay_ty[6]; int step_j,step4; double p1_out[5],z2_out,p2_out[5],rkd1[25],rkd2[25]; //---------------------------------------------------------------- qp = p_in[4]; ierror = (fabs(qp) > fQpCurls) ? 1 : 0; h = z_out - z_in; hC = h * c_light; x0[0] = p_in[0]; x0[1] = p_in[1]; x0[2] = p_in[2]; x0[3] = p_in[3]; // // Runge-Kutta step // int i, j, step; for (step = 0; step < 6; ++step) { for(i=0; i < 4; ++i) { x[i] = x0[i]; step_j = ((step-1)*step)/2 + 1; for(j=0; j < step; ++j) { x[i] += b[step_j + j] * k[j*4+i]; } } fPoint[0] = x[0]; fPoint[1] = x[1]; fPoint[2] = z_in + a[step] * h; fMagneticField->GetFieldValue(fPoint, fB); //fPoint.setX( x[0] ); //fPoint.setY( x[1] ); //fPoint.setZ( z_in + a[step] * h ); //m_pIMF->fieldVector( fPoint, fB ); tx = x[2]; ty = x[3]; tx2 = tx * tx; ty2 = ty * ty; txty = tx * ty; tx2ty21= 1.0 + tx2 + ty2; I_tx2ty21 = 1.0 / tx2ty21 * qp; tx2ty2 = sqrt(tx2ty21 ) ; I_tx2ty2 = qp * hC / tx2ty2 ; tx2ty2 *= hC; tx2ty2qp = tx2ty2 * qp; //Ax[step] = ( txty*fB.x() + ty*fB.z() - ( 1.0 + tx2 )*fB.y() ) * tx2ty2; //Ay[step] = (-txty*fB.y() - tx*fB.z() + ( 1.0 + ty2 )*fB.x() ) * tx2ty2; Ax[step] = ( txty*fB[0] + ty*fB[2] - ( 1.0 + tx2 )*fB[1] ) * tx2ty2; Ay[step] = (-txty*fB[1] - tx*fB[2] + ( 1.0 + ty2 )*fB[0] ) * tx2ty2; //Ax_tx[step] = Ax[step]*tx*I_tx2ty21 + (ty*fB.x()-2.*tx*fB.y())*tx2ty2qp; //Ax_ty[step] = Ax[step]*ty*I_tx2ty21 + (tx*fB.x()+fB.z())*tx2ty2qp; //Ay_tx[step] = Ay[step]*tx*I_tx2ty21 + (-ty*fB.y()-fB.z())*tx2ty2qp; //Ay_ty[step] = Ay[step]*ty*I_tx2ty21 + (-tx*fB.y()+2.*ty*fB.x())*tx2ty2qp; Ax_tx[step] = Ax[step]*tx*I_tx2ty21 + (ty*fB[0]-2.*tx*fB[1])*tx2ty2qp; Ax_ty[step] = Ax[step]*ty*I_tx2ty21 + (tx*fB[0]+fB[2])*tx2ty2qp; Ay_tx[step] = Ay[step]*tx*I_tx2ty21 + (-ty*fB[1]-fB[2])*tx2ty2qp; Ay_ty[step] = Ay[step]*ty*I_tx2ty21 + (-tx*fB[1]+2.*ty*fB[0])*tx2ty2qp; step4 = step * 4; k[step4 ] = tx * h; k[step4+1] = ty * h; k[step4+2] = Ax[step] * qp; k[step4+3] = Ay[step] * qp; } // end of Runge-Kutta steps for(i=0; i < 4; ++i) { p_out[i]=x0[i]+c[0]*k[i]+c[2]*k[8+i]+c[3]*k[12+i]+c[5]*k[20+i]; } p_out[4]=p_in[4]; // printf("p_out %8f3 %8f3 %8f3 %8f3 \n" // ,p_out[0],p_out[1],p_out[2],p_out[3]); // // The embedded fourth-order formula for x and y // for (i = 0; i < 2; ++i) { p1_out[i]=x0[i]+c1[0]*k[i]+c1[2]*k[8+i]+c1[3]*k[12+i]+c1[4]*k[16+i] +c1[5]*k[20+i]; } // // Derivatives dx/dqp // x0[0] = 0.; x0[1] = 0.; x0[2] = 0.; x0[3] = 0.; // // Runge-Kutta step for derivatives dx/dqp for (step = 0; step < 6; ++step) { for(i=0; i < 4; ++i) { x[i] = x0[i]; step_j = ((step-1)*step)/2 + 1; for(j=0; j < step; ++j) { x[i] += b[step_j + j] * k1[j*4+i]; } } step4 = step * 4; k1[step4 ] = x[2] * h; k1[step4+1] = x[3] * h; k1[step4+2] = Ax[step] + Ax_tx[step] * x[2] + Ax_ty[step] * x[3]; k1[step4+3] = Ay[step] + Ay_tx[step] * x[2] + Ay_ty[step] * x[3]; } // end of Runge-Kutta steps for derivatives dx/dqp for (i = 0; i < 4; ++i ) { rkd[20+i]=x0[i]+c[0]*k1[i]+c[2]*k1[8+i]+c[3]*k1[12+i]+c[5]*k1[20+i]; } rkd[24] = 1.; // end of derivatives dx/dqp // // Derivatives dx/tx // x0[0] = 0.0; x0[1] = 0.0; x0[2] = 1.0; x0[3] = 0.0; // // Runge-Kutta step for derivatives dx/dtx // for (step = 0; step < 6; ++step) { for(i=0; i < 4; ++i) { x[i] = x0[i]; step_j = ((step-1)*step)/2 + 1; for(j=0; j < step; ++j) { if(i != 2) {x[i] += b[step_j + j] * k1[j*4+i];} // tx fixed } } step4 = step * 4; k1[step4 ] = x[2] * h; k1[step4+1] = x[3] * h; // k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3]; k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3]; } // end of Runge-Kutta steps for derivatives dx/dtx for (i = 0; i < 4; ++i ) { if(i != 2) { rkd[10+i]=x0[i]+c[0]*k1[i]+c[2]*k1[8+i]+c[3]*k1[12+i]+c[5]*k1[20+i]; } } // end of derivatives dx/dtx rkd[12] = 1.; rkd[14] = 0.; // Derivatives dx/ty // x0[0] = 0; x0[1] = 0.; x0[2] = 0.; x0[3] = 1.; // // Runge-Kutta step for derivatives dx/dty // for (step = 0; step < 6; ++step) { for(i=0; i < 4; ++i) { x[i] = x0[i]; step_j = ((step-1)*step)/2 + 1; for(j=0; j < step; ++j) { if( i != 3){ x[i] += b[step_j + j] * k1[j*4+i];} // ty fixed } } step4 = step * 4; k1[step4 ] = x[2] * h; k1[step4+1] = x[3] * h; k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3]; // k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3]; } // end of Runge-Kutta steps for derivatives dx/dty for (i = 0; i < 3; ++i ) { rkd[15+i]=x0[i]+c[0]*k1[i]+c[2]*k1[8+i]+c[3]*k1[12+i]+c[5]*k1[20+i]; } // end of derivatives dx/dty rkd[18] = 1.; rkd[19] = 0.; // // derivatives dx/dx and dx/dy for(i = 0; i < 10; ++i){ rkd[i] = 0.;} rkd[0] = 1.; rkd[6] = 1.; // // stepsize control // if( (fabs(p1_out[0]-p_out[0]) > error ) || (fabs(p1_out[1]-p_out[1]) > error ) ) { if(fabs(h) > fStepMinRK5 ) { z2_out = z_in + 0.5 * h; rk5order(z_in , p_in, error, z2_out, p2_out, rkd1, ierror); rk5order(z2_out, p2_out, error, z_out, p_out , rkd2, ierror); rkd[0] = 1.; rkd[1] = 0.; rkd[2] = 0.; rkd[3] = 0.; rkd[4] = 0; rkd[5] = 0.; rkd[6] = 1.; rkd[7] = 0.; rkd[8] = 0.; rkd[4] = 0; rkd[10] = rkd1[10] + rkd2[10] + rkd2[15] * rkd1[13]; rkd[11] = rkd1[11] + rkd2[11] + rkd2[16] * rkd1[13]; rkd[12] = 1.; rkd[13] = rkd2[13] + rkd1[13]; rkd[14] = 0.; rkd[15] = rkd1[15] + rkd2[10] * rkd1[17] + rkd2[15]; rkd[16] = rkd1[16] + rkd2[11] * rkd1[17] + rkd2[16]; rkd[17] = rkd1[17] + rkd2[17]; rkd[18] = 1.; rkd[19] = 0.; rkd[20] = rkd1[20] + rkd2[10] * rkd1[22] + rkd2[15] * rkd1[23] +rkd2[20]; rkd[21] = rkd1[21] + rkd2[11] * rkd1[22] + rkd2[16] * rkd1[23] +rkd2[21]; rkd[22] = rkd1[22] + rkd2[17] * rkd1[23] + rkd2[22]; rkd[23] = rkd2[13] * rkd1[22] + rkd1[23] +rkd2[23]; rkd[24] = 1.; } } // end of stepsize control } // end of rk5order //=========================================================================== //-- Author : A.Spiridonov 26.03.98 //=========================================================================== void CbmLitExtrapolator::rk5fast( double& z_in , // z value for input parameters double* p_in, // input track parameters (x,y,tx,ty,Q/p) double& error, // acceptable errors in mm double& z_out, // z value for output parameters double* p_out, // output track parameters double* rkd, // derivatives d p_out[0-4] / d p_in[0-4] int& ierror) // = 0 ok, = 1 track curls // // Fifth-order Runge-Kutta method with adaptive stepsize control // for solution of the equation of motion of a particle with // parameter qp = Q /P in the magnetic field B() // // | x | tx // | y | ty // d/dz = | tx| = ft * ( ty * ( B(3)+tx*b(1) ) - (1+tx**2)*B(2) ) // | ty| ft * (-tx * ( B(3)+ty+b(2) - (1+ty**2)*B(1) ) , // // where ft = C*qp*sqrt ( 1 + tx**2 + ty**2 ) . // // In the following for RK step : // x=x[0], y=x[1], tx=x[3], ty=x[4]. // { static double a[6] = {0.0, 0.2, 0.3, 0.6, 1.0, 7.0/8.0}; static double c[6] = {37.0/378.0, 0.0, 250.0/621.0, 125.0/594.0, 0.0, 512.0/1771.0}; static double c1[6] = {2825.0/27648.0, 0.0, 18575.0/48384.0, 13525.0/55296.0, 277.0/14336.,1.0/4.0}; static double b[16] = {0.0, 1.0/5.0, 3.0/40.0, 9.0/40.0, 3.0/10.0, -9.0/10.0, 6.0/5.0, -11.0/54.0, 5.0/2.0, -70.0/27.0, 35.0/27.0, 1631.0/55296.0, 175.0/512.0, 575.0/13824.0, 44275.0/110592.0, 253.0/4096.0}; int step_j,step4; double qp,hC,h; double k[24],x0[4],x[4],k1[24]; double tx,ty,tx2,ty2,txty,tx2ty2; double Ax[6],Ay[6]; double p1_out[5],z2_out,p2_out[5]; qp = p_in[4]; ierror = (fabs(qp) > fQpCurls) ? 1 : 0; h = z_out - z_in; hC = h * c_light; x0[0] = p_in[0]; x0[1] = p_in[1]; x0[2] = p_in[2]; x0[3] = p_in[3]; // // Runge-Kutta step // int i, j, step; for (step = 0; step < 6; ++step){ for(i=0; i < 4; ++i){ x[i] = x0[i]; step_j = ((step-1)*step)/2 + 1; for(j=0; j < step; ++j){ x[i] += b[step_j + j] * k[j*4+i]; } // j } //i //fPoint.setX( x[0] ); //fPoint.setY( x[1] ); //fPoint.setZ( z_in + a[step] * h ); //m_pIMF->fieldVector( fPoint, fB ); fPoint[0] = x[0]; fPoint[1] = x[1]; fPoint[2] = z_in + a[step] * h; fMagneticField->GetFieldValue(fPoint, fB); tx = x[2]; ty = x[3]; tx2 = tx * tx; ty2 = ty * ty; txty = tx * ty; tx2ty2 = sqrt( 1.0 + tx2 + ty2 ) * hC; //Ax[step] = ( txty*fB.x() + ty*fB.z() - ( 1.0 + tx2 )*fB.y() ) * tx2ty2; //Ay[step] = (-txty*fB.y() - tx*fB.z() + ( 1.0 + ty2 )*fB.x() ) * tx2ty2; Ax[step] = ( txty*fB[0] + ty*fB[2] - ( 1.0 + tx2 )*fB[1] ) * tx2ty2; Ay[step] = (-txty*fB[1] - tx*fB[2] + ( 1.0 + ty2 )*fB[0] ) * tx2ty2; step4 = step * 4; k[step4 ] = tx * h; k[step4+1] = ty * h; k[step4+2] = Ax[step] * qp; k[step4+3] = Ay[step] * qp; } // end of Runge-Kutta steps for(i=0; i < 4; ++i){ p_out[i]=x0[i]+c[0]*k[i]+c[2]*k[8+i]+c[3]*k[12+i]+c[5]*k[20+i]; } p_out[4]=p_in[4]; // // The embedded fourth-order formula for x // p1_out[0]=x0[0]+c1[0]*k[0]+c1[2]*k[8]+c1[3]*k[12]+c1[4]*k[16]+c1[5]*k[20]; // Derivatives dx/dqp // // // Runge-Kutta step for derivatives dx/dqp for (step = 0; step < 6; ++step) { x[0] = 0.0; step_j = ((step-1)*step)/2 + 1; for(j=0; j < step; ++j) { x[0] += b[step_j + j] * k1[j*4 ]; } x[2] = 0.0; step_j = ((step-1)*step)/2 + 1; for(j=0; j < step; ++j) { x[2] += b[step_j + j] * k1[j*4+2]; } step4 = step * 4; k1[step4 ] = x[2] * h; k1[step4+2] = Ax[step]; } // end of Runge-Kutta steps for derivatives dx/dqp rkd[20]=c[0]*k1[0]+c[2]*k1[8 ]+c[3]*k1[12]+c[5]*k1[20]; rkd[21]=0.; rkd[22]=c[0]*k1[2]+c[2]*k1[10]+c[3]*k1[14]+c[5]*k1[22]; rkd[23]=0.; rkd[24] = 1.; // end of derivatives dx/dqp // // // other derivatives for(i = 0; i <= 19; ++i){ rkd[i] = 0.; } rkd[0] = 1.; rkd[6] = 1.; rkd[10] = h; rkd[12] = 1.; rkd[16] = h; rkd[18] = 1.; // // stepsize control // if ( fabs(p1_out[0]-p_out[0]) > error ){ if(fabs(h) > fStepMin *3.){ z2_out = z_in + 0.5 * h; rk5fast(z_in , p_in, error, z2_out, p2_out); rk5fast(z2_out, p2_out, error, z_out, p_out ); } else if(fabs(h) > fStepMin){ z2_out = z_in + 0.5 * h; rk4fast(z_in , p_in, z2_out, p2_out); rk4fast(z2_out, p2_out, z_out, p_out ); } } // end of stepsize control } // end of rk5fast //=========================================================================== //-- Author : A.Spiridonov 26.03.98 //=========================================================================== void CbmLitExtrapolator::rk5fast( // Without derivatives double& z_in , // z value for input parameters double* p_in, // input track parameters (x,y,tx,ty,Q/p) double& error, // acceptable errors in mm double& z_out, // z value for output parameters double* p_out )// output track parameters // // Fifth-order Runge-Kutta method with adaptive stepsize control // for solution of the equation of motion of a particle with // parameter qp = Q /P in the magnetic field B() // // | x | tx // | y | ty // d/dz = | tx| = ft * ( ty * ( B(3)+tx*b(1) ) - (1+tx**2)*B(2) ) // | ty| ft * (-tx * ( B(3)+ty+b(2) - (1+ty**2)*B(1) ) , // // where ft = C*qp*sqrt ( 1 + tx**2 + ty**2 ) . // // In the following for RK step : // x=x[0], y=x[1], tx=x[3], ty=x[4]. // { static double a[6] = {0.0, 0.2, 0.3, 0.6, 1.0, 7.0/8.0}; static double c[6] = {37.0/378.0, 0.0, 250.0/621.0, 125.0/594.0, 0.0, 512.0/1771.0}; static double c1[6] = {2825.0/27648.0, 0.0, 18575.0/48384.0, 13525.0/55296.0, 277.0/14336.0,1.0/4.0}; static double b[16] = {0.0, 1.0/5.0, 3.0/40.0, 9.0/40.0, 3.0/10.0, -9.0/10.0, 6.0/5.0, -11.0/54.0, 5.0/2.0, -70.0/27.0, 35.0/27.0, 1631.0/55296., 175.0/512.0, 575.0/13824.0, 44275.0/110592.0, 253.0/4096.0}; int step_j,step4; double qp,hC,h; double k[24],x0[4],x[4]; double tx,ty,tx2,ty2,txty,tx2ty2; double Ax[6],Ay[6]; double p1_out[5],z2_out,p2_out[5]; //---------------------------------------------------------------- qp = p_in[4]; h = z_out - z_in; hC = h * c_light; x0[0] = p_in[0]; x0[1] = p_in[1]; x0[2] = p_in[2]; x0[3] = p_in[3]; // // Runge-Kutta step // int i,j,step; for (step = 0; step < 6; ++step){ for(i=0; i < 4; ++i){ x[i] = x0[i]; step_j = ((step-1)*step)/2 + 1; for(j=0; j < step; ++j){ x[i] += b[step_j + j] * k[j*4+i]; } // i } // j //fPoint.setX( x[0] ); //fPoint.setY( x[1] ); //fPoint.setZ( z_in + a[step] * h ); //m_pIMF->fieldVector( fPoint, fB ); fPoint[0] = x[0]; fPoint[1] = x[1]; fPoint[2] = z_in + a[step] * h; fMagneticField->GetFieldValue(fPoint, fB); tx = x[2]; ty = x[3]; tx2 = tx * tx; ty2 = ty * ty; txty = tx * ty; tx2ty2 = sqrt( 1.0 + tx2 + ty2 ) * hC; //Ax[step] = ( txty*fB.x() + ty*fB.z() - ( 1.0 + tx2 )*fB.y() ) * tx2ty2; //Ay[step] = (-txty*fB.y() - tx*fB.z() + ( 1.0 + ty2 )*fB.x() ) * tx2ty2; Ax[step] = ( txty*fB[0] + ty*fB[2] - ( 1.0 + tx2 )*fB[1] ) * tx2ty2; Ay[step] = (-txty*fB[1] - tx*fB[2] + ( 1.0 + ty2 )*fB[0] ) * tx2ty2; step4 = step * 4; k[step4 ] = tx * h; k[step4+1] = ty * h; k[step4+2] = Ax[step] * qp; k[step4+3] = Ay[step] * qp; } // end of Runge-Kutta steps for(i=0; i < 4; ++i){ p_out[i]=x0[i]+c[0]*k[i]+c[2]*k[8+i]+c[3]*k[12+i]+c[5]*k[20+i]; } p_out[4]=p_in[4]; // // The embedded fourth-order formula for x // p1_out[0]=x0[0]+c1[0]*k[0]+c1[2]*k[8]+c1[3]*k[12]+c1[4]*k[16]+c1[5]*k[20]; // // stepsize control // if ( fabs(p1_out[0]-p_out[0]) > error ){ if(fabs(h) > fStepMin *3.) { z2_out = z_in + 0.5 * h; rk5fast(z_in , p_in, error, z2_out, p2_out); rk5fast(z2_out, p2_out, error, z_out, p_out ); } else if(fabs(h) > fStepMin ){ z2_out = z_in + 0.5 * h; rk4fast(z_in , p_in, z2_out, p2_out); rk4fast(z2_out, p2_out, z_out, p_out ); } } // end of stepsize control } // end of rk5fast without derivatives //=========================================================================== //-- Author : A.Spiridonov 09.04.98 //=========================================================================== void CbmLitExtrapolator::rk5numde( // Numerical Derivatives ( quite slow ) double& z_in , // z value for input parameters double* p_in, // input track parameters (x,y,tx,ty,Q/p) double& error, // acceptable errors in mm double& z_out, // z value for output parameters double* p_out, // output track parameters double* rkd, // derivatives d p_out[0-4] / d p_in[0-4] int& ierror) // = 0 ok, = 1 track curls // // Fifth-order Runge-Kutta method with adaptive stepsize control // for solution of the equation of motion of a particle with // parameter qp = Q /P in the magnetic field B() // // | x | tx // | y | ty // d/dz = | tx| = ft * ( ty * ( B(3)+tx*B(1) ) - (1+tx**2)*B(2) ) // | ty| ft * (-tx * ( B(3)+ty+B(2) - (1+ty**2)*B(1) ) , // // where ft = C*qp*sqrt ( 1 + tx**2 + ty**2 ) . // // Derivatives are calculated by "numerical differentiation" // of p_out as function of p_in // { double qp; static double delta[5] = {2.5, 2.5, 0.01, 0.01, 0.05}; double p1_out[5],p1_in[5],d_p[5]; //---------------------------------------------------------------- int i,j; qp = p_in[4]; ierror = (fabs(qp) > fQpCurls) ? 1 : 0; rk5fast(z_in , p_in, error, z_out, p_out); for(i=0; i < 4; ++i){ d_p[i] = delta[i]; } // loop i d_p[4] = qp * delta[4]; for(j=0; j < 5 ; ++j){ for( i=0; i < 5; ++i){ p1_in[i] = p_in[i]; } // i p1_in[j] += d_p[j]; rk5fast(z_in , p1_in, error, z_out, p1_out); for( i=0; i < 5; ++i){ rkd[5*j+i] = (p1_out[i]-p_out[i])/d_p[j]; } // i } // j } // end of rk5nuder //=========================================================================== //-- Author : A.Spiridonov 23.03.98 //=========================================================================== void CbmLitExtrapolator::rk4order( double& z_in , // z value for input parameters double* p_in, // input track parameters (x,y,tx,ty,Q/p) double& z_out, // z value for output parameters double* p_out, // output track parameters double* rkd, // derivatives d p_out(1-5) / d p_in(1-5) int& ierror) // = 0 ok, = 1 track curls // // Forth-order Runge-Kutta method for solution of the equation // of motion of a particle with parameter qp = Q /P // in the magnetic field B() // // | x | tx // | y | ty // d/dz = | tx| = ft * ( ty * ( B(3)+tx*b(1) ) - (1+tx**2)*B(2) ) // | ty| ft * (-tx * ( B(3)+ty+b(2) - (1+ty**2)*B(1) ) , // // where ft = C*qp*sqrt ( 1 + tx**2 + ty**2 ) . // // In the following for RK step : // x=x[0], y=x[1], tx=x[3], ty=x[4]. // { static double a[4] = {0.0, 0.5, 0.5, 1.0}; static double c[4] = {1.0/6.0, 1.0/3.0, 1.0/3.0, 1.0/6.0}; static double b[4] = {0.0, 0.5, 0.5, 1.0}; int step4; double k[16],x0[4],x[4],k1[16]; double Ax[4],Ay[4],Ax_tx[4],Ay_tx[4],Ax_ty[4],Ay_ty[4]; double qp = p_in[4]; ierror = (fabs(qp) > fQpCurls) ? 1 : 0; double h = z_out - z_in; double hC = h * c_light; x0[0] = p_in[0]; x0[1] = p_in[1]; x0[2] = p_in[2]; x0[3] = p_in[3]; // // Runge-Kutta step // int step; int i; for (step = 0; step < 4; ++step) { for(i=0; i < 4; ++i) { if(step == 0) { x[i] = x0[i]; } else { x[i] = x0[i] + b[step] * k[step*4-4+i]; } } //fPoint.setX( x[0] ); //fPoint.setY( x[1] ); //fPoint.setZ( z_in + a[step] * h ); //m_pIMF->fieldVector( fPoint, fB ); fPoint[0] = x[0]; fPoint[1] = x[1]; fPoint[2] = z_in + a[step] * h; fMagneticField->GetFieldValue(fPoint, fB); fB[0] = fB[0] * 0.10; fB[1] = fB[1] * 0.10; fB[2] = fB[2] * 0.10; double tx = x[2]; double ty = x[3]; double tx2 = tx * tx; double ty2 = ty * ty; double txty = tx * ty; double tx2ty21= 1.0 + tx2 + ty2; double I_tx2ty21 = 1.0 / tx2ty21 * qp; double tx2ty2 = sqrt(tx2ty21 ) ; // double I_tx2ty2 = qp * hC / tx2ty2 ; unsused ??? tx2ty2 *= hC; double tx2ty2qp = tx2ty2 * qp; //Ax[step] = ( txty*fB.x() + ty*fB.z() - ( 1.0 + tx2 )*fB.y() ) * tx2ty2; //Ay[step] = (-txty*fB.y() - tx*fB.z() + ( 1.0 + ty2 )*fB.x() ) * tx2ty2; Ax[step] = ( txty*fB[0] + ty*fB[2] - ( 1.0 + tx2 )*fB[1] ) * tx2ty2; Ay[step] = (-txty*fB[1] - tx*fB[2] + ( 1.0 + ty2 )*fB[0] ) * tx2ty2; //Ax_tx[step] = Ax[step]*tx*I_tx2ty21 + (ty*fB.x()-2.0*tx*fB.y())*tx2ty2qp; //Ax_ty[step] = Ax[step]*ty*I_tx2ty21 + (tx*fB.x()+fB.z())*tx2ty2qp; //Ay_tx[step] = Ay[step]*tx*I_tx2ty21 + (-ty*fB.y()-fB.z())*tx2ty2qp; //Ay_ty[step] = Ay[step]*ty*I_tx2ty21 + (-tx*fB.y()+2.0*ty*fB.x())*tx2ty2qp; Ax_tx[step] = Ax[step]*tx*I_tx2ty21 + (ty*fB[0]-2.0*tx*fB[1])*tx2ty2qp; Ax_ty[step] = Ax[step]*ty*I_tx2ty21 + (tx*fB[0]+fB[2])*tx2ty2qp; Ay_tx[step] = Ay[step]*tx*I_tx2ty21 + (-ty*fB[1]-fB[2])*tx2ty2qp; Ay_ty[step] = Ay[step]*ty*I_tx2ty21 + (-tx*fB[1]+2.0*ty*fB[0])*tx2ty2qp; step4 = step * 4; k[step4 ] = tx * h; k[step4+1] = ty * h; k[step4+2] = Ax[step] * qp; k[step4+3] = Ay[step] * qp; } // end of Runge-Kutta steps for(i=0; i < 4; ++i) { p_out[i]=x0[i]+c[0]*k[i]+c[1]*k[4+i]+c[2]*k[8+i]+c[3]*k[12+i]; } p_out[4]=p_in[4]; // // Derivatives dx/dqp // x0[0] = 0.0; x0[1] = 0.0; x0[2] = 0.0; x0[3] = 0.0; // // Runge-Kutta step for derivatives dx/dqp for (step = 0; step < 4; ++step) { for(i=0; i < 4; ++i) { if(step == 0) { x[i] = x0[i]; } else { x[i] = x0[i] + b[step] * k1[step*4-4+i]; } } step4 = step * 4; k1[step4 ] = x[2] * h; k1[step4+1] = x[3] * h; k1[step4+2] = Ax[step] + Ax_tx[step] * x[2] + Ax_ty[step] * x[3]; k1[step4+3] = Ay[step] + Ay_tx[step] * x[2] + Ay_ty[step] * x[3]; } // end of Runge-Kutta steps for derivatives dx/dqp for (i = 0; i < 4; ++i ) { rkd[20+i]=x0[i]+c[0]*k1[i]+c[1]*k1[4+i]+c[2]*k1[8+i]+c[3]*k1[12+i]; } rkd[24] = 1.; // // end of derivatives dx/dqp // // Derivatives dx/tx // x0[0] = 0.0; x0[1] = 0.0; x0[2] = 1.0; x0[3] = 0.0; // // Runge-Kutta step for derivatives dx/dtx // for (step = 0; step < 4; ++step) { for(i=0; i < 4; ++i) { if(step == 0) { x[i] = x0[i]; } else if ( i!=2 ){ x[i] = x0[i] + b[step] * k1[step*4-4+i]; } } step4 = step * 4; k1[step4 ] = x[2] * h; k1[step4+1] = x[3] * h; // k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3]; k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3]; } // end of Runge-Kutta steps for derivatives dx/dtx for (i = 0; i < 4; ++i ) { if(i != 2) { rkd[10+i]=x0[i]+c[0]*k1[i]+c[1]*k1[4+i]+c[2]*k1[8+i]+c[3]*k1[12+i]; } } // end of derivatives dx/dtx rkd[12] = 1.0; rkd[14] = 0.0; // Derivatives dx/ty // x0[0] = 0.0; x0[1] = 0.0; x0[2] = 0.0; x0[3] = 1.0; // // Runge-Kutta step for derivatives dx/dty // for (step = 0; step < 4; ++step) { for(i=0; i < 4; ++i) { if(step == 0) { x[i] = x0[i]; // ty fixed } else if(i!=3) { x[i] = x0[i] + b[step] * k1[step*4-4+i]; } } step4 = step * 4; k1[step4 ] = x[2] * h; k1[step4+1] = x[3] * h; k1[step4+2] = Ax_tx[step] * x[2] + Ax_ty[step] * x[3]; // k1[step4+3] = Ay_tx[step] * x[2] + Ay_ty[step] * x[3]; } // end of Runge-Kutta steps for derivatives dx/dty for (i = 0; i < 3; ++i ) { rkd[15+i]=x0[i]+c[0]*k1[i]+c[1]*k1[4+i]+c[2]*k1[8+i]+c[3]*k1[12+i]; } // end of derivatives dx/dty rkd[18] = 1.; rkd[19] = 0.; // // derivatives dx/dx and dx/dy for(i = 0; i < 10; ++i){ rkd[i] = 0.;} rkd[0] = 1.; rkd[6] = 1.; } // end of rk4order //=========================================================================== //-- Author : A.Spiridonov 27.03.98 //=========================================================================== void CbmLitExtrapolator::rk4fast( double& z_in , // z value for input parameters double* p_in, // input track parameters (x,y,tx,ty,Q/p) double& z_out, // z value for output parameters double* p_out, // output track parameters double* rkd, // derivatives d p_out[0-4] / d p_in[0-4] int& ierror) // = 0 ok, = 1 track curls // // Forth-order Runge-Kutta method for solution of the equation // of motion of a particle with parameter qp = Q /P // in the magnetic field B() // // | x | tx // | y | ty // d/dz = | tx| = ft * ( ty * ( B(3)+tx*b(1) ) - (1+tx**2)*B(2) ) // | ty| ft * (-tx * ( B(3)+ty+b(2) - (1+ty**2)*B(1) ) , // // where ft = C*qp*sqrt ( 1 + tx**2 + ty**2 ) . // // In the following for RK step : // x=x[0], y=x[1], tx=x[3], ty=x[4]. // { static double a[4] = {0.0, 0.5, 0.5, 1.0}; static double c[4] = {1.0/6.0, 1.0/3.0, 1.0/3.0, 1.0/6.0}; static double b[4] = {0.0, 0.5, 0.5, 1.0}; int step4; double qp,hC,h; double k[16],x0[4],x[4],k1[16]; double tx,ty,tx2,ty2,txty,tx2ty2; double Ax[4],Ay[4]; //---------------------------------------------------------------- qp = p_in[4]; ierror = (fabs(qp) > fQpCurls) ? 1 : 0; h = z_out - z_in; hC = h * c_light; x0[0] = p_in[0]; x0[1] = p_in[1]; x0[2] = p_in[2]; x0[3] = p_in[3]; // // Runge-Kutta step // int step, i; for (step = 0; step < 4; ++step) { for(i=0; i < 4; ++i) { if(step == 0) { x[i] = x0[i]; } else { x[i] = x0[i] + b[step] * k[step*4-4+i]; } } //fPoint.setX( x[0] ); //fPoint.setY( x[1] ); //fPoint.setZ( z_in + a[step] * h ); //m_pIMF->fieldVector( fPoint, fB ); fPoint[0] = x[0]; fPoint[1] = x[1]; fPoint[2] = z_in + a[step] * h; fMagneticField->GetFieldValue(fPoint, fB); tx = x[2]; ty = x[3]; tx2 = tx * tx; ty2 = ty * ty; txty = tx * ty; tx2ty2 = sqrt( 1.0 + tx2 + ty2 ) * hC; //Ax[step] = ( txty*fB.x() + ty*fB.z() - ( 1.0 + tx2 )*fB.y() ) * tx2ty2; //Ay[step] = (-txty*fB.y() - tx*fB.z() + ( 1.0 + ty2 )*fB.x() ) * tx2ty2; Ax[step] = ( txty*fB[0] + ty*fB[2] - ( 1.0 + tx2 )*fB[1] ) * tx2ty2; Ay[step] = (-txty*fB[1] - tx*fB[2] + ( 1.0 + ty2 )*fB[0] ) * tx2ty2; step4 = step * 4; k[step4 ] = tx * h; k[step4+1] = ty * h; k[step4+2] = Ax[step] * qp; k[step4+3] = Ay[step] * qp; } // end of Runge-Kutta steps for(i=0; i < 4; ++i) { p_out[i]=x0[i]+c[0]*k[i]+c[1]*k[4+i]+c[2]*k[8+i]+c[3]*k[12+i]; } p_out[4]=p_in[4]; // // Derivatives dx/dqp and dtx/dqp // // Runge-Kutta step for derivatives dx/dqp for (step = 0; step < 4; ++step) { for(i=0; i < 4; ++i) { if(step == 0) { x[0] = 0.0; x[2] = 0.0; } else { x[0] = b[step] * k1[step*4-4]; x[2] = b[step] * k1[step*4-2]; } } step4 = step * 4; k1[step4 ] = x[2] * h; k1[step4+2] = Ax[step] ; } // end of Runge-Kutta steps for derivatives dx/dqp rkd[20] = c[0]*k1[0]+c[1]*k1[4]+c[2]*k1[8]+c[3]*k1[12]; rkd[21] = 0.; rkd[22] = c[0]*k1[2]+c[1]*k1[6]+c[2]*k1[10]+c[3]*k1[14]; rkd[23] = 0.; rkd[24] = 1.; // end of derivatives dx/dqp // // other derivatives for(i = 0; i <= 19; ++i){ rkd[i] = 0.;} rkd[0] = 1.; rkd[6] = 1.; rkd[10] = h; rkd[12] = 1.; rkd[16] = h; rkd[18] = 1.; } // end of rk4fast //=========================================================================== //-- Author : A.Spiridonov 27.03.98 //=========================================================================== void CbmLitExtrapolator::rk4fast( // Without derivatives double& z_in , // z value for input parameters double* p_in, // input track parameters (x,y,tx,ty,Q/p) double& z_out, // z value for output parameters double* p_out) // output track parameters // // Forth-order Runge-Kutta method for solution of the equation // of motion of a particle with parameter qp = Q /P // in the magnetic field B() // // | x | tx // | y | ty // d/dz = | tx| = ft * ( ty * ( B(3)+tx*b(1) ) - (1+tx**2)*B(2) ) // | ty| ft * (-tx * ( B(3)+ty+b(2) - (1+ty**2)*B(1) ) , // // where ft = C*qp*sqrt ( 1 + tx**2 + ty**2 ) . // // In the following for RK step : // x=x[0], y=x[1], tx=x[3], ty=x[4]. // { static double a[4] = {0.0, 0.5, 0.5, 1.0}; static double c[4] = {1.0/6.0, 1.0/3.0, 1.0/3.0, 1.0/6.0}; static double b[4] = {0.0, 0.5, 0.5, 1.}; int step4; double qp,hC,h; double k[16],x0[4],x[4]; double tx,ty,tx2,ty2,txty,tx2ty2; double Ax[4],Ay[4]; //---------------------------------------------------------------- qp = p_in[4]; h = z_out - z_in; hC = h * c_light; x0[0] = p_in[0]; x0[1] = p_in[1]; x0[2] = p_in[2]; x0[3] = p_in[3]; // // Runge-Kutta step // int i, step; for (step = 0; step < 4; ++step) { for(i=0; i < 4; ++i) { if(step == 0) { x[i] = x0[i]; } else { x[i] = x0[i] + b[step] * k[step*4-4+i]; } } //fPoint.setX( x[0] ); //fPoint.setY( x[1] ); //fPoint.setZ( z_in + a[step] * h ); //m_pIMF->fieldVector( fPoint, fB ); fPoint[0] = x[0]; fPoint[1] = x[1]; fPoint[2] = z_in + a[step] * h; fMagneticField->GetFieldValue(fPoint, fB); tx = x[2]; ty = x[3]; tx2 = tx * tx; ty2 = ty * ty; txty = tx * ty; tx2ty2 = sqrt( 1.0 + tx2 + ty2 ) * hC; //Ax[step] = ( txty*fB.x() + ty*fB.z() - ( 1.0 + tx2 )*fB.y() ) * tx2ty2; //Ay[step] = (-txty*fB.y() - tx*fB.z() + ( 1.0 + ty2 )*fB.x() ) * tx2ty2; Ax[step] = ( txty*fB[0] + ty*fB[2] - ( 1.0 + tx2 )*fB[1] ) * tx2ty2; Ay[step] = (-txty*fB[1] - tx*fB[2] + ( 1.0 + ty2 )*fB[0] ) * tx2ty2; step4 = step * 4; k[step4 ] = tx * h; k[step4+1] = ty * h; k[step4+2] = Ax[step] * qp; k[step4+3] = Ay[step] * qp; } // end of Runge-Kutta steps for(i=0; i < 4; ++i) { p_out[i]=x0[i]+c[0]*k[i]+c[1]*k[4+i]+c[2]*k[8+i]+c[3]*k[12+i]; } p_out[4]=p_in[4]; } // end of rk4fast without derivatives ClassImp(CbmLitExtrapolator)