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();
}
}
// 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_;
}
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;
}
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();
}
}
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();
}
}
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;
}
/*===========================================================================*/
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 );
}
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 );
}