Particle Sensor::getMeasurement(const Particles &input_measurement)
{
//
auto __input = input_measurement.getParticles();
//
float len = __input.size();
//
auto it = __input.begin();
//
Particle vals = Particle(_size);
//
std::vector<float> __vals(_size);
//
for(;it<__input.end();it++)
{
//
auto in = it->getStates().begin();
//
auto vl = __vals.begin();
//
for(;in<it->getStates().end();in++,vl++)
{
//
*vl+=(*in)*(it->getWeight());
}
}
//
vals.setStates(__vals);
//
return vals;
}
Particle ExampleSystem::getState(const Particles &input_state)
{
//
auto __input = input_state.getParticles();
//
auto it = __input.begin();
//
Particle vals = Particle(_size);
//std::cout<<"size:"<<_size<<std::endl;
//
std::vector<float> __vals(_size);
//
float len = __input.size();
//
for(;it<__input.end();it++)
{
//
auto in = it->getStates().begin();
//
auto vl = __vals.begin();
//
for(;in<it->getStates().end();in++,vl++)
{
//
//std::cout<<*vl<<std::endl;
*vl+=*in*(it->getWeight());
}
}
//
vals.setStates(__vals);
//
return vals;
}
bool Sensor::updateMeasurement(const Particle &input, const Particles &input_state, Particles &output_measuresments)
{
//
const std::vector<Particle>& __input = input_state.getParticles();
//
std::vector<Particle>& __output = output_measuresments.getParticles();
//
auto val = __output.begin();
//
for(auto it=__input.begin();it<__input.end();it++,val++)
{
//
update(input,*it,*val);
}
//
return true;
}
void Particles::copyFrom(Particles &plist)
{
clear();
for(Particle* p : plist.getParticles()) {
Particle* n = new Particle();
n->setPos(p->getPos());
n->setType(p->getType());
particles.append(n);
}
}
void NBHList::Create(Particles& orgList)
{
float threshold = 0.01;
Particles plist;
plist.copyFrom(orgList);
list.clear();
Particle* p = plist[0];
plist.getParticles().removeAt(0);
}
