// ----------------------------------------------------------------------
shawn::Vec
RandomProcessPointGenerator::
random_point( void )
throw()
{
return Vec( box().lower().x() + ( uniform_random_0i_1i() *(box().upper().x()-box().lower().x()) ),
box().lower().y() + ( uniform_random_0i_1i() *(box().upper().y()-box().lower().y()) ),
box().lower().z() + ( uniform_random_0i_1i() *(box().upper().z()-box().lower().z()) ) );
}
// ----------------------------------------------------------------------
Vec
RectWorldFactory::
new_position( void )
const throw()
{
assert( width() > 0.0 );
assert( height() > 0.0 );
return Vec( uniform_random_0i_1i()*width(),
uniform_random_0i_1i()*height() );
}
// ----------------------------------------------------------------------
bool
Uniform25DPointGenerator::
make_feasible( shawn::Vec& v )
throw()
{
assert( lower_ != NULL ); assert( upper_ != NULL );
double l = lower_->value(v);
double u = upper_->value(v);
if( u<l ) return false;
v = Vec(v.x(),v.y(), l + uniform_random_0i_1i()*(u-l) );
return true;
}
// ----------------------------------------------------------------------
void
LocalizationIterLaterationModule::
iter_lateration_step( void )
throw()
{
if ( state_ == il_finished || !sound_ )
return;
if ( iteration_cnt_ >= iteration_limit_ )
state_ = il_finished;
++iteration_cnt_;
neighborhood_w().reassign_twins( twin_measure_ * observer().comm_range() );
if ( neighborhood().confident_neighbor_cnt() < min_confident_nbrs_ )
return;
Vec est_pos;
NeighborInfoList neighbors;
collect_neighbors( neighborhood(), lat_confident, neighbors );
// try to update position. if lateration fails, confidence is set to 0,
// else position is updated and confidence is set to average of all
// neighbor confidences
if ( est_pos_lateration( neighbors, est_pos, lat_confident, false ) &&
est_pos_lateration( neighbors, est_pos, lat_confident, true ) )
{
set_confidence( neighborhood().avg_neighbor_confidence() );
// check validity of new position. if check fails, there is a chance
// of 'res_acceptance_', which is normally set to 0.1, to accept
// the position anyway. this is done to avoid being trapped in a
// local minimum. moreover, if the bad position is accepted, the
// confidence is reduced by 50%.
if ( !check_residue( neighbors, est_pos, lat_confident, observer().comm_range() ) )
{
if ( res_acceptance_ > uniform_random_0i_1i() )
set_confidence( observer().confidence() / 2 );
else
{
if ( observer().confidence() == 0 ) return;
set_confidence( 0 );
node_w().clear_est_position();
send( new LocalizationIterLaterationMessage( observer().confidence() ) );
return;
}
}
// check, whether the new position is very close to the old one or
// not. if so, refinement is finished.
if ( node().has_est_position() )
{
double pos_diff = euclidean_distance( est_pos, node().est_position() );
if ( pos_diff < abort_pos_update_ * observer().comm_range() )
state_ = il_finished;
}
node_w().set_est_position( est_pos );
}
else
{
if ( observer().confidence() == 0 ) return;
set_confidence( 0 );
node_w().clear_est_position();
}
send( new LocalizationIterLaterationMessage( observer().confidence() ) );
}
