Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Newer
Older
100644 569 lines (428 sloc) 12.981 kb
e65967f powerpacks now set charging flag correctly
rtv authored
1 #include "stage.hh"
2 #include "worldfile.hh"
3 #include "canvas.hh"
4 #include "texture_manager.hh"
5 using namespace Stg;
6
7 // speech bubble colors
4ccc5cd replaced stg_color_t with Color class and added static named constructor...
rtv authored
8 static const Color BUBBLE_FILL( 1.0, 0.8, 0.8 );// light blue/grey
9 static const Color BUBBLE_BORDER( 0,0,0 ); // black
10 static const Color BUBBLE_TEXT( 0,0,0 ); // black
e65967f powerpacks now set charging flag correctly
rtv authored
11
12 void Model::DrawSelected()
13 {
14 glPushMatrix();
15
16 glTranslatef( pose.x, pose.y, pose.z+0.01 ); // tiny Z offset raises rect above grid
17
18 Pose gpose = GetGlobalPose();
19
20 char buf[64];
21 snprintf( buf, 63, "%s [%.2f %.2f %.2f %.2f]",
22 token, gpose.x, gpose.y, gpose.z, rtod(gpose.a) );
23
24 PushColor( 0,0,0,1 ); // text color black
25 Gl::draw_string( 0.5,0.5,0.5, buf );
26
27 glRotatef( rtod(pose.a), 0,0,1 );
28
29 Gl::pose_shift( geom.pose );
30
31 double dx = geom.size.x / 2.0 * 1.6;
32 double dy = geom.size.y / 2.0 * 1.6;
33
34 PopColor();
35
36 PushColor( 0,1,0,0.4 ); // highlight color blue
37 glRectf( -dx, -dy, dx, dy );
38 PopColor();
39
40 PushColor( 0,1,0,0.8 ); // highlight color blue
41 glLineWidth( 1 );
42 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
43 glRectf( -dx, -dy, dx, dy );
44 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
45 PopColor();
46
47 glPopMatrix();
48 }
49
50
51 void Model::DrawTrailFootprint()
52 {
53 for( int i=trail->len-1; i>=0; i-- )
54 {
55 stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i );
56
57 glPushMatrix();
58 Gl::pose_shift( checkpoint->pose );
59 Gl::pose_shift( geom.pose );
60
4ccc5cd replaced stg_color_t with Color class and added static named constructor...
rtv authored
61 //stg_color_unpack( checkpoint->color, &r, &g, &b, &a );
62 Color c = checkpoint->color;
63 c.a = 0.1;
64 PushColor( c );
e65967f powerpacks now set charging flag correctly
rtv authored
65
66 blockgroup.DrawFootPrint( geom );
67
68 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
4ccc5cd replaced stg_color_t with Color class and added static named constructor...
rtv authored
69 PushColor( c.r/2, c.g/2, c.b/2, 0.1 );
e65967f powerpacks now set charging flag correctly
rtv authored
70
71 blockgroup.DrawFootPrint( geom );
72
73 PopColor();
74 PopColor();
75 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
76 glPopMatrix();
77 }
78 }
79
80 void Model::DrawTrailBlocks()
81 {
82 double timescale = 0.0000001;
83
84 for( int i=trail->len-1; i>=0; i-- )
85 {
86 stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i );
87
88 Pose pose;
89 memcpy( &pose, &checkpoint->pose, sizeof(pose));
90 pose.z = (world->sim_time - checkpoint->time) * timescale;
91
92 PushLocalCoords();
93 //blockgroup.Draw( this );
94
95 DrawBlocksTree();
96 PopCoords();
97 }
98 }
99
100 void Model::DrawTrailArrows()
101 {
102 double dx = 0.2;
103 double dy = 0.07;
104 double timescale = 0.0000001;
105
106 for( unsigned int i=0; i<trail->len; i++ )
107 {
108 stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i );
109
110 Pose pose;
111 memcpy( &pose, &checkpoint->pose, sizeof(pose));
112 pose.z = (world->sim_time - checkpoint->time) * timescale;
113
114 PushLocalCoords();
115
116 // set the height proportional to age
117
118 PushColor( checkpoint->color );
119
120 glEnable(GL_POLYGON_OFFSET_FILL);
121 glPolygonOffset(1.0, 1.0);
122
123 glBegin( GL_TRIANGLES );
124 glVertex3f( 0, -dy, 0);
125 glVertex3f( dx, 0, 0 );
126 glVertex3f( 0, +dy, 0 );
127 glEnd();
128 glDisable(GL_POLYGON_OFFSET_FILL);
129
130 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
131
4ccc5cd replaced stg_color_t with Color class and added static named constructor...
rtv authored
132 Color c = checkpoint->color;
133 c.r /= 2.0;
134 c.g /= 2.0;
135 c.b /= 2.0;
136 PushColor( c ); // darker color
e65967f powerpacks now set charging flag correctly
rtv authored
137
138 glDepthMask(GL_FALSE);
139 glBegin( GL_TRIANGLES );
140 glVertex3f( 0, -dy, 0);
141 glVertex3f( dx, 0, 0 );
142 glVertex3f( 0, +dy, 0 );
143 glEnd();
144 glDepthMask(GL_TRUE);
145
146 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
147 PopColor();
148 PopColor();
149 PopCoords();
150 }
151 }
152
153 void Model::DrawOriginTree()
154 {
155 DrawPose( GetGlobalPose() );
5dee8f1 more STL-ization. may reduce performance slightly in worlds with few mod...
rtv authored
156
157 FOR_EACH( it, children )
158 (*it)->DrawOriginTree();
e65967f powerpacks now set charging flag correctly
rtv authored
159 }
160
161
162 void Model::DrawBlocksTree( )
163 {
164 PushLocalCoords();
5dee8f1 more STL-ization. may reduce performance slightly in worlds with few mod...
rtv authored
165
166 FOR_EACH( it, children )
167 (*it)->DrawBlocksTree();
168
e65967f powerpacks now set charging flag correctly
rtv authored
169 DrawBlocks();
170 PopCoords();
171 }
172
173 void Model::DrawPose( Pose pose )
174 {
175 PushColor( 0,0,0,1 );
176 glPointSize( 4 );
177
178 glBegin( GL_POINTS );
179 glVertex3f( pose.x, pose.y, pose.z );
180 glEnd();
181
182 PopColor();
183 }
184
185 void Model::DrawBlocks( )
186 {
187 blockgroup.CallDisplayList( this );
188 }
189
190 void Model::DrawBoundingBoxTree()
191 {
192 PushLocalCoords();
5dee8f1 more STL-ization. may reduce performance slightly in worlds with few mod...
rtv authored
193
194 FOR_EACH( it, children )
195 (*it)->DrawBoundingBoxTree();
196
e65967f powerpacks now set charging flag correctly
rtv authored
197 DrawBoundingBox();
198 PopCoords();
199 }
200
201 void Model::DrawBoundingBox()
202 {
203 Gl::pose_shift( geom.pose );
204
205 PushColor( color );
206
207 glBegin( GL_QUAD_STRIP );
208
209 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
210 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
211
212 glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
213 glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, 0 );
214
215 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
216 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );
217
218 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
219 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );
220
221 glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
222 glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, 0 );
223
224 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
225 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
226
227 glEnd();
228
229 glBegin( GL_LINES );
230 glVertex2f( -0.02, 0 );
231 glVertex2f( +0.02, 0 );
232
233 glVertex2f( 0, -0.02 );
234 glVertex2f( 0, +0.02 );
235 glEnd();
236
237 PopColor();
238 }
239
240 // move into this model's local coordinate frame
241 void Model::PushLocalCoords()
242 {
243 glPushMatrix();
244
245 if( parent )
246 glTranslatef( 0,0, parent->geom.size.z );
247
248 Gl::pose_shift( pose );
249 }
250
251 void Model::PopCoords()
252 {
253 glPopMatrix();
254 }
255
a9b91ec more STLing
rtv authored
256 void Model::AddVisualizer( Visualizer* cv, bool on_by_default )
e65967f powerpacks now set charging flag correctly
rtv authored
257 {
a9b91ec more STLing
rtv authored
258 if( !cv )
64382ca cleaned up cmdline args
rtv authored
259 return;
260
261 // If there's no GUI, ignore this request
262 if( ! world_gui )
263 return;
5e9262a reworked CustomVisualizer => Visualizer - tweaked API. See ModelLaser fo...
rtv authored
264
e65967f powerpacks now set charging flag correctly
rtv authored
265 //save visual instance
a9b91ec more STLing
rtv authored
266 //cv_list = g_list_append(cv_list, cv );
267 cv_list.push_back( cv );
e65967f powerpacks now set charging flag correctly
rtv authored
268
269 //register option for all instances which share the same name
270 Canvas* canvas = world_gui->GetCanvas();
a9b91ec more STLing
rtv authored
271 std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( cv->GetMenuName() );
e65967f powerpacks now set charging flag correctly
rtv authored
272 if( i == canvas->_custom_options.end() ) {
a9b91ec more STLing
rtv authored
273 Option* op = new Option( cv->GetMenuName(),
274 cv->GetWorldfileName(),
5e9262a reworked CustomVisualizer => Visualizer - tweaked API. See ModelLaser fo...
rtv authored
275 "",
427aa67 working on power models
rtv authored
276 on_by_default,
5e9262a reworked CustomVisualizer => Visualizer - tweaked API. See ModelLaser fo...
rtv authored
277 world_gui );
a9b91ec more STLing
rtv authored
278 canvas->_custom_options[ cv->GetMenuName() ] = op;
4cba3e6 cleaned up vis options and dialog
rtv authored
279 RegisterOption( op );
e65967f powerpacks now set charging flag correctly
rtv authored
280 }
281 }
282
a9b91ec more STLing
rtv authored
283 void Model::RemoveVisualizer( Visualizer* cv )
e65967f powerpacks now set charging flag correctly
rtv authored
284 {
a9b91ec more STLing
rtv authored
285 if( cv )
286 cv_list.erase( remove( cv_list.begin(), cv_list.end(), cv ));
287
e65967f powerpacks now set charging flag correctly
rtv authored
288
289 //TODO unregister option - tricky because there might still be instances attached to different models which have the same name
290 }
291
292
293 void Model::DrawStatusTree( Camera* cam )
294 {
295 PushLocalCoords();
296 DrawStatus( cam );
5dee8f1 more STL-ization. may reduce performance slightly in worlds with few mod...
rtv authored
297 FOR_EACH( it, children )
298 (*it)->DrawStatusTree( cam );
e65967f powerpacks now set charging flag correctly
rtv authored
299 PopCoords();
300 }
301
302 void Model::DrawStatus( Camera* cam )
303 {
304 // quick hack
305 // if( power_pack && power_pack->stored < 0.0 )
306 // {
307 // glPushMatrix();
308 // glTranslatef( 0.3, 0, 0.0 );
309 // DrawImage( TextureManager::getInstance()._mains_texture_id, cam, 0.85 );
310 // glPopMatrix();
311 // }
312
313 if( say_string || power_pack )
314 {
315 float yaw, pitch;
316 pitch = - cam->pitch();
317 yaw = - cam->yaw();
318
319 Pose gpz = GetGlobalPose();
320
321 float robotAngle = -rtod(gpz.a);
322 glPushMatrix();
203687c working on energy monitoring
rtv authored
323
e65967f powerpacks now set charging flag correctly
rtv authored
324 // move above the robot
325 glTranslatef( 0, 0, 0.5 );
326
327 // rotate to face screen
328 glRotatef( robotAngle - yaw, 0,0,1 );
329 glRotatef( -pitch, 1,0,0 );
330
2db3b91 fixed crash bug when a saystring was present without a powerpack
rtv authored
331 if( power_pack )
e65967f powerpacks now set charging flag correctly
rtv authored
332 power_pack->Visualize( cam );
333
334 if( say_string )
335 {
aadcd1b vis stuff
rtv authored
336 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
337
e65967f powerpacks now set charging flag correctly
rtv authored
338 //get raster positition, add gl_width, then project back to world coords
339 glRasterPos3f( 0, 0, 0 );
340 GLfloat pos[ 4 ];
341 glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
342
343 GLboolean valid;
344 glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid );
345
346 if( valid )
2db3b91 fixed crash bug when a saystring was present without a powerpack
rtv authored
347 {
aadcd1b vis stuff
rtv authored
348 //fl_font( FL_HELVETICA, 12 );
e65967f powerpacks now set charging flag correctly
rtv authored
349 float w = gl_width( this->say_string ); // scaled text width
350 float h = gl_height(); // scaled text height
351
352 GLdouble wx, wy, wz;
353 GLint viewport[4];
354 glGetIntegerv(GL_VIEWPORT, viewport);
355
356 GLdouble modelview[16];
357 glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
358
359 GLdouble projection[16];
360 glGetDoublev(GL_PROJECTION_MATRIX, projection);
361
362 //get width and height in world coords
363 gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz );
364 w = wx;
365 gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz );
366 h = wy;
367
368 // calculate speech bubble margin
369 const float m = h/10;
370
371 // draw inside of bubble
372 PushColor( BUBBLE_FILL );
373 glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT );
374 glPolygonMode( GL_FRONT, GL_FILL );
375 glEnable( GL_POLYGON_OFFSET_FILL );
376 glPolygonOffset( 1.0, 1.0 );
377 Gl::draw_octagon( w, h, m );
378 glDisable( GL_POLYGON_OFFSET_FILL );
379 PopColor();
380
381 // draw outline of bubble
382 PushColor( BUBBLE_BORDER );
383 glLineWidth( 1 );
384 glEnable( GL_LINE_SMOOTH );
385 glPolygonMode( GL_FRONT, GL_LINE );
386 Gl::draw_octagon( w, h, m );
387 glPopAttrib();
388 PopColor();
389
390 PushColor( BUBBLE_TEXT );
391 // draw text inside the bubble
aadcd1b vis stuff
rtv authored
392 Gl::draw_string( m, 2.5*m, 0, this->say_string );
e65967f powerpacks now set charging flag correctly
rtv authored
393 PopColor();
394 }
395 }
396 glPopMatrix();
397 }
398
399 if( stall )
400 {
401 DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 );
402 }
403 }
404
405 void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width, double height )
406 {
407 float yaw, pitch;
408 pitch = - cam->pitch();
409 yaw = - cam->yaw();
410 float robotAngle = -rtod(pose.a);
411
8b06ea1 fixed broken badges
rtv authored
412 glPolygonMode( GL_FRONT, GL_FILL );
413
e65967f powerpacks now set charging flag correctly
rtv authored
414 glEnable(GL_TEXTURE_2D);
415 glBindTexture( GL_TEXTURE_2D, texture_id );
416
417 glColor4f( 1.0, 1.0, 1.0, alpha );
418 glPushMatrix();
419
420 //position image above the robot
421 glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 );
422
423 // rotate to face screen
424 glRotatef( robotAngle - yaw, 0,0,1 );
425 glRotatef( -pitch - 90, 1,0,0 );
426
427 //draw a square, with the textured image
428 glBegin(GL_QUADS);
429 glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f );
430 glTexCoord2f(width, 0.0f); glVertex3f( 0.25f, 0, -0.25f );
431 glTexCoord2f(width, height); glVertex3f( 0.25f, 0, 0.25f );
432 glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0, 0.25f );
433 glEnd();
434
435 glPopMatrix();
436 glBindTexture( GL_TEXTURE_2D, 0 );
437 glDisable(GL_TEXTURE_2D);
438 }
439
440
441 void Model::DrawFlagList( void )
442 {
2c36066 fixed color bug in flags, and more STLing
rtv authored
443 if( flag_list.size() < 1 )
e65967f powerpacks now set charging flag correctly
rtv authored
444 return;
445
446 PushLocalCoords();
447
448 glPolygonMode( GL_FRONT, GL_FILL );
449
450 GLUquadric* quadric = gluNewQuadric();
451 glTranslatef(0,0,1); // jump up
452 Pose gpose = GetGlobalPose();
453 glRotatef( 180 + rtod(-gpose.a),0,0,1 );
454
2c36066 fixed color bug in flags, and more STLing
rtv authored
455 for( std::list<Flag*>::reverse_iterator it( flag_list.rbegin());
456 it != flag_list.rend();
457 it++ )
458 {
459 Flag* flag = *it;
e65967f powerpacks now set charging flag correctly
rtv authored
460
2c36066 fixed color bug in flags, and more STLing
rtv authored
461 glTranslatef( 0, 0, flag->size/2.0 );
e65967f powerpacks now set charging flag correctly
rtv authored
462
2c36066 fixed color bug in flags, and more STLing
rtv authored
463 flag->Draw( quadric );
464
e65967f powerpacks now set charging flag correctly
rtv authored
465 glTranslatef( 0, 0, flag->size/2.0 );
466 }
467
468
469 gluDeleteQuadric( quadric );
470
471 PopCoords();
472 }
473
474
475 void Model::DrawBlinkenlights()
476 {
477 PushLocalCoords();
478
479 GLUquadric* quadric = gluNewQuadric();
480 //glTranslatef(0,0,1); // jump up
481 //Pose gpose = GetGlobalPose();
482 //glRotatef( 180 + rtod(-gpose.a),0,0,1 );
483
484 for( unsigned int i=0; i<blinkenlights->len; i++ )
485 {
486 stg_blinkenlight_t* b =
487 (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
488 assert(b);
489
490 glTranslatef( b->pose.x, b->pose.y, b->pose.z );
491
492 PushColor( b->color );
493
494 if( b->enabled )
2c36066 fixed color bug in flags, and more STLing
rtv authored
495 gluQuadricDrawStyle( quadric, GLU_FILL );
e65967f powerpacks now set charging flag correctly
rtv authored
496 else
2c36066 fixed color bug in flags, and more STLing
rtv authored
497 gluQuadricDrawStyle( quadric, GLU_LINE );
498
e65967f powerpacks now set charging flag correctly
rtv authored
499 gluSphere( quadric, b->size/2.0, 8,8 );
500
501 PopColor();
502 }
503
504 gluDeleteQuadric( quadric );
505
506 PopCoords();
507 }
508
509 void Model::DrawPicker( void )
510 {
511 //PRINT_DEBUG1( "Drawing %s", token );
512 PushLocalCoords();
513
514 // draw the boxes
515 blockgroup.DrawSolid( geom );
516
517 // recursively draw the tree below this model
5dee8f1 more STL-ization. may reduce performance slightly in worlds with few mod...
rtv authored
518 FOR_EACH( it, children )
519 (*it)->DrawPicker();
e65967f powerpacks now set charging flag correctly
rtv authored
520
521 PopCoords();
522 }
523
524 void Model::DataVisualize( Camera* cam )
525 {
526 }
527
528 void Model::DataVisualizeTree( Camera* cam )
529 {
530 PushLocalCoords();
531 DataVisualize( cam ); // virtual function overridden by most model types
532
a9b91ec more STLing
rtv authored
533 for( std::list<Visualizer*>::iterator it = cv_list.begin();
534 it != cv_list.end();
535 it++ )
536 {
537 Visualizer* vis = *it;
538 if( world_gui->GetCanvas()->_custom_options[ vis->GetMenuName() ]->isEnabled() )
539 vis->Visualize( this, cam );
540 }
541
e65967f powerpacks now set charging flag correctly
rtv authored
542 // and draw the children
5dee8f1 more STL-ization. may reduce performance slightly in worlds with few mod...
rtv authored
543 FOR_EACH( it, children )
544 (*it)->DataVisualizeTree( cam );
e65967f powerpacks now set charging flag correctly
rtv authored
545
546 PopCoords();
547 }
548
549 void Model::DrawGrid( void )
550 {
551 if ( gui.grid )
552 {
553 PushLocalCoords();
554
555 stg_bounds3d_t vol;
556 vol.x.min = -geom.size.x/2.0;
557 vol.x.max = geom.size.x/2.0;
558 vol.y.min = -geom.size.y/2.0;
559 vol.y.max = geom.size.y/2.0;
560 vol.z.min = 0;
561 vol.z.max = geom.size.z;
562
563 PushColor( 0,0,1,0.4 );
564 Gl::draw_grid(vol);
565 PopColor();
566 PopCoords();
567 }
568 }
Something went wrong with that request. Please try again.