// ----------------------------------------------------------------------
void
LocalizationDLSModule::
initialize( void )
throw(){
shawn::Node* linearizationTool;
shawn::Vec* tool;
beacons_= new std::vector<shawn::Node*>();
{
shawn::World::node_iterator iter = owner_w().owner_w().world_w().begin_nodes_w();
shawn::World::node_iterator end = owner_w().owner_w().world_w().end_nodes_w();
for( ; iter != end ; iter++ ){
LocalizationProcessor* temp= iter->get_processor_of_type_w<LocalizationProcessor>();
if(temp!=NULL){
if(temp->is_anchor() && !temp->is_server() ){
beacons_->push_back(&temp->owner_w());
}
}
}
}
linearizationTool=beacons_->front();
if(linearizationTool->has_est_position()){
shawn::Vec est_pos(linearizationTool->est_position());
tool = &est_pos;
}
else
{
shawn::Vec real_pos(linearizationTool->real_position());
tool = &real_pos;
}
beacons_->erase(beacons_->begin());
matrix_a_ = new SimpleMatrix<double>(beacons_->size(),3);
vector_r_ =new SimpleMatrix<double>(beacons_->size(),1);
std::vector<shawn::Node*>::iterator iter=beacons_->begin();
std::vector<shawn::Node*>::iterator end = beacons_->end();
for(int count =0; iter!= end; iter++,count++){
shawn::Vec* tmp_pos;
if((*iter)->has_est_position())
{
shawn::Vec est_pos((*iter)->est_position());
tmp_pos=&est_pos;
}
else
{
shawn::Vec real_pos((*iter)->real_position());
tmp_pos=&real_pos;
}
shawn::Vec pos( *tmp_pos - *tool );
matrix_a_->at(count,0)=pos.x();
matrix_a_->at(count,1)=pos.y();
matrix_a_->at(count,2)=pos.z();
vector_r_->at(count,0)=(pos.x()*pos.x()) + (pos.y()*pos.y()) +(pos.z()*pos.z()); //pos.euclidean_norm();
}
covariance_a_ = new SimpleMatrix<double>(matrix_a_->covariance());
*matrix_a_ = matrix_a_->transposed();
*matrix_a_= *covariance_a_ * *matrix_a_;
send( new LocalizationDLSInitMessage(matrix_a_,vector_r_,covariance_a_,linearizationTool,beacons_));
}
void HookSvc::ProcessDVDMark(int idx, const Point& pos)
{
DRect user_lct(GetDVDLabelLocation(idx, pos));
DPoint real_pos(PhisPos());
cont->device_to_user(real_pos.x, real_pos.y);
if( user_lct.Contains(real_pos) )
{
DPoint int_pos(real_pos - user_lct.A());
if( IsAtPicture(GetDVDMark(false), Point((int)int_pos.x, (int)int_pos.y)) )
{
pAction->AtDVDMark(idx);
}
}
}
static void show_status_line(const editor_t *editor)
{
screen_move_clear(-1);
if (editor->hide_status_line)
return;
bool mail = chkmail();
vector_size_t x;
text_line_size_t y;
real_pos(editor, &x, &y);
screen_printf("\033[1;33;44m[%s] [\033[32m%s\033[33m]"
" [\033[32m按\033[31mCtrl-Q\033[32m求救\033[33m]"
" [\033[32m%d\033[33m,\033[32m%d\033[33m]\033[K\033[m",
mail ? "\033[5;32m信\033[m\033[1;33;44m" : " ",
format_time(fb_time(), TIME_FORMAT_UTF8_ZH) + 7,
x, y);
}
