00001 #include "stage.hh"
00002 #include "worldfile.hh"
00003 #include "canvas.hh"
00004 #include "texture_manager.hh"
00005 using namespace Stg;
00006
00007
00008 static const Color BUBBLE_FILL( 1.0, 0.8, 0.8 );
00009 static const Color BUBBLE_BORDER( 0,0,0 );
00010 static const Color BUBBLE_TEXT( 0,0,0 );
00011
00012 void Model::DrawSelected()
00013 {
00014 glPushMatrix();
00015
00016 glTranslatef( pose.x, pose.y, pose.z+0.01 );
00017
00018 Pose gp = GetGlobalPose();
00019
00020 char buf[64];
00021 snprintf( buf, 63, "%s [%.2f %.2f %.2f %.2f]",
00022 Token(), gp.x, gp.y, gp.z, rtod(gp.a) );
00023
00024 PushColor( 0,0,0,1 );
00025 Gl::draw_string( 0.5,0.5,0.5, buf );
00026
00027 glRotatef( rtod(pose.a), 0,0,1 );
00028
00029 Gl::pose_shift( geom.pose );
00030
00031 double dx = geom.size.x / 2.0 * 1.6;
00032 double dy = geom.size.y / 2.0 * 1.6;
00033
00034 PopColor();
00035
00036 PushColor( 0,1,0,0.4 );
00037 glRectf( -dx, -dy, dx, dy );
00038 PopColor();
00039
00040 PushColor( 0,1,0,0.8 );
00041 glLineWidth( 1 );
00042 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
00043 glRectf( -dx, -dy, dx, dy );
00044 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
00045 PopColor();
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121 glPopMatrix();
00122
00123 }
00124
00125
00126 void Model::DrawTrailFootprint()
00127 {
00128 double darkness = 0;
00129 double fade = 0.5 / (double)(trail_length+1);
00130
00131 PushColor( 0,0,0,1 );
00132
00133
00134 for( unsigned int i=0; i<trail_length; i++ )
00135 {
00136
00137 TrailItem& checkpoint =
00138 trail[ (i + trail_index) % trail_length ];
00139
00140
00141 if( checkpoint.time == 0 )
00142 continue;
00143
00144 glPushMatrix();
00145 Pose pz = checkpoint.pose;
00146
00147 Gl::pose_shift( pz );
00148 Gl::pose_shift( geom.pose );
00149
00150 darkness += fade;
00151 Color c = checkpoint.color;
00152 c.a = darkness;
00153 glColor4f( c.r, c.g, c.b, c.a );
00154
00155 blockgroup.DrawFootPrint( geom );
00156
00157 glPopMatrix();
00158 }
00159
00160 PopColor();
00161 }
00162
00163 void Model::DrawTrailBlocks()
00164 {
00165 double timescale = 0.0000001;
00166
00167 FOR_EACH( it, trail )
00168 {
00169 TrailItem& checkpoint = *it;
00170
00171 glPushMatrix();
00172 Pose pz = checkpoint.pose;
00173 pz.z = (world->sim_time - checkpoint.time) * timescale;
00174
00175 Gl::pose_shift( pz );
00176 Gl::pose_shift( geom.pose );
00177
00178 DrawBlocks();
00179
00180 glPopMatrix();
00181 }
00182 }
00183
00184 void Model::DrawTrailArrows()
00185 {
00186 double dx = 0.2;
00187 double dy = 0.07;
00188 double timescale = 1e-7;
00189
00190 PushColor( 0,0,0,1 );
00191
00192 FOR_EACH( it, trail )
00193 {
00194 TrailItem& checkpoint = *it;
00195
00196 glPushMatrix();
00197 Pose pz = checkpoint.pose;
00198
00199 pz.z = (world->sim_time - checkpoint.time) * timescale;
00200
00201 Gl::pose_shift( pz );
00202 Gl::pose_shift( geom.pose );
00203
00204 Color& c = checkpoint.color;
00205 glColor4f( c.r, c.g, c.b, c.a );
00206
00207 glBegin( GL_TRIANGLES );
00208 glVertex3f( 0, -dy, 0);
00209 glVertex3f( dx, 0, 0 );
00210 glVertex3f( 0, +dy, 0 );
00211 glEnd();
00212
00213 glPopMatrix();
00214 }
00215
00216 PopColor();
00217 }
00218
00219 void Model::DrawOriginTree()
00220 {
00221 DrawPose( GetGlobalPose() );
00222
00223 FOR_EACH( it, children )
00224 (*it)->DrawOriginTree();
00225 }
00226
00227
00228 void Model::DrawBlocksTree( )
00229 {
00230 PushLocalCoords();
00231
00232 FOR_EACH( it, children )
00233 (*it)->DrawBlocksTree();
00234
00235 DrawBlocks();
00236 PopCoords();
00237 }
00238
00239 void Model::DrawPose( Pose pose )
00240 {
00241 PushColor( 0,0,0,1 );
00242 glPointSize( 4 );
00243
00244 glBegin( GL_POINTS );
00245 glVertex3f( pose.x, pose.y, pose.z );
00246 glEnd();
00247
00248 PopColor();
00249 }
00250
00251 void Model::DrawBlocks( )
00252 {
00253 blockgroup.CallDisplayList( this );
00254 }
00255
00256 void Model::DrawBoundingBoxTree()
00257 {
00258 PushLocalCoords();
00259
00260 FOR_EACH( it, children )
00261 (*it)->DrawBoundingBoxTree();
00262
00263 DrawBoundingBox();
00264 PopCoords();
00265 }
00266
00267 void Model::DrawBoundingBox()
00268 {
00269 Gl::pose_shift( geom.pose );
00270
00271 PushColor( color );
00272
00273 glBegin( GL_QUAD_STRIP );
00274
00275 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
00276 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
00277
00278 glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
00279 glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, 0 );
00280
00281 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
00282 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );
00283
00284 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
00285 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );
00286
00287 glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
00288 glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, 0 );
00289
00290 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
00291 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
00292
00293 glEnd();
00294
00295 glBegin( GL_LINES );
00296 glVertex2f( -0.02, 0 );
00297 glVertex2f( +0.02, 0 );
00298
00299 glVertex2f( 0, -0.02 );
00300 glVertex2f( 0, +0.02 );
00301 glEnd();
00302
00303 PopColor();
00304 }
00305
00306
00307 void Model::PushLocalCoords()
00308 {
00309 glPushMatrix();
00310
00311 if( parent && parent->stack_children )
00312 glTranslatef( 0,0, parent->geom.size.z );
00313
00314 Gl::pose_shift( pose );
00315 }
00316
00317 void Model::PopCoords()
00318 {
00319 glPopMatrix();
00320 }
00321
00322 void Model::AddVisualizer( Visualizer* cv, bool on_by_default )
00323 {
00324 if( !cv )
00325 return;
00326
00327
00328 if( ! world_gui )
00329 return;
00330
00331
00332 cv_list.push_back( cv );
00333
00334
00335 Canvas* canvas = world_gui->GetCanvas();
00336 std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( cv->GetMenuName() );
00337 if( i == canvas->_custom_options.end() ) {
00338 Option* op = new Option( cv->GetMenuName(),
00339 cv->GetWorldfileName(),
00340 "",
00341 on_by_default,
00342 world_gui );
00343 canvas->_custom_options[ cv->GetMenuName() ] = op;
00344 RegisterOption( op );
00345 }
00346 }
00347
00348 void Model::RemoveVisualizer( Visualizer* cv )
00349 {
00350 if( cv )
00351 EraseAll( cv, cv_list );
00352
00353
00354 }
00355
00356
00357 void Model::DrawStatusTree( Camera* cam )
00358 {
00359 PushLocalCoords();
00360 DrawStatus( cam );
00361 FOR_EACH( it, children )
00362 (*it)->DrawStatusTree( cam );
00363 PopCoords();
00364 }
00365
00366 void Model::DrawStatus( Camera* cam )
00367 {
00368 if( power_pack || !say_string.empty() )
00369 {
00370 float pitch = - cam->pitch();
00371 float yaw = - cam->yaw();
00372
00373 Pose gpz = GetGlobalPose();
00374
00375 float robotAngle = -rtod(gpz.a);
00376 glPushMatrix();
00377
00378
00379 glTranslatef( 0, 0, 0.5 );
00380
00381
00382 glRotatef( robotAngle - yaw, 0,0,1 );
00383 glRotatef( -pitch, 1,0,0 );
00384
00385
00386
00387
00388 if( !say_string.empty() )
00389 {
00390 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
00391
00392
00393 glRasterPos3f( 0, 0, 0 );
00394 GLfloat pos[ 4 ];
00395 glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
00396
00397 GLboolean valid;
00398 glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid );
00399
00400 if( valid )
00401 {
00402
00403 float w = gl_width( this->say_string.c_str() );
00404 float h = gl_height();
00405
00406 GLdouble wx, wy, wz;
00407 GLint viewport[4];
00408 glGetIntegerv(GL_VIEWPORT, viewport);
00409
00410 GLdouble modelview[16];
00411 glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
00412
00413 GLdouble projection[16];
00414 glGetDoublev(GL_PROJECTION_MATRIX, projection);
00415
00416
00417 gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz );
00418 w = wx;
00419 gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz );
00420 h = wy;
00421
00422
00423 const float m = h/10;
00424
00425
00426 PushColor( BUBBLE_FILL );
00427 glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT );
00428 glPolygonMode( GL_FRONT, GL_FILL );
00429 glEnable( GL_POLYGON_OFFSET_FILL );
00430 glPolygonOffset( 1.0, 1.0 );
00431 Gl::draw_octagon( w, h, m );
00432 glDisable( GL_POLYGON_OFFSET_FILL );
00433 PopColor();
00434
00435
00436 PushColor( BUBBLE_BORDER );
00437 glLineWidth( 1 );
00438 glEnable( GL_LINE_SMOOTH );
00439 glPolygonMode( GL_FRONT, GL_LINE );
00440 Gl::draw_octagon( w, h, m );
00441 glPopAttrib();
00442 PopColor();
00443
00444 PushColor( BUBBLE_TEXT );
00445
00446 Gl::draw_string( m, 2.5*m, 0, this->say_string.c_str() );
00447 PopColor();
00448 }
00449 }
00450 glPopMatrix();
00451 }
00452
00453 if( stall )
00454 {
00455 DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 );
00456 }
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467 }
00468
00469 void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha,
00470 double width, double height )
00471 {
00472 (void)alpha;
00473
00474 float yaw, pitch;
00475 pitch = - cam->pitch();
00476 yaw = - cam->yaw();
00477
00478 float robotAngle = -rtod( GetGlobalPose().a);
00479
00480 glPolygonMode( GL_FRONT, GL_FILL );
00481
00482 glEnable(GL_TEXTURE_2D);
00483 glBindTexture( GL_TEXTURE_2D, texture_id );
00484
00485 glColor4f( 1.0, 1.0, 1.0, 1.0 );
00486 glPushMatrix();
00487
00488
00489
00490 glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 );
00491
00492
00493 glRotatef( robotAngle - yaw, 0,0,1 );
00494 glRotatef( -pitch - 90, 1,0,0 );
00495
00496
00497 glBegin(GL_QUADS);
00498 glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f );
00499 glTexCoord2f(width, 0.0f); glVertex3f( 0.25f, 0, -0.25f );
00500 glTexCoord2f(width, height); glVertex3f( 0.25f, 0, 0.25f );
00501 glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0, 0.25f );
00502 glEnd();
00503
00504 glBindTexture( GL_TEXTURE_2D, 0 );
00505 glDisable(GL_TEXTURE_2D);
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516 glPopMatrix();
00517 }
00518
00519
00520
00521
00522 void Model::DrawFlagList( void )
00523 {
00524 if( flag_list.size() < 1 )
00525 return;
00526
00527 Pose gp = GetGlobalPose();
00528 GLfloat z = 1.0;
00529
00530 for( std::list<Flag*>::reverse_iterator it( flag_list.rbegin());
00531 it != flag_list.rend();
00532 it++ )
00533 {
00534 double sz = (*it)->GetSize();
00535 double d = sz/2.0;
00536
00537 (*it)->GetColor().GLSet();
00538
00539 glVertex3f( gp.x+d, gp.y+0, gp.z+0 +z);
00540 glVertex3f( gp.x+0, gp.y+d, gp.z+0 +z);
00541 glVertex3f( gp.x+0, gp.y+0, gp.z+d +z);
00542
00543 glVertex3f( gp.x+d, gp.y+0, gp.z+0 +z);
00544 glVertex3f( gp.x+0, gp.y+d, gp.z+0 +z);
00545 glVertex3f( gp.x+0, gp.y+0, gp.z-d +z);
00546
00547
00548 glVertex3f( gp.x-d, gp.y+0, gp.z+0 +z);
00549 glVertex3f( gp.x+0, gp.y-d, gp.z+0 +z);
00550 glVertex3f( gp.x+0, gp.y+0, gp.z+d +z);
00551
00552 glVertex3f( gp.x-d, gp.y+0, gp.z+0 +z);
00553 glVertex3f( gp.x+0, gp.y+d, gp.z+0 +z);
00554 glVertex3f( gp.x+0, gp.y+0, gp.z-d +z);
00555
00556 glVertex3f( gp.x+d, gp.y+0, gp.z+0 +z);
00557 glVertex3f( gp.x+0, gp.y-d, gp.z+0 +z);
00558 glVertex3f( gp.x+0, gp.y+0, gp.z-d +z);
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573 z += sz;
00574 }
00575 }
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612 void Model::DrawPicker( void )
00613 {
00614
00615 PushLocalCoords();
00616
00617
00618 blockgroup.DrawSolid( geom );
00619
00620
00621 FOR_EACH( it, children )
00622 (*it)->DrawPicker();
00623
00624 PopCoords();
00625 }
00626
00627 void Model::DataVisualize( Camera* cam )
00628 {
00629 (void)cam;
00630 }
00631
00632 void Model::DataVisualizeTree( Camera* cam )
00633 {
00634 PushLocalCoords();
00635
00636 if( subs > 0 )
00637 {
00638 DataVisualize( cam );
00639
00640 FOR_EACH( it, cv_list )
00641 {
00642 Visualizer* vis = *it;
00643 if( world_gui->GetCanvas()->_custom_options[ vis->GetMenuName() ]->isEnabled() )
00644 vis->Visualize( this, cam );
00645 }
00646 }
00647
00648
00649 FOR_EACH( it, children )
00650 (*it)->DataVisualizeTree( cam );
00651
00652 PopCoords();
00653 }
00654
00655 void Model::DrawGrid( void )
00656 {
00657 if ( gui.grid )
00658 {
00659 PushLocalCoords();
00660
00661 bounds3d_t vol;
00662 vol.x.min = -geom.size.x/2.0;
00663 vol.x.max = geom.size.x/2.0;
00664 vol.y.min = -geom.size.y/2.0;
00665 vol.y.max = geom.size.y/2.0;
00666 vol.z.min = 0;
00667 vol.z.max = geom.size.z;
00668
00669 PushColor( 0,0,1,0.4 );
00670 Gl::draw_grid(vol);
00671 PopColor();
00672 PopCoords();
00673 }
00674 }