Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Newer
Older
100644 591 lines (441 sloc) 14.15 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
8 static const stg_color_t BUBBLE_FILL = 0xFFC8C8FF; // light blue/grey
9 static const stg_color_t BUBBLE_BORDER = 0xFF000000; // black
10 static const stg_color_t BUBBLE_TEXT = 0xFF000000; // black
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 double r,g,b,a;
54
55 for( int i=trail->len-1; i>=0; i-- )
56 {
57 stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i );
58
59 glPushMatrix();
60 Gl::pose_shift( checkpoint->pose );
61 Gl::pose_shift( geom.pose );
62
63 stg_color_unpack( checkpoint->color, &r, &g, &b, &a );
64 PushColor( r, g, b, 0.1 );
65
66 blockgroup.DrawFootPrint( geom );
67
68 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
69 PushColor( r/2, g/2, b/2, 0.1 );
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 r,g,b,a;
103
104 double dx = 0.2;
105 double dy = 0.07;
106 double timescale = 0.0000001;
107
108 for( unsigned int i=0; i<trail->len; i++ )
109 {
110 stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i );
111
112 Pose pose;
113 memcpy( &pose, &checkpoint->pose, sizeof(pose));
114 pose.z = (world->sim_time - checkpoint->time) * timescale;
115
116 PushLocalCoords();
117
118 // set the height proportional to age
119
120 PushColor( checkpoint->color );
121
122 glEnable(GL_POLYGON_OFFSET_FILL);
123 glPolygonOffset(1.0, 1.0);
124
125 glBegin( GL_TRIANGLES );
126 glVertex3f( 0, -dy, 0);
127 glVertex3f( dx, 0, 0 );
128 glVertex3f( 0, +dy, 0 );
129 glEnd();
130 glDisable(GL_POLYGON_OFFSET_FILL);
131
132 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
133
134 stg_color_unpack( checkpoint->color, &r, &g, &b, &a );
135 PushColor( r/2, g/2, b/2, 1 ); // darker color
136
137 glDepthMask(GL_FALSE);
138 glBegin( GL_TRIANGLES );
139 glVertex3f( 0, -dy, 0);
140 glVertex3f( dx, 0, 0 );
141 glVertex3f( 0, +dy, 0 );
142 glEnd();
143 glDepthMask(GL_TRUE);
144
145 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
146 PopColor();
147 PopColor();
148 PopCoords();
149 }
150 }
151
152 void Model::DrawOriginTree()
153 {
154 DrawPose( GetGlobalPose() );
155 for( GList* it=children; it; it=it->next )
156 ((Model*)it->data)->DrawOriginTree();
157 }
158
159
160 void Model::DrawBlocksTree( )
161 {
162 PushLocalCoords();
163 LISTMETHOD( children, Model*, DrawBlocksTree );
164 DrawBlocks();
165 PopCoords();
166 }
167
168 void Model::DrawPose( Pose pose )
169 {
170 PushColor( 0,0,0,1 );
171 glPointSize( 4 );
172
173 glBegin( GL_POINTS );
174 glVertex3f( pose.x, pose.y, pose.z );
175 glEnd();
176
177 PopColor();
178 }
179
180 void Model::DrawBlocks( )
181 {
182 blockgroup.CallDisplayList( this );
183 }
184
185 void Model::DrawBoundingBoxTree()
186 {
187 PushLocalCoords();
188 LISTMETHOD( children, Model*, DrawBoundingBoxTree );
189 DrawBoundingBox();
190 PopCoords();
191 }
192
193 void Model::DrawBoundingBox()
194 {
195 Gl::pose_shift( geom.pose );
196
197 PushColor( color );
198
199 glBegin( GL_QUAD_STRIP );
200
201 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
202 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
203
204 glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
205 glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, 0 );
206
207 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
208 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );
209
210 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
211 glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );
212
213 glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
214 glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, 0 );
215
216 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
217 glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
218
219 glEnd();
220
221 glBegin( GL_LINES );
222 glVertex2f( -0.02, 0 );
223 glVertex2f( +0.02, 0 );
224
225 glVertex2f( 0, -0.02 );
226 glVertex2f( 0, +0.02 );
227 glEnd();
228
229 PopColor();
230 }
231
232 // move into this model's local coordinate frame
233 void Model::PushLocalCoords()
234 {
235 glPushMatrix();
236
237 if( parent )
238 glTranslatef( 0,0, parent->geom.size.z );
239
240 Gl::pose_shift( pose );
241 }
242
243 void Model::PopCoords()
244 {
245 glPopMatrix();
246 }
247
248 void Model::AddCustomVisualizer( CustomVisualizer* custom_visual )
249 {
250 if( !custom_visual )
251 return;
252
253 //Visualizations can only be added to stage when run in a GUI
254 if( world_gui == NULL ) {
255 printf( "Unable to add custom visualization - it must be run with a GUI world\n" );
256 return;
257 }
258
259 //save visual instance
260 custom_visual_list = g_list_append(custom_visual_list, custom_visual );
261
262 //register option for all instances which share the same name
263 Canvas* canvas = world_gui->GetCanvas();
264 std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( custom_visual->name() );
265 if( i == canvas->_custom_options.end() ) {
266 Option* op = new Option( custom_visual->name(), custom_visual->name(), "", true, world_gui );
267 canvas->_custom_options[ custom_visual->name() ] = op;
268 registerOption( op );
269 }
270 }
271
272 void Model::RemoveCustomVisualizer( CustomVisualizer* custom_visual )
273 {
274 if( custom_visual )
275 custom_visual_list = g_list_remove(custom_visual_list, custom_visual );
276
277 //TODO unregister option - tricky because there might still be instances attached to different models which have the same name
278 }
279
280
281 void Model::DrawStatusTree( Camera* cam )
282 {
283 PushLocalCoords();
284 DrawStatus( cam );
285 LISTMETHODARG( children, Model*, DrawStatusTree, cam );
286 PopCoords();
287 }
288
289 void Model::DrawStatus( Camera* cam )
290 {
291 // quick hack
292 // if( power_pack && power_pack->stored < 0.0 )
293 // {
294 // glPushMatrix();
295 // glTranslatef( 0.3, 0, 0.0 );
296 // DrawImage( TextureManager::getInstance()._mains_texture_id, cam, 0.85 );
297 // glPopMatrix();
298 // }
299
300 if( say_string || power_pack )
301 {
302 float yaw, pitch;
303 pitch = - cam->pitch();
304 yaw = - cam->yaw();
305
306 Pose gpz = GetGlobalPose();
307
308 float robotAngle = -rtod(gpz.a);
309 glPushMatrix();
310
311
312 // move above the robot
313 glTranslatef( 0, 0, 0.5 );
314
315 // rotate to face screen
316 glRotatef( robotAngle - yaw, 0,0,1 );
317 glRotatef( -pitch, 1,0,0 );
318
319
320 //if( ! parent )
321 // glRectf( 0,0,1,1 );
322
323 //if( power_pack->stored > 0.0 )
324 power_pack->Visualize( cam );
325
326 if( say_string )
327 {
328 //get raster positition, add gl_width, then project back to world coords
329 glRasterPos3f( 0, 0, 0 );
330 GLfloat pos[ 4 ];
331 glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
332
333 GLboolean valid;
334 glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid );
335
336 if( valid )
337 {
338
339 fl_font( FL_HELVETICA, 12 );
340 float w = gl_width( this->say_string ); // scaled text width
341 float h = gl_height(); // scaled text height
342
343 GLdouble wx, wy, wz;
344 GLint viewport[4];
345 glGetIntegerv(GL_VIEWPORT, viewport);
346
347 GLdouble modelview[16];
348 glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
349
350 GLdouble projection[16];
351 glGetDoublev(GL_PROJECTION_MATRIX, projection);
352
353 //get width and height in world coords
354 gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz );
355 w = wx;
356 gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz );
357 h = wy;
358
359 // calculate speech bubble margin
360 const float m = h/10;
361
362 // draw inside of bubble
363 PushColor( BUBBLE_FILL );
364 glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT );
365 glPolygonMode( GL_FRONT, GL_FILL );
366 glEnable( GL_POLYGON_OFFSET_FILL );
367 glPolygonOffset( 1.0, 1.0 );
368 Gl::draw_octagon( w, h, m );
369 glDisable( GL_POLYGON_OFFSET_FILL );
370 PopColor();
371
372 // draw outline of bubble
373 PushColor( BUBBLE_BORDER );
374 glLineWidth( 1 );
375 glEnable( GL_LINE_SMOOTH );
376 glPolygonMode( GL_FRONT, GL_LINE );
377 Gl::draw_octagon( w, h, m );
378 glPopAttrib();
379 PopColor();
380
381 PushColor( BUBBLE_TEXT );
382 // draw text inside the bubble
383 Gl::draw_string( 2.5*m, 2.5*m, 0, this->say_string );
384 PopColor();
385 }
386 }
387 glPopMatrix();
388 }
389
390 if( stall )
391 {
392 DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 );
393 }
394 }
395
396 void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width, double height )
397 {
398 float yaw, pitch;
399 pitch = - cam->pitch();
400 yaw = - cam->yaw();
401 float robotAngle = -rtod(pose.a);
402
403 glEnable(GL_TEXTURE_2D);
404 glBindTexture( GL_TEXTURE_2D, texture_id );
405
406 glColor4f( 1.0, 1.0, 1.0, alpha );
407 glPushMatrix();
408
409 //position image above the robot
410 glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 );
411
412 // rotate to face screen
413 glRotatef( robotAngle - yaw, 0,0,1 );
414 glRotatef( -pitch - 90, 1,0,0 );
415
416 //draw a square, with the textured image
417 glBegin(GL_QUADS);
418 glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f );
419 glTexCoord2f(width, 0.0f); glVertex3f( 0.25f, 0, -0.25f );
420 glTexCoord2f(width, height); glVertex3f( 0.25f, 0, 0.25f );
421 glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0, 0.25f );
422 glEnd();
423
424 glPopMatrix();
425 glBindTexture( GL_TEXTURE_2D, 0 );
426 glDisable(GL_TEXTURE_2D);
427 }
428
429
430 void Model::DrawFlagList( void )
431 {
432 if( flag_list == NULL )
433 return;
434
435 PushLocalCoords();
436
437 glPolygonMode( GL_FRONT, GL_FILL );
438
439 GLUquadric* quadric = gluNewQuadric();
440 glTranslatef(0,0,1); // jump up
441 Pose gpose = GetGlobalPose();
442 glRotatef( 180 + rtod(-gpose.a),0,0,1 );
443
444
445 GList* list = g_list_copy( flag_list );
446 list = g_list_reverse(list);
447
448 for( GList* item = list; item; item = item->next )
449 {
450
451 Flag* flag = (Flag*)item->data;
452
453 glTranslatef( 0, 0, flag->size/2.0 );
454
455 PushColor( flag->color );
456
457
458 glEnable(GL_POLYGON_OFFSET_FILL);
459 glPolygonOffset(1.0, 1.0);
460 gluQuadricDrawStyle( quadric, GLU_FILL );
461 gluSphere( quadric, flag->size/2.0, 4,2 );
462 glDisable(GL_POLYGON_OFFSET_FILL);
463
464 // draw the edges darker version of the same color
465 double r,g,b,a;
466 stg_color_unpack( flag->color, &r, &g, &b, &a );
467 PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a ));
468
469 gluQuadricDrawStyle( quadric, GLU_LINE );
470 gluSphere( quadric, flag->size/2.0, 4,2 );
471
472 PopColor();
473 PopColor();
474
475 glTranslatef( 0, 0, flag->size/2.0 );
476 }
477
478 g_list_free( list );
479
480 gluDeleteQuadric( quadric );
481
482 PopCoords();
483 }
484
485
486 void Model::DrawBlinkenlights()
487 {
488 PushLocalCoords();
489
490 GLUquadric* quadric = gluNewQuadric();
491 //glTranslatef(0,0,1); // jump up
492 //Pose gpose = GetGlobalPose();
493 //glRotatef( 180 + rtod(-gpose.a),0,0,1 );
494
495 for( unsigned int i=0; i<blinkenlights->len; i++ )
496 {
497 stg_blinkenlight_t* b =
498 (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
499 assert(b);
500
501 glTranslatef( b->pose.x, b->pose.y, b->pose.z );
502
503 PushColor( b->color );
504
505 if( b->enabled )
506 gluQuadricDrawStyle( quadric, GLU_FILL );
507 else
508 gluQuadricDrawStyle( quadric, GLU_LINE );
509
510 gluSphere( quadric, b->size/2.0, 8,8 );
511
512 PopColor();
513 }
514
515 gluDeleteQuadric( quadric );
516
517 PopCoords();
518 }
519
520 void Model::DrawPicker( void )
521 {
522 //PRINT_DEBUG1( "Drawing %s", token );
523 PushLocalCoords();
524
525 // draw the boxes
526 blockgroup.DrawSolid( geom );
527
528 // recursively draw the tree below this model
529 LISTMETHOD( this->children, Model*, DrawPicker );
530
531 PopCoords();
532 }
533
534 void Model::DataVisualize( Camera* cam )
535 {
536 // if( power_pack )
537 // {
538 // // back into global coords to get rid of my rotation
539 // glPushMatrix();
540 // gl_pose_inverse_shift( GetGlobalPose() );
541
542 // // shift to the top left corner of the model (roughly)
543 // glTranslatef( pose.x - geom.size.x/2.0,
544 // pose.y + geom.size.y/2.0,
545 // pose.z + geom.size.z );
546
547 // power_pack->Visualize( cam );
548 // glPopMatrix();
549 // }
550 }
551
552 void Model::DataVisualizeTree( Camera* cam )
553 {
554 PushLocalCoords();
555 DataVisualize( cam ); // virtual function overridden by most model types
556
557 CustomVisualizer* vis;
558 for( GList* item = custom_visual_list; item; item = item->next ) {
559 vis = static_cast< CustomVisualizer* >( item->data );
560 if( world_gui->GetCanvas()->_custom_options[ vis->name() ]->isEnabled() )
561 vis->DataVisualize( cam );
562 }
563
564
565 // and draw the children
566 LISTMETHODARG( children, Model*, DataVisualizeTree, cam );
567
568 PopCoords();
569 }
570
571 void Model::DrawGrid( void )
572 {
573 if ( gui.grid )
574 {
575 PushLocalCoords();
576
577 stg_bounds3d_t vol;
578 vol.x.min = -geom.size.x/2.0;
579 vol.x.max = geom.size.x/2.0;
580 vol.y.min = -geom.size.y/2.0;
581 vol.y.max = geom.size.y/2.0;
582 vol.z.min = 0;
583 vol.z.max = geom.size.z;
584
585 PushColor( 0,0,1,0.4 );
586 Gl::draw_grid(vol);
587 PopColor();
588 PopCoords();
589 }
590 }
Something went wrong with that request. Please try again.