Skip to content

HTTPS clone URL

Subversion checkout URL

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