Skip to content
Newer
Older
100644 581 lines (434 sloc) 13.5 KB
e65967f powerpacks now set charging flag correctly
rtv authored Feb 9, 2009
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
5e9262a reworked CustomVisualizer => Visualizer - tweaked API. See ModelLaser…
rtv authored Mar 27, 2009
248 void Model::AddVisualizer( Visualizer* custom_visual )
e65967f powerpacks now set charging flag correctly
rtv authored Feb 9, 2009
249 {
250 if( !custom_visual )
251 return;
252
253 //Visualizations can only be added to stage when run in a GUI
5e9262a reworked CustomVisualizer => Visualizer - tweaked API. See ModelLaser…
rtv authored Mar 27, 2009
254 if( world_gui == NULL )
255 {
256 printf( "Unable to add custom visualization - it must be run with a GUI world\n" );
257 return;
258 }
259
e65967f powerpacks now set charging flag correctly
rtv authored Feb 9, 2009
260 //save visual instance
261 custom_visual_list = g_list_append(custom_visual_list, custom_visual );
262
263 //register option for all instances which share the same name
264 Canvas* canvas = world_gui->GetCanvas();
5e9262a reworked CustomVisualizer => Visualizer - tweaked API. See ModelLaser…
rtv authored Mar 27, 2009
265 std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( custom_visual->GetMenuName() );
e65967f powerpacks now set charging flag correctly
rtv authored Feb 9, 2009
266 if( i == canvas->_custom_options.end() ) {
5e9262a reworked CustomVisualizer => Visualizer - tweaked API. See ModelLaser…
rtv authored Mar 27, 2009
267 Option* op = new Option( custom_visual->GetMenuName(),
268 custom_visual->GetWorldfileName(),
269 "",
270 true,
271 world_gui );
272 canvas->_custom_options[ custom_visual->GetMenuName() ] = op;
4cba3e6 cleaned up vis options and dialog
rtv authored Feb 17, 2009
273 RegisterOption( op );
e65967f powerpacks now set charging flag correctly
rtv authored Feb 9, 2009
274 }
275 }
276
5e9262a reworked CustomVisualizer => Visualizer - tweaked API. See ModelLaser…
rtv authored Mar 27, 2009
277 void Model::RemoveVisualizer( Visualizer* custom_visual )
e65967f powerpacks now set charging flag correctly
rtv authored Feb 9, 2009
278 {
279 if( custom_visual )
280 custom_visual_list = g_list_remove(custom_visual_list, custom_visual );
281
282 //TODO unregister option - tricky because there might still be instances attached to different models which have the same name
283 }
284
285
286 void Model::DrawStatusTree( Camera* cam )
287 {
288 PushLocalCoords();
289 DrawStatus( cam );
290 LISTMETHODARG( children, Model*, DrawStatusTree, cam );
291 PopCoords();
292 }
293
294 void Model::DrawStatus( Camera* cam )
295 {
296 // quick hack
297 // if( power_pack && power_pack->stored < 0.0 )
298 // {
299 // glPushMatrix();
300 // glTranslatef( 0.3, 0, 0.0 );
301 // DrawImage( TextureManager::getInstance()._mains_texture_id, cam, 0.85 );
302 // glPopMatrix();
303 // }
304
305 if( say_string || power_pack )
306 {
307 float yaw, pitch;
308 pitch = - cam->pitch();
309 yaw = - cam->yaw();
310
311 Pose gpz = GetGlobalPose();
312
313 float robotAngle = -rtod(gpz.a);
314 glPushMatrix();
315
316
317 // move above the robot
318 glTranslatef( 0, 0, 0.5 );
319
320 // rotate to face screen
321 glRotatef( robotAngle - yaw, 0,0,1 );
322 glRotatef( -pitch, 1,0,0 );
323
324
325 //if( ! parent )
326 // glRectf( 0,0,1,1 );
327
328 //if( power_pack->stored > 0.0 )
329 power_pack->Visualize( cam );
330
331 if( say_string )
332 {
333 //get raster positition, add gl_width, then project back to world coords
334 glRasterPos3f( 0, 0, 0 );
335 GLfloat pos[ 4 ];
336 glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
337
338 GLboolean valid;
339 glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid );
340
341 if( valid )
342 {
343
344 fl_font( FL_HELVETICA, 12 );
345 float w = gl_width( this->say_string ); // scaled text width
346 float h = gl_height(); // scaled text height
347
348 GLdouble wx, wy, wz;
349 GLint viewport[4];
350 glGetIntegerv(GL_VIEWPORT, viewport);
351
352 GLdouble modelview[16];
353 glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
354
355 GLdouble projection[16];
356 glGetDoublev(GL_PROJECTION_MATRIX, projection);
357
358 //get width and height in world coords
359 gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz );
360 w = wx;
361 gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz );
362 h = wy;
363
364 // calculate speech bubble margin
365 const float m = h/10;
366
367 // draw inside of bubble
368 PushColor( BUBBLE_FILL );
369 glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT );
370 glPolygonMode( GL_FRONT, GL_FILL );
371 glEnable( GL_POLYGON_OFFSET_FILL );
372 glPolygonOffset( 1.0, 1.0 );
373 Gl::draw_octagon( w, h, m );
374 glDisable( GL_POLYGON_OFFSET_FILL );
375 PopColor();
376
377 // draw outline of bubble
378 PushColor( BUBBLE_BORDER );
379 glLineWidth( 1 );
380 glEnable( GL_LINE_SMOOTH );
381 glPolygonMode( GL_FRONT, GL_LINE );
382 Gl::draw_octagon( w, h, m );
383 glPopAttrib();
384 PopColor();
385
386 PushColor( BUBBLE_TEXT );
387 // draw text inside the bubble
388 Gl::draw_string( 2.5*m, 2.5*m, 0, this->say_string );
389 PopColor();
390 }
391 }
392 glPopMatrix();
393 }
394
395 if( stall )
396 {
397 DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 );
398 }
399 }
400
401 void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width, double height )
402 {
403 float yaw, pitch;
404 pitch = - cam->pitch();
405 yaw = - cam->yaw();
406 float robotAngle = -rtod(pose.a);
407
408 glEnable(GL_TEXTURE_2D);
409 glBindTexture( GL_TEXTURE_2D, texture_id );
410
411 glColor4f( 1.0, 1.0, 1.0, alpha );
412 glPushMatrix();
413
414 //position image above the robot
415 glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 );
416
417 // rotate to face screen
418 glRotatef( robotAngle - yaw, 0,0,1 );
419 glRotatef( -pitch - 90, 1,0,0 );
420
421 //draw a square, with the textured image
422 glBegin(GL_QUADS);
423 glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f );
424 glTexCoord2f(width, 0.0f); glVertex3f( 0.25f, 0, -0.25f );
425 glTexCoord2f(width, height); glVertex3f( 0.25f, 0, 0.25f );
426 glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0, 0.25f );
427 glEnd();
428
429 glPopMatrix();
430 glBindTexture( GL_TEXTURE_2D, 0 );
431 glDisable(GL_TEXTURE_2D);
432 }
433
434
435 void Model::DrawFlagList( void )
436 {
437 if( flag_list == NULL )
438 return;
439
440 PushLocalCoords();
441
442 glPolygonMode( GL_FRONT, GL_FILL );
443
444 GLUquadric* quadric = gluNewQuadric();
445 glTranslatef(0,0,1); // jump up
446 Pose gpose = GetGlobalPose();
447 glRotatef( 180 + rtod(-gpose.a),0,0,1 );
448
449
450 GList* list = g_list_copy( flag_list );
451 list = g_list_reverse(list);
452
453 for( GList* item = list; item; item = item->next )
454 {
455
456 Flag* flag = (Flag*)item->data;
457
458 glTranslatef( 0, 0, flag->size/2.0 );
459
460 PushColor( flag->color );
461
462
463 glEnable(GL_POLYGON_OFFSET_FILL);
464 glPolygonOffset(1.0, 1.0);
465 gluQuadricDrawStyle( quadric, GLU_FILL );
466 gluSphere( quadric, flag->size/2.0, 4,2 );
467 glDisable(GL_POLYGON_OFFSET_FILL);
468
469 // draw the edges darker version of the same color
470 double r,g,b,a;
471 stg_color_unpack( flag->color, &r, &g, &b, &a );
472 PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a ));
473
474 gluQuadricDrawStyle( quadric, GLU_LINE );
475 gluSphere( quadric, flag->size/2.0, 4,2 );
476
477 PopColor();
478 PopColor();
479
480 glTranslatef( 0, 0, flag->size/2.0 );
481 }
482
483 g_list_free( list );
484
485 gluDeleteQuadric( quadric );
486
487 PopCoords();
488 }
489
490
491 void Model::DrawBlinkenlights()
492 {
493 PushLocalCoords();
494
495 GLUquadric* quadric = gluNewQuadric();
496 //glTranslatef(0,0,1); // jump up
497 //Pose gpose = GetGlobalPose();
498 //glRotatef( 180 + rtod(-gpose.a),0,0,1 );
499
500 for( unsigned int i=0; i<blinkenlights->len; i++ )
501 {
502 stg_blinkenlight_t* b =
503 (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
504 assert(b);
505
506 glTranslatef( b->pose.x, b->pose.y, b->pose.z );
507
508 PushColor( b->color );
509
510 if( b->enabled )
511 gluQuadricDrawStyle( quadric, GLU_FILL );
512 else
513 gluQuadricDrawStyle( quadric, GLU_LINE );
514
515 gluSphere( quadric, b->size/2.0, 8,8 );
516
517 PopColor();
518 }
519
520 gluDeleteQuadric( quadric );
521
522 PopCoords();
523 }
524
525 void Model::DrawPicker( void )
526 {
527 //PRINT_DEBUG1( "Drawing %s", token );
528 PushLocalCoords();
529
530 // draw the boxes
531 blockgroup.DrawSolid( geom );
532
533 // recursively draw the tree below this model
534 LISTMETHOD( this->children, Model*, DrawPicker );
535
536 PopCoords();
537 }
538
539 void Model::DataVisualize( Camera* cam )
540 {
541 }
542
543 void Model::DataVisualizeTree( Camera* cam )
544 {
545 PushLocalCoords();
546 DataVisualize( cam ); // virtual function overridden by most model types
547
5e9262a reworked CustomVisualizer => Visualizer - tweaked API. See ModelLaser…
rtv authored Mar 27, 2009
548 Visualizer* vis;
e65967f powerpacks now set charging flag correctly
rtv authored Feb 9, 2009
549 for( GList* item = custom_visual_list; item; item = item->next ) {
5e9262a reworked CustomVisualizer => Visualizer - tweaked API. See ModelLaser…
rtv authored Mar 27, 2009
550 vis = static_cast<Visualizer* >( item->data );
551 if( world_gui->GetCanvas()->_custom_options[ vis->GetMenuName() ]->isEnabled() )
552 vis->Visualize( this, cam );
e65967f powerpacks now set charging flag correctly
rtv authored Feb 9, 2009
553 }
554
555 // and draw the children
556 LISTMETHODARG( children, Model*, DataVisualizeTree, cam );
557
558 PopCoords();
559 }
560
561 void Model::DrawGrid( void )
562 {
563 if ( gui.grid )
564 {
565 PushLocalCoords();
566
567 stg_bounds3d_t vol;
568 vol.x.min = -geom.size.x/2.0;
569 vol.x.max = geom.size.x/2.0;
570 vol.y.min = -geom.size.y/2.0;
571 vol.y.max = geom.size.y/2.0;
572 vol.z.min = 0;
573 vol.z.max = geom.size.z;
574
575 PushColor( 0,0,1,0.4 );
576 Gl::draw_grid(vol);
577 PopColor();
578 PopCoords();
579 }
580 }
Something went wrong with that request. Please try again.