void ParticleFilterLocalizer::resample() {
ParticleList resampledParticles;
int particleCount = (int)particles.size();
int index = Math::randomInt(0, particleCount - 1);
float beta = 0.0f;
float maxProbability = 1.0f;
for (int i = 0; i < particleCount; i++) {
beta += Math::randomFloat() * 2.0f * maxProbability;
while (beta > particles[index]->probability) {
beta -= particles[index]->probability;
index = (index + 1) % particleCount;
}
resampledParticles.push_back(new Particle(
particles[index]->x,
particles[index]->y,
particles[index]->orientation,
particles[index]->probability
));
}
for (ParticleList::const_iterator it = particles.begin(); it != particles.end(); it++) {
delete *it;
}
particles.clear();
particles = resampledParticles;
}
void Cell::removeLostParticles(ParticleList &lost) {
for (ParticleList::iterator it = particles.begin(); it != particles.end();) {
const Particle &particle = *it;
if (!isInside(particle)) { // particle moved outside of cell
lost.push_back(particle); // add to lost
it = particles.erase(it); // erase particle
} else
it++;
}
}
static void SeedParticles(ParticleList& list, float dt)
{
static float time = 0;
time += dt;
unsigned int num_new = (unsigned int) (time * ParticlesPerSecond);
if (num_new == 0)
return;
time = 0;
list.reserve(list.size() + num_new);
for (unsigned int i = 0; i < num_new; ++i) {
float theta = randhashf(Seed++, 0, M_PI * 2);
float r = randhashf(Seed++, 0, SeedRadius);
float y = randhashf(Seed++, 0, InitialBand);
Particle p;
p.Px = r*std::cos(theta);
p.Py = PlumeBase + y;
p.Pz = r*std::sin(theta) + 0.125f; // Nudge the emitter towards the viewer
p.ToB = Time;
list.push_back(p);
}
}
int main( int argc, char **argv )
{
//
// process command line parameters
//
if( find_option( argc, argv, "-h" ) >= 0 )
{
printf( "Options:\n" );
printf( "-h to see this help\n" );
printf( "-n <int> to set the number of particles\n" );
printf( "-o <filename> to specify the output file name\n" );
return 0;
}
int n = read_int( argc, argv, "-n", 1000 );
char *savename = read_string( argc, argv, "-o", NULL );
//
// set up MPI
//
int n_proc, rank;
set_size(n);
int num_cells = get_num_cells();
MPI_Init( &argc, &argv );
MPI_Comm_size( MPI_COMM_WORLD, &n_proc );
MPI_Comm_rank( MPI_COMM_WORLD, &rank );
//
// allocate generic resources
//
// FILE *fsave = savename && rank == 0 ? fopen( savename, "w" ) : NULL;
FILE *fsave = savename ? fopen( savename, "w" ) : NULL;
particle_t *particles = (particle_t*) malloc( n * sizeof(particle_t) );
MPI_Datatype PARTICLE;
MPI_Type_contiguous( 6, MPI_DOUBLE, &PARTICLE );
MPI_Type_commit( &PARTICLE );
ParticleList reference_particles; // the particles needed to compute correct forces on my_particles.
ParticleList my_particles; // the particles that this process is responsible for updating.
CellMatrix cells(num_cells);
// dummy particle
particle_t dummy;
dummy.x = -1;
dummy.y = -1;
//
// set up the data partitioning across processors
//
int *partition_offsets = (int*) malloc( (n_proc+1) * sizeof(int) );
int *partition_sizes = (int*) malloc( n_proc * sizeof(int) );
//
// allocate storage for local partition
//
int rows_per_thread = (num_cells + n_proc - 1) / n_proc;
int top_row = min( rank * rows_per_thread, num_cells); // the top row that the process is responsible for.
int bottom_row = min( (rank+1) * rows_per_thread, num_cells); // the bottow row that this process needs but is not responsible for
int my_amount;
//
// initialize and distribute the particles (that's fine to leave it unoptimized)
//
ParticleList all_particles;
all_particles.clear();
init_cell_matrix(cells);
if( rank == 0 )
{
all_particles.clear();
init_particles( n, particles );
update_cells(particles, cells, n);
for(int rankId = 0; rankId < n_proc; rankId++)
{
partition_offsets[rankId] = all_particles.size();
int first_row = min( rankId * rows_per_thread, num_cells);
int last_row = min( (rankId+1) * rows_per_thread, num_cells);
ParticleList tmp;
tmp.clear();
get_particles_from_rows(first_row, last_row, &tmp, cells);
all_particles.insert(all_particles.end(), tmp.begin(), tmp.end());
partition_sizes[rankId] = tmp.size();
}
partition_offsets[n_proc] = n;
}
// broadcast all offsets and sizes so we can scatter later.
MPI_Bcast(partition_offsets, n_proc+1, MPI_INT, 0, MPI_COMM_WORLD);
MPI_Bcast(partition_sizes, n_proc, MPI_INT, 0, MPI_COMM_WORLD);
// get my_amount from the partition sizes array and rezise the my_particles vector.
my_amount = partition_sizes[rank];
my_particles.resize(my_amount);
MPI_Scatterv( &all_particles.front(), partition_sizes, partition_offsets, PARTICLE, &my_particles.front(), my_amount, PARTICLE, 0, MPI_COMM_WORLD );
clear_cells(top_row-1, bottom_row+1, cells);
add_particles_mpi(my_particles, cells);
//
// simulate a number of time steps
//
double simulation_time = read_timer( );
for( int step = 0; step < NSTEPS; step++ )
{
//
// save current step if necessary (slightly different semantics than in other codes)
//
if( fsave && (step%SAVEFREQ) == 0 )
{
if(rank == 0) // root saves to file. First it recieves particles from other processes then write to file
{
int num_waiting = n - my_particles.size();
int i;
int count = num_waiting;
// put root particles in particles array
for(i = 0; i < my_particles.size(); i++)
{
particles[i] = my_particles[i];
}
particle_t other;
// recieve particles from other processes.
while(num_waiting > 0)
{
MPI_Status stat;
MPI_Recv(&other, 1, PARTICLE, MPI_ANY_SOURCE, SAVE, MPI_COMM_WORLD, &stat);
particles[i] = other;
i++;
num_waiting--;
}
save( fsave, n, particles );
}
else // all other processes will send their particles to root(0)
{
for(int i = 0; i < my_particles.size(); i++)
{
MPI_Request req;
MPI_Isend(&my_particles[i], 1 , PARTICLE, 0, SAVE, MPI_COMM_WORLD, &req );
}
}
}
//
// compute all forces
//
ParticleList::iterator iter = my_particles.begin();
while(iter != my_particles.end())
{
particle_t *curr_particle = &(*iter);
curr_particle->ax = 0;
curr_particle->ay = 0;
apply_force(curr_particle, cells);
++iter;
}
reference_particles.clear();
ParticleList out_of_bounds;
out_of_bounds.clear();
ParticleList bounds;
bounds.clear();
iter = my_particles.begin();
/*
Iterate through all my particles and send all particles that are not our responsibility
any longer to other processes. Send first and last row particles as references to other
processes aswell.
*/
while(iter != my_particles.end())
{
move(*iter);
Point p = get_cell_index(*iter);
if(p.y < top_row || p.y >= bottom_row) // check if out of bounds
{
particle_t tmp = (*iter);
int index = out_of_bounds.size();
out_of_bounds.push_back(tmp);
iter = my_particles.erase(iter);
// send to process above or below
int target = (p.y < top_row)? rank-1 : rank+1;
MPI_Request request;
MPI_Isend(&out_of_bounds[index], 1, PARTICLE, target, NEW, MPI_COMM_WORLD, &request ); // non blocking send
continue;
}
else if(p.y == top_row && top_row > 0) // send our top row particles to the process above except if we are root
{
particle_t tmp = (*iter);
int index = bounds.size();
bounds.push_back(tmp);
MPI_Request request;
MPI_Isend(&bounds[index], 1, PARTICLE, rank-1, BOUND, MPI_COMM_WORLD, &request ); // non blocking send
}
else if(p.y == bottom_row-1 && rank < n_proc-1) // send our top row particles to the process below except if we are last
{
particle_t tmp = (*iter);
int index = bounds.size();
bounds.push_back(tmp);
MPI_Request request;
MPI_Isend(&bounds[index], 1, PARTICLE, rank+1, BOUND, MPI_COMM_WORLD, &request ); // non blocking send
}
++iter;
}
MPI_Request req;
// send message to process below and above that we are done sending particles.
if(top_row > 0) MPI_Isend(&dummy, 1, PARTICLE, rank - 1, DONE, MPI_COMM_WORLD, &req);
if(bottom_row < num_cells) MPI_Isend(&dummy, 1, PARTICLE, rank + 1, DONE, MPI_COMM_WORLD, &req);
// get ready to recieve new particles from other processes.
int isDone = 2;
if(top_row == 0) isDone--; // we are root, should only recieve from process below.
if(bottom_row == num_cells) isDone--; // we are last process, we should only recieve from process above.
particle_t new_particle;
while(isDone > 0)
{
MPI_Status status;
MPI_Recv(&new_particle, 1, PARTICLE, MPI_ANY_SOURCE, MPI_ANY_TAG, MPI_COMM_WORLD, &status);
// check what sort of particle was recieved.
if(status.MPI_TAG == DONE)
{
isDone--;
continue;
}
else if(status.MPI_TAG == NEW)
{
my_particles.push_back(new_particle);
}
else if(status.MPI_TAG == BOUND)
{
reference_particles.push_back(new_particle);
}
}
MPI_Barrier(MPI_COMM_WORLD); // wait in order to synchronize with same frame.
// update our cells with my particles and the reference particles.
clear_cells(top_row-1, bottom_row+1, cells);
add_particles_mpi(my_particles, cells);
add_particles_mpi(reference_particles, cells);
}
simulation_time = read_timer( ) - simulation_time;
if( rank == 0 )
printf( "n = %d, n_procs = %d, simulation time = %g s\n", n, n_proc, simulation_time );
//
// release resources
//
free( partition_offsets );
free( partition_sizes );
free( particles );
if( fsave )
fclose( fsave );
MPI_Finalize( );
return 0;
}
void ParticleEmitter::spawn(float delta, const AnimationTime& at, ParticleList& ps, const Matrix& m)
{
if (!mVisibility.getFrame(at.current))
{
return;
}
//换算成秒
float timeFactor = delta / 1000.0f;
mCurrentEmission += mEmitRate.getFrame(at) * timeFactor;
while(mCurrentEmission >= 1.0f)
{
if (ps.size() >= mLimitNumber /*|| !ps.empty()*/)
{
return;
}
Particle p;
p.mEmitter = this;
float speed;
float width;
float length;
float height;
float latitude;
float variation;
p.mVertices[0].texture_.x = 0;
p.mVertices[0].texture_.y = 0;
p.mVertices[1].texture_.x = 0;
p.mVertices[1].texture_.y = 1;
p.mVertices[2].texture_.x = 1;
p.mVertices[2].texture_.y = 1;
p.mVertices[3].texture_.x = 1;
p.mVertices[3].texture_.y = 0;
p.mStopMove = false;
p.mColorStart = mColorStart;
p.mColorMiddle = mColorMiddle;
p.mColorEnd = mColorEnd;
p.mAlpha = mAlpha;
speed = mSpeedKFs.getFrame(at);
width = mWidth.getFrame(at);
length = mLength.getFrame(at);
height = mHeigth.getFrame(at);
latitude = mLatitude.getFrame(at);
variation = mVariationKFs.getFrame(at);
speed *= (1.0f + randomReal(0.0f, variation));
p.mPosition = Vector3(randomReal(-width,width), randomReal(-height,height), randomReal(-length,length));
p.mPosition = m.applyVector(p.mPosition);
p.mOriginalPosition = p.mPosition;
p.mNodeOriginalPosition = m.applyToOrigin();
Matrix mtxX;
{
Quaternion q;
q.fromAngleAxis(randomReal(0,latitude * TwoPI / 360.f), Vector3::AxisX);
mtxX.make(Vector3::Zero, Vector3::One, q);
}
Matrix mtxY;
{
Quaternion q;
q.fromAngleAxis(randomReal(0,TwoPI), Vector3::AxisY);
mtxY.make(Vector3::Zero, Vector3::One, q);
}
Matrix rot;
rot.multiply(mtxX, mtxY);
p.mVelocity = rot.applyVector(Vector3(0,1,0));
p.mVelocity.normalise();
p.mVelocity = m.applyVectorNormal(p.mVelocity);
p.mVelocity *= speed;
p.mAge = 0.0f;
if(mLifeSpan < mLifeVar)
p.mLife = mLifeSpan;
else
p.mLife= randomReal(mLifeSpan - mLifeVar, mLifeSpan + mLifeVar);
p.mGravity = mGravity.getFrame(at);
p.mExplosiveForce = mExplosiveForce.getFrame(at);
//每个粒子的缩放比例都可能不同
p.mScale.x = randomReal(mScale.x - mScaleVar.x, mScale.x + mScaleVar.x);
if(mFixedSize)
{
p.mScale.y = p.mScale.z = p.mScale.x;
}
else
{
p.mScale.y = randomReal(mScale.y - mScaleVar.y, mScale.y + mScaleVar.y);
p.mScale.z = randomReal(mScale.z - mScaleVar.z, mScale.z + mScaleVar.z);
}
//粒子旋转的角度
p.mAngle = randomReal(mInitAngleBegin, mInitAngleEnd);
p.mRotateSpeed = randomReal(mRotateSpeed - mRotateSpeedVar, mRotateSpeed + mRotateSpeedVar) * timeFactor * 15.0f;
if(mHead)
{
p.mHeadFramesNum = ((mHeadLifeSpan.y - mHeadLifeSpan.x + 1.0f) * mHeadLifeSpan.z);
p.mHeadDecalFramesNum = ((mHeadDecay.y - mHeadDecay.x + 1.0f) * mHeadDecay.z);
}
else
{
p.mHeadFramesNum = 0;
p.mHeadDecalFramesNum = 0;
}
if(mTail)
{
p.mTailFramesNum = ((mTailLifeSpan.y - mTailLifeSpan.x + 1.0f) * mTailLifeSpan.z);
p.mTailDecalFramesNum = ((mTailDecay.y - mTailDecay.x + 1.0f) * mTailDecay.z);
}
else
{
p.mTailFramesNum = 0;
p.mTailDecalFramesNum = 0;
}
if(mWander)
{
p.mWanderRadius = mWanderRadius;
p.mWanderSpeed = mWanderSpeed;
p.mWander = true;
p.mWanderS = 0.0f;
Matrix m1,m2;
for(int i = 0;i < 4;i++)
{
{
Quaternion q;
q.fromAngleAxis(randomReal(0,TwoPI), Vector3::AxisX);
m1.make(Vector3::Zero, Vector3::One, q);
}
{
Quaternion q;
q.fromAngleAxis(randomReal(0,TwoPI), Vector3::AxisY);
m2.make(Vector3::Zero, Vector3::One, q);
}
Matrix mt;
mt.multiply(m1, m2);
p.mWanderCatmullRom[i] = mWanderRadius * (mt.applyVector(Vector3::AxisZ));
}
}
//
ps.push_back(p);
//
mCurrentEmission -= 1.0f;
}
}