void HHV4Vector::Rotate(const HHV4Vector & q) { Double_t ex, ey, ez, en; Double_t a, b, c, d, px1, py1, pz1, px2, py2, pz2, px3, py3, pz3; px1 = Px(); py1 = Py(); pz1 = Pz(); ex = q.Px(); ey = q.Py(); ez = q.Pz(); en = sqrt(ex * ex + ey * ey + ez * ez); ex = ex / en; ey = ey / en; ez = ez / en; // cout << "Rotate e: " << ex << " " << ey << " " << ez << endl; a = ez; b = sqrt(1. - a * a); c = ex / b; d = -ey / b; px2 = a * px1 + b * pz1; // rotation around y-axis py2 = py1; pz2 = -b * px1 + a * pz1; // cout << "Rotate p2: " << px2 << " " << py2 << " " << pz2 << endl; px3 = c * px2 + d * py2; // rotation around z-axis py3 = -d * px2 + c * py2; pz3 = pz2; // cout << "Rotate p3: " << px3 << " " << py3 << " " << pz3 << endl; SetPxPyPzE(px3, py3, pz3, E()); }
void HHV4Vector::Boost(Double_t bx, Double_t by, Double_t bz) { //Boost this Lorentz vector //cout << "Boost: beta " << bx << " " << by << " " << bz << " " << bx**2+by**2+bz**2 << endl; // cout << "Boost: P " << Px()<<" " << Py()<<" " << Pz()<<" " << endl; Double_t b2 = bx * bx + by * by + bz * bz; Double_t gamma = 1.0 / sqrt(1.0 - b2); Double_t bp = bx * Px() + by * Py() + bz * Pz(); Double_t gamma2 = b2 > 0 ? (gamma - 1.0) / b2 : 0.0; Double_t px, py, pz; // cout << "Boost: b2,g,bp,g2: " << b2 << " " << gamma << " " << bp << " " << gamma2 << endl; px = Px() + gamma2 * bp * bx + gamma * bx * E(); py = Py() + gamma2 * bp * by + gamma * by * E(); pz = Pz() + gamma2 * bp * bz + gamma * bz * E(); m_e = gamma * (E() + bp); m_phi = atan2(py, px); m_eta = -log(tan(0.5 * acos(pz / sqrt(px * px + py * py + pz * pz)))); // cout << "Boost: E " << E << " px " << px<< " py " << py<< " pz " << pz // << " Phi " <<Phi<<" Eta " <<Eta << endl; }
void PreconditionerBlockMS<space_type>::initAMS( void ) { M_grad = Grad( _domainSpace=M_Qh, _imageSpace=M_Vh); // This preconditioner is linked to that backend : the backend will // automatically use the preconditioner. auto prec = preconditioner(_pc=pcTypeConvertStrToEnum(soption(M_prefix_11+".pc-type")), _backend=backend(_name=M_prefix_11), _prefix=M_prefix_11, _matrix=M_11 ); prec->setMatrix(M_11); prec->attachAuxiliarySparseMatrix("G",M_grad.matPtr()); if(boption(M_prefix_11+".useEdge")) { LOG(INFO) << "[ AMS ] : using SetConstantEdgeVector \n"; ozz.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(1),cst(0),cst(0))); zoz.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(0),cst(1),cst(0))); zzo.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(0),cst(0),cst(1))); *M_ozz = ozz; M_ozz->close(); *M_zoz = zoz; M_zoz->close(); *M_zzo = zzo; M_zzo->close(); prec->attachAuxiliaryVector("Px",M_ozz); prec->attachAuxiliaryVector("Py",M_zoz); prec->attachAuxiliaryVector("Pz",M_zzo); } else { LOG(INFO) << "[ AMS ] : using SetCoordinates \n"; X.on(_range=elements(M_Vh->mesh()),_expr=Px()); Y.on(_range=elements(M_Vh->mesh()),_expr=Py()); Z.on(_range=elements(M_Vh->mesh()),_expr=Pz()); *M_X = X; M_X->close(); *M_Y = Y; M_Y->close(); *M_Z = Z; M_Z->close(); prec->attachAuxiliaryVector("X",M_X); prec->attachAuxiliaryVector("Y",M_Y); prec->attachAuxiliaryVector("Z",M_Z); } }
void HHV4Vector::PrintEPxPyPzM(int pos) const { if (pos >= 0) { PSTools::coutf(4, pos); std::cout << " "; } PSTools::coutf(10, m_name); PSTools::coutf(4, ID()); PSTools::coutf(9, 2, E()); PSTools::coutf(9, 2, Px()); PSTools::coutf(9, 2, Py()); PSTools::coutf(9, 2, Pz()); PSTools::coutf(9, 2, M()); PSTools::coutf(5, Mother()); for (int i = 0; i < nDaughter(); i++) { PSTools::coutf(4, Daughter(i)); } std::cout << std::endl; // cout << Name << " " << ID << " "; // Printf(" (EPxPyPzM)= %8.2f %8.2f %8.2f %8.2f %8.2f", E(), Px(), Py(), // Pz(), M()); }
void FifthCKF::FifthCKFfiltering(double *z, UINT m_length) { CMatrix Pplus(4, 4); Pplus = glb.Pplus; CMatrix xhat(4, 1); xhat = glb.xhat; StoreData *fifthckf = new StoreData(xhat(1, 1), xhat(2, 1), xhat(3, 1), xhat(4, 1)); X.Add(fifthckf); CMatrix Shat(dim, dim); CMatrix rjpoint1(dim, 1); CMatrix rjpoint2(dim, cpoints); CMatrix rjpoint3(dim, hcpoints - cpoints - 1); CMatrix Xminus1(dim, 1); CMatrix Xminus2(dim, cpoints); CMatrix Repmat2(dim, cpoints); CMatrix Xminus3(dim, hcpoints - cpoints - 1); CMatrix Repmat3(dim, hcpoints - cpoints - 1); CMatrix jtemp(dim, 1); CMatrix Zhat1(1, 1); CMatrix Zhat2(1, cpoints); CMatrix RepmatZ2(1, cpoints); CMatrix Zhat3(1, hcpoints - cpoints - 1); CMatrix RepmatZ3(1, hcpoints - cpoints - 1); CMatrix W(dim, 1); CMatrix Pz(1, 1); CMatrix Xtemp(dim, 1); CMatrix Pxz(4, 1); int num = 1;//第0个未知存放的是占位符0,并不是真正的测量值,因此应当从下标1开始取测量值 for (UINT count = TimeInterval; count <= m_length; count += TimeInterval) { //Time Update //Evaluate the Cholesky factor Shat = Pplus.Cholesky(); //Evaluate the cubature points and the propagated cubature points//Estimate the predicted state CMatrix CX(4, 1); for (int i = 1; i <= hcpoints; ++i) { for (int j = 1; j <= dim; ++j) { jtemp(j, 1) = glb.kesi_FifthCKF(j, i); } jtemp = Shat * jtemp + xhat; Xtemp = glb.fai * jtemp; if ( 1 == i) { for (int j = 1; j <= dim; ++j) { Xminus1(j, 1) = Xtemp(j, 1); } CX = CX + c5_w2 * Xtemp; } else if (i > 1 && i <= cpoints + 1)//此编程风格会提高代码的冗余度,但提高了程序的运行效率,下同 { for (int j = 1; j <= dim; ++j) { Xminus2(j, i - 1) = Xtemp(j, 1); } CX = CX + c5_w1 * Xtemp; } else { for (int j = 1; j <= dim; ++j) { Xminus3(j, i - cpoints - 1) = Xtemp(j, 1); } CX = CX + c5_w4 * Xtemp; } } //xhat = CX; //Estimate the predicted error covariance for (int i = 1; i <= cpoints; ++i) { #pragma omp parallel for for (int j = 1; j <= dim; ++j) { Repmat2(j, i) = CX(j, 1); } } for (int i = 1; i <= hcpoints - cpoints - 1; ++i) { #pragma omp parallel for for(int j = 1; j <= dim; ++j) { Repmat3(j, i) = CX(j, 1); } } Pplus = c5_w2 * ((Xminus1 - CX) * (~(Xminus1 - CX))) + c5_w1 * (Xminus2 - Repmat2) * (~(Xminus2 - Repmat2)) + c5_w4 * (Xminus3 - Repmat3) * (~(Xminus3 - Repmat3)) + glb.gama * glb.Im * (~glb.gama); //Measurement Update //Evaluate the Cholesky factor Shat = Pplus.Cholesky(); //Evaluate the cubature points and the propagated cubature points//Estimate the predicted measurement CMatrix Z(1, 1); for (int i = 1; i <= hcpoints; ++i) { for (int j = 1; j <= dim; ++j) { jtemp(j, 1) = glb.kesi_FifthCKF(j, i); } jtemp = Shat * jtemp + CX; if (1 == i) { Zhat1(1, 1) = atan(jtemp(3, 1) / jtemp(1, 1)); Z(1, 1) = Z(1, 1) + c5_w2 * Zhat1(1, 1); #pragma omp parallel for for (int j = 1; j<= dim; ++j) { rjpoint1(j, 1) = jtemp(j, 1); } } else if (i > 1 && i <= cpoints + 1) { Zhat2(1, i - 1) = atan(jtemp(3, 1) / jtemp(1, 1)); Z(1, 1) = Z(1, 1) + c5_w1 * Zhat2(1, i - 1); #pragma omp parallel for for (int j = 1; j <= dim; ++j) { rjpoint2(j, i - 1) = jtemp(j, 1); } } else { Zhat3(1, i - cpoints - 1) = atan(jtemp(3, 1) / jtemp(1, 1)); Z(1, 1) = Z(1, 1) + c5_w4 * Zhat3(1, i - cpoints - 1); #pragma omp parallel for for (int j = 1; j <= dim; ++j) { rjpoint3(j, i - cpoints - 1) = jtemp(j, 1); } } } //Estimate the innovation covariance matrix Pz(1, 1) = Rn;//For saving memory #pragma omp parallel for for (int i = 1; i <= cpoints; ++i) { RepmatZ2(1, i) = Z(1, 1); } #pragma omp parallel for for (int i = 1; i <= hcpoints - cpoints - 1; ++i) { RepmatZ3(1, i) = Z(1, 1); } Pz = c5_w2 * (Zhat1 - Z) * (~(Zhat1 - Z)) + c5_w1 * (Zhat2 - RepmatZ2)* (~(Zhat2 - RepmatZ2)) + c5_w4 * (Zhat3 - RepmatZ3) * (~(Zhat3 - RepmatZ3)) + Pz; //Estimate the cross-covariance matrix Pxz = c5_w2 * (rjpoint1 - CX) * (~(Zhat1 - Z)) + c5_w1 *(rjpoint2 - Repmat2) * (~(Zhat2 - RepmatZ2)) + c5_w4 * (rjpoint3 - Repmat3) * (~(Zhat3 - RepmatZ3)); //Estimate the Kalman gain W = ((double)1 / Pz(1, 1)) * Pxz; //Estimate the updated state //Znum(1, 1) = z[num]; xhat = CX + W * (z[num] - Z(1, 1)); ++num; Pplus = Pplus - Pz(1, 1) * W * (~W); StoreData *fifthckf = new StoreData(xhat(1, 1), xhat(2, 1), xhat(3, 1), xhat(4, 1)); X.Add(fifthckf); } }
Vector3< Momentum > PseudoJet::Spatial() const { return {Px(), Py(), Pz()}; }
boca::LorentzVector< Momentum > PseudoJet::LorentzVector() const { return {Px(), Py(), Pz(), Energy()}; }
PreconditionerBlockMS<space_type>::PreconditionerBlockMS(space_ptrtype Xh, // (u)x(p) ModelProperties model, // model std::string const& p, // prefix sparse_matrix_ptrtype AA ) // The matrix : M_backend(backend()), // the backend associated to the PC M_Xh( Xh ), M_Vh( Xh->template functionSpace<0>() ), // Potential M_Qh( Xh->template functionSpace<1>() ), // Lagrange M_Vh_indices( M_Vh->nLocalDofWithGhost() ), M_Qh_indices( M_Qh->nLocalDofWithGhost() ), M_uin( M_backend->newVector( M_Vh ) ), M_uout( M_backend->newVector( M_Vh ) ), M_pin( M_backend->newVector( M_Qh ) ), M_pout( M_backend->newVector( M_Qh ) ), U( M_Xh, "U" ), M_mass(M_backend->newMatrix(M_Vh,M_Vh)), M_L(M_backend->newMatrix(M_Qh,M_Qh)), M_er( 1. ), M_model( model ), M_prefix( p ), M_prefix_11( p+".11" ), M_prefix_22( p+".22" ), u(M_Vh, "u"), ozz(M_Vh, "ozz"), zoz(M_Vh, "zoz"), zzo(M_Vh, "zzo"), M_ozz(M_backend->newVector( M_Vh )), M_zoz(M_backend->newVector( M_Vh )), M_zzo(M_backend->newVector( M_Vh )), X(M_Qh, "X"), Y(M_Qh, "Y"), Z(M_Qh, "Z"), M_X(M_backend->newVector( M_Qh )), M_Y(M_backend->newVector( M_Qh )), M_Z(M_backend->newVector( M_Qh )), phi(M_Qh, "phi") { tic(); LOG(INFO) << "[PreconditionerBlockMS] setup starts"; this->setMatrix( AA ); this->setName(M_prefix); /* Indices are need to extract sub matrix */ std::iota( M_Vh_indices.begin(), M_Vh_indices.end(), 0 ); std::iota( M_Qh_indices.begin(), M_Qh_indices.end(), M_Vh->nLocalDofWithGhost() ); M_11 = AA->createSubMatrix( M_Vh_indices, M_Vh_indices, true, true); /* Boundary conditions */ BoundaryConditions M_bc = M_model.boundaryConditions(); map_vector_field<FEELPP_DIM,1,2> m_dirichlet_u { M_bc.getVectorFields<FEELPP_DIM> ( "u", "Dirichlet" ) }; map_scalar_field<2> m_dirichlet_p { M_bc.getScalarFields<2> ( "phi", "Dirichlet" ) }; /* Compute the mass matrix (needed in first block, constant) */ auto f2A = form2(_test=M_Vh, _trial=M_Vh, _matrix=M_mass); auto f1A = form1(_test=M_Vh); f2A = integrate(_range=elements(M_Vh->mesh()), _expr=inner(idt(u),id(u))); // M for(auto const & it : m_dirichlet_u ) { LOG(INFO) << "Applying " << it.second << " on " << it.first << " for "<<M_prefix_11<<"\n"; f2A += on(_range=markedfaces(M_Vh->mesh(),it.first), _expr=it.second,_rhs=f1A, _element=u, _type="elimination_symmetric"); } /* Compute the L (= er * grad grad) matrix (the second block) */ auto f2L = form2(_test=M_Qh,_trial=M_Qh, _matrix=M_L); for(auto it : M_model.materials() ) { f2L += integrate(_range=markedelements(M_Qh->mesh(),marker(it)), _expr=M_er*inner(gradt(phi), grad(phi))); } auto f1LQ = form1(_test=M_Qh); for(auto const & it : m_dirichlet_p) { LOG(INFO) << "Applying " << it.second << " on " << it.first << " for "<<M_prefix_22<<"\n"; f2L += on(_range=markedfaces(M_Qh->mesh(),it.first),_element=phi, _expr=it.second, _rhs=f1LQ, _type="elimination_symmetric"); } if(soption(_name="pc-type", _prefix=M_prefix_11) == "ams") #if FEELPP_DIM == 3 { M_grad = Grad( _domainSpace=M_Qh, _imageSpace=M_Vh); // This preconditioner is linked to that backend : the backend will // automatically use the preconditioner. auto prec = preconditioner(_pc=pcTypeConvertStrToEnum(soption(M_prefix_11+".pc-type")), _backend=backend(_name=M_prefix_11), _prefix=M_prefix_11, _matrix=M_11 ); prec->setMatrix(M_11); prec->attachAuxiliarySparseMatrix("G",M_grad.matPtr()); if(boption(M_prefix_11+".useEdge")) { LOG(INFO) << "[ AMS ] : using SetConstantEdgeVector \n"; ozz.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(1),cst(0),cst(0))); zoz.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(0),cst(1),cst(0))); zzo.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(0),cst(0),cst(1))); *M_ozz = ozz; M_ozz->close(); *M_zoz = zoz; M_zoz->close(); *M_zzo = zzo; M_zzo->close(); prec->attachAuxiliaryVector("Px",M_ozz); prec->attachAuxiliaryVector("Py",M_zoz); prec->attachAuxiliaryVector("Pz",M_zzo); } else { LOG(INFO) << "[ AMS ] : using SetCoordinates \n"; X.on(_range=elements(M_Vh->mesh()),_expr=Px()); Y.on(_range=elements(M_Vh->mesh()),_expr=Py()); Z.on(_range=elements(M_Vh->mesh()),_expr=Pz()); *M_X = X; M_X->close(); *M_Y = Y; M_Y->close(); *M_Z = Z; M_Z->close(); prec->attachAuxiliaryVector("X",M_X); prec->attachAuxiliaryVector("Y",M_Y); prec->attachAuxiliaryVector("Z",M_Z); } } #else std::cerr << "ams preconditioner is not interfaced in two dimensions\n"; #endif toc( "[PreconditionerBlockMS] setup done ", FLAGS_v > 0 ); }