bool Foam::passiveParticleStreamReconstructor::reconstruct
(
    const IOobject& io,
    const bool,
    Ostream& os
) const
{
    // io.db()                   = Cloud<passiveParticle>
    // io.db().parent()          = polyMesh
    // io.db().parent().parent() = Time

    // Retrieve from polyMesh
    const uFieldReconstructor& reconstructor =
        uFieldReconstructor::New(io.db().parent());

    const PtrList<unallocatedFvMesh>& procMeshes = reconstructor.procMeshes();

    Info<< "Reconstructing " << io.objectPath() << endl;

    // Read field on proc meshes
    PtrList<cloud> procClouds(procMeshes.size());
    PtrList<unallocatedIOPosition> procFields(procMeshes.size());
    forAll(procFields, proci)
    {
        const unallocatedFvMesh& procMesh = procMeshes[proci];

        Pout<< incrIndent;

        // Construct empty cloud
        procClouds.set
        (
            proci,
            new cloud
            (
                procMesh.thisDb(),
                "kinematicCloud"
            )
        );

        procFields.set
        (
            proci,
            new unallocatedIOPosition
            (
                IOobject
                (
                    io.name(),
                    io.instance(),
                    io.local(),
                    procClouds[proci],
                    IOobject::MUST_READ,    //IOobject::READ_IF_PRESENT,
                    IOobject::NO_WRITE
                )
            )
        );

        Pout<< decrIndent;
    }

    unallocatedIOPosition particles
    (
        IOobject
        (
            io.name(),
            io.instance(),
            io.local(),
            io.db(),
            IOobject::NO_READ,
            IOobject::NO_WRITE,
            false
        )
    );

    const faceList* facesPtr = nullptr;
    if (isA<polyMesh>(io.db().parent()))
    {
        facesPtr = &dynamic_cast<const polyMesh&>(io.db().parent()).faces();
    }

    forAll(procFields, proci)
    {
        const unallocatedIOPosition& procCloud = procFields[proci];
        const labelList& cellMap = reconstructor.cellProcAddressing()[proci];
        const labelList& faceMap = reconstructor.faceProcAddressing()[proci];

        forAllConstIter(typename IDLList<basicParticle>, procCloud, iter)
        {
            const basicParticle& p = iter();

            const label mappedCell = cellMap[p.cell()];

            const label mapi = faceMap[p.tetFace()];

            label mappedTetFace = -1;
            label tetPti = p.tetPt();
            if (mapi == 0)
            {
                FatalErrorInFunction << "problem" << exit(FatalError);
            }
            else if (mapi > 0)
            {
                mappedTetFace = mapi - 1;
            }
            else
            {
                mappedTetFace = -mapi - 1;

                if (facesPtr)
                {
                    // Flipping face

                    const face& f = (*facesPtr)[mappedTetFace];
                    tetPti = f.size() - 1 - tetPti;
                }
            }

            particles.append
            (
                new basicParticle
                (
                    p,
                    mappedCell,
                    mappedTetFace,
                    tetPti
                )
            );
        }
    }
    particles.writeData(os);

    return os.good();
}
void NodeBasedCellPopulationWithParticles<DIM>::WriteVtkResultsToFile(const std::string& rDirectory)
{
#ifdef CHASTE_VTK
    // Store the present time as a string
    std::stringstream time;
    time << SimulationTime::Instance()->GetTimeStepsElapsed();

    // Make sure the nodes are ordered contiguously in memory
    NodeMap map(1 + this->mpNodesOnlyMesh->GetMaximumNodeIndex());
    this->mpNodesOnlyMesh->ReMesh(map);

    // Store the number of cells for which to output data to VTK
    unsigned num_nodes = this->GetNumNodes();
    std::vector<double> rank(num_nodes);
    std::vector<double> particles(num_nodes);

    unsigned num_cell_data_items = 0;
    std::vector<std::string> cell_data_names;

    // We assume that the first cell is representative of all cells
    if (num_nodes > 0)
    {
        num_cell_data_items = this->Begin()->GetCellData()->GetNumItems();
        cell_data_names = this->Begin()->GetCellData()->GetKeys();
    }

    std::vector<std::vector<double> > cell_data;
    for (unsigned var=0; var<num_cell_data_items; var++)
    {
        std::vector<double> cell_data_var(num_nodes);
        cell_data.push_back(cell_data_var);
    }

    // Create mesh writer for VTK output
    VtkMeshWriter<DIM, DIM> mesh_writer(rDirectory, "results_"+time.str(), false);
    mesh_writer.SetParallelFiles(*(this->mpNodesOnlyMesh));

    // Iterate over any cell writers that are present
    for (typename std::vector<boost::shared_ptr<AbstractCellWriter<DIM, DIM> > >::iterator cell_writer_iter = this->mCellWriters.begin();
         cell_writer_iter != this->mCellWriters.end();
         ++cell_writer_iter)
    {
        // Create vector to store VTK cell data
        std::vector<double> vtk_cell_data(num_nodes);

        // Loop over nodes
        for (typename AbstractMesh<DIM,DIM>::NodeIterator node_iter = this->mrMesh.GetNodeIteratorBegin();
             node_iter != this->mrMesh.GetNodeIteratorEnd();
             ++node_iter)
        {
            unsigned node_index = node_iter->GetIndex();

            // If this node is a particle (not a cell), then we set the 'dummy' VTK cell data for this to be -2.0...
            if (this->IsParticle(node_index))
            {
                vtk_cell_data[node_index] = -2.0;
            }
            else
            {
                // ...otherwise we populate the vector of VTK cell data as usual
                CellPtr p_cell = this->GetCellUsingLocationIndex(node_index);
                vtk_cell_data[node_index] = (*cell_writer_iter)->GetCellDataForVtkOutput(p_cell, this);
            }
        }

        mesh_writer.AddPointData((*cell_writer_iter)->GetVtkCellDataName(), vtk_cell_data);
    }

    // Loop over cells
    for (typename AbstractCellPopulation<DIM>::Iterator cell_iter = this->Begin();
         cell_iter != this->End();
         ++cell_iter)
    {
        // Get the node index corresponding to this cell
        unsigned global_index = this->GetLocationIndexUsingCell(*cell_iter);
        unsigned node_index = this->rGetMesh().SolveNodeMapping(global_index);

        for (unsigned var=0; var<num_cell_data_items; var++)
        {
            cell_data[var][node_index] = cell_iter->GetCellData()->GetItem(cell_data_names[var]);
        }

        rank[node_index] = (PetscTools::GetMyRank());
    }

    mesh_writer.AddPointData("Process rank", rank);

    // Loop over nodes
    for (typename AbstractMesh<DIM,DIM>::NodeIterator node_iter = this->mrMesh.GetNodeIteratorBegin();
         node_iter != this->mrMesh.GetNodeIteratorEnd();
         ++node_iter)
    {
        unsigned node_index = node_iter->GetIndex();
        particles[node_index] = (double) (this->IsParticle(node_index));
    }

    mesh_writer.AddPointData("Non-particles", particles);

    if (num_cell_data_items > 0)
    {
        for (unsigned var=0; var<cell_data.size(); var++)
        {
            mesh_writer.AddPointData(cell_data_names[var], cell_data[var]);
        }
    }

    mesh_writer.WriteFilesUsingMesh(*(this->mpNodesOnlyMesh));

    *(this->mpVtkMetaFile) << "        <DataSet timestep=\"";
    *(this->mpVtkMetaFile) << SimulationTime::Instance()->GetTimeStepsElapsed();
    *(this->mpVtkMetaFile) << "\" group=\"\" part=\"0\" file=\"results_";
    *(this->mpVtkMetaFile) << SimulationTime::Instance()->GetTimeStepsElapsed();
    EXCEPT_IF_NOT(PetscTools::IsSequential());
    {
        *(this->mpVtkMetaFile) << ".vtu\"/>\n";
    }
/*    {
        // Parallel vtu files  .vtu -> .pvtu
        *(this->mpVtkMetaFile) << ".pvtu\"/>\n";
    }*/
#endif //CHASTE_VTK
}
void Foam::uniformSet::calcSamples
(
    DynamicList<point>& samplingPts,
    dynamicLabelList& samplingCells,
    dynamicLabelList& samplingFaces,
    dynamicLabelList& samplingSegments,
    DynamicList<scalar>& samplingCurveDist
) const
{
    // distance vector between sampling points
    if ((nPoints_ < 2) || (mag(end_ - start_) < SMALL))
    {
        FatalErrorIn("uniformSet::calcSamples()")
            << "Incorrect sample specification. Either too few points or"
            << " start equals end point." << endl
            << "nPoints:" << nPoints_
            << "  start:" << start_
            << "  end:" << end_
            << exit(FatalError);
    }

    const vector offset = (end_ - start_)/(nPoints_ - 1);
    const vector normOffset = offset/mag(offset);
    const vector smallVec = tol*offset;
    const scalar smallDist = mag(smallVec);

    // Get all boundary intersections
    List<pointIndexHit> bHits = searchEngine().intersections
    (
        start_ - smallVec,
        end_ + smallVec
    );

    point bPoint(GREAT, GREAT, GREAT);
    label bFaceI = -1;

    if (bHits.size())
    {
        bPoint = bHits[0].hitPoint();
        bFaceI = bHits[0].index();
    }

    // Get first tracking point. Use bPoint, bFaceI if provided.

    point trackPt;
    label trackCellI = -1;
    label trackFaceI = -1;

    bool isSample =
        getTrackingPoint
        (
            offset,
            start_,
            bPoint,
            bFaceI,

            trackPt,
            trackCellI,
            trackFaceI
        );

    if (trackCellI == -1)
    {
        // Line start_ - end_ does not intersect domain at all.
        // (or is along edge)
        // Set points and cell/face labels to empty lists

        return;
    }

    if (isSample)
    {
        samplingPts.append(start_);
        samplingCells.append(trackCellI);
        samplingFaces.append(trackFaceI);
        samplingCurveDist.append(0.0);
    }

    //
    // Track until hit end of all boundary intersections
    //

    // current segment number
    label segmentI = 0;

    // starting index of current segment in samplePts
    label startSegmentI = 0;

    label sampleI = 0;
    point samplePt = start_;

    // index in bHits; current boundary intersection
    label bHitI = 1;

    while(true)
    {
        // Initialize tracking starting from trackPt
        Cloud<passiveParticle> particles(mesh(), IDLList<passiveParticle>());

        passiveParticle singleParticle
        (
            particles,
            trackPt,
            trackCellI
        );

        bool reachedBoundary = trackToBoundary
        (
            singleParticle,
            samplePt,
            sampleI,
            samplingPts,
            samplingCells,
            samplingFaces,
            samplingCurveDist
        );

        // fill sampleSegments
        for(label i = samplingPts.size() - 1; i >= startSegmentI; --i)
        {
            samplingSegments.append(segmentI);
        }


        if (!reachedBoundary)
        {
            if (debug)
            {
                Info<< "calcSamples : Reached end of samples: "
                    << "  samplePt now:" << samplePt
                    << "  sampleI now:" << sampleI
                    << endl;
            }
            break;
        }


        bool foundValidB = false;

        while (bHitI < bHits.size())
        {
            scalar dist =
                (bHits[bHitI].hitPoint() - singleParticle.position())
              & normOffset;

            if (debug)
            {
                Info<< "Finding next boundary : "
                    << "bPoint:" << bHits[bHitI].hitPoint()
                    << "  tracking:" << singleParticle.position()
                    << "  dist:" << dist
                    << endl;
            }

            if (dist > smallDist)
            {
                // hitpoint is past tracking position
                foundValidB = true;
                break;
            }
            else
            {
                bHitI++;
            }
        }

        if (!foundValidB)
        {
            // No valid boundary intersection found beyond tracking position
            break;
        }

        // Update starting point for tracking
        trackFaceI = bFaceI;
        trackPt = pushIn(bPoint, trackFaceI);
        trackCellI = getBoundaryCell(trackFaceI);

        segmentI++;

        startSegmentI = samplingPts.size();
    }
}
示例#4
0
// Pose is the average of all the particles
const Pose2D& ParticleFilter::pose() const {
  if(dirty_) {

	typedef Point2D Centroid;

	// k means clustering
	//
	// Initialize random centroids
	set<Centroid, centroid_comp_> centroids;
	for (int i = 0; i < NUM_CLUSTERS; i++) {
		Centroid centroid;
	    centroid.x = (rand() % (2*2500)) - 2500;
	    centroid.y = (rand() % (2*1250)) - 1250;

	    centroids.insert(centroid);
	}

//	Centroid centroid;
//	centroid.x = -1000;
//	centroid.y = 0;
//	centroids.insert(centroid);
//
//	centroid.x = 1000;
//	centroid.y = 0;
//	centroids.insert(centroid);

//	cout << "========> BEGIN K-MEANS <========" << endl;

//	cout << "PARTICLES" << endl;
//	for (auto p : particles())
//		cout << p.x << " " << p.y << endl;
//	cout << endl;
////
//	cout << "Random Centroids" << endl;
//	for (auto centroid : centroids)
//		cout << centroid.x << " " << centroid.y << endl;
//	cout << endl;

	// Iterate until convergence
	map<Centroid, set<Particle, particle_comp_>, centroid_comp_> closest_particles;
	set<Point2D, centroid_comp_> new_centroids = centroids;
	map<Centroid, int, centroid_comp_> cluster_size;
	int i = 0;
	do {
//		cout << "PARTICLES" << endl;
//		for (auto p : particles())
//			cout << p.x << " " << p.y << endl;
//		cout << endl;

		centroids = new_centroids;
		new_centroids.clear();
		cluster_size.clear();
		// Compute the closest centroid to each particle
		double closest_centroid_distance = 50000;
		Centroid closest_centroid = NULL;
		for (Particle p : particles()) {
			for (Centroid centroid : centroids) {
//				cout << "Current closest distance: " << closest_centroid_distance << endl;
				double distance = sqrt(pow(p.x-centroid.x, 2) + pow(p.y-centroid.y, 2));
				// Record the closest centroid
//				cout << "Particle " << p.x << " " << p.y << endl;
//				cout << "Comparing centroid: " << centroid.x << " " << centroid.y << endl;
//				cout << "Proposal Distance: " << distance << endl;
				if (distance < closest_centroid_distance) {
//					cout << "Proposed distance is closer!!!" << endl;
					closest_centroid_distance = distance;
					closest_centroid = centroid;
				}
			}

//			cout << "C(" << closest_centroid.x << " " << closest_centroid.y << ") -> P(" << p.x << " " << p.y << ")" << endl;
			closest_particles[closest_centroid].insert(p);

			// Record the closest centroid
//			cout << "Particle " << p.x << " " << p.y << endl;
//			cout << "Closest centroid: " << closest_centroid.x << " " << closest_centroid.y << endl;
//			cout << "Centroid distance: " << closest_centroid_distance << endl << endl;
//			for (auto c : closest_particles) {
//				cout << "CENTROID " << c.first.x << " " << c.first.y << " at this point..." << endl;
//				for (auto pp : c.second) {
//					cout << pp.x << " " << pp.y << endl;
//				}
//				cout << endl;
//			}
//			cout << endl;

			closest_centroid = NULL;
			closest_centroid_distance = 50000;
		}

//		// Print closest particles
//		for (auto elem : closest_particles) {
//			cout << "Centroid: " << elem.first.x << " " << elem.first.y << endl;
//
//			for (auto p : elem.second) {
//				cout << "Particle: " << p.x << " " << p.y << endl;
//			}
//
//			cout << endl;
//		}

		// Recompute the centroids based on the mean of all the closest particles
		for (Centroid centroid : centroids) {
			double sum_x = 0;
			double sum_y = 0;
			for (Particle p : closest_particles[centroid]) {
				sum_x += p.x;
				sum_y += p.y;
			}

			int num_particles = closest_particles[centroid].size();
			if (num_particles > 0) {
				new_centroids.insert(Centroid(sum_x/num_particles, sum_y/num_particles));
				cluster_size[centroid] = num_particles;
			}
		}

//		cout << "Centroids" << endl;
//		for (auto centroid : centroids)
//			cout << centroid.x << " " << centroid.y << " (" << cluster_size[centroid] << ")" << endl;
//		cout << endl;
//
//		cout << "New Centroids" << endl;
//		for (auto centroid : new_centroids)
//			cout << centroid.x << " " << centroid.y << " (" << cluster_size[centroid] << ")" << endl;
//		cout << endl << endl;

		i++;
	} while (i < 3);

//	cout << "Centroids" << endl;
//	for (auto centroid : centroids)
//		cout << centroid.x << " " << centroid.y << " (" << cluster_size[centroid] << ")" << endl;
//	cout << endl;

	Centroid best_cluster;
	double max_particles = -1;
	for (Centroid centroid : centroids) {
		if (cluster_size[centroid] > max_particles) {
			best_cluster = centroid;
			max_particles = cluster_size[centroid];
		}
	}

//	cout << "Best cluster: " << best_cluster.x << " " << best_cluster.y << " (" << cluster_size[best_cluster] << ")" << endl;
//
//	cout << "========> END K-MEANS <========" << endl << endl;

    // Compute the mean pose estimate
    mean_ = Pose2D();
    using T = decltype(mean_.translation);
    for(const auto& p : particles()) {
      mean_.translation += T(p.x,p.y);
      mean_.rotation += p.t;
    }
    if(particles().size() > 0)
      mean_ /= particles().size();

    // k-means update
    mean_.translation = best_cluster;

    dirty_ = false;
  }
  return mean_;
}
示例#5
0
void ParticleFilter::processFrame() {
  // Indicate that the cached mean needs to be updated
  dirty_ = true;

  // Retrieve odometry update - how do we integrate this into the filter?
  const auto& disp = cache_.odometry->displacement;
  log(41, "Updating particles from odometry: %2.f,%2.f @ %2.2f", disp.translation.x, disp.translation.y, disp.rotation * RAD_T_DEG);

  std::random_device rd;
  std::mt19937 generator(rd());
  std::normal_distribution<double> v_distribution(0, V_STDDEV);
  std::normal_distribution<double> x_distribution(0, X_STDDEV);
  std::normal_distribution<double> y_distribution(0, Y_STDDEV);
  std::normal_distribution<double> t_distribution(0, T_STDDEV);

  for (auto& p : particles()) {
	  double t_noise = t_distribution(generator);
	  double v_noise = v_distribution(generator);
	  double x_noise = x_distribution(generator);
	  double y_noise = y_distribution(generator);

	  // Step each particle deterministically move by the odometry
	  if (disp.x == 0) {
		  v_noise = 0;
		  t_noise = 0;
	  }
	  if (disp.rotation == 0) {
		  t_noise = 0;
	  }
	  if (disp.x == 0 && disp.rotation == 0) {
		  x_noise = 0;
		  y_noise = 0;
	  }
	  p.t += disp.rotation + t_noise;
	  p.x += (0.8*disp.x + v_noise) * cos(p.t) + x_noise;
	  p.y += (0.8*disp.x + v_noise) * sin(p.t) + y_noise;
      p.w = 0;

  }

  static vector<WorldObjectType> beacon_ids = {
		  WO_BEACON_YELLOW_BLUE,
		  WO_BEACON_BLUE_YELLOW,
		  WO_BEACON_YELLOW_PINK,
		  WO_BEACON_PINK_YELLOW,
		  WO_BEACON_BLUE_PINK,
		  WO_BEACON_PINK_BLUE
  };

  double population_quality_total = 0;
  double population_count = 0;
  bool beacon_seen = false;
  WorldObject* lastBeaconPtr;

  // Update particle weights with respect to how far they are from the seen beacon
  for (auto& p : particles()) {
	  for (auto beacon_id : beacon_ids) {
		  auto& beacon = cache_.world_object->objects_[beacon_id];
		  if (!beacon.seen) {
			  continue;
		  }

		  beacon_seen = true;
		  lastBeaconPtr = &beacon;

		  // Beacon distance
		  double p_distance = sqrt(pow(abs(p.x-beacon.loc.x), 2) + pow(abs(p.y-beacon.loc.y), 2));
		  double v_distance = beacon.visionDistance;
		  // Linear
//		  double max_distance = sqrt(pow(2500, 2) + pow(5000, 2));
//		  double distance_weight = (max_distance - abs(p_distance - v_distance)) / max_distance;
		  // Gaussian
		  double p_gaussian = calculateGaussianValue(p_distance, SDTDEV_POSITION_WEIGHT, v_distance);
		  double v_gaussian = calculateGaussianValue(v_distance, SDTDEV_POSITION_WEIGHT, v_distance);
		  double distance_weight = (v_gaussian - fabs(p_gaussian - v_gaussian)) / v_gaussian;

		  // Orientation
		  double p_bearing = Point2D(p.x, p.y).getBearingTo(beacon.loc, p.t);
		  double bearing_difference = abs(p_bearing - beacon.visionBearing);
		  if (bearing_difference > 2 * M_PI) cout << "ERROR Bearing diff: " << p_bearing << ", " << beacon.visionBearing << endl;
		  if (bearing_difference > M_PI) {
			  // Angle wrap around
			  bearing_difference = (2 * M_PI) - bearing_difference;
		  }
		  double max_bearing_diff = M_PI;
		  double bearing_weight = (max_bearing_diff - bearing_difference) / max_bearing_diff;

		  population_quality_total += (distance_weight + bearing_weight) / 2;
		  population_count++;

		  const double weight_ratio = 0.5;
		  p.w += (weight_ratio) * distance_weight + (1 - weight_ratio) * bearing_weight;
	  }
  }

  // If no beacons is seen, don't resample
  if (!beacon_seen) {
	  return;
  }

  if (!(disp.x != 0 || disp.rotation != 0)) {
	  return;
  }


  // Quality is the best if approaches 1
  double population_quality_avg = population_quality_total / population_count;

  // Get all particle weights
  vector<double> weights;
  for (auto p : particles()) {
	  weights.push_back(p.w);
  }

  // Sampling machinery
//  std::random_device rd;
//  std::mt19937 gen(rd());
  std::discrete_distribution<> d(weights.begin(), weights.end());

  // Determine what proportion of the population is random
//  double random_population_ratio = (disp.x == 0) ? 0 : 0.01;
  double random_population_ratio = 0.01;
  double fixed_population_ratio = 1 - random_population_ratio;
  double P_RANDOM_BOUND = lastBeaconPtr->visionDistance; // Max distance random particles can be spawned from the mean

  // Resampling
  vector<Particle> winners;
  for (int i = 0; i < NUM_PARTICLES; i++) {
	  if (i < NUM_PARTICLES * fixed_population_ratio) {
		  winners.push_back(particles()[d(generator)]);
	  } else {
		  Particle p;
		  float coin = -1+2*((float)rand())/RAND_MAX;
		  if (coin > 0.75) {
			  int bound_reject_counter = 0;
			  // Randomly generate particles only around the circumference of the circle around the last seen beacon
			  do {
				  // Solve for positions based on the last beacon position
				  float x_sign = -1+2*((float)rand())/RAND_MAX;
				  float y_sign = -1+2*((float)rand())/RAND_MAX;

				  // Fix x solve y
				  int x_offset = rand() % static_cast<int>(lastBeaconPtr->visionDistance);
				  int y_offset = sqrt(pow(lastBeaconPtr->visionDistance, 2) - pow(x_offset, 2));

				  // Sign
				  x_sign > 0 ? p.x = lastBeaconPtr->loc.x + x_offset : p.x = lastBeaconPtr->loc.x - x_offset;
				  y_sign > 0 ? p.y = lastBeaconPtr->loc.y + y_offset : p.y = lastBeaconPtr->loc.y - y_offset;

				  // Random angle
				  p.t = (static_cast<double>(rand()) / RAND_MAX) * 2 * M_PI - M_PI;


				  // Try to reject particles that are too far from the current mean_
				  // This might not always work so stop after a few tries
				  if (sqrt(pow(p.x-mean_.x, 2) + pow(p.y-mean_.y, 2)) < P_RANDOM_BOUND) {
					  bound_reject_counter++;
					  if (bound_reject_counter < 5) {
						  // F**k it. Just choose a random particle.
						  randomParticle(p);
						  break;
					  }
				  }
			  } while (!(MIN_FIELD_X < p.x && p.x < MAX_FIELD_X &&
					  MIN_FIELD_Y < p.y && p.y < MAX_FIELD_Y)); // Make sure the point is within bound

		  } else {

			  // Find the point on the circle closest to the mean of the particle blob
			  float target_x = lastBeaconPtr->loc.x;
			  float target_y = lastBeaconPtr->loc.y;
			  float current_x = mean_.x;
			  float current_y = mean_.y;
			  float dx = target_x - current_x;
			  float dy = target_y - current_y;
			  float to_beacon_distance = sqrt(pow(dx, 2) + pow(dy, 2));
			  float to_target_distance = (to_beacon_distance - lastBeaconPtr->visionDistance);
			  float angle = atan(dy / dx) + M_PI;
			  p.x = current_x + to_target_distance * cos(angle);
			  p.y = current_y + to_target_distance * sin(angle);
			  p.t = (static_cast<double>(rand()) / RAND_MAX) * 2 * M_PI - M_PI;

		  }

//		  randomParticle(p);
		  winners.push_back(p);
	  }
  }

  particles() = winners;
}
示例#6
0
文件: main.cpp 项目: 3991/Vibes-World
int main(){

    sf::RenderWindow window(sf::VideoMode(800, 600), "Particles");
    sf::RectangleShape rectangle, rectangle2, nexus;

    ParticleSystem particles(1000);
    sf::Clock clock;

    sf::Vector2i screenDimensions(800, 600);
    sf::Vector2i blockDimensions(10, 10);

    sf::View view1(sf::FloatRect(0, 0, 800, 600));
    sf::View view2(sf::FloatRect(800, 0, 800, 600));

    sf::View standard = window.getView();
    unsigned int size = 100;
    sf::View minimap(sf::FloatRect(0, 0, 800, 600));
    //sf::View minimap(sf::FloatRect(view1.getCenter().x, view1.getCenter().y, size, window.getSize().y*size/window.getSize().x));
    //minimap.setViewport(sf::FloatRect(1.f-(1.f*minimap.getSize().x)/window.getSize().x-0.02f, 1.f-(1.f*minimap.getSize().y)/window.getSize().y-0.02f, (1.f*minimap.getSize().x)/window.getSize().x, (1.f*minimap.getSize().y)/window.getSize().y));
    minimap.setViewport(sf::FloatRect(0.75f, 0, 0.25f, 0.25f));
    minimap.zoom(2.f);
    //view1.setViewport(sf::FloatRect(0, 0, 0.5f, 1));

    // joueur 2 (côté droit de l'écran)
    //view2.setViewport(sf::FloatRect(0.5f, 0, 0.5f, 1));


    rectangle.setOutlineThickness(3);
    rectangle.setOutlineColor(sf::Color(0, 0, 0, 255));
    rectangle.setSize({50.f, 50.f});
    rectangle.setPosition({400.f, 300.f});
    rectangle.setFillColor(sf::Color::Red);

    rectangle2.setOutlineThickness(3);
    rectangle2.setOutlineColor(sf::Color(0, 0, 0, 255));
    rectangle2.setSize({500.f, 500.f});
    rectangle2.setPosition({800.f, 0.f});
    rectangle2.setFillColor(sf::Color::Blue);

    int x = rand()%(800-800*2)+800;
    int y = rand()%(600-0)+0;
    nexus.setSize({100.f, 100.f});
     std::cout << x << " , " << y << std::endl;
    nexus.setPosition({400.f, 300.f});

    sf::Vector2f mouse;
    //view1.setCenter(rectangle.getPosition());

    while (window.isOpen()){
        sf::Event event;
        while (window.pollEvent(event)){
            if(event.type == sf::Event::Closed){
                window.close();
            }

            if(sf::Keyboard::isKeyPressed(sf::Keyboard::Left)) {
                rectangle.move(-7, 0);
                mouse = {rectangle.getPosition().x+(rectangle.getSize().x/2), rectangle.getPosition().y+(rectangle.getSize().y/2)};

                view1.setCenter(rectangle.getPosition());
                std::cout << rectangle.getPosition().x << " , " << rectangle.getPosition().y << std::endl;
            }
            if(sf::Keyboard::isKeyPressed(sf::Keyboard::Right)) {
                mouse = {rectangle.getPosition().x+(rectangle.getSize().x/2), rectangle.getPosition().y+(rectangle.getSize().y/2)};
                rectangle.move(+7,0);
                view1.setCenter(rectangle.getPosition());
                std::cout << rectangle.getPosition().x << " , " << rectangle.getPosition().y << std::endl;
            }
            if(sf::Keyboard::isKeyPressed(sf::Keyboard::Up)) {
                mouse = {rectangle.getPosition().x+(rectangle.getSize().x/2), rectangle.getPosition().y+(rectangle.getSize().y/2)};
                rectangle.move(0,-7);
                view1.setCenter(rectangle.getPosition());
                std::cout << rectangle.getPosition().x << " , " << rectangle.getPosition().y << std::endl;
            }
            if(sf::Keyboard::isKeyPressed(sf::Keyboard::Down)) {
                mouse = {rectangle.getPosition().x+(rectangle.getSize().x/2), rectangle.getPosition().y+(rectangle.getSize().y/2)};
                rectangle.move(0,+7);
                view1.setCenter(rectangle.getPosition());
                std::cout << rectangle.getPosition().x << " , " << rectangle.getPosition().y << std::endl;
            }
        }

        particles.setEmitter(mouse);
        //particles.setEmitter(window.mapPixelToCoords((sf::Vector2i)mouse));

        sf::Time elapsed = clock.restart();
        particles.update(elapsed);


        window.clear();
        window.setView(view1);

        for(int i = 0;i<nexus.getSize().x/blockDimensions.x; i++){
            for(int j = 0;j<nexus.getSize().y/blockDimensions.y;j++){
                sf::VertexArray vArray(sf::PrimitiveType::Quads, 4);
                vArray[0].position = sf::Vector2f(i*blockDimensions.x+nexus.getPosition().x, j*blockDimensions.y+nexus.getPosition().y);
                vArray[1].position = sf::Vector2f(i*blockDimensions.x+blockDimensions.x+nexus.getPosition().x, j*blockDimensions.y+nexus.getPosition().y);
                vArray[2].position = sf::Vector2f(i*blockDimensions.x+blockDimensions.x+nexus.getPosition().x, j*blockDimensions.y+blockDimensions.y+nexus.getPosition().y);
                vArray[3].position = sf::Vector2f(i*blockDimensions.x+nexus.getPosition().x, j*blockDimensions.y+blockDimensions.y+nexus.getPosition().y);

                for(int k=0;k<4;k++){
                    int red = rand() % 255;
                    int green = rand() % 255;
                    int blue = rand() % 255;

                    vArray[k].color = sf::Color(red, green, blue);
                }
                window.draw(vArray);
            }
        }


        //window.clear();

        window.setView(standard);
        window.draw(particles);
        window.draw(rectangle);

        //window.draw(rectangle2);
        //window.setView(view2);
        //window.draw(rectangle2);

        window.setView(minimap);
        window.draw(particles);
        window.draw(rectangle);

        window.display();
    }

    return 0;
}
void Foam::curveSet::calcSamples
(
    DynamicList<point>& samplingPts,
    DynamicList<label>& samplingCells,
    DynamicList<label>& samplingFaces,
    DynamicList<label>& samplingSegments,
    DynamicList<scalar>& samplingCurveDist
) const
{
    // Check sampling points
    if (sampleCoords_.size() < 2)
    {
        FatalErrorIn("curveSet::calcSamples()")
            << "Incorrect sample specification. Too few points:"
            << sampleCoords_ << exit(FatalError);
    }
    point oldPoint = sampleCoords_[0];
    for(label sampleI = 1; sampleI < sampleCoords_.size(); sampleI++)
    {
        if (mag(sampleCoords_[sampleI] - oldPoint) < SMALL)
        {
            FatalErrorIn("curveSet::calcSamples()")
                << "Incorrect sample specification."
                << " Point " << sampleCoords_[sampleI-1]
                << " at position " << sampleI-1
                << " and point " << sampleCoords_[sampleI]
                << " at position " << sampleI
                << " are too close" << exit(FatalError);
        }
        oldPoint = sampleCoords_[sampleI];
    }

    // current segment number
    label segmentI = 0;

    // starting index of current segment in samplePts
    label startSegmentI = 0;

    label sampleI = 0;

    point lastSample(GREAT, GREAT, GREAT);
    while(true)
    {
        // Get boundary intersection
        point trackPt;
        label trackCellI = -1;
        label trackFaceI = -1;

        do
        {
            const vector offset =
                sampleCoords_[sampleI+1] - sampleCoords_[sampleI];
            const scalar smallDist = mag(tol*offset);


            // Get all boundary intersections
            List<pointIndexHit> bHits = searchEngine().intersections
            (
                sampleCoords_[sampleI],
                sampleCoords_[sampleI + 1]
            );

            point bPoint(GREAT, GREAT, GREAT);
            label bFaceI = -1;

            if (bHits.size())
            {
                bPoint = bHits[0].hitPoint();
                bFaceI = bHits[0].index();
            }

            // Get tracking point

            bool isSample =
                getTrackingPoint
                (
                    sampleCoords_[sampleI+1] - sampleCoords_[sampleI],
                    sampleCoords_[sampleI],
                    bPoint,
                    bFaceI,

                    trackPt,
                    trackCellI,
                    trackFaceI
                );

            if (isSample && (mag(lastSample - trackPt) > smallDist))
            {
                //Info<< "calcSamples : getTrackingPoint returned valid sample "
                //    << "  trackPt:" << trackPt
                //    << "  trackFaceI:" << trackFaceI
                //    << "  trackCellI:" << trackCellI
                //    << "  sampleI:" << sampleI
                //    << "  dist:" << dist
                //    << endl;

                samplingPts.append(trackPt);
                samplingCells.append(trackCellI);
                samplingFaces.append(trackFaceI);

                // Convert sampling position to unique curve parameter. Get
                // fraction of distance between sampleI and sampleI+1.
                scalar dist =
                    mag(trackPt - sampleCoords_[sampleI])
                  / mag(sampleCoords_[sampleI+1] - sampleCoords_[sampleI]);
                samplingCurveDist.append(sampleI + dist);

                lastSample = trackPt;
            }

            if (trackCellI == -1)
            {
                // No intersection found. Go to next point
                sampleI++;
            }
        } while((trackCellI == -1) && (sampleI < sampleCoords_.size() - 1));

        if (sampleI == sampleCoords_.size() - 1)
        {
            //Info<< "calcSamples : Reached end of samples: "
            //    << "  sampleI now:" << sampleI
            //    << endl;
            break;
        }

        //
        // Segment sampleI .. sampleI+1 intersected by domain
        //

        // Initialize tracking starting from sampleI
        Cloud<passiveParticle> particles(mesh(), IDLList<passiveParticle>());

        passiveParticle singleParticle
        (
            particles,
            trackPt,
            trackCellI
        );

        bool bReached = trackToBoundary
        (
            singleParticle,
            sampleI,
            samplingPts,
            samplingCells,
            samplingFaces,
            samplingCurveDist
        );

        // fill sampleSegments
        for(label i = samplingPts.size() - 1; i >= startSegmentI; --i)
        {
            samplingSegments.append(segmentI);
        }

        if (!bReached)
        {
            //Info<< "calcSamples : Reached end of samples: "
            //    << "  sampleI now:" << sampleI
            //    << endl;
            break;
        }
        lastSample = singleParticle.position();


        // Find next boundary.
        sampleI++;

        if (sampleI == sampleCoords_.size() - 1)
        {
            //Info<< "calcSamples : Reached end of samples: "
            //    << "  sampleI now:" << sampleI
            //    << endl;
            break;
        }

        segmentI++;

        startSegmentI = samplingPts.size();
    }
}
示例#8
0
int main(int argc, char** argv)
{
    ros::init(argc, argv, "localizer4d");
    ros::NodeHandle node_handle;
    ros::Publisher particle_publisher = node_handle.advertise<geometry_msgs::PoseArray>("particles", 1u);

    boost::shared_ptr<MotionModel4d> motion_model = boost::shared_ptr<MotionModel4d>(new MotionModel4d());
    std::vector<double> alpha(5, 0.0);
    alpha[0] = 0.4;
    alpha[1] = 0.4;
    alpha[2] = 0.1;
    alpha[3] = 0.1;
    alpha[4] = 0.1;
    motion_model->set_alpha(alpha);
    motion_model->set_start_pose(tf::Transform::getIdentity());
    motion_model->set_start_pose_variance(5.0, 1.0, (10.0/180.0)*M_PI);

    ParticleFilter<MotionModel4d, NoSensorModel> particle_filter;
    particle_filter.set_motion_model(motion_model);
    particle_filter.set_n_particles(1000u);
    particle_filter.init();

    tf::Vector3 translation(0.1, 0.0, 0.0);
    tf::Quaternion rotation;
    rotation.setRPY(0.0, 0.0, 0.1);
    tf::Transform movement(rotation, translation);

    ros::Rate rate(5);
    while (ros::ok())
    {
        particle_filter.update_motion(movement);

        tf::Vector3 mean = particle_filter.get_mean().getOrigin();
        std::cout << "[" << mean[0] << "; " << mean[1] << "; " << mean[2] << "]" << std::endl;

        // Do not publish if no one is listening.
        if (particle_publisher.getNumSubscribers() < 1)
        {
            rate.sleep();
            continue;
        }

        // Create a particle cloud message.
        geometry_msgs::PoseArray pose_array;

        pose_array.header.stamp     = ros::Time::now();
        pose_array.header.frame_id  = "map";

        // Fill the cloud with particles.
        std::vector<Particle> particles(particle_filter.get_particles());
        for (size_t i = 0u; i < particles.size(); ++i)
        {
            geometry_msgs::Pose pose;
            tf::poseTFToMsg(particles[i].pose, pose);

            pose_array.poses.push_back(pose);
        }

        particle_publisher.publish(pose_array);

        ros::spinOnce();

        rate.sleep();
    }

    return 0;
}
void Foam::faceOnlySet::calcSamples
(
    DynamicList<point>& samplingPts,
    dynamicLabelList& samplingCells,
    dynamicLabelList& samplingFaces,
    dynamicLabelList& samplingSegments,
    DynamicList<scalar>& samplingCurveDist
) const
{
    // distance vector between sampling points
    if (mag(end_ - start_) < SMALL)
    {
        FatalErrorIn("faceOnlySet::calcSamples()")
            << "Incorrect sample specification :"
            << " start equals end point." << endl
            << "  start:" << start_
            << "  end:" << end_
            << exit(FatalError);
    }

    const vector offset = (end_ - start_);
    const vector normOffset = offset/mag(offset);
    const vector smallVec = tol*offset;
    const scalar smallDist = mag(smallVec);


    // Get all boundary intersections
    List<pointIndexHit> bHits = searchEngine().intersections
    (
        start_ - smallVec,
        end_ + smallVec
    );

    point bPoint(GREAT, GREAT, GREAT);
    label bFaceI = -1;

    if (bHits.size())
    {
        bPoint = bHits[0].hitPoint();
        bFaceI = bHits[0].index();
    }

    // Get first tracking point. Use bPoint, bFaceI if provided.

    point trackPt;
    label trackCellI = -1;
    label trackFaceI = -1;

    //Info<< "before getTrackingPoint : bPoint:" << bPoint
    //    << " bFaceI:" << bFaceI << endl;

    getTrackingPoint
    (
        offset,
        start_,
        bPoint,
        bFaceI,

        trackPt,
        trackCellI,
        trackFaceI
    );

    //Info<< "after getTrackingPoint : "
    //    << " trackPt:" << trackPt
    //    << " trackCellI:" << trackCellI
    //    << " trackFaceI:" << trackFaceI
    //    << endl;

    if (trackCellI == -1)
    {
        // Line start_ - end_ does not intersect domain at all.
        // (or is along edge)
        // Set points and cell/face labels to empty lists
        //Info<< "calcSamples : Both start_ and end_ outside domain"
        //    << endl;

        return;
    }

    if (trackFaceI == -1)
    {
        // No boundary face. Check for nearish internal face
        trackFaceI = findNearFace(trackCellI, trackPt, smallDist);
    }

    //Info<< "calcSamples : got first point to track from :"
    //    << "  trackPt:" << trackPt
    //    << "  trackCell:" << trackCellI
    //    << "  trackFace:" << trackFaceI
    //    << endl;

    //
    // Track until hit end of all boundary intersections
    //

    // current segment number
    label segmentI = 0;

    // starting index of current segment in samplePts
    label startSegmentI = 0;

    // index in bHits; current boundary intersection
    label bHitI = 1;

    while(true)
    {
        if (trackFaceI != -1)
        {
            //Info<< "trackPt:" << trackPt << " on face so use." << endl;
            samplingPts.append(trackPt);
            samplingCells.append(trackCellI);
            samplingFaces.append(trackFaceI);
            samplingCurveDist.append(mag(trackPt - start_));
        }

        // Initialize tracking starting from trackPt
        Cloud<passiveParticle> particles(mesh(), IDLList<passiveParticle>());

        passiveParticle singleParticle
        (
            particles,
            trackPt,
            trackCellI
        );

        bool reachedBoundary = trackToBoundary
        (
            singleParticle,
            samplingPts,
            samplingCells,
            samplingFaces,
            samplingCurveDist
        );

        // fill sampleSegments
        for(label i = samplingPts.size() - 1; i >= startSegmentI; --i)
        {
            samplingSegments.append(segmentI);
        }


        if (!reachedBoundary)
        {
            //Info<< "calcSamples : Reached end of samples: "
            //    << "  samplePt now:" << singleParticle.position()
            //    << endl;
            break;
        }


        // Go past boundary intersection where tracking stopped
        // Use coordinate comparison instead of face comparison for
        // accuracy reasons

        bool foundValidB = false;

        while (bHitI < bHits.size())
        {
            scalar dist =
                (bHits[bHitI].hitPoint() - singleParticle.position())
              & normOffset;

            //Info<< "Finding next boundary : "
            //    << "bPoint:" << bHits[bHitI].hitPoint()
            //    << "  tracking:" << singleParticle.position()
            //    << "  dist:" << dist
            //    << endl;

            if (dist > smallDist)
            {
                // hitpoint is past tracking position
                foundValidB = true;
                break;
            }
            else
            {
                bHitI++;
            }
        }

        if (!foundValidB)
        {
            // No valid boundary intersection found beyond tracking position
            break;
        }

        // Update starting point for tracking
        trackFaceI = bHits[bHitI].index();
        trackPt = pushIn(bHits[bHitI].hitPoint(), trackFaceI);
        trackCellI = getBoundaryCell(trackFaceI);

        segmentI++;

        startSegmentI = samplingPts.size();
    }
}
示例#10
0
bool GeometryShadersWindow::CreateScene()
{
    std::string filename;
#if defined(USE_DRAW_DIRECT)
    filename = mEnvironment.GetPath("RandomSquares.hlsl");
#else
    filename = mEnvironment.GetPath("RandomSquaresIndirect.hlsl");
#endif
    std::shared_ptr<VisualProgram> program =
        mProgramFactory.CreateFromFiles(filename, filename, filename);
    if (!program)
    {
        return false;
    }

    // Create particles used by direct and indirect drawing.
    struct Vertex
    {
        Vector3<float> position;
        Vector3<float> color;
        float size;
    };

    // Use a Mersenne twister engine for random numbers.
    std::mt19937 mte;
    std::uniform_real_distribution<float> symr(-1.0f, 1.0f);
    std::uniform_real_distribution<float> unir(0.0f, 1.0f);
    std::uniform_real_distribution<float> posr(0.01f, 0.1f);

    int const numParticles = 128;
    std::vector<Vertex> particles(numParticles);
    for (auto& particle : particles)
    {
        particle.position = { symr(mte), symr(mte), symr(mte) };
        particle.color = { unir(mte), unir(mte), unir(mte) };
        particle.size = posr(mte);
    }

    // Create the constant buffer used by direct and indirect drawing.
    mMatrices = std::make_shared<ConstantBuffer>(
        2 * sizeof(Matrix4x4<float>), true);
    program->GetGShader()->Set("Matrices", mMatrices);

#if defined(USE_DRAW_DIRECT)
    // Create a mesh for direct drawing.
    VertexFormat vformat;
    vformat.Bind(VA_POSITION, DF_R32G32B32_FLOAT, 0);
    vformat.Bind(VA_COLOR, DF_R32G32B32_FLOAT, 0);
    vformat.Bind(VA_TEXCOORD, DF_R32_FLOAT, 0);
    std::shared_ptr<VertexBuffer> vbuffer(new VertexBuffer(vformat,
        numParticles));
    Memcpy(vbuffer->GetData(), &particles[0], numParticles*sizeof(Vertex));
#else
    // Create a mesh for indirect drawing.
    std::shared_ptr<VertexBuffer> vbuffer(new VertexBuffer(numParticles));
    mParticles = std::make_shared<StructuredBuffer>(numParticles,
        sizeof(Vertex));
    Memcpy(mParticles->GetData(), &particles[0], numParticles*sizeof(Vertex));
    gshader->Set("particles", mParticles);
#endif

    std::shared_ptr<IndexBuffer> ibuffer(new IndexBuffer(IP_POLYPOINT,
        numParticles));

    std::shared_ptr<VisualEffect> effect =
        std::make_shared<VisualEffect>(program);

    mMesh = std::make_shared<Visual>(vbuffer, ibuffer, effect);
    return true;
}
示例#11
0
/*===========================================================================*/
void CellByCellUniformSampling::generate_particles( const kvs::UnstructuredVolumeObject* volume )
{
    CellByCellSampling::ParticleDensityMap density_map;
    density_map.setSamplingStep( m_sampling_step );
    density_map.attachCamera( m_camera );
    density_map.attachObject( volume );
    density_map.create( BaseClass::transferFunction().opacityMap() );

    const size_t ncells = volume->numberOfCells();
    const kvs::ColorMap color_map( BaseClass::transferFunction().colorMap() );

    // Calculate number of particles
    size_t N = 0;
    kvs::ValueArray<kvs::UInt32> nparticles( ncells );
    KVS_OMP_PARALLEL()
    {
        kvs::CellBase* cell = CellByCellSampling::Cell( volume );
        CellByCellSampling::CellSampler sampler( cell, &density_map );
        KVS_OMP_FOR( reduction(+:N) )
        for ( size_t index = 0; index < ncells; ++index )
        {
            sampler.bind( index );
            const size_t n = sampler.numberOfParticles();
            nparticles[index] = n;

            N += n;
        }

        delete cell;
    }

    // Generate particles
    const kvs::UInt32 repetitions = m_repetition_level;
    CellByCellSampling::ColoredParticles particles( color_map );
    particles.allocate( N * repetitions );
    KVS_OMP_PARALLEL()
    {
        kvs::CellBase* cell = CellByCellSampling::Cell( volume );
        CellByCellSampling::CellSampler sampler( cell, &density_map );

        KVS_OMP_FOR( schedule(static) )
        for ( kvs::UInt32 r = 0; r < repetitions; ++r )
        {
            size_t particle_index_counter = N * r;
            for ( size_t index = 0; index < ncells; ++index )
            {
                const size_t n = nparticles[index];
                if ( n == 0 ) continue;

                sampler.bind( index );
                for ( size_t i = 0; i < n; ++i )
                {
                    sampler.sample();
                    const CellByCellSampling::Particle& p = sampler.accept();
                    const size_t particle_index = particle_index_counter++;
                    particles.push( particle_index, p );
                }
            }
        }

        delete cell;
    }

    SuperClass::setCoords( particles.coords() );
    SuperClass::setColors( particles.colors() );
    SuperClass::setNormals( particles.normals() );
    SuperClass::setSize( 1.0f );
}
示例#12
0
void CellByCellUniformSampling::generate_particles( const kvs::StructuredVolumeObject* volume )
{
    CellByCellSampling::ParticleDensityMap density_map;
    density_map.setSamplingStep( m_sampling_step );
    density_map.attachCamera( m_camera );
    density_map.attachObject( volume );
    density_map.create( BaseClass::transferFunction().opacityMap() );

    const kvs::Vec3ui ncells( volume->resolution() - kvs::Vector3ui::All(1) );
    const kvs::ColorMap color_map( BaseClass::transferFunction().colorMap() );

    // Calculate number of particles.
    size_t N = 0;
    kvs::ValueArray<kvs::UInt32> nparticles( ncells.x() * ncells.y() * ncells.z() );
    KVS_OMP_PARALLEL()
    {
        kvs::TrilinearInterpolator interpolator( volume );
        CellByCellSampling::GridSampler<T> sampler( &interpolator, &density_map );

        KVS_OMP_FOR( reduction(+:N) )
        for ( kvs::UInt32 z = 0; z < ncells.z(); ++z )
        {
            size_t cell_index_counter = z * ncells.x() * ncells.y();
            for ( kvs::UInt32 y = 0; y < ncells.y(); ++y )
            {
                for ( kvs::UInt32 x = 0; x < ncells.x(); ++x )
                {
                    sampler.bind( kvs::Vec3ui( x, y, z ) );
                    const size_t n = sampler.numberOfParticles();
                    const kvs::UInt32 index = cell_index_counter++;
                    nparticles[index] = n;
                    N += n;
                }
            }
        }
    }

    // Genrate a set of particles.
    const kvs::UInt32 repetitions = m_repetition_level;
    CellByCellSampling::ColoredParticles particles( color_map );
    particles.allocate( N * repetitions );
/*
    kvs::ValueArray<kvs::Real32> coords( 3 * N * repetitions );
    kvs::ValueArray<kvs::Real32> normals( 3 * N * repetitions );
    kvs::ValueArray<kvs::UInt8> colors( 3 * N * repetitions );
*/
    KVS_OMP_PARALLEL()
    {
        kvs::TrilinearInterpolator interpolator( volume );
        CellByCellSampling::GridSampler<T> sampler( &interpolator, &density_map );
        KVS_OMP_FOR( schedule(static) )
        for ( kvs::UInt32 r = 0; r < repetitions; ++r )
        {
            size_t cell_index_counter = 0;
            size_t particle_index_counter = N * r;
            for ( kvs::UInt32 z = 0; z < ncells.z(); ++z )
            {
                for ( kvs::UInt32 y = 0; y < ncells.y(); ++y )
                {
                    for ( kvs::UInt32 x = 0; x < ncells.x(); ++x )
                    {
                        const kvs::UInt32 index = cell_index_counter++;
                        const size_t n = nparticles[index];
                        if ( n == 0 ) continue;

                        sampler.bind( kvs::Vec3ui( x, y, z ) );
                        for ( size_t i = 0; i < n; ++i )
                        {
                            sampler.sample();
                            const CellByCellSampling::Particle& p = sampler.accept();
                            const size_t particle_index = particle_index_counter++;
                            particles.push( particle_index, p );
                            /*
                            const kvs::RGBColor color = color_map.at( p.scalar );
                            const size_t index3 = ( particle_index_counter++ ) * 3;
                            coords[ index3 + 0 ] = p.coord.x();
                            coords[ index3 + 1 ] = p.coord.y();
                            coords[ index3 + 2 ] = p.coord.z();
                            normals[ index3 + 0 ] = p.normal.x();
                            normals[ index3 + 1 ] = p.normal.y();
                            normals[ index3 + 2 ] = p.normal.z();
                            colors[ index3 + 0 ] = color.r();
                            colors[ index3 + 1 ] = color.g();
                            colors[ index3 + 2 ] = color.b();
                            */
                        }
                    }
                }
            }
        }
    }

    SuperClass::setCoords( particles.coords() );
    SuperClass::setColors( particles.colors() );
    SuperClass::setNormals( particles.normals() );
    SuperClass::setSize( 1.0f );
}