Revision: 7321
http://playerstage.svn.sourceforge.net/playerstage/?rev=7321&view=rev
Author: rtv
Date: 2009-02-09 03:58:32 +0000 (Mon, 09 Feb 2009)
Log Message:
-----------
powerpacks now set charging flag correctly
Modified Paths:
--------------
code/stage/trunk/libstage/CMakeLists.txt
code/stage/trunk/libstage/blockgroup.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/powerpack.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
Added Paths:
-----------
code/stage/trunk/libstage/model_draw.cc
code/stage/trunk/libstage/model_getset.cc
Modified: code/stage/trunk/libstage/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstage/CMakeLists.txt 2009-02-07 02:18:31 UTC (rev
7320)
+++ code/stage/trunk/libstage/CMakeLists.txt 2009-02-09 03:58:32 UTC (rev
7321)
@@ -16,8 +16,10 @@
model_blinkenlight.cc
model_blobfinder.cc
model_callbacks.cc
+ model_draw.cc
model_camera.cc
model_fiducial.cc
+ model_getset.cc
model_laser.cc
model_load.cc
model_position.cc
Modified: code/stage/trunk/libstage/blockgroup.cc
===================================================================
--- code/stage/trunk/libstage/blockgroup.cc 2009-02-07 02:18:31 UTC (rev
7320)
+++ code/stage/trunk/libstage/blockgroup.cc 2009-02-09 03:58:32 UTC (rev
7321)
@@ -27,8 +27,6 @@
{
blocks = g_list_append( blocks, block );
++count;
-
- block->mod->map_caches_are_invalid = true;
}
void BlockGroup::Clear()
@@ -84,9 +82,6 @@
maxx = maxy = -billion;
size.z = 0.0; // grow to largest z we see
-
- if( blocks )
- ((Block*)blocks->data)->mod->map_caches_are_invalid = true;
for( GList* it=blocks; it; it=it->next ) // examine all the blocks
{
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-02-07 02:18:31 UTC (rev 7320)
+++ code/stage/trunk/libstage/model.cc 2009-02-09 03:58:32 UTC (rev 7321)
@@ -113,15 +113,8 @@
//#define DEBUG 0
#include "stage.hh"
#include "worldfile.hh"
-#include "canvas.hh"
-#include "texture_manager.hh"
using namespace Stg;
-// speech bubble colors
-static const stg_color_t BUBBLE_FILL = 0xFFC8C8FF; // light blue/grey
-static const stg_color_t BUBBLE_BORDER = 0xFF000000; // black
-static const stg_color_t BUBBLE_TEXT = 0xFF000000; // black
-
// static members
uint32_t Model::count = 0;
GHashTable* Model::modelsbyid = g_hash_table_new( NULL, NULL );
@@ -216,7 +209,6 @@
initfunc(NULL),
interval((stg_usec_t)1e4), // 10msec
last_update(0),
- map_caches_are_invalid( true ),
map_resolution(0.1),
mass(0),
on_update_list( false ),
@@ -318,7 +310,6 @@
blockgroup.CalcSize();
UnMap(); // remove any old cruft rendered during startup
- map_caches_are_invalid = true;
Map();
if( initfunc )
@@ -366,8 +357,6 @@
{
UnMap();
blockgroup.Clear();
- map_caches_are_invalid = true;
-
//no need to Map() - we have no blocks
NeedRedraw();
}
@@ -550,66 +539,7 @@
}
-// get the model's velocity in the global frame
-Velocity Model::GetGlobalVelocity()
-{
- Pose gpose = GetGlobalPose();
- double cosa = cos( gpose.a );
- double sina = sin( gpose.a );
-
- Velocity gv;
- gv.x = velocity.x * cosa - velocity.y * sina;
- gv.y = velocity.x * sina + velocity.y * cosa;
- gv.a = velocity.a;
-
- //printf( "local velocity %.2f %.2f %.2f\nglobal velocity %.2f %.2f %.2f\n",
- // mod->velocity.x, mod->velocity.y, mod->velocity.a,
- // gvel->x, gvel->y, gvel->a );
-
- return gv;
-}
-
-// set the model's velocity in the global frame
-void Model::SetGlobalVelocity( Velocity gv )
-{
- Pose gpose = GetGlobalPose();
-
- double cosa = cos( gpose.a );
- double sina = sin( gpose.a );
-
- Velocity lv;
- lv.x = gv.x * cosa + gv.y * sina;
- lv.y = -gv.x * sina + gv.y * cosa;
- lv.a = gv.a;
-
- this->SetVelocity( lv );
-}
-
-// get the model's position in the global frame
-Pose Model::GetGlobalPose()
-{
- // if I'm a top level model, my global pose is my local pose
- if( parent == NULL )
- return pose;
-
- // otherwise
-
- Pose global_pose = pose_sum( parent->GetGlobalPose(), pose );
-
- // we are on top of our parent
- global_pose.z += parent->geom.size.z;
-
- // PRINT_DEBUG4( "GET GLOBAL POSE [x:%.2f y:%.2f z:%.2f a:%.2f]",
- // global_pose.x,
- // global_pose.y,
- // global_pose.z,
- // global_pose.a );
-
- return global_pose;
-}
-
-
inline Pose Model::LocalToGlobal( Pose pose )
{
return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose );
@@ -640,7 +570,6 @@
// render all blocks in the group at my global pose and size
blockgroup.Map();
- map_caches_are_invalid = false;
}
@@ -770,390 +699,7 @@
last_update = world->sim_time;
}
-void Model::DrawSelected()
-{
- glPushMatrix();
-
- glTranslatef( pose.x, pose.y, pose.z+0.01 ); // tiny Z offset raises rect
above grid
-
- Pose gpose = GetGlobalPose();
-
- char buf[64];
- snprintf( buf, 63, "%s [%.2f %.2f %.2f %.2f]",
- token, gpose.x, gpose.y, gpose.z, rtod(gpose.a) );
-
- PushColor( 0,0,0,1 ); // text color black
- Gl::draw_string( 0.5,0.5,0.5, buf );
-
- glRotatef( rtod(pose.a), 0,0,1 );
-
- Gl::pose_shift( geom.pose );
-
- double dx = geom.size.x / 2.0 * 1.6;
- double dy = geom.size.y / 2.0 * 1.6;
-
- PopColor();
- PushColor( 0,1,0,0.4 ); // highlight color blue
- glRectf( -dx, -dy, dx, dy );
- PopColor();
-
- PushColor( 0,1,0,0.8 ); // highlight color blue
- glLineWidth( 1 );
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- glRectf( -dx, -dy, dx, dy );
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
- PopColor();
-
- glPopMatrix();
-}
-
-
-void Model::DrawTrailFootprint()
-{
- double r,g,b,a;
-
- for( int i=trail->len-1; i>=0; i-- )
- {
- stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t,
i );
-
- glPushMatrix();
- Gl::pose_shift( checkpoint->pose );
- Gl::pose_shift( geom.pose );
-
- stg_color_unpack( checkpoint->color, &r, &g, &b, &a );
- PushColor( r, g, b, 0.1 );
-
- blockgroup.DrawFootPrint( geom );
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- PushColor( r/2, g/2, b/2, 0.1 );
-
- blockgroup.DrawFootPrint( geom );
-
- PopColor();
- PopColor();
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
- glPopMatrix();
- }
-}
-
-void Model::DrawTrailBlocks()
-{
- double timescale = 0.0000001;
-
- for( int i=trail->len-1; i>=0; i-- )
- {
- stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t,
i );
-
- Pose pose;
- memcpy( &pose, &checkpoint->pose, sizeof(pose));
- pose.z = (world->sim_time - checkpoint->time) * timescale;
-
- PushLocalCoords();
- //blockgroup.Draw( this );
-
- DrawBlocksTree();
- PopCoords();
- }
-}
-
-void Model::DrawTrailArrows()
-{
- double r,g,b,a;
-
- double dx = 0.2;
- double dy = 0.07;
- double timescale = 0.0000001;
-
- for( unsigned int i=0; i<trail->len; i++ )
- {
- stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t,
i );
-
- Pose pose;
- memcpy( &pose, &checkpoint->pose, sizeof(pose));
- pose.z = (world->sim_time - checkpoint->time) * timescale;
-
- PushLocalCoords();
-
- // set the height proportional to age
-
- PushColor( checkpoint->color );
-
- glEnable(GL_POLYGON_OFFSET_FILL);
- glPolygonOffset(1.0, 1.0);
-
- glBegin( GL_TRIANGLES );
- glVertex3f( 0, -dy, 0);
- glVertex3f( dx, 0, 0 );
- glVertex3f( 0, +dy, 0 );
- glEnd();
- glDisable(GL_POLYGON_OFFSET_FILL);
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
-
- stg_color_unpack( checkpoint->color, &r, &g, &b, &a );
- PushColor( r/2, g/2, b/2, 1 ); // darker color
-
- glDepthMask(GL_FALSE);
- glBegin( GL_TRIANGLES );
- glVertex3f( 0, -dy, 0);
- glVertex3f( dx, 0, 0 );
- glVertex3f( 0, +dy, 0 );
- glEnd();
- glDepthMask(GL_TRUE);
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
- PopColor();
- PopColor();
- PopCoords();
- }
-}
-
-void Model::DrawOriginTree()
-{
- DrawPose( GetGlobalPose() );
- for( GList* it=children; it; it=it->next )
- ((Model*)it->data)->DrawOriginTree();
-}
-
-
-void Model::DrawBlocksTree( )
-{
- PushLocalCoords();
- LISTMETHOD( children, Model*, DrawBlocksTree );
- DrawBlocks();
- PopCoords();
-}
-
-void Model::DrawPose( Pose pose )
-{
- PushColor( 0,0,0,1 );
- glPointSize( 4 );
-
- glBegin( GL_POINTS );
- glVertex3f( pose.x, pose.y, pose.z );
- glEnd();
-
- PopColor();
-}
-
-void Model::DrawBlocks( )
-{
- blockgroup.CallDisplayList( this );
-}
-
-void Model::DrawBoundingBoxTree()
-{
- PushLocalCoords();
- LISTMETHOD( children, Model*, DrawBoundingBoxTree );
- DrawBoundingBox();
- PopCoords();
-}
-
-void Model::DrawBoundingBox()
-{
- Gl::pose_shift( geom.pose );
-
- PushColor( color );
-
- glBegin( GL_QUAD_STRIP );
-
- glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
- glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
-
- glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
- glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, 0 );
-
- glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
- glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );
-
- glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
- glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );
-
- glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
- glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, 0 );
-
- glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
- glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
-
- glEnd();
-
- glBegin( GL_LINES );
- glVertex2f( -0.02, 0 );
- glVertex2f( +0.02, 0 );
-
- glVertex2f( 0, -0.02 );
- glVertex2f( 0, +0.02 );
- glEnd();
-
- PopColor();
-}
-
-// move into this model's local coordinate frame
-void Model::PushLocalCoords()
-{
- glPushMatrix();
-
- if( parent )
- glTranslatef( 0,0, parent->geom.size.z );
-
- Gl::pose_shift( pose );
-}
-
-void Model::PopCoords()
-{
- glPopMatrix();
-}
-
-void Model::AddCustomVisualizer( CustomVisualizer* custom_visual )
-{
- if( !custom_visual )
- return;
-
- //Visualizations can only be added to stage when run in a GUI
- if( world_gui == NULL ) {
- printf( "Unable to add custom visualization - it must be run
with a GUI world\n" );
- return;
- }
-
- //save visual instance
- custom_visual_list = g_list_append(custom_visual_list, custom_visual );
-
- //register option for all instances which share the same name
- Canvas* canvas = world_gui->GetCanvas();
- std::map< std::string, Option* >::iterator i =
canvas->_custom_options.find( custom_visual->name() );
- if( i == canvas->_custom_options.end() ) {
- Option* op = new Option( custom_visual->name(),
custom_visual->name(), "", true, world_gui );
- canvas->_custom_options[ custom_visual->name() ] = op;
- registerOption( op );
- }
-}
-
-void Model::RemoveCustomVisualizer( CustomVisualizer* custom_visual )
-{
- if( custom_visual )
- custom_visual_list = g_list_remove(custom_visual_list,
custom_visual );
-
- //TODO unregister option - tricky because there might still be
instances attached to different models which have the same name
-}
-
-
-void Model::DrawStatusTree( Camera* cam )
-{
- PushLocalCoords();
- DrawStatus( cam );
- LISTMETHODARG( children, Model*, DrawStatusTree, cam );
- PopCoords();
-}
-
-void Model::DrawStatus( Camera* cam )
-{
- // quick hack
-// if( power_pack && power_pack->stored < 0.0 )
-// {
-// glPushMatrix();
-// glTranslatef( 0.3, 0, 0.0 );
-// DrawImage( TextureManager::getInstance()._mains_texture_id,
cam, 0.85 );
-// glPopMatrix();
-// }
-
- if( say_string || power_pack )
- {
- float yaw, pitch;
- pitch = - cam->pitch();
- yaw = - cam->yaw();
-
- Pose gpz = GetGlobalPose();
-
- float robotAngle = -rtod(gpz.a);
- glPushMatrix();
-
-
- // move above the robot
- glTranslatef( 0, 0, 0.5 );
-
- // rotate to face screen
- glRotatef( robotAngle - yaw, 0,0,1 );
- glRotatef( -pitch, 1,0,0 );
-
-
- //if( ! parent )
- // glRectf( 0,0,1,1 );
-
- //if( power_pack->stored > 0.0 )
- power_pack->Visualize( cam );
-
- if( say_string )
- {
- //get raster positition, add gl_width, then project
back to world coords
- glRasterPos3f( 0, 0, 0 );
- GLfloat pos[ 4 ];
- glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
-
- GLboolean valid;
- glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID,
&valid );
-
- if( valid )
- {
-
- fl_font( FL_HELVETICA, 12 );
- float w = gl_width( this->say_string ); //
scaled text width
- float h = gl_height(); // scaled text height
-
- GLdouble wx, wy, wz;
- GLint viewport[4];
- glGetIntegerv(GL_VIEWPORT, viewport);
-
- GLdouble modelview[16];
- glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
-
- GLdouble projection[16];
- glGetDoublev(GL_PROJECTION_MATRIX,
projection);
-
- //get width and height in world coords
- gluUnProject( pos[0] + w, pos[1], pos[2],
modelview, projection, viewport, &wx, &wy, &wz );
- w = wx;
- gluUnProject( pos[0], pos[1] + h, pos[2],
modelview, projection, viewport, &wx, &wy, &wz );
- h = wy;
-
- // calculate speech bubble margin
- const float m = h/10;
-
- // draw inside of bubble
- PushColor( BUBBLE_FILL );
- glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT );
- glPolygonMode( GL_FRONT, GL_FILL );
- glEnable( GL_POLYGON_OFFSET_FILL );
- glPolygonOffset( 1.0, 1.0 );
- Gl::draw_octagon( w, h, m );
- glDisable( GL_POLYGON_OFFSET_FILL );
- PopColor();
-
- // draw outline of bubble
- PushColor( BUBBLE_BORDER );
- glLineWidth( 1 );
- glEnable( GL_LINE_SMOOTH );
- glPolygonMode( GL_FRONT, GL_LINE );
- Gl::draw_octagon( w, h, m );
- glPopAttrib();
- PopColor();
-
- PushColor( BUBBLE_TEXT );
- // draw text inside the bubble
- Gl::draw_string( 2.5*m, 2.5*m, 0,
this->say_string );
- PopColor();
- }
- }
- glPopMatrix();
- }
-
- if( stall )
- {
- DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 );
- }
-}
-
stg_meters_t Model::ModelHeight()
{
stg_meters_t m_child = 0; //max size of any child
@@ -1170,272 +716,8 @@
}
-void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double
width, double height )
-{
- float yaw, pitch;
- pitch = - cam->pitch();
- yaw = - cam->yaw();
- float robotAngle = -rtod(pose.a);
- glEnable(GL_TEXTURE_2D);
- glBindTexture( GL_TEXTURE_2D, texture_id );
- glColor4f( 1.0, 1.0, 1.0, alpha );
- glPushMatrix();
-
- //position image above the robot
- glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 );
-
- // rotate to face screen
- glRotatef( robotAngle - yaw, 0,0,1 );
- glRotatef( -pitch - 90, 1,0,0 );
-
- //draw a square, with the textured image
- glBegin(GL_QUADS);
- glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f );
- glTexCoord2f(width, 0.0f); glVertex3f( 0.25f, 0, -0.25f );
- glTexCoord2f(width, height); glVertex3f( 0.25f, 0, 0.25f );
- glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0, 0.25f );
- glEnd();
-
- glPopMatrix();
- glBindTexture( GL_TEXTURE_2D, 0 );
- glDisable(GL_TEXTURE_2D);
-}
-
-
-void Model::DrawFlagList( void )
-{
- if( flag_list == NULL )
- return;
-
- PushLocalCoords();
-
- glPolygonMode( GL_FRONT, GL_FILL );
-
- GLUquadric* quadric = gluNewQuadric();
- glTranslatef(0,0,1); // jump up
- Pose gpose = GetGlobalPose();
- glRotatef( 180 + rtod(-gpose.a),0,0,1 );
-
-
- GList* list = g_list_copy( flag_list );
- list = g_list_reverse(list);
-
- for( GList* item = list; item; item = item->next )
- {
-
- Flag* flag = (Flag*)item->data;
-
- glTranslatef( 0, 0, flag->size/2.0 );
-
- PushColor( flag->color );
-
-
- glEnable(GL_POLYGON_OFFSET_FILL);
- glPolygonOffset(1.0, 1.0);
- gluQuadricDrawStyle( quadric, GLU_FILL );
- gluSphere( quadric, flag->size/2.0, 4,2 );
- glDisable(GL_POLYGON_OFFSET_FILL);
-
- // draw the edges darker version of the same color
- double r,g,b,a;
- stg_color_unpack( flag->color, &r, &g, &b, &a );
- PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a ));
-
- gluQuadricDrawStyle( quadric, GLU_LINE );
- gluSphere( quadric, flag->size/2.0, 4,2 );
-
- PopColor();
- PopColor();
-
- glTranslatef( 0, 0, flag->size/2.0 );
- }
-
- g_list_free( list );
-
- gluDeleteQuadric( quadric );
-
- PopCoords();
-}
-
-
-void Model::DrawBlinkenlights()
-{
- PushLocalCoords();
-
- GLUquadric* quadric = gluNewQuadric();
- //glTranslatef(0,0,1); // jump up
- //Pose gpose = GetGlobalPose();
- //glRotatef( 180 + rtod(-gpose.a),0,0,1 );
-
- for( unsigned int i=0; i<blinkenlights->len; i++ )
- {
- stg_blinkenlight_t* b =
- (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
- assert(b);
-
- glTranslatef( b->pose.x, b->pose.y, b->pose.z );
-
- PushColor( b->color );
-
- if( b->enabled )
- gluQuadricDrawStyle( quadric, GLU_FILL );
- else
- gluQuadricDrawStyle( quadric, GLU_LINE );
-
- gluSphere( quadric, b->size/2.0, 8,8 );
-
- PopColor();
- }
-
- gluDeleteQuadric( quadric );
-
- PopCoords();
-}
-
-void Model::DrawPicker( void )
-{
- //PRINT_DEBUG1( "Drawing %s", token );
- PushLocalCoords();
-
- // draw the boxes
- blockgroup.DrawSolid( geom );
-
- // recursively draw the tree below this model
- LISTMETHOD( this->children, Model*, DrawPicker );
-
- PopCoords();
-}
-
-void Model::DataVisualize( Camera* cam )
-{
-// if( power_pack )
-// {
-// // back into global coords to get rid of my rotation
-// glPushMatrix();
-// gl_pose_inverse_shift( GetGlobalPose() );
-
-// // shift to the top left corner of the model (roughly)
-// glTranslatef( pose.x - geom.size.x/2.0,
-// pose.y + geom.size.y/2.0,
-// pose.z + geom.size.z );
-
-// power_pack->Visualize( cam );
-// glPopMatrix();
-// }
-}
-
-void Model::DataVisualizeTree( Camera* cam )
-{
- PushLocalCoords();
- DataVisualize( cam ); // virtual function overridden by most model types
-
- CustomVisualizer* vis;
- for( GList* item = custom_visual_list; item; item = item->next ) {
- vis = static_cast< CustomVisualizer* >( item->data );
- if( world_gui->GetCanvas()->_custom_options[ vis->name() ]->isEnabled()
)
- vis->DataVisualize( cam );
- }
-
-
- // and draw the children
- LISTMETHODARG( children, Model*, DataVisualizeTree, cam );
-
- PopCoords();
-}
-
-void Model::DrawGrid( void )
-{
- if ( gui.grid )
- {
- PushLocalCoords();
-
- stg_bounds3d_t vol;
- vol.x.min = -geom.size.x/2.0;
- vol.x.max = geom.size.x/2.0;
- vol.y.min = -geom.size.y/2.0;
- vol.y.max = geom.size.y/2.0;
- vol.z.min = 0;
- vol.z.max = geom.size.z;
-
- PushColor( 0,0,1,0.4 );
- Gl::draw_grid(vol);
- PopColor();
- PopCoords();
- }
-}
-
-inline bool velocity_is_zero( Velocity& v )
-{
- return( !(v.x || v.y || v.z || v.a) );
-}
-
-void Model::SetVelocity( Velocity vel )
-{
- // printf( "SetVelocity %.2f %.2f %.2f %.2f\n",
- // vel.x,
- // vel.y,
- // vel.z,
- // vel.a );
-
- assert( ! isnan(vel.x) );
- assert( ! isnan(vel.y) );
- assert( ! isnan(vel.z) );
- assert( ! isnan(vel.a) );
-
- this->velocity = vel;
-
- if( on_velocity_list && velocity_is_zero( vel ) )
- {
- world->StopUpdatingModelPose( this );
- on_velocity_list = false;
- }
-
- if( (!on_velocity_list) && (!velocity_is_zero( vel )) )
- {
- world->StartUpdatingModelPose( this );
- on_velocity_list = true;
- }
-
- CallCallbacks( &this->velocity );
-}
-
-void Model::NeedRedraw( void )
-{
- this->rebuild_displaylist = true;
-
- if( parent )
- parent->NeedRedraw();
- else
- world->NeedRedraw();
-}
-
-void Model::SetPose( Pose newpose )
-{
- //PRINT_DEBUG5( "%s.SetPose(%.2f %.2f %.2f %.2f)",
- // this->token, pose->x, pose->y, pose->z, pose->a );
-
- // if the pose has changed, we need to do some work
- if( memcmp( &pose, &newpose, sizeof(Pose) ) != 0 )
- {
- pose = newpose;
- pose.a = normalize(pose.a);
-
- if( isnan( pose.a ) )
- printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n",
- token, pose.x, pose.y, pose.z, pose.a );
-
- NeedRedraw();
- map_caches_are_invalid = true;
- MapWithChildren();
- world->dirty = true;
- }
-
- // register a model change even if the pose didn't actually change
- CallCallbacks( &this->pose );
-}
-
void Model::AddToPose( double dx, double dy, double dz, double da )
{
if( dx || dy || dz || da )
@@ -1455,155 +737,8 @@
this->AddToPose( pose.x, pose.y, pose.z, pose.a );
}
-void Model::SetGeom( Geom geom )
-{
- //printf( "MODEL \"%s\" SET GEOM (%.2f %.2f %.2f)[%.2f %.2f]\n",
- // this->token,
- // geom->pose.x, geom->pose.y, geom->pose.a,
- // geom->size.x, geom->size.y );
- UnMapWithChildren();
-
- this->geom = geom;
-
- blockgroup.CalcSize();
-
- NeedRedraw();
- MapWithChildren();
-
- CallCallbacks( &geom );
-}
-
-void Model::SetColor( stg_color_t col )
-{
- this->color = col;
- NeedRedraw();
- CallCallbacks( &color );
-}
-
-void Model::SetMass( stg_kg_t mass )
-{
- this->mass = mass;
- CallCallbacks( &mass );
-}
-
-void Model::SetStall( stg_bool_t stall )
-{
- this->stall = stall;
- CallCallbacks( &stall );
-}
-
-void Model::SetGripperReturn( int val )
-{
- vis.gripper_return = val;
- CallCallbacks( &vis.gripper_return );
-}
-
-void Model::SetFiducialReturn( int val )
-{
- vis.fiducial_return = val;
- CallCallbacks( &vis.fiducial_return );
-}
-
-void Model::SetFiducialKey( int key )
-{
- vis.fiducial_key = key;
- CallCallbacks( &vis.fiducial_key );
-}
-
-void Model::SetLaserReturn( stg_laser_return_t val )
-{
- vis.laser_return = val;
- CallCallbacks( &vis.laser_return );
-}
-
-void Model::SetObstacleReturn( int val )
-{
- vis.obstacle_return = val;
- CallCallbacks( &vis.obstacle_return );
-}
-
-void Model::SetBlobReturn( int val )
-{
- vis.blob_return = val;
- CallCallbacks( &vis.blob_return );
-}
-
-void Model::SetRangerReturn( int val )
-{
- vis.ranger_return = val;
- CallCallbacks( &vis.ranger_return );
-}
-
-void Model::SetBoundary( int val )
-{
- boundary = val;
- CallCallbacks( &boundary );
-}
-
-void Model::SetGuiNose( int val )
-{
- gui.nose = val;
- CallCallbacks( &gui.nose );
-}
-
-void Model::SetGuiMask( int val )
-{
- gui.mask = val;
- CallCallbacks( &gui.mask );
-}
-
-void Model::SetGuiGrid( int val )
-{
- gui.grid = val;
- CallCallbacks( &this->gui.grid );
-}
-
-void Model::SetGuiOutline( int val )
-{
- gui.outline = val;
- CallCallbacks( &gui.outline );
-}
-
-void Model::SetWatts( stg_watts_t watts )
-{
- watts = watts;
- CallCallbacks( &watts );
-}
-
-void Model::SetMapResolution( stg_meters_t res )
-{
- map_resolution = res;
- CallCallbacks( &map_resolution );
-}
-
-// set the pose of model in global coordinates
-void Model::SetGlobalPose( Pose gpose )
-{
- SetPose( parent ? parent->GlobalToLocal( gpose ) : gpose );
-
- //printf( "setting global pose %.2f %.2f %.2f = local pose %.2f %.2f %.2f\n",
- // gpose->x, gpose->y, gpose->a, lpose.x, lpose.y, lpose.a );
-}
-
-int Model::SetParent( Model* newparent)
-{
- // remove the model from its old parent (if it has one)
- if( this->parent )
- this->parent->children = g_list_remove( this->parent->children, this );
-
- if( newparent )
- newparent->children = g_list_append( newparent->children, this );
-
- // link from the model to its new parent
- this->parent = newparent;
-
- CallCallbacks( &this->parent );
- return 0; //ok
-}
-
-
void Model::PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax,
stg_meters_t ymin, stg_meters_t ymax )
{
@@ -1617,6 +752,7 @@
return blockgroup.AppendTouchingModels( list );
}
+
Model* Model::TestCollision()
{
//printf( "mod %s test collision...\n", token );
@@ -1641,21 +777,22 @@
void Model::UpdateCharge()
{
- //printf( "model %s is a charger. testing to see if anyone is touching\n",
- // token );x
-
assert( watts_give > 0 );
PowerPack* mypp = FindPowerPack();
assert( mypp );
+ // make sure we only charge a powerpack once
+ GList* ppacks_serviced = NULL;
+
+ // run through and update all appropriate touchers
for( GList* touchers = AppendTouchingModels( NULL );
touchers;
touchers = touchers->next )
{
- Model* toucher = (Model*)touchers->data;
-
- PowerPack* hispp =toucher-> FindPowerPack();
+ Model* toucher = (Model*)touchers->data;
+ PowerPack* hispp =toucher->FindPowerPack();
+
if( hispp && toucher->watts_take > 0.0)
{
//printf( " toucher %s can take up to %.2f wats\n",
@@ -1672,7 +809,6 @@
hispp->charging = true;
}
}
-
}
void Model::CommitTestedPose()
@@ -1792,6 +928,16 @@
return NULL;
}
+void Model::NeedRedraw( void )
+{
+ this->rebuild_displaylist = true;
+
+ if( parent )
+ parent->NeedRedraw();
+ else
+ world->NeedRedraw();
+}
+
Model* Model::GetUnusedModelOfType( stg_model_type_t type )
{
//printf( "searching for type %d in model %s type %d\n", type, token,
this->type );
Added: code/stage/trunk/libstage/model_draw.cc
===================================================================
--- code/stage/trunk/libstage/model_draw.cc (rev 0)
+++ code/stage/trunk/libstage/model_draw.cc 2009-02-09 03:58:32 UTC (rev
7321)
@@ -0,0 +1,590 @@
+#include "stage.hh"
+#include "worldfile.hh"
+#include "canvas.hh"
+#include "texture_manager.hh"
+using namespace Stg;
+
+// speech bubble colors
+static const stg_color_t BUBBLE_FILL = 0xFFC8C8FF; // light blue/grey
+static const stg_color_t BUBBLE_BORDER = 0xFF000000; // black
+static const stg_color_t BUBBLE_TEXT = 0xFF000000; // black
+
+void Model::DrawSelected()
+{
+ glPushMatrix();
+
+ glTranslatef( pose.x, pose.y, pose.z+0.01 ); // tiny Z offset raises rect
above grid
+
+ Pose gpose = GetGlobalPose();
+
+ char buf[64];
+ snprintf( buf, 63, "%s [%.2f %.2f %.2f %.2f]",
+ token, gpose.x, gpose.y, gpose.z, rtod(gpose.a) );
+
+ PushColor( 0,0,0,1 ); // text color black
+ Gl::draw_string( 0.5,0.5,0.5, buf );
+
+ glRotatef( rtod(pose.a), 0,0,1 );
+
+ Gl::pose_shift( geom.pose );
+
+ double dx = geom.size.x / 2.0 * 1.6;
+ double dy = geom.size.y / 2.0 * 1.6;
+
+ PopColor();
+
+ PushColor( 0,1,0,0.4 ); // highlight color blue
+ glRectf( -dx, -dy, dx, dy );
+ PopColor();
+
+ PushColor( 0,1,0,0.8 ); // highlight color blue
+ glLineWidth( 1 );
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+ glRectf( -dx, -dy, dx, dy );
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+ PopColor();
+
+ glPopMatrix();
+}
+
+
+void Model::DrawTrailFootprint()
+{
+ double r,g,b,a;
+
+ for( int i=trail->len-1; i>=0; i-- )
+ {
+ stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t,
i );
+
+ glPushMatrix();
+ Gl::pose_shift( checkpoint->pose );
+ Gl::pose_shift( geom.pose );
+
+ stg_color_unpack( checkpoint->color, &r, &g, &b, &a );
+ PushColor( r, g, b, 0.1 );
+
+ blockgroup.DrawFootPrint( geom );
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+ PushColor( r/2, g/2, b/2, 0.1 );
+
+ blockgroup.DrawFootPrint( geom );
+
+ PopColor();
+ PopColor();
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+ glPopMatrix();
+ }
+}
+
+void Model::DrawTrailBlocks()
+{
+ double timescale = 0.0000001;
+
+ for( int i=trail->len-1; i>=0; i-- )
+ {
+ stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t,
i );
+
+ Pose pose;
+ memcpy( &pose, &checkpoint->pose, sizeof(pose));
+ pose.z = (world->sim_time - checkpoint->time) * timescale;
+
+ PushLocalCoords();
+ //blockgroup.Draw( this );
+
+ DrawBlocksTree();
+ PopCoords();
+ }
+}
+
+void Model::DrawTrailArrows()
+{
+ double r,g,b,a;
+
+ double dx = 0.2;
+ double dy = 0.07;
+ double timescale = 0.0000001;
+
+ for( unsigned int i=0; i<trail->len; i++ )
+ {
+ stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t,
i );
+
+ Pose pose;
+ memcpy( &pose, &checkpoint->pose, sizeof(pose));
+ pose.z = (world->sim_time - checkpoint->time) * timescale;
+
+ PushLocalCoords();
+
+ // set the height proportional to age
+
+ PushColor( checkpoint->color );
+
+ glEnable(GL_POLYGON_OFFSET_FILL);
+ glPolygonOffset(1.0, 1.0);
+
+ glBegin( GL_TRIANGLES );
+ glVertex3f( 0, -dy, 0);
+ glVertex3f( dx, 0, 0 );
+ glVertex3f( 0, +dy, 0 );
+ glEnd();
+ glDisable(GL_POLYGON_OFFSET_FILL);
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+
+ stg_color_unpack( checkpoint->color, &r, &g, &b, &a );
+ PushColor( r/2, g/2, b/2, 1 ); // darker color
+
+ glDepthMask(GL_FALSE);
+ glBegin( GL_TRIANGLES );
+ glVertex3f( 0, -dy, 0);
+ glVertex3f( dx, 0, 0 );
+ glVertex3f( 0, +dy, 0 );
+ glEnd();
+ glDepthMask(GL_TRUE);
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+ PopColor();
+ PopColor();
+ PopCoords();
+ }
+}
+
+void Model::DrawOriginTree()
+{
+ DrawPose( GetGlobalPose() );
+ for( GList* it=children; it; it=it->next )
+ ((Model*)it->data)->DrawOriginTree();
+}
+
+
+void Model::DrawBlocksTree( )
+{
+ PushLocalCoords();
+ LISTMETHOD( children, Model*, DrawBlocksTree );
+ DrawBlocks();
+ PopCoords();
+}
+
+void Model::DrawPose( Pose pose )
+{
+ PushColor( 0,0,0,1 );
+ glPointSize( 4 );
+
+ glBegin( GL_POINTS );
+ glVertex3f( pose.x, pose.y, pose.z );
+ glEnd();
+
+ PopColor();
+}
+
+void Model::DrawBlocks( )
+{
+ blockgroup.CallDisplayList( this );
+}
+
+void Model::DrawBoundingBoxTree()
+{
+ PushLocalCoords();
+ LISTMETHOD( children, Model*, DrawBoundingBoxTree );
+ DrawBoundingBox();
+ PopCoords();
+}
+
+void Model::DrawBoundingBox()
+{
+ Gl::pose_shift( geom.pose );
+
+ PushColor( color );
+
+ glBegin( GL_QUAD_STRIP );
+
+ glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
+ glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
+
+ glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
+ glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, 0 );
+
+ glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
+ glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );
+
+ glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
+ glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );
+
+ glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
+ glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, 0 );
+
+ glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
+ glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
+
+ glEnd();
+
+ glBegin( GL_LINES );
+ glVertex2f( -0.02, 0 );
+ glVertex2f( +0.02, 0 );
+
+ glVertex2f( 0, -0.02 );
+ glVertex2f( 0, +0.02 );
+ glEnd();
+
+ PopColor();
+}
+
+// move into this model's local coordinate frame
+void Model::PushLocalCoords()
+{
+ glPushMatrix();
+
+ if( parent )
+ glTranslatef( 0,0, parent->geom.size.z );
+
+ Gl::pose_shift( pose );
+}
+
+void Model::PopCoords()
+{
+ glPopMatrix();
+}
+
+void Model::AddCustomVisualizer( CustomVisualizer* custom_visual )
+{
+ if( !custom_visual )
+ return;
+
+ //Visualizations can only be added to stage when run in a GUI
+ if( world_gui == NULL ) {
+ printf( "Unable to add custom visualization - it must be run
with a GUI world\n" );
+ return;
+ }
+
+ //save visual instance
+ custom_visual_list = g_list_append(custom_visual_list, custom_visual );
+
+ //register option for all instances which share the same name
+ Canvas* canvas = world_gui->GetCanvas();
+ std::map< std::string, Option* >::iterator i =
canvas->_custom_options.find( custom_visual->name() );
+ if( i == canvas->_custom_options.end() ) {
+ Option* op = new Option( custom_visual->name(),
custom_visual->name(), "", true, world_gui );
+ canvas->_custom_options[ custom_visual->name() ] = op;
+ registerOption( op );
+ }
+}
+
+void Model::RemoveCustomVisualizer( CustomVisualizer* custom_visual )
+{
+ if( custom_visual )
+ custom_visual_list = g_list_remove(custom_visual_list,
custom_visual );
+
+ //TODO unregister option - tricky because there might still be
instances attached to different models which have the same name
+}
+
+
+void Model::DrawStatusTree( Camera* cam )
+{
+ PushLocalCoords();
+ DrawStatus( cam );
+ LISTMETHODARG( children, Model*, DrawStatusTree, cam );
+ PopCoords();
+}
+
+void Model::DrawStatus( Camera* cam )
+{
+ // quick hack
+// if( power_pack && power_pack->stored < 0.0 )
+// {
+// glPushMatrix();
+// glTranslatef( 0.3, 0, 0.0 );
+// DrawImage( TextureManager::getInstance()._mains_texture_id,
cam, 0.85 );
+// glPopMatrix();
+// }
+
+ if( say_string || power_pack )
+ {
+ float yaw, pitch;
+ pitch = - cam->pitch();
+ yaw = - cam->yaw();
+
+ Pose gpz = GetGlobalPose();
+
+ float robotAngle = -rtod(gpz.a);
+ glPushMatrix();
+
+
+ // move above the robot
+ glTranslatef( 0, 0, 0.5 );
+
+ // rotate to face screen
+ glRotatef( robotAngle - yaw, 0,0,1 );
+ glRotatef( -pitch, 1,0,0 );
+
+
+ //if( ! parent )
+ // glRectf( 0,0,1,1 );
+
+ //if( power_pack->stored > 0.0 )
+ power_pack->Visualize( cam );
+
+ if( say_string )
+ {
+ //get raster positition, add gl_width, then project
back to world coords
+ glRasterPos3f( 0, 0, 0 );
+ GLfloat pos[ 4 ];
+ glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
+
+ GLboolean valid;
+ glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID,
&valid );
+
+ if( valid )
+ {
+
+ fl_font( FL_HELVETICA, 12 );
+ float w = gl_width( this->say_string ); //
scaled text width
+ float h = gl_height(); // scaled text height
+
+ GLdouble wx, wy, wz;
+ GLint viewport[4];
+ glGetIntegerv(GL_VIEWPORT, viewport);
+
+ GLdouble modelview[16];
+ glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
+
+ GLdouble projection[16];
+ glGetDoublev(GL_PROJECTION_MATRIX,
projection);
+
+ //get width and height in world coords
+ gluUnProject( pos[0] + w, pos[1], pos[2],
modelview, projection, viewport, &wx, &wy, &wz );
+ w = wx;
+ gluUnProject( pos[0], pos[1] + h, pos[2],
modelview, projection, viewport, &wx, &wy, &wz );
+ h = wy;
+
+ // calculate speech bubble margin
+ const float m = h/10;
+
+ // draw inside of bubble
+ PushColor( BUBBLE_FILL );
+ glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT );
+ glPolygonMode( GL_FRONT, GL_FILL );
+ glEnable( GL_POLYGON_OFFSET_FILL );
+ glPolygonOffset( 1.0, 1.0 );
+ Gl::draw_octagon( w, h, m );
+ glDisable( GL_POLYGON_OFFSET_FILL );
+ PopColor();
+
+ // draw outline of bubble
+ PushColor( BUBBLE_BORDER );
+ glLineWidth( 1 );
+ glEnable( GL_LINE_SMOOTH );
+ glPolygonMode( GL_FRONT, GL_LINE );
+ Gl::draw_octagon( w, h, m );
+ glPopAttrib();
+ PopColor();
+
+ PushColor( BUBBLE_TEXT );
+ // draw text inside the bubble
+ Gl::draw_string( 2.5*m, 2.5*m, 0,
this->say_string );
+ PopColor();
+ }
+ }
+ glPopMatrix();
+ }
+
+ if( stall )
+ {
+ DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 );
+ }
+}
+
+void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double
width, double height )
+{
+ float yaw, pitch;
+ pitch = - cam->pitch();
+ yaw = - cam->yaw();
+ float robotAngle = -rtod(pose.a);
+
+ glEnable(GL_TEXTURE_2D);
+ glBindTexture( GL_TEXTURE_2D, texture_id );
+
+ glColor4f( 1.0, 1.0, 1.0, alpha );
+ glPushMatrix();
+
+ //position image above the robot
+ glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 );
+
+ // rotate to face screen
+ glRotatef( robotAngle - yaw, 0,0,1 );
+ glRotatef( -pitch - 90, 1,0,0 );
+
+ //draw a square, with the textured image
+ glBegin(GL_QUADS);
+ glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f );
+ glTexCoord2f(width, 0.0f); glVertex3f( 0.25f, 0, -0.25f );
+ glTexCoord2f(width, height); glVertex3f( 0.25f, 0, 0.25f );
+ glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0, 0.25f );
+ glEnd();
+
+ glPopMatrix();
+ glBindTexture( GL_TEXTURE_2D, 0 );
+ glDisable(GL_TEXTURE_2D);
+}
+
+
+void Model::DrawFlagList( void )
+{
+ if( flag_list == NULL )
+ return;
+
+ PushLocalCoords();
+
+ glPolygonMode( GL_FRONT, GL_FILL );
+
+ GLUquadric* quadric = gluNewQuadric();
+ glTranslatef(0,0,1); // jump up
+ Pose gpose = GetGlobalPose();
+ glRotatef( 180 + rtod(-gpose.a),0,0,1 );
+
+
+ GList* list = g_list_copy( flag_list );
+ list = g_list_reverse(list);
+
+ for( GList* item = list; item; item = item->next )
+ {
+
+ Flag* flag = (Flag*)item->data;
+
+ glTranslatef( 0, 0, flag->size/2.0 );
+
+ PushColor( flag->color );
+
+
+ glEnable(GL_POLYGON_OFFSET_FILL);
+ glPolygonOffset(1.0, 1.0);
+ gluQuadricDrawStyle( quadric, GLU_FILL );
+ gluSphere( quadric, flag->size/2.0, 4,2 );
+ glDisable(GL_POLYGON_OFFSET_FILL);
+
+ // draw the edges darker version of the same color
+ double r,g,b,a;
+ stg_color_unpack( flag->color, &r, &g, &b, &a );
+ PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a ));
+
+ gluQuadricDrawStyle( quadric, GLU_LINE );
+ gluSphere( quadric, flag->size/2.0, 4,2 );
+
+ PopColor();
+ PopColor();
+
+ glTranslatef( 0, 0, flag->size/2.0 );
+ }
+
+ g_list_free( list );
+
+ gluDeleteQuadric( quadric );
+
+ PopCoords();
+}
+
+
+void Model::DrawBlinkenlights()
+{
+ PushLocalCoords();
+
+ GLUquadric* quadric = gluNewQuadric();
+ //glTranslatef(0,0,1); // jump up
+ //Pose gpose = GetGlobalPose();
+ //glRotatef( 180 + rtod(-gpose.a),0,0,1 );
+
+ for( unsigned int i=0; i<blinkenlights->len; i++ )
+ {
+ stg_blinkenlight_t* b =
+ (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
+ assert(b);
+
+ glTranslatef( b->pose.x, b->pose.y, b->pose.z );
+
+ PushColor( b->color );
+
+ if( b->enabled )
+ gluQuadricDrawStyle( quadric, GLU_FILL );
+ else
+ gluQuadricDrawStyle( quadric, GLU_LINE );
+
+ gluSphere( quadric, b->size/2.0, 8,8 );
+
+ PopColor();
+ }
+
+ gluDeleteQuadric( quadric );
+
+ PopCoords();
+}
+
+void Model::DrawPicker( void )
+{
+ //PRINT_DEBUG1( "Drawing %s", token );
+ PushLocalCoords();
+
+ // draw the boxes
+ blockgroup.DrawSolid( geom );
+
+ // recursively draw the tree below this model
+ LISTMETHOD( this->children, Model*, DrawPicker );
+
+ PopCoords();
+}
+
+void Model::DataVisualize( Camera* cam )
+{
+// if( power_pack )
+// {
+// // back into global coords to get rid of my rotation
+// glPushMatrix();
+// gl_pose_inverse_shift( GetGlobalPose() );
+
+// // shift to the top left corner of the model (roughly)
+// glTranslatef( pose.x - geom.size.x/2.0,
+// pose.y + geom.size.y/2.0,
+// pose.z + geom.size.z );
+
+// power_pack->Visualize( cam );
+// glPopMatrix();
+// }
+}
+
+void Model::DataVisualizeTree( Camera* cam )
+{
+ PushLocalCoords();
+ DataVisualize( cam ); // virtual function overridden by most model types
+
+ CustomVisualizer* vis;
+ for( GList* item = custom_visual_list; item; item = item->next ) {
+ vis = static_cast< CustomVisualizer* >( item->data );
+ if( world_gui->GetCanvas()->_custom_options[ vis->name() ]->isEnabled()
)
+ vis->DataVisualize( cam );
+ }
+
+
+ // and draw the children
+ LISTMETHODARG( children, Model*, DataVisualizeTree, cam );
+
+ PopCoords();
+}
+
+void Model::DrawGrid( void )
+{
+ if ( gui.grid )
+ {
+ PushLocalCoords();
+
+ stg_bounds3d_t vol;
+ vol.x.min = -geom.size.x/2.0;
+ vol.x.max = geom.size.x/2.0;
+ vol.y.min = -geom.size.y/2.0;
+ vol.y.max = geom.size.y/2.0;
+ vol.z.min = 0;
+ vol.z.max = geom.size.z;
+
+ PushColor( 0,0,1,0.4 );
+ Gl::draw_grid(vol);
+ PopColor();
+ PopCoords();
+ }
+}
Added: code/stage/trunk/libstage/model_getset.cc
===================================================================
--- code/stage/trunk/libstage/model_getset.cc (rev 0)
+++ code/stage/trunk/libstage/model_getset.cc 2009-02-09 03:58:32 UTC (rev
7321)
@@ -0,0 +1,245 @@
+#include "stage.hh"
+using namespace Stg;
+
+void Model::SetGeom( Geom geom )
+{
+ UnMapWithChildren();
+
+ this->geom = geom;
+
+ blockgroup.CalcSize();
+
+ NeedRedraw();
+
+ MapWithChildren();
+
+ CallCallbacks( &geom );
+}
+
+void Model::SetColor( stg_color_t col )
+{
+ this->color = col;
+ NeedRedraw();
+ CallCallbacks( &color );
+}
+
+void Model::SetMass( stg_kg_t mass )
+{
+ this->mass = mass;
+ CallCallbacks( &mass );
+}
+
+void Model::SetStall( stg_bool_t stall )
+{
+ this->stall = stall;
+ CallCallbacks( &stall );
+}
+
+void Model::SetGripperReturn( int val )
+{
+ vis.gripper_return = val;
+ CallCallbacks( &vis.gripper_return );
+}
+
+void Model::SetFiducialReturn( int val )
+{
+ vis.fiducial_return = val;
+ CallCallbacks( &vis.fiducial_return );
+}
+
+void Model::SetFiducialKey( int key )
+{
+ vis.fiducial_key = key;
+ CallCallbacks( &vis.fiducial_key );
+}
+
+void Model::SetLaserReturn( stg_laser_return_t val )
+{
+ vis.laser_return = val;
+ CallCallbacks( &vis.laser_return );
+}
+
+void Model::SetObstacleReturn( int val )
+{
+ vis.obstacle_return = val;
+ CallCallbacks( &vis.obstacle_return );
+}
+
+void Model::SetBlobReturn( int val )
+{
+ vis.blob_return = val;
+ CallCallbacks( &vis.blob_return );
+}
+
+void Model::SetRangerReturn( int val )
+{
+ vis.ranger_return = val;
+ CallCallbacks( &vis.ranger_return );
+}
+
+void Model::SetBoundary( int val )
+{
+ boundary = val;
+ CallCallbacks( &boundary );
+}
+
+void Model::SetGuiNose( int val )
+{
+ gui.nose = val;
+ CallCallbacks( &gui.nose );
+}
+
+void Model::SetGuiMask( int val )
+{
+ gui.mask = val;
+ CallCallbacks( &gui.mask );
+}
+
+void Model::SetGuiGrid( int val )
+{
+ gui.grid = val;
+ CallCallbacks( &this->gui.grid );
+}
+
+void Model::SetGuiOutline( int val )
+{
+ gui.outline = val;
+ CallCallbacks( &gui.outline );
+}
+
+void Model::SetWatts( stg_watts_t watts )
+{
+ watts = watts;
+ CallCallbacks( &watts );
+}
+
+void Model::SetMapResolution( stg_meters_t res )
+{
+ map_resolution = res;
+ CallCallbacks( &map_resolution );
+}
+
+// set the pose of model in global coordinates
+void Model::SetGlobalPose( Pose gpose )
+{
+ SetPose( parent ? parent->GlobalToLocal( gpose ) : gpose );
+}
+
+int Model::SetParent( Model* newparent)
+{
+ // remove the model from its old parent (if it has one)
+ if( this->parent )
+ this->parent->children = g_list_remove( this->parent->children, this );
+
+ if( newparent )
+ newparent->children = g_list_append( newparent->children, this );
+
+ // link from the model to its new parent
+ this->parent = newparent;
+
+ CallCallbacks( &this->parent );
+ return 0; //ok
+}
+
+// get the model's velocity in the global frame
+Velocity Model::GetGlobalVelocity()
+{
+ Pose gpose = GetGlobalPose();
+
+ double cosa = cos( gpose.a );
+ double sina = sin( gpose.a );
+
+ Velocity gv;
+ gv.x = velocity.x * cosa - velocity.y * sina;
+ gv.y = velocity.x * sina + velocity.y * cosa;
+ gv.a = velocity.a;
+
+ return gv;
+}
+
+// set the model's velocity in the global frame
+void Model::SetGlobalVelocity( Velocity gv )
+{
+ Pose gpose = GetGlobalPose();
+
+ double cosa = cos( gpose.a );
+ double sina = sin( gpose.a );
+
+ Velocity lv;
+ lv.x = gv.x * cosa + gv.y * sina;
+ lv.y = -gv.x * sina + gv.y * cosa;
+ lv.a = gv.a;
+
+ this->SetVelocity( lv );
+}
+
+// get the model's position in the global frame
+Pose Model::GetGlobalPose()
+{
+ // if I'm a top level model, my global pose is my local pose
+ if( parent == NULL )
+ return pose;
+
+ // otherwise
+
+ Pose global_pose = pose_sum( parent->GetGlobalPose(), pose );
+
+ // we are on top of our parent
+ global_pose.z += parent->geom.size.z;
+
+ // PRINT_DEBUG4( "GET GLOBAL POSE [x:%.2f y:%.2f z:%.2f a:%.2f]",
+ // global_pose.x,
+ // global_pose.y,
+ // global_pose.z,
+ // global_pose.a );
+
+ return global_pose;
+}
+
+
+void Model::SetVelocity( Velocity vel )
+{
+// assert( ! isnan(vel.x) );
+// assert( ! isnan(vel.y) );
+// assert( ! isnan(vel.z) );
+// assert( ! isnan(vel.a) );
+
+ this->velocity = vel;
+
+ if( on_velocity_list && vel.IsZero() )
+ {
+ world->StopUpdatingModelPose( this );
+ on_velocity_list = false;
+ }
+
+ if( (!on_velocity_list) && (!vel.IsZero()) )
+ {
+ world->StartUpdatingModelPose( this );
+ on_velocity_list = true;
+ }
+
+ CallCallbacks( &this->velocity );
+}
+
+
+// set the model's pose in the local frame
+void Model::SetPose( Pose newpose )
+{
+ // if the pose has changed, we need to do some work
+ if( memcmp( &pose, &newpose, sizeof(Pose) ) != 0 )
+ {
+ pose = newpose;
+ pose.a = normalize(pose.a);
+
+ if( isnan( pose.a ) )
+ printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n",
+ token, pose.x, pose.y, pose.z, pose.a
);
+
+ NeedRedraw();
+ MapWithChildren();
+ world->dirty = true;
+ }
+
+ CallCallbacks( &this->pose );
+}
+
Modified: code/stage/trunk/libstage/powerpack.cc
===================================================================
--- code/stage/trunk/libstage/powerpack.cc 2009-02-07 02:18:31 UTC (rev
7320)
+++ code/stage/trunk/libstage/powerpack.cc 2009-02-09 03:58:32 UTC (rev
7321)
@@ -12,15 +12,29 @@
PowerPack::PowerPack( Model* mod ) :
mod( mod), stored( 0.0 ), capacity( 0.0 ), charging( false )
{
- // nothing to do
+ // tell the world about this new pp
+ mod->world->AddPowerPack( this );
};
+PowerPack::~PowerPack()
+{
+ // tell the world about this new pp
+ mod->world->RemovePowerPack( this );
+}
+
void PowerPack::Print( char* prefix )
{
printf( "%s stored %.2f/%.2f joules\n", prefix, stored, capacity );
}
+// this is called every world update cycle - can we get away without doing
this?
+void PowerPack::Update()
+{
+ charging = false;
+}
+
+
/** OpenGL visualization of the powerpack state */
void PowerPack::Visualize( Camera* cam )
{
@@ -45,84 +59,83 @@
// glVertex2i( 5, 4);
// glEnd();
//}
-
+
double percent = stored/capacity * 100.0;
-
+
const double alpha = 0.5;
- if( percent > 50 )
- glColor4f( 0,1,0, alpha ); // green
+ if( percent > 50 )
+ glColor4f( 0,1,0, alpha ); // green
else if( percent > 25 )
glColor4f( 1,0,1, alpha ); // magenta
else
glColor4f( 1,0,0, alpha ); // red
+
+ static char buf[6];
+ snprintf( buf, 6, "%.0f", percent );
+
+ glTranslatef( -width, 0.0, 0.0 );
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+
+ GLfloat fullness = height * (percent * 0.01);
+ glRectf( 0,0,width, fullness);
+
+ // outline the charge-o-meter
+ glTranslatef( 0,0,0.001 );
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+
+ glColor4f( 0,0,0,0.7 );
+
+ glRectf( 0,0,width, height );
+
+ glBegin( GL_LINES );
+ glVertex2f( 0, fullness );
+ glVertex2f( width, fullness );
+ glEnd();
+
+ if( stored < 0.0 ) // inifinite supply!
+ {
+ // draw an arrow toward the top
+ glBegin( GL_LINES );
+ glVertex2f( width/3.0, height/3.0 );
+ glVertex2f( 2.0 * width/3, height/3.0 );
- static char buf[6];
- snprintf( buf, 6, "%.0f", percent );
+ glVertex2f( width/3.0, height/3.0 );
+ glVertex2f( width/3.0, height - height/5.0 );
- glTranslatef( -width, 0.0, 0.0 );
+ glVertex2f( width/3.0, height - height/5.0 );
+ glVertex2f( 0, height - height/5.0 );
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+ glVertex2f( 0, height - height/5.0 );
+ glVertex2f( width/2.0, height );
- GLfloat fullness = height * (percent * 0.01);
- glRectf( 0,0,width, fullness);
+ glVertex2f( width/2.0, height );
+ glVertex2f( width, height - height/5.0 );
- // outline the charge-o-meter
- glTranslatef( 0,0,0.001 );
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+ glVertex2f( width, height - height/5.0 );
+ glVertex2f( 2.0 * width/3.0, height - height/5.0 );
- glColor4f( 0,0,0,0.7 );
+ glVertex2f( 2.0 * width/3.0, height - height/5.0 );
+ glVertex2f( 2.0 * width/3, height/3.0 );
+ glEnd();
+ }
+
+
+ if( charging )
+ {
+ glLineWidth( 6.0 );
+ glColor4f( 1,0,0,0.7 );
+
glRectf( 0,0,width, height );
- glBegin( GL_LINES );
- glVertex2f( 0, fullness );
- glVertex2f( width, fullness );
- glEnd();
-
- if( stored < 0.0 ) // inifinite supply!
- {
- // draw an arrow toward the top
- glBegin( GL_LINES );
- glVertex2f( width/3.0, height/3.0 );
- glVertex2f( 2.0 * width/3, height/3.0 );
-
- glVertex2f( width/3.0, height/3.0 );
- glVertex2f( width/3.0, height - height/5.0 );
-
- glVertex2f( width/3.0, height - height/5.0 );
- glVertex2f( 0, height - height/5.0 );
-
- glVertex2f( 0, height - height/5.0 );
- glVertex2f( width/2.0, height );
-
- glVertex2f( width/2.0, height );
- glVertex2f( width, height - height/5.0 );
-
- glVertex2f( width, height - height/5.0 );
- glVertex2f( 2.0 * width/3.0, height - height/5.0 );
-
- glVertex2f( 2.0 * width/3.0, height - height/5.0 );
- glVertex2f( 2.0 * width/3, height/3.0 );
-
- glEnd();
-
- }
-
-
- if( charging )
- {
- glLineWidth( 6.0 );
- glColor4f( 1,0,0,0.7 );
-
- glRectf( 0,0,width, height );
-
- glLineWidth( 1.0 );
- }
-
- // draw the percentage
- //gl_draw_string( -0.2, 0, 0, buf );
-
+ glLineWidth( 1.0 );
+ }
+
+ // draw the percentage
+ //gl_draw_string( -0.2, 0, 0, buf );
+
// ?
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
}
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-02-07 02:18:31 UTC (rev 7320)
+++ code/stage/trunk/libstage/stage.hh 2009-02-09 03:58:32 UTC (rev 7321)
@@ -273,6 +273,8 @@
printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
prefix, x,y,z,a );
}
+
+ bool IsZero(){ return( !(x || y || z || a )); };
void Load( Worldfile* wf, int section, const char* keyword );
void Save( Worldfile* wf, int section, const char* keyword );
@@ -304,9 +306,7 @@
{
printf( "%s velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
prefix, x,y,z,a );
- }
-
- bool IsZero(){ return( !(x || y || z || a )); };
+ }
};
/** Specify an object's basic geometry: position and rectangular
@@ -373,38 +373,38 @@
Bounds x, y, z;
} stg_bbox3d_t;
- /** define a field-of-view: an angle and range bounds */
+ /** Define a field-of-view: an angle and range bounds */
typedef struct
{
Bounds range; ///< min and max range of sensor
stg_radians_t angle; ///< width of viewing angle of sensor
} stg_fov_t;
- /** define a point on a 2d plane */
+ /** Define a point on a 2d plane */
typedef struct
{
stg_meters_t x, y;
} stg_point_t;
- /** define a point in 3d space */
+ /** Define a point in 3d space */
typedef struct
{
float x, y, z;
} stg_vertex_t;
- /** define vertex and its color */
+ /** Define vertex and its color */
typedef struct
{
float x, y, z, r, g, b, a;
} stg_colorvertex_t;
- /** define a point in 3d space */
+ /** Define a point in 3d space */
typedef struct
{
stg_meters_t x, y, z;
} stg_point3_t;
- /** define an integer point on the 2d plane */
+ /** Define an integer point on the 2d plane */
typedef struct
{
int32_t x,y;
@@ -639,7 +639,7 @@
stg_cb_t* cb_create( stg_model_callback_t callback, void* arg );
void cb_destroy( stg_cb_t* cb );
- /** defines a rectangle of [size] located at [pose] */
+ /** Defines a rectangle of [size] located at [pose] */
typedef struct
{
Pose pose;
@@ -846,6 +846,7 @@
stg_bounds3d_t extent; ///< Describes the 3D volume of the world
bool graphics;///< true iff we have a GUI
stg_usec_t interval_sim; ///< temporal resolution: microseconds that
elapse between simulated time steps
+ GList* powerpack_list; ///< List of all the powerpacks attached to
models in the world
GList* ray_list;///< List of rays traced for debug visualization
stg_usec_t sim_time; ///< the current sim time in this world in ms
GHashTable* superregions;
@@ -924,6 +925,9 @@
virtual void AddModel( Model* mod );
virtual void RemoveModel( Model* mod );
+
+ void AddPowerPack( PowerPack* pp );
+ void RemovePowerPack( PowerPack* pp );
GList* GetRayList(){ return ray_list; };
void ClearRays();
@@ -1371,13 +1375,12 @@
};
- class Camera;
-
/** energy data packet */
class PowerPack
{
public:
PowerPack( Model* mod );
+ ~PowerPack();
/** The model that owns this object */
Model* mod;
@@ -1415,6 +1418,10 @@
void Print( const char* prefix )
{ printf( "%s PowerPack %.2f/%.2f J\n", prefix, stored, capacity ); }
+
+ /** Called exactly once for each pack on every world update
+ cycle */
+ void Update();
};
class Visibility
@@ -1571,7 +1578,6 @@
ctrlinit_t* initfunc;
stg_usec_t interval; ///< time between updates in us
stg_usec_t last_update; ///< time of last update in us
- bool map_caches_are_invalid;
stg_meters_t map_resolution;
stg_kg_t mass;
bool on_update_list;
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-02-07 02:18:31 UTC (rev 7320)
+++ code/stage/trunk/libstage/world.cc 2009-02-09 03:58:32 UTC (rev 7321)
@@ -77,6 +77,7 @@
destroy( false ),
dirty( true ),
models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ),
+ powerpack_list( NULL ),
ppm( ppm ), // raytrace resolution
quit( false ),
quit_time( 0 ),
@@ -192,17 +193,14 @@
void World::LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable )
{
-// // lookup the group in which this was defined
-// Ancestor* anc = (Ancestor*)g_hash_table_lookup( entitytable,
-//
(gpointer)wf->GetEntityParent( entity ) );
-
-
Puck* puck = new Puck();
puck->Load( wf, entity );
puck_list = g_list_prepend( puck_list, puck );
}
+
+
void World::LoadModel( Worldfile* wf, int entity, GHashTable* entitytable )
{
int parent_entity = wf->GetEntityParent( entity );
@@ -424,14 +422,17 @@
if( IsGUI() == false )
return true;
}
-
+
// upate all positions first
LISTMETHOD( velocity_list, Model*, UpdatePose );
+ // upate all powerpacks
+ LISTMETHOD( powerpack_list, PowerPack*, Update );
+
// test all models that supply charge to see if they are touching
// something that takes charge
LISTMETHOD( charge_list, Model*, UpdateCharge );
-
+
// then update all sensors
if( worker_threads == 0 ) // do all the work in this thread
{
@@ -441,31 +442,31 @@
{
// push the update for every model that needs it into the thread pool
for( GList* it = update_list; it; it=it->next )
- {
- Model* mod = (Model*)it->data;
-
- if( mod->UpdateDue() )
- {
- if( mod->thread_safe ) // do update in a worker thread
- {
- g_mutex_lock( thread_mutex );
- update_jobs_pending++;
- g_mutex_unlock( thread_mutex );
- g_thread_pool_push( threadpool, mod, NULL );
- }
- else
- mod->Update(); // do update in this thread
- }
- }
-
+ {
+ Model* mod = (Model*)it->data;
+
+ if( mod->UpdateDue() )
+ {
+ if( mod->thread_safe ) // do update in a
worker thread
+ {
+ g_mutex_lock( thread_mutex );
+ update_jobs_pending++;
+ g_mutex_unlock( thread_mutex );
+ g_thread_pool_push( threadpool,
mod, NULL );
+ }
+ else
+ mod->Update(); // do update in this
thread
+ }
+ }
+
// wait for all the last update job to complete - it will
// signal the worker_threads_done condition var
g_mutex_lock( thread_mutex );
while( update_jobs_pending )
- g_cond_wait( worker_threads_done, thread_mutex );
+ g_cond_wait( worker_threads_done, thread_mutex );
g_mutex_unlock( thread_mutex );
}
-
+
this->sim_time += this->interval_sim;
this->updates++;
@@ -938,4 +939,12 @@
}
+void World::AddPowerPack( PowerPack* pp )
+{
+ powerpack_list = g_list_append( powerpack_list, pp );
+}
+void World::RemovePowerPack( PowerPack* pp )
+{
+ powerpack_list = g_list_remove( powerpack_list, pp );
+}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Create and Deploy Rich Internet Apps outside the browser with Adobe(R)AIR(TM)
software. With Adobe AIR, Ajax developers can use existing skills and code to
build responsive, highly engaging applications that combine the power of local
resources and data with the reach of the web. Download the Adobe AIR SDK and
Ajax docs to start building applications today-http://p.sf.net/sfu/adobe-com
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
