C++ Particles类代码示例

int main(int argc, char ** argv)
{
	invalid_argument er(
		"Bond angle correlation function\n"
#ifdef use_periodic
		"Syntax : periodic_g6 [path]filename NbOfBins Nb dx dy dz [mode=0 [l=6]]\n"
#else
		"Syntax : g6 [path]filename radius NbOfBins range [mode=0 [l=6 [zmin zmax]]]\n"
		" range is in diameter unit\n"
#endif
		" mode\t0 raw boo\n"
		"     \t1 coarse grained boo\n"
		);

    try
    {

		if(argc<3) throw er;

		const string filename(argv[1]);
		const string inputPath = filename.substr(0,filename.find_last_of("."));

		vector<size_t> inside;

		//construct the particle container out of the datafile
#ifdef use_periodic
		if(argc<7) throw er;
		const size_t Nbins = atoi(argv[2]);
		const size_t Nb = atoi(argv[3]);
		double nbDiameterCutOff =atof(argv[4]);
		BoundingBox b;
		for(size_t d=0;d<3;++d)
		{
			b.edges[d].first=0.0;
			b.edges[d].second = atof(argv[4+d]);
			if(nbDiameterCutOff>b.edges[d].second)
                nbDiameterCutOff=b.edges[d].second;
		}
		nbDiameterCutOff /=4.0;
		const bool mode = (argc<8)?0:atoi(argv[7]);
		const size_t l = (argc<9)?6:atoi(argv[8]);
		PeriodicParticles Centers(Nb,b,filename,1.0);
		cout << "With periodic boundary conditions"<<endl;
#else
        const double radius = atof(argv[2]),
				nbDiameterCutOff = atof(argv[4]);
        const size_t Nbins = atoi(argv[3]);
        const bool mode = (argc<6)?0:atoi(argv[5]);
        const size_t l = (argc<7)?6:atoi(argv[6]);
		Particles Centers(filename,radius);
#endif
		cout << Centers.size() << " particles ... ";

		//read q6m
		vector<BooData> rawBoo, allBoo;
		Centers.load_qlm(inputPath+".qlm", rawBoo);
		if(mode)
		{
		    Centers.makeNgbList(loadBonds(inputPath+".bonds"));
		    //select all particles who's all neighbours' qlms are not null
		    vector<size_t> second_inside;
		    second_inside.reserve(Centers.size());
		    for(size_t p=0; p<Centers.getNgbList().size(); ++p)
		    {
		        bool hasNullNgb = false;
		        for(size_t n=0; n<Centers.getNgbList()[p].size(); ++n)
                    hasNullNgb = (hasNullNgb || rawBoo[Centers.getNgbList()[p][n]].isnull());
                if(!hasNullNgb)
                    second_inside.push_back(p);
		    }
		    cout<< second_inside.size()<< " have coarse grained qlms ... ";
		    Centers.getCgBOOs(second_inside, rawBoo, allBoo);
		}
		else
            allBoo=rawBoo;

#ifndef use_periodic
        vector<size_t> slab;
        slab.reserve(Centers.size());
        //Case where we ask to discard all points out of the interval [zmin, zmax]
        if(argc>8)
        {
            const double zmin = atof(argv[7]),
                        zmax = atof(argv[8]);
            //look for the particles that are in the slab and have non null qlm
            for(size_t p=0; p<Centers.size(); ++p)
                if(Centers[p][2]>zmin && Centers[p][2]<zmax && !allBoo[p].isnull())
                    slab.push_back(p);
        }
        else
            for(size_t p=0; p<Centers.size(); ++p)
                if(!allBoo[p].isnull())
                    slab.push_back(p);

        //reduce the centers list
        Particles cen;
        cen.radius = Centers.radius;
        cen.reserve(slab.size());
        for(size_t p=0; p<slab.size(); ++p)
            cen.push_back(Centers[slab[p]]);
//.........这里部分代码省略.........

void WaterSimApp::resetParticles()
{
	particles->clear_particles();
}

algebra::Vector3Ds CoarseCC::calc_derivatives(const DensityMap *em_map,
                                              const DensityMap *model_map,
                                              const Particles &model_ps,
                                              const FloatKey &w_key,
                                              KernelParameters *kernel_params,
                                              const float &scalefac,
                                              const algebra::Vector3Ds &dv) {
  algebra::Vector3Ds dv_out;
  dv_out.insert(dv_out.end(), dv.size(), algebra::Vector3D(0., 0., 0.));
  double tdvx = 0., tdvy = 0., tdvz = 0., tmp, rsq;
  int iminx, iminy, iminz, imaxx, imaxy, imaxz;

  const DensityHeader *model_header = model_map->get_header();
  const DensityHeader *em_header = em_map->get_header();
  const float *x_loc = em_map->get_x_loc();
  const float *y_loc = em_map->get_y_loc();
  const float *z_loc = em_map->get_z_loc();
  IMP_INTERNAL_CHECK(model_ps.size() == dv.size(),
                     "input derivatives array size does not match "
                         << "the number of particles in the model map\n");
  core::XYZRs model_xyzr = core::XYZRs(model_ps);
  // this would go away once we have XYZRW decorator
  const emreal *em_data = em_map->get_data();
  float lim = kernel_params->get_lim();
  long ivox;
  // validate that the model and em maps are not empty
  IMP_USAGE_CHECK(em_header->rms >= EPS,
                  "EM map is empty ! em_header->rms = " << em_header->rms);
  // it may be that CG takes a too large step, which causes the particles
  // to go outside of the density
  // if (model_header->rms <= EPS){
  // IMP_WARN("Model map is empty ! model_header->rms = " << model_header->rms
  //           <<" derivatives are not calculated. the model centroid is : " <<
  //           core::get_centroid(core::XYZs(model_ps))<<
  //           " the map centroid is " << em_map->get_centroid()<<
  //                 "number of particles in model:"<<model_ps.size()
  //<<std::endl);
  // return;
  // }
  // Compute the derivatives
  int nx = em_header->get_nx();
  int ny = em_header->get_ny();
  // int nz=em_header->get_nz();
  IMP_INTERNAL_CHECK(em_map->get_rms_calculated(),
                     "RMS should be calculated for calculating derivatives \n");
  long nvox = em_header->get_number_of_voxels();
  double lower_comp = 1. * nvox * em_header->rms * model_header->rms;

  for (unsigned int ii = 0; ii < model_ps.size(); ii++) {
    float x, y, z;
    x = model_xyzr[ii].get_x();
    y = model_xyzr[ii].get_y();
    z = model_xyzr[ii].get_z();
    IMP_IF_LOG(VERBOSE) {
      algebra::Vector3D vv(x, y, z);
      IMP_LOG_VERBOSE(
          "start value:: (" << x << "," << y << "," << z << " ) "
                            << em_map->get_value(x, y, z) << " : "
                            << em_map->get_dim_index_by_location(vv, 0) << ","
                            << em_map->get_dim_index_by_location(vv, 1) << ","
                            << em_map->get_dim_index_by_location(vv, 2)
                            << std::endl);
    }
    const RadiusDependentKernelParameters &params =
        kernel_params->get_params(model_xyzr[ii].get_radius());
    calc_local_bounding_box(  // em_map,
        model_map, x, y, z, params.get_kdist(), iminx, iminy, iminz, imaxx,
        imaxy, imaxz);
    IMP_LOG_WRITE(VERBOSE, params.show());
    IMP_LOG_VERBOSE("local bb: [" << iminx << "," << iminy << "," << iminz
                                  << "] [" << imaxx << "," << imaxy << ","
                                  << imaxz << "] \n");
    tdvx = .0;
    tdvy = .0;
    tdvz = .0;
    for (int ivoxz = iminz; ivoxz <= imaxz; ivoxz++) {
      for (int ivoxy = iminy; ivoxy <= imaxy; ivoxy++) {
        ivox = ivoxz * nx * ny + ivoxy * nx + iminx;
        for (int ivoxx = iminx; ivoxx <= imaxx; ivoxx++) {
          /*          if (em_data[ivox]<EPS) {
            ivox++;
            continue;
            }*/
          float dx = x_loc[ivox] - x;
          float dy = y_loc[ivox] - y;
          float dz = z_loc[ivox] - z;
          rsq = dx * dx + dy * dy + dz * dz;
          rsq = EXP(-rsq * params.get_inv_sigsq());
          tmp = (x - x_loc[ivox]) * rsq;
          if (std::abs(tmp) > lim) {
            tdvx += tmp * em_data[ivox];
          }
          tmp = (y - y_loc[ivox]) * rsq;
          if (std::abs(tmp) > lim) {
            tdvy += tmp * em_data[ivox];
          }
          tmp = (z - z_loc[ivox]) * rsq;
          if (std::abs(tmp) > lim) {
            tdvz += tmp * em_data[ivox];
          }
//.........这里部分代码省略.........

void PusherPonderomotivePositionBorisV::operator()( Particles &particles, SmileiMPI *smpi, int istart, int iend, int ithread, int ipart_ref )
{
/////////// not vectorized

    std::vector<double> *Phi_mpart     = &( smpi->dynamics_PHI_mpart[ithread] );
    std::vector<double> *GradPhi_mpart = &( smpi->dynamics_GradPHI_mpart[ithread] );
    
    double charge_sq_over_mass_dts4, charge_over_mass_sq;
    double gamma0, gamma0_sq, gamma_ponderomotive;
    double pxsm, pysm, pzsm;
    
    //int* cell_keys;
    
    double *momentum[3];
    for( int i = 0 ; i<3 ; i++ ) {
        momentum[i] =  &( particles.momentum( i, 0 ) );
    }
    double *position[3];
    for( int i = 0 ; i<nDim_ ; i++ ) {
        position[i] =  &( particles.position( i, 0 ) );
    }
#ifdef  __DEBUG
    double *position_old[3];
    for( int i = 0 ; i<nDim_ ; i++ ) {
        position_old[i] =  &( particles.position_old( i, 0 ) );
    }
#endif
    
    short *charge = &( particles.charge( 0 ) );
    
    int nparts = GradPhi_mpart->size()/3;
    
    
    double *Phi_m       = &( ( *Phi_mpart )[0*nparts] );
    double *GradPhi_mx = &( ( *GradPhi_mpart )[0*nparts] );
    double *GradPhi_my = &( ( *GradPhi_mpart )[1*nparts] );
    double *GradPhi_mz = &( ( *GradPhi_mpart )[2*nparts] );
    
    //particles.cell_keys.resize(nparts);
    //cell_keys = &( particles.cell_keys[0]);
    
    #pragma omp simd
    for( int ipart=istart ; ipart<iend; ipart++ ) { // begin loop on particles
    
        // ! ponderomotive force is proportional to charge squared and the field is divided by 4 instead of 2
        charge_sq_over_mass_dts4 = ( double )( charge[ipart] )*( double )( charge[ipart] )*one_over_mass_*dts4;
        // (charge over mass)^2
        charge_over_mass_sq      = ( double )( charge[ipart] )*one_over_mass_*( charge[ipart] )*one_over_mass_;
        
        // compute initial ponderomotive gamma
        gamma0_sq = 1. + momentum[0][ipart]*momentum[0][ipart] + momentum[1][ipart]*momentum[1][ipart] + momentum[2][ipart]*momentum[2][ipart] + ( *( Phi_m+ipart-ipart_ref ) )*charge_over_mass_sq;
        gamma0    = sqrt( gamma0_sq ) ;
        
        // ponderomotive force for ponderomotive gamma advance (Grad Phi is interpolated in time, hence the division by 2)
        pxsm = charge_sq_over_mass_dts4 * ( *( GradPhi_mx+ipart-ipart_ref ) ) / gamma0_sq ;
        pysm = charge_sq_over_mass_dts4 * ( *( GradPhi_my+ipart-ipart_ref ) ) / gamma0_sq ;
        pzsm = charge_sq_over_mass_dts4 * ( *( GradPhi_mz+ipart-ipart_ref ) ) / gamma0_sq ;
        
        // update of gamma ponderomotive
        gamma_ponderomotive = gamma0 + ( pxsm*momentum[0][ipart]+pysm*momentum[1][ipart]+pzsm*momentum[2][ipart] ) ;
        
        // Move the particle
#ifdef  __DEBUG
        for( int i = 0 ; i<nDim_ ; i++ ) {
            position_old[i][ipart] = position[i][ipart];
        }
#endif
        for( int i = 0 ; i<nDim_ ; i++ ) {
            position[i][ipart]     += dt*momentum[i][ipart]/gamma_ponderomotive;
        }
        
    } // end loop on particles
    
    //#pragma omp simd
    //for (int ipart=0 ; ipart<nparts; ipart++ ) {
    //
    //    for ( int i = 0 ; i<nDim_ ; i++ ){
    //        cell_keys[ipart] *= nspace[i];
    //        cell_keys[ipart] += round( (position[i][ipart]-min_loc_vec[i]) * dx_inv_[i] );
    //    }
    //
    //} // end loop on particles
    
    
}

// ---------------------------------------------------------------------------------------------------------------------
//! Project charge : frozen & diagFields timstep
// ---------------------------------------------------------------------------------------------------------------------
void Projector2D4Order::basic( double *rhoj, Particles &particles, unsigned int ipart, unsigned int type )
{
    //Warning : this function is used for frozen species or initialization only and doesn't use the standard scheme.
    //rho type = 0
    //Jx type = 1
    //Jy type = 2
    //Jz type = 3
    
    int iloc;
    int ny( nprimy );
    // (x,y,z) components of the current density for the macro-particle
    double charge_weight = inv_cell_volume * ( double )( particles.charge( ipart ) )*particles.weight( ipart );
    
    if( type > 0 ) {
        charge_weight *= 1./sqrt( 1.0 + particles.momentum( 0, ipart )*particles.momentum( 0, ipart )
                                  + particles.momentum( 1, ipart )*particles.momentum( 1, ipart )
                                  + particles.momentum( 2, ipart )*particles.momentum( 2, ipart ) );
                                  
        if( type == 1 ) {
            charge_weight *= particles.momentum( 0, ipart );
        } else if( type == 2 ) {
            charge_weight *= particles.momentum( 1, ipart );
            ny ++;
        } else {
            charge_weight *= particles.momentum( 2, ipart );
        }
    }
    
    // variable declaration
    double xpn, ypn;
    double delta, delta2, delta3, delta4;
    // arrays used for the Esirkepov projection method
    double  Sx1[7], Sy1[7];
    
    for( unsigned int i=0; i<7; i++ ) {
        Sx1[i] = 0.;
        Sy1[i] = 0.;
    }
    
    // --------------------------------------------------------
    // Locate particles & Calculate Esirkepov coef. S, DS and W
    // --------------------------------------------------------
    // locate the particle on the primal grid at current time-step & calculate coeff. S1
    xpn = particles.position( 0, ipart ) * dx_inv_;
    int ip        = round( xpn + 0.5 * ( type==1 ) );                       // index of the central node
    delta  = xpn - ( double )ip;
    delta2 = delta*delta;
    delta3 = delta2*delta;
    delta4 = delta3*delta;
    
    Sx1[1] = dble_1_ov_384   - dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 - dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    Sx1[2] = dble_19_ov_96   - dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 + dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sx1[3] = dble_115_ov_192 - dble_5_ov_8   * delta2 + dble_1_ov_4  * delta4;
    Sx1[4] = dble_19_ov_96   + dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 - dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sx1[5] = dble_1_ov_384   + dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 + dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    
    ypn = particles.position( 1, ipart ) * dy_inv_;
    int jp = round( ypn + 0.5*( type==2 ) );
    delta  = ypn - ( double )jp;
    delta2 = delta*delta;
    delta3 = delta2*delta;
    delta4 = delta3*delta;
    
    Sy1[1] = dble_1_ov_384   - dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 - dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    Sy1[2] = dble_19_ov_96   - dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 + dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sy1[3] = dble_115_ov_192 - dble_5_ov_8   * delta2 + dble_1_ov_4  * delta4;
    Sy1[4] = dble_19_ov_96   + dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 - dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sy1[5] = dble_1_ov_384   + dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 + dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    
    // ---------------------------
    // Calculate the total current
    // ---------------------------
    ip -= i_domain_begin + 3;
    jp -= j_domain_begin + 3;
    
    for( unsigned int i=0 ; i<7 ; i++ ) {
        iloc = ( i+ip )*ny+jp;
        for( unsigned int j=0 ; j<7 ; j++ ) {
            rhoj[iloc+j] += charge_weight * Sx1[i]*Sy1[j];
        }
    }//i
}

unsigned int _take_particles(const Particles &ps) {
  for (unsigned int i = 0; i < ps.size(); ++i) {
    IMP_CHECK_OBJECT(ps[i]);
  }
  return ps.size();
}

ParticleIndexesAdaptor::ParticleIndexesAdaptor(const Particles &ps)
    : tmp_(new ParticleIndexes(ps.size())), val_(tmp_.get()) {
  *tmp_ = get_indexes(ParticlesTemp(ps.begin(), ps.end()));
}

// ---------------------------------------------------------------------------------------------------------------------
// 2nd Order Interpolation of the fields at a the particle position (3 nodes are used)
// ---------------------------------------------------------------------------------------------------------------------
void Interpolator2D1Order::operator() (ElectroMagn* EMfields, Particles &particles, int ipart, LocalFields* ELoc, LocalFields* BLoc)
{
    // Static cast of the electromagnetic fields
    Field2D* Ex2D = static_cast<Field2D*>(EMfields->Ex_);
    Field2D* Ey2D = static_cast<Field2D*>(EMfields->Ey_);
    Field2D* Ez2D = static_cast<Field2D*>(EMfields->Ez_);
    Field2D* Bx2D = static_cast<Field2D*>(EMfields->Bx_m);
    Field2D* By2D = static_cast<Field2D*>(EMfields->By_m);
    Field2D* Bz2D = static_cast<Field2D*>(EMfields->Bz_m);


    // Normalized particle position
    double xpn = particles.position(0, ipart)*dx_inv_;
    double ypn = particles.position(1, ipart)*dy_inv_;


    // Indexes of the central nodes
    ip_ = floor(xpn);
    jp_ = floor(ypn);


    // Declaration and calculation of the coefficient for interpolation
    double delta;

    delta   = xpn - (double)ip_;
    coeffxp_[0] = 1.0 - delta;
    coeffxp_[1] = delta;


    delta   = ypn - (double)jp_;
    coeffyp_[0] = 1.0 - delta;
    coeffyp_[1] = delta;

    //!\todo CHECK if this is correct for both primal & dual grids !!!
    // First index for summation
    ip_ = ip_ - i_domain_begin;
    jp_ = jp_ - j_domain_begin;


    // -------------------------
    // Interpolation of Ex^(d,p)
    // -------------------------
    (*ELoc).x =  compute( coeffxp_, coeffyp_, Ex2D, ip_, jp_);

    // -------------------------
    // Interpolation of Ey^(p,d)
    // -------------------------
    (*ELoc).y = compute( coeffxp_, coeffyp_, Ey2D, ip_, jp_);

    // -------------------------
    // Interpolation of Ez^(p,p)
    // -------------------------
    (*ELoc).z = compute( coeffxp_, coeffyp_, Ez2D, ip_, jp_);

    // -------------------------
    // Interpolation of Bx^(p,d)
    // -------------------------
    (*BLoc).x = compute( coeffxp_, coeffyp_, Bx2D, ip_, jp_);

    // -------------------------
    // Interpolation of By^(d,p)
    // -------------------------
    (*BLoc).y = compute( coeffxp_, coeffyp_, By2D, ip_, jp_);

    // -------------------------
    // Interpolation of Bz^(d,d)
    // -------------------------
    (*BLoc).z = compute( coeffxp_, coeffyp_, Bz2D, ip_, jp_);

} // END Interpolator2D1Order

//----------------------------------------------------------------------------
void ParticleSystems::CreateScene ()
{
    mScene = new0 Node();
    mWireState = new0 WireState();
    mRenderer->SetOverrideWireState(mWireState);

    VertexFormat* vformat = VertexFormat::Create(2,
        VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
        VertexFormat::AU_TEXCOORD, VertexFormat::AT_FLOAT2, 0);
    int vstride = vformat->GetStride();

    const int numParticles = 32;
    VertexBuffer* vbuffer = new0 VertexBuffer(4*numParticles, vstride);
    Float4* positionSizes = new1<Float4>(numParticles);
    for (int i = 0; i < numParticles; ++i)
    {
        positionSizes[i][0] = Mathf::SymmetricRandom();
        positionSizes[i][1] = Mathf::SymmetricRandom();
        positionSizes[i][2] = Mathf::SymmetricRandom();
        positionSizes[i][3] = 0.25f*Mathf::UnitRandom();
    }

    Particles* particles = new0 Particles(vformat, vbuffer, sizeof(int),
        positionSizes, 1.0f);

    particles->AttachController(new0 BloodCellController());
    mScene->AttachChild(particles);

    // Create an image with transparency.
    const int xsize = 32, ysize = 32;
    Texture2D* texture = new0 Texture2D(Texture::TF_A8R8G8B8, xsize,
        ysize, 1);
    unsigned char* data = (unsigned char*)texture->GetData(0);

    float factor = 1.0f/(xsize*xsize + ysize*ysize);
    for (int y = 0, i = 0; y < ysize; ++y)
    {
        for (int x = 0; x < xsize; ++x)
        {
            // The image is red.
            data[i++] = 0;
            data[i++] = 0;
            data[i++] = 255;

            // Semitransparent within a disk, dropping off to zero outside the
            // disk.
            int dx = 2*x - xsize;
            int dy = 2*y - ysize;
            float value = factor*(dx*dx + dy*dy);
            if (value < 0.125f)
            {
                value = Mathf::Cos(4.0f*Mathf::PI*value);
            }
            else
            {
                value = 0.0f;
            }
            data[i++] = (unsigned char)(255.0f*value);
        }
    }

    Texture2DEffect* effect = new0 Texture2DEffect(Shader::SF_LINEAR);
    effect->GetAlphaState(0, 0)->BlendEnabled = true;
    effect->GetDepthState(0, 0)->Enabled = false;
    particles->SetEffectInstance(effect->CreateInstance(texture));
}

void MapModel::verifyPoses(Particles& particles){
  double minX, minY, minZ, maxX, maxY, maxZ;
  m_map->getMetricMin(minX, minY, minZ);
  m_map->getMetricMax(maxX, maxY, maxZ);

  // find min. particle weight:
  double minWeight = std::numeric_limits<double>::max();
  for (Particles::iterator it = particles.begin(); it != particles.end(); ++it) {
    if (it->weight < minWeight)
      minWeight = it->weight;

  }

  minWeight -= 200;

  unsigned numWall = 0;
  unsigned numOut = 0;
  unsigned numMotion = 0;


  // TODO possible speedup: cluster particles by grid voxels first?
  // iterate over samples, multi-threaded:
#pragma omp parallel for
  for (unsigned i = 0; i < particles.size(); ++i){

    octomap::point3d position(particles[i].pose.getOrigin().getX(),
                              particles[i].pose.getOrigin().getY(), particles[i].pose.getOrigin().getZ());

    // see if outside of map bounds:
    if (position(0) < minX || position(0) > maxX
        ||	position(1) < minY || position(1) > maxY
        ||	position(2) < minZ || position(2) > maxZ)
    {
      particles[i].weight = minWeight;
#pragma omp atomic
      numOut++;
    } else {

      // see if occupied cell:
      if (this->isOccupied(position)){
        particles[i].weight = minWeight;
#pragma omp atomic
        numWall++;
      } else {
        // see if current pose is a valid walking pose:
        // TODO: need to check height over floor!
        if (m_motionRangeZ >= 0.0 &&
            std::abs(particles[i].pose.getOrigin().getZ() - m_motionMeanZ) > m_motionRangeZ)
        {
          particles[i].weight = minWeight;
#pragma omp atomic
          numMotion++;
        } else if (m_motionRangePitch >= 0.0 || m_motionRangeRoll >= 0.0){

          double yaw, pitch, roll;
          particles[i].pose.getBasis().getRPY(roll, pitch, yaw);

          if ((m_motionRangePitch >= 0.0 && std::abs(pitch) > m_motionRangePitch)
              || (m_motionRangeRoll >= 0.0 && std::abs(roll) > m_motionRangeRoll))
          {
            particles[i].weight = minWeight;
#pragma omp atomic
            numMotion++;
          }
        }
      }
    }
  } // end loop over particles

  if (numWall > 0 || numOut > 0 || numMotion > 0){
    ROS_INFO("Particle weights minimized: %d out of map, %d in obstacles, %d out of motion range", numOut, numWall, numMotion);
  }

  if (numOut + numWall >= particles.size()){
    ROS_WARN("All particles are out of the valid map area or in obstacles!");
  }

}

Field TriCubicHermite::scalarCurl2D( const Particles& aParticles ) const
{
	assert(data->nDim() == 2);
	Field result("n/a","n/a", 1, aParticles.N());
	return result;
};

void FMM_load_balance(string file, int nbParticles, double dist, double tolerance, double maxEdge, int LBStrategy)
{
    int size;
    MPI_Comm_size(MPI_COMM_WORLD,&size);
    decompo nb1ers(size);
    int first = 0;
    int last = nbParticles-1;
    vec3D center(0,0,0);

    Node<Particles> * treeHead = nullptr;
    Gaspi_communicator * gComm = nullptr;

    // tree creation
    Particles p;
    if ( LBStrategy == HIST_EXACT ||
            LBStrategy == HIST_APPROX ||
            LBStrategy == MORTON_MPI_SYNC)
    {
        p.loadCoordinatesASCII(nbParticles, file);
    }
    else
    {
        gComm = new Gaspi_communicator(512, nbParticles);
        p.loadCoordinatesASCII(nbParticles, file, gComm);
    }

    p.scale();
    treeHead = new Node<Particles>(p);

    // Node Owners array
    i64 * nodeOwners = nullptr;
    // Load Balancing
    LB_Base * LBB = nullptr;
    switch (LBStrategy)
    {
    case HIST_EXACT :
        cout <<"--> Exact Histograms" << endl;
        LBB = new LoadBalancer<Particles, HistExact> (treeHead, nb1ers, dist, tolerance, first, last, maxEdge, center, gComm, nullptr, nodeOwners, 0);
        LBB->run();
        break;

    case HIST_APPROX :
        cout <<"--> Approx Histograms" << endl;
        LBB = new LoadBalancer<Particles, HistApprox> (treeHead, nb1ers, dist, tolerance, first, last, maxEdge, center, gComm, nullptr, nodeOwners, 0);
        LBB->run();
        break;

    case MORTON_MPI_SYNC :
        cout <<"--> Morton DFS, with tolerance : " << tolerance << endl;
        LBB = new LoadBalancer<Particles, MortonSyncMPI>(treeHead, nb1ers, dist, tolerance, first, last, maxEdge, center, gComm, nullptr, nodeOwners, 0);
        LBB->run();
        break;

    case MORTON_GASPI_ASYNC :
        cout <<"--> Morton DFS Async, with tolerance : " << tolerance << endl;
        LBB = new LoadBalancer<Particles, MortonAsyncGASPI>(treeHead, nb1ers, dist, tolerance, first, last, maxEdge, center, gComm, nullptr, nodeOwners, 0);
        LBB->run();
        break;

    default :
        cerr << "No load balancing strategy detected !";
        exit(5);
    }
}

// ---------------------------------------------------------------------------------------------------------------------
//! Project current densities : main projector
// ---------------------------------------------------------------------------------------------------------------------
void Projector1D4Order::currents( double *Jx, double *Jy, double *Jz, Particles &particles, unsigned int ipart, double invgf, int *iold, double *delta )
{
    // Declare local variables
    int ipo, ip;
    int ip_m_ipo;
    double charge_weight = inv_cell_volume * ( double )( particles.charge( ipart ) )*particles.weight( ipart );
    double xjn, xj_m_xipo, xj_m_xipo2, xj_m_xipo3, xj_m_xipo4, xj_m_xip, xj_m_xip2, xj_m_xip3, xj_m_xip4;
    double crx_p = charge_weight*dx_ov_dt;                // current density for particle moving in the x-direction
    double cry_p = charge_weight*particles.momentum( 1, ipart )*invgf;  // current density in the y-direction of the macroparticle
    double crz_p = charge_weight*particles.momentum( 2, ipart )*invgf;  // current density allow the y-direction of the macroparticle
    double S0[7], S1[7], Wl[7], Wt[7], Jx_p[7];            // arrays used for the Esirkepov projection method
    // Initialize variables
    for( unsigned int i=0; i<7; i++ ) {
        S0[i]=0.;
        S1[i]=0.;
        Wl[i]=0.;
        Wt[i]=0.;
        Jx_p[i]=0.;
    }//i
    
    
    // Locate particle old position on the primal grid
    xj_m_xipo  = *delta;                   // normalized distance to the nearest grid point
    xj_m_xipo2 = xj_m_xipo  * xj_m_xipo;                 // square of the normalized distance to the nearest grid point
    xj_m_xipo3 = xj_m_xipo2 * xj_m_xipo;              // cube of the normalized distance to the nearest grid point
    xj_m_xipo4 = xj_m_xipo3 * xj_m_xipo;              // 4th power of the normalized distance to the nearest grid point
    
    // Locate particle new position on the primal grid
    xjn       = particles.position( 0, ipart ) * dx_inv_;
    ip        = round( xjn );                         // index of the central node
    xj_m_xip  = xjn - ( double )ip;                   // normalized distance to the nearest grid point
    xj_m_xip2 = xj_m_xip  * xj_m_xip;                    // square of the normalized distance to the nearest grid point
    xj_m_xip3 = xj_m_xip2 * xj_m_xip;                 // cube of the normalized distance to the nearest grid point
    xj_m_xip4 = xj_m_xip3 * xj_m_xip;                 // 4th power of the normalized distance to the nearest grid point
    
    
    // coefficients 4th order interpolation on 5 nodes
    
    S0[1] = dble_1_ov_384   - dble_1_ov_48  * xj_m_xipo  + dble_1_ov_16 * xj_m_xipo2 - dble_1_ov_12 * xj_m_xipo3 + dble_1_ov_24 * xj_m_xipo4;
    S0[2] = dble_19_ov_96   - dble_11_ov_24 * xj_m_xipo  + dble_1_ov_4 * xj_m_xipo2  + dble_1_ov_6  * xj_m_xipo3 - dble_1_ov_6  * xj_m_xipo4;
    S0[3] = dble_115_ov_192 - dble_5_ov_8   * xj_m_xipo2 + dble_1_ov_4 * xj_m_xipo4;
    S0[4] = dble_19_ov_96   + dble_11_ov_24 * xj_m_xipo  + dble_1_ov_4 * xj_m_xipo2  - dble_1_ov_6  * xj_m_xipo3 - dble_1_ov_6  * xj_m_xipo4;
    S0[5] = dble_1_ov_384   + dble_1_ov_48  * xj_m_xipo  + dble_1_ov_16 * xj_m_xipo2 + dble_1_ov_12 * xj_m_xipo3 + dble_1_ov_24 * xj_m_xipo4;
    
    // coefficients 2nd order interpolation on 5 nodes
    ipo        = *iold;                          // index of the central node
    ip_m_ipo = ip-ipo-index_domain_begin;
    
    S1[ip_m_ipo+1] = dble_1_ov_384   - dble_1_ov_48  * xj_m_xip  + dble_1_ov_16 * xj_m_xip2 - dble_1_ov_12 * xj_m_xip3 + dble_1_ov_24 * xj_m_xip4;
    S1[ip_m_ipo+2] = dble_19_ov_96   - dble_11_ov_24 * xj_m_xip  + dble_1_ov_4 * xj_m_xip2  + dble_1_ov_6  * xj_m_xip3 - dble_1_ov_6  * xj_m_xip4;
    S1[ip_m_ipo+3] = dble_115_ov_192 - dble_5_ov_8   * xj_m_xip2 + dble_1_ov_4 * xj_m_xip4;
    S1[ip_m_ipo+4] = dble_19_ov_96   + dble_11_ov_24 * xj_m_xip  + dble_1_ov_4 * xj_m_xip2  - dble_1_ov_6  * xj_m_xip3 - dble_1_ov_6  * xj_m_xip4;
    S1[ip_m_ipo+5] = dble_1_ov_384   + dble_1_ov_48  * xj_m_xip  + dble_1_ov_16 * xj_m_xip2 + dble_1_ov_12 * xj_m_xip3 + dble_1_ov_24 * xj_m_xip4;
    
    // coefficients used in the Esirkepov method
    for( unsigned int i=0; i<7; i++ ) {
        Wl[i] = S0[i] - S1[i];           // for longitudinal current (x)
        Wt[i] = 0.5 * ( S0[i] + S1[i] ); // for transverse currents (y,z)
    }//i
    
    // local current created by the particle
    // calculate using the charge conservation equation
    for( unsigned int i=1; i<7; i++ ) {
        Jx_p[i] = Jx_p[i-1] + crx_p * Wl[i-1];
    }
    
    ipo -= 3 ;
    
    // 4th order projection for the total currents & charge density
    // At the 4th order, oversize = 3.
    for( unsigned int i=0; i<7; i++ ) {
        Jx[i  + ipo]  += Jx_p[i];
        Jy[i  + ipo]  += cry_p * Wt[i];
        Jz[i  + ipo]  += crz_p * Wt[i];
    }//i
    
}

Particles ResamplingResidual::resample(Particles* particles){

    Particles resampledSet;
    Particles stage1;
    int count = 0;
    resampledSet.samples = fmat(particles->samples.n_rows,particles->samples.n_cols);
    resampledSet.weights = frowvec(particles->weights.n_cols);

    fvec cumsum;
    fvec random;

    unsigned int number = particles->samples.n_cols;
    unsigned int numberOfStage1 = 0;

    // Generating copie information of every particle
    ivec copies = zeros<ivec>(particles->samples.n_cols);
    for (unsigned int i=0;i<particles->samples.n_cols;++i){
        copies = (int) floor(number*particles->weights(i));
    }

    numberOfStage1 = sum(copies);

    stage1.samples =  fmat(particles->samples.n_rows,numberOfStage1);
    stage1.weights = frowvec(numberOfStage1);

    //Picking N_i = N*w_i copies of i-th particle
    for (unsigned int i=1; i < copies.n_rows;++i){
        for (int j=0;j<copies(i);++j){
            for (unsigned int k=0;k<particles->samples.n_rows;++k){
                stage1.samples(k,count) = particles->samples(k,i);
            }
            stage1.weights(count) = particles->weights(i) - (float)copies(i)/number;
            count++;
        }
    }

    // multinomial resampling with residuum weights w_i = w_i - N_i/N
    cumsum = zeros<fvec>(numberOfStage1);
    for (unsigned int i=1; i < stage1.weights.n_cols;++i){
        cumsum(i) = cumsum(i-1) + stage1.weights(i);
    }

    // generate sorted random set
    random = randu<fvec>(numberOfStage1);
    random=random*cumsum(cumsum.n_rows-1);
    random = sort(random);

    for (unsigned int j=0; j < random.n_rows; ++j){
        for (unsigned int i=0 ; i < cumsum.n_rows; ++i){
            if (random(j) <= cumsum(i)){
                if(i > 0){
                    if(random(j) >= cumsum(i-1)) {
                        for (unsigned int k=0;k<stage1.samples.n_rows;++k){
                            resampledSet.samples(k,j) = stage1.samples(k,i);
                            assignmentVec.push_back(i);
                        }
                        break;
                    }
                }
                else {
                    for (unsigned int k=0; k<stage1.samples.n_rows; ++k){
                        resampledSet.samples(k,j) = stage1.samples(k,i);
                        assignmentVec.push_back(i);
                    }
                    break;

                }

            }
            // Normalize weights
            resampledSet.weights(j) = 1.0f/particles->weights.n_cols;
        }
    }

    return resampledSet;
}

// ---------------------------------------------------------------------------------------------------------------------
//! Project current densities : main projector
// ---------------------------------------------------------------------------------------------------------------------
void Projector2D4Order::currents( double *Jx, double *Jy, double *Jz, Particles &particles, unsigned int ipart, double invgf, int *iold, double *deltaold )
{
    int nparts = particles.size();
    
    // -------------------------------------
    // Variable declaration & initialization
    // -------------------------------------
    
    int iloc;
    // (x,y,z) components of the current density for the macro-particle
    double charge_weight = inv_cell_volume * ( double )( particles.charge( ipart ) )*particles.weight( ipart );
    double crx_p = charge_weight*dx_ov_dt;
    double cry_p = charge_weight*dy_ov_dt;
    double crz_p = charge_weight*one_third*particles.momentum( 2, ipart )*invgf;
    
    // variable declaration
    double xpn, ypn;
    double delta, delta2, delta3, delta4;
    // arrays used for the Esirkepov projection method
    double  Sx0[7], Sx1[7], Sy0[7], Sy1[7], DSx[7], DSy[7], tmpJx[7];
    
    for( unsigned int i=0; i<7; i++ ) {
        Sx1[i] = 0.;
        Sy1[i] = 0.;
        tmpJx[i] = 0.;
    }
    Sx0[0] = 0.;
    Sx0[6] = 0.;
    Sy0[0] = 0.;
    Sy0[6] = 0.;
    
    // --------------------------------------------------------
    // Locate particles & Calculate Esirkepov coef. S, DS and W
    // --------------------------------------------------------
    
    // locate the particle on the primal grid at former time-step & calculate coeff. S0
    delta = deltaold[0*nparts];
    delta2 = delta*delta;
    delta3 = delta2*delta;
    delta4 = delta3*delta;
    
    Sx0[1] = dble_1_ov_384   - dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 - dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    Sx0[2] = dble_19_ov_96   - dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 + dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sx0[3] = dble_115_ov_192 - dble_5_ov_8   * delta2 + dble_1_ov_4  * delta4;
    Sx0[4] = dble_19_ov_96   + dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 - dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sx0[5] = dble_1_ov_384   + dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 + dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    
    delta = deltaold[1*nparts];
    delta2 = delta*delta;
    delta3 = delta2*delta;
    delta4 = delta3*delta;
    
    Sy0[1] = dble_1_ov_384   - dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 - dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    Sy0[2] = dble_19_ov_96   - dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 + dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sy0[3] = dble_115_ov_192 - dble_5_ov_8   * delta2 + dble_1_ov_4  * delta4;
    Sy0[4] = dble_19_ov_96   + dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 - dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sy0[5] = dble_1_ov_384   + dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 + dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    
    
    // locate the particle on the primal grid at current time-step & calculate coeff. S1
    xpn = particles.position( 0, ipart ) * dx_inv_;
    int ip = round( xpn );
    int ipo = iold[0*nparts];
    int ip_m_ipo = ip-ipo-i_domain_begin;
    delta  = xpn - ( double )ip;
    delta2 = delta*delta;
    delta3 = delta2*delta;
    delta4 = delta3*delta;
    
    Sx1[ip_m_ipo+1] = dble_1_ov_384   - dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 - dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    Sx1[ip_m_ipo+2] = dble_19_ov_96   - dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 + dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sx1[ip_m_ipo+3] = dble_115_ov_192 - dble_5_ov_8   * delta2 + dble_1_ov_4  * delta4;
    Sx1[ip_m_ipo+4] = dble_19_ov_96   + dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 - dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sx1[ip_m_ipo+5] = dble_1_ov_384   + dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 + dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    
    ypn = particles.position( 1, ipart ) * dy_inv_;
    int jp = round( ypn );
    int jpo = iold[1*nparts];
    int jp_m_jpo = jp-jpo-j_domain_begin;
    delta  = ypn - ( double )jp;
    delta2 = delta*delta;
    delta3 = delta2*delta;
    delta4 = delta3*delta;
    
    Sy1[jp_m_jpo+1] = dble_1_ov_384   - dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 - dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    Sy1[jp_m_jpo+2] = dble_19_ov_96   - dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 + dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sy1[jp_m_jpo+3] = dble_115_ov_192 - dble_5_ov_8   * delta2 + dble_1_ov_4  * delta4;
    Sy1[jp_m_jpo+4] = dble_19_ov_96   + dble_11_ov_24 * delta  + dble_1_ov_4  * delta2 - dble_1_ov_6  * delta3 - dble_1_ov_6  * delta4;
    Sy1[jp_m_jpo+5] = dble_1_ov_384   + dble_1_ov_48  * delta  + dble_1_ov_16 * delta2 + dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
    
    for( unsigned int i=0; i < 7; i++ ) {
        DSx[i] = Sx1[i] - Sx0[i];
        DSy[i] = Sy1[i] - Sy0[i];
    }
    
    // calculate Esirkepov coeff. Wx, Wy, Wz when used
    double tmp, tmp2, tmp3, tmpY;
//.........这里部分代码省略.........