Revision: 7935
          http://playerstage.svn.sourceforge.net/playerstage/?rev=7935&view=rev
Author:   rtv
Date:     2009-07-01 17:50:43 +0000 (Wed, 01 Jul 2009)

Log Message:
-----------
cleaning up and STLization of BlockGroup

Modified Paths:
--------------
    code/stage/trunk/libstage/ancestor.cc
    code/stage/trunk/libstage/block.cc
    code/stage/trunk/libstage/blockgroup.cc
    code/stage/trunk/libstage/canvas.cc
    code/stage/trunk/libstage/model.cc
    code/stage/trunk/libstage/model_getset.cc
    code/stage/trunk/libstage/stage.cc
    code/stage/trunk/libstage/stage.hh
    code/stage/trunk/libstage/world.cc

Modified: code/stage/trunk/libstage/ancestor.cc
===================================================================
--- code/stage/trunk/libstage/ancestor.cc       2009-07-01 15:37:14 UTC (rev 
7934)
+++ code/stage/trunk/libstage/ancestor.cc       2009-07-01 17:50:43 UTC (rev 
7935)
@@ -5,7 +5,6 @@
 Ancestor::Ancestor() :
   children(),
   debug( false ),
-  puck_list( NULL ),
   token( NULL )
 {
   for( int i=0; i<MODEL_TYPE_COUNT; i++ )

Modified: code/stage/trunk/libstage/block.cc
===================================================================
--- code/stage/trunk/libstage/block.cc  2009-07-01 15:37:14 UTC (rev 7934)
+++ code/stage/trunk/libstage/block.cc  2009-07-01 17:50:43 UTC (rev 7935)
@@ -19,47 +19,47 @@
   mpts(),
   pt_count( pt_count ),
   pts(),
-       local_z( zmin, zmax ),
+  local_z( zmin, zmax ),
   color( color ),
   inherit_color( inherit_color ),
-  rendered_cells( new std::vector<Cell*> ), 
-  candidate_cells( new std::vector<Cell*> ),
-       gpts()
+  rendered_cells( new CellPtrVec ), 
+  candidate_cells( new CellPtrVec ),
+  gpts()
 {
   assert( mod );
   assert( pt_count > 0 );
-
-       // copy the argument point data into the member vector
-       this->pts.reserve( pt_count );
-       for( size_t p=0; p<pt_count; p++ )
-               this->pts.push_back( pts[p] );  
+  
+  // copy the argument point data into the member vector
+  this->pts.reserve( pt_count );
+  for( size_t p=0; p<pt_count; p++ )
+       this->pts.push_back( pts[p] );  
 }
 
 /** A from-file  constructor */
 Block::Block(  Model* mod,
-              Worldfile* wf,
-              int entity)
+                          Worldfile* wf,
+                          int entity)
   : mod( mod ),
-               mpts(),
+       mpts(),
     pt_count(0),
     pts(),
     color(0),
     inherit_color(true),
-               rendered_cells( new std::vector<Cell*> ), 
-               candidate_cells( new std::vector<Cell*> ) 
+       rendered_cells( new CellPtrVec ), 
+       candidate_cells( new CellPtrVec ) 
 {
   assert(mod);
   assert(wf);
   assert(entity);
-
+  
   Load( wf, entity );
 }
 
 Block::~Block()
 {
   if( mapped ) UnMap();
-
-       delete rendered_cells;
+  
+  delete rendered_cells;
   delete candidate_cells;
 }
 
@@ -70,7 +70,7 @@
       pts[p].x += x;
       pts[p].y += y;
     }
-
+  
   mod->blockgroup.BuildDisplayList( mod );
 }
 
@@ -78,13 +78,13 @@
 {
   double min = billion;
   double max = -billion;
-
+  
   for( unsigned int p=0; p<pt_count; p++)
     {
       if( pts[p].y > max ) max = pts[p].y;
       if( pts[p].y < min ) min = pts[p].y;
     }
-
+  
   // return the value half way between max and min
   return( min + (max - min)/2.0 );
 }
@@ -93,7 +93,7 @@
 {
   double min = billion;
   double max = -billion;
-
+  
   for( unsigned int p=0; p<pt_count; p++)
     {
       if( pts[p].x > max ) max = pts[p].x;
@@ -134,34 +134,21 @@
   mod->blockgroup.BuildDisplayList( mod );
 }
 
-
 stg_color_t Block::GetColor()
 {
   return( inherit_color ? mod->color : color );
 }
 
-GList* Block::AppendTouchingModels( GList* l )
+void Block::AppendTouchingModels( ModelPtrSet& touchers )
 {
   // for every cell we are rendered into
-  for( unsigned int i=0; i<rendered_cells->size(); i++ )
-    {
-               Cell* c = (*rendered_cells)[i];
-
-      // for every block rendered into that cell
-               for( std::vector<Block*>::iterator it = c->blocks.begin();
-                         it != c->blocks.end();
-                         ++it )
-                 {
-                        //Block* testblock = *it;
-                        Model* testmod = (*it)->mod;
-
-                        if( !mod->IsRelated( testmod ))
-                               if( ! g_list_find( l, testmod ) )
-                                 l = g_list_append( l, testmod );
-                 }
-    }
-
-  return l;
+  FOR_EACH( cell_it, *rendered_cells )
+       // for every block rendered into that cell
+       FOR_EACH( block_it, (*cell_it)->blocks )
+       {
+         if( !mod->IsRelated( (*block_it)->mod ))
+               touchers.insert( (*block_it)->mod );
+       }
 }
 
 Model* Block::TestCollision()
@@ -170,33 +157,29 @@
 
   // find the set of cells we would render into given the current global pose
   GenerateCandidateCells();
-
+  
   if( mod->vis.obstacle_return )
     // for every cell we may be rendered into
-    for( unsigned int i=0; i<candidate_cells->size(); i++ )
+       FOR_EACH( cell_it, *candidate_cells )
       {
-                 Cell* c = (*candidate_cells)[i];
-                 
-                 // for every rendered into that cell
-                 for( std::vector<Block*>::iterator it = c->blocks.begin();
-                                it != c->blocks.end();
-                                ++it )
-                        {
-                               Model* testmod = (*it)->mod;
-
-                               //printf( "   testing block %p of model %s\n", 
testblock, testmod->Token() );
-
-                               // if the tested model is an obstacle and it's 
not attached to this model
-                               if( (testmod != this->mod) &&
-                                        testmod->vis.obstacle_return &&
-                                        !mod->IsRelated( testmod ))
-                                 {
-                                        //puts( "HIT");
-                                        return testmod; // bail immediately 
with the bad news
-                                 }
-                        }
+               // for every rendered into that cell
+               FOR_EACH( block_it, (*cell_it)->blocks )
+                 {
+                       Model* testmod = (*block_it)->mod;
+                       
+                       //printf( "   testing block %p of model %s\n", 
testblock, testmod->Token() );
+                       
+                       // if the tested model is an obstacle and it's not 
attached to this model
+                       if( (testmod != this->mod) &&
+                               testmod->vis.obstacle_return &&
+                               !mod->IsRelated( testmod ))
+                         {
+                               //puts( "HIT");
+                               return testmod; // bail immediately with the 
bad news
+                         }
+                 }
       }
-
+  
   //printf( "model %s block %p collision done. no hits.\n", mod->Token(), this 
);
   return NULL; // no hit
 }
@@ -218,20 +201,16 @@
   mapped = false;
 }
 
-void Block::RemoveFromCellArray( std::vector<Cell*> * cells )
+void Block::RemoveFromCellArray( CellPtrVec *cells )
 {
-  for( std::vector<Cell*>::iterator it = cells->begin();
-                it != cells->end();
-                ++it )  
-        (*it)->RemoveBlock( this);
+  FOR_EACH( it, *cells )
+       (*it)->RemoveBlock( this);
 }
 
-void Block::AddToCellArray( std::vector<Cell*> * cells )
+void Block::AddToCellArray( CellPtrVec *cells )
 {
-  for( std::vector<Cell*>::iterator it = cells->begin();
-                it != cells->end();
-                ++it )  
-        (*it)->AddBlock( this);
+  FOR_EACH( it, *cells )
+       (*it)->AddBlock( this);
 }
 
 void Block::SwitchToTestedCells()
@@ -240,7 +219,7 @@
   AddToCellArray( candidate_cells );
 
   // switch current and candidate cell pointers
-  std::vector<Cell*> * tmp = rendered_cells;
+  CellPtrVec *tmp = rendered_cells;
   rendered_cells = candidate_cells;
   candidate_cells = tmp;
 
@@ -253,47 +232,47 @@
   stg_point3_t bgoffset = mod->blockgroup.GetOffset();
 
   return stg_point_t( (bpt.x - bgoffset.x) * (mod->geom.size.x/bgsize.x),
-                                                        (bpt.y - bgoffset.y) * 
(mod->geom.size.y/bgsize.y));
+                                         (bpt.y - bgoffset.y) * 
(mod->geom.size.y/bgsize.y));
 }
 
 void Block::InvalidateModelPointCache()
 {
-       // this doesn't happen often, so this simple strategy isn't too wasteful
-       mpts.clear();
+  // this doesn't happen often, so this simple strategy isn't too wasteful
+  mpts.clear();
 }
 
 void Block::GenerateCandidateCells()
 {
   candidate_cells->clear();
-       
+  
   if( mpts.size() == 0 )
-               {
-                       // no valid cache of model coord points, so generate 
them
-                       mpts.resize( pt_count );
-                       for( unsigned int i=0; i<pt_count; i++ )
-                               mpts[i] = BlockPointToModelMeters( pts[i] );
-               }
-       
+       {
+         // no valid cache of model coord points, so generate them
+         mpts.resize( pt_count );
+         for( unsigned int i=0; i<pt_count; i++ )
+               mpts[i] = BlockPointToModelMeters( pts[i] );
+       }
+  
   // convert the mpts in model coords into global pixel coords
-       gpts.resize(pt_count);
-       
+  gpts.resize(pt_count);
+  
   for( unsigned int i=0; i<pt_count; i++ )
-               gpts[i] = mod->world->MetersToPixels( mod->LocalToGlobal( 
mpts[i] ));
+       gpts[i] = mod->world->MetersToPixels( mod->LocalToGlobal( mpts[i] ));
   
   for( unsigned int i=0; i<pt_count; i++ )
-               mod->world->ForEachCellInLine( gpts[i], 
-                                                                               
                                                         gpts[(i+1)%pt_count], 
-                                                                               
                                                         *candidate_cells );  
+       mod->world->ForEachCellInLine( gpts[i], 
+                                                                  
gpts[(i+1)%pt_count], 
+                                                                  
*candidate_cells );  
   // set global Z
   Pose gpose = mod->GetGlobalPose();
   gpose.z += mod->geom.pose.z;
   double scalez = mod->geom.size.z /  mod->blockgroup.GetSize().z;
   stg_meters_t z = gpose.z - mod->blockgroup.GetOffset().z;
-
+  
   // store the block's absolute z bounds at this rendering
   global_z.min = (scalez * local_z.min) + z;
   global_z.max = (scalez * local_z.max) + z;
-
+  
   mapped = true;
 }
 
@@ -305,74 +284,74 @@
 }
 
 void Block::Rasterize( uint8_t* data,
-                                                         unsigned int width,
-                                                         unsigned int height,
-                                                         stg_meters_t 
cellwidth,
-                                                         stg_meters_t 
cellheight )
+                                          unsigned int width,
+                                          unsigned int height,
+                                          stg_meters_t cellwidth,
+                                          stg_meters_t cellheight )
 {
   //printf( "rasterize block %p : w: %u h: %u  scale %.2f %.2f  offset %.2f 
%.2f\n",
   //    this, width, height, scalex, scaley, offsetx, offsety );
-
+  
   for( unsigned int i=0; i<pt_count; i++ )
     {
-               // convert points from local to model coords
-               stg_point_t mpt1 = BlockPointToModelMeters( pts[i] );
-               stg_point_t mpt2 = BlockPointToModelMeters( pts[(i+1)%pt_count] 
);
-
-               // record for debug visualization
-               mod->rastervis.AddPoint( mpt1.x, mpt1.y );
-
-               // shift to the bottom left of the model
-               mpt1.x += mod->geom.size.x/2.0;
-               mpt1.y += mod->geom.size.y/2.0;
-               mpt2.x += mod->geom.size.x/2.0;
-               mpt2.y += mod->geom.size.y/2.0;
-
-               // convert from meters to cells
-               stg_point_int_t a( floor( mpt1.x / cellwidth  ),
-                                                                floor( mpt1.y 
/ cellheight ));
-               stg_point_int_t b( floor( mpt2.x / cellwidth  ),
-                                                                floor( mpt2.y 
/ cellheight ) );
-
-               bool steep = abs( b.y-a.y ) > abs( b.x-a.x );
-               if( steep )
-                 {
-                        swap( a.x, a.y );
-                        swap( b.x, b.y );
-                 }
-
-               if( a.x > b.x )
-                 {
-                        swap( a.x, b.x );
-                        swap( a.y, b.y );
-                 }
-
-               double dydx = (double) (b.y - a.y) / (double) (b.x - a.x);
-               double y = a.y;
-               for(int x=a.x; x<=b.x; x++)
-                 {
-                        if( steep )
-                               {
-                                 if( ! (floor(y) >= 0) ) continue;
-                                 if( ! (floor(y) < (int)width) ) continue;
-                                 if( ! (x >= 0) ) continue;
-                                 if( ! (x < (int)height) ) continue;
-                               }
-                        else
-                               {
-                                 if( ! (x >= 0) ) continue;
-                                 if( ! (x < (int)width) ) continue;
-                                 if( ! (floor(y) >= 0) ) continue;
-                                 if( ! (floor(y) < (int)height) ) continue;
-                               }
-
-                        if( steep )
-                               data[ (int)floor(y) + (x * width)] = 1;
-                        else
-                               data[ x + ((int)floor(y) * width)] = 1;
-                        y += dydx;
-                 }
-        }
+         // convert points from local to model coords
+         stg_point_t mpt1 = BlockPointToModelMeters( pts[i] );
+         stg_point_t mpt2 = BlockPointToModelMeters( pts[(i+1)%pt_count] );
+         
+         // record for debug visualization
+         mod->rastervis.AddPoint( mpt1.x, mpt1.y );
+         
+         // shift to the bottom left of the model
+         mpt1.x += mod->geom.size.x/2.0;
+         mpt1.y += mod->geom.size.y/2.0;
+         mpt2.x += mod->geom.size.x/2.0;
+         mpt2.y += mod->geom.size.y/2.0;
+         
+         // convert from meters to cells
+         stg_point_int_t a( floor( mpt1.x / cellwidth  ),
+                                                floor( mpt1.y / cellheight ));
+         stg_point_int_t b( floor( mpt2.x / cellwidth  ),
+                                                floor( mpt2.y / cellheight ) );
+         
+         bool steep = abs( b.y-a.y ) > abs( b.x-a.x );
+         if( steep )
+               {
+                 swap( a.x, a.y );
+                 swap( b.x, b.y );
+               }
+         
+         if( a.x > b.x )
+               {
+                 swap( a.x, b.x );
+                 swap( a.y, b.y );
+               }
+         
+         double dydx = (double) (b.y - a.y) / (double) (b.x - a.x);
+         double y = a.y;
+         for(int x=a.x; x<=b.x; x++)
+               {
+                 if( steep )
+                       {
+                         if( ! (floor(y) >= 0) ) continue;
+                         if( ! (floor(y) < (int)width) ) continue;
+                         if( ! (x >= 0) ) continue;
+                         if( ! (x < (int)height) ) continue;
+                       }
+                 else
+                       {
+                         if( ! (x >= 0) ) continue;
+                         if( ! (x < (int)width) ) continue;
+                         if( ! (floor(y) >= 0) ) continue;
+                         if( ! (floor(y) < (int)height) ) continue;
+                       }
+                 
+                 if( steep )
+                       data[ (int)floor(y) + (x * width)] = 1;
+                 else
+                       data[ x + ((int)floor(y) * width)] = 1;
+                 y += dydx;
+               }
+       }
 }
 
 void Block::DrawTop()
@@ -412,26 +391,26 @@
 {
   // draw filled color polygons
   stg_color_t col = inherit_color ? mod->color : color;
-
-   mod->PushColor( col );
-   glEnable(GL_POLYGON_OFFSET_FILL);
-   glPolygonOffset(1.0, 1.0);
-   DrawSides();
-   DrawTop();
-   glDisable(GL_POLYGON_OFFSET_FILL);
-
+  
+  mod->PushColor( col );
+  glEnable(GL_POLYGON_OFFSET_FILL);
+  glPolygonOffset(1.0, 1.0);
+  DrawSides();
+  DrawTop();
+  glDisable(GL_POLYGON_OFFSET_FILL);
+  
   //   // draw the block outline in a darker version of the same color
   double r,g,b,a;
   stg_color_unpack( col, &r, &g, &b, &a );
   mod->PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a ));
-
+  
   glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
   glDepthMask(GL_FALSE);
   DrawTop();
   DrawSides();
   glDepthMask(GL_TRUE);
   glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
-
+  
   mod->PopColor();
   mod->PopColor();
 }
@@ -442,10 +421,6 @@
   DrawTop();
 }
 
-
-//#define DEBUG 1
-
-
 void Block::Load( Worldfile* wf, int entity )
 {
   //printf( "Block::Load entity %d\n", entity );
@@ -455,18 +430,18 @@
 
   //printf( "reading %d points\n",
   //    pt_count );
-
+  
   char key[128];
   for( unsigned int p=0; p<pt_count; p++ )           {
     snprintf(key, sizeof(key), "point[%d]", p );
-
+       
     pts[p].x = wf->ReadTupleLength(entity, key, 0, 0);
     pts[p].y = wf->ReadTupleLength(entity, key, 1, 0);
   }
-
+  
   local_z.min = wf->ReadTupleLength( entity, "z", 0, 0.0 );
   local_z.max = wf->ReadTupleLength( entity, "z", 1, 1.0 );
-
+  
   const char* colorstr = wf->ReadString( entity, "color", NULL );
   if( colorstr )
     {
@@ -476,6 +451,3 @@
   else
     inherit_color = true;
 }
-
-
-

Modified: code/stage/trunk/libstage/blockgroup.cc
===================================================================
--- code/stage/trunk/libstage/blockgroup.cc     2009-07-01 15:37:14 UTC (rev 
7934)
+++ code/stage/trunk/libstage/blockgroup.cc     2009-07-01 17:50:43 UTC (rev 
7935)
@@ -11,11 +11,11 @@
 
 BlockGroup::BlockGroup() 
   : displaylist(0),
-        blocks(), 
-        minx(0),
-        maxx(0),
-        miny(0),
-        maxy(0)
+       blocks(), 
+       minx(0),
+       maxx(0),
+       miny(0),
+       maxy(0)
 { /* empty */ }
 
 BlockGroup::~BlockGroup()
@@ -31,7 +31,7 @@
 void BlockGroup::Clear()
 {
   FOR_EACH( it, blocks )
-        delete *it;
+       delete *it;
   
   blocks.clear();
 }
@@ -40,23 +40,22 @@
 {
   // confirm the tentative pose for all blocks
   FOR_EACH( it, blocks )
-        (*it)->SwitchToTestedCells();  
+       (*it)->SwitchToTestedCells();  
 }
 
-GList* BlockGroup::AppendTouchingModels( GList* list )
+void BlockGroup::AppendTouchingModels( ModelPtrSet &v )
 {
   FOR_EACH( it, blocks )
-        list = (*it)->AppendTouchingModels( list );  
-  return list;
+       (*it)->AppendTouchingModels( v );  
 }
 
 Model* BlockGroup::TestCollision()
 {
   Model* hitmod = NULL;
-  
+   
   FOR_EACH( it, blocks )
-        if( (hitmod = (*it)->TestCollision()))
-               break; // bail on the earliest collision
+       if( (hitmod = (*it)->TestCollision()))
+         break; // bail on the earliest collision
 
   return hitmod; // NULL if no collision
 }
@@ -74,21 +73,21 @@
   size.z = 0.0; // grow to largest z we see
   
   FOR_EACH( it, blocks )
-        {
-               // examine all the points in the polygon
-               Block* block = *it;
-               
-               for( unsigned int p=0; p < block->pt_count; p++ )
-                 {
-                        stg_point_t* pt = &block->pts[p];
-                        if( pt->x < minx ) minx = pt->x;
-                        if( pt->y < miny ) miny = pt->y;
-                        if( pt->x > maxx ) maxx = pt->x;
-                        if( pt->y > maxy ) maxy = pt->y;
-                 }
-               
-               size.z = MAX( block->local_z.max, size.z );
-        }
+       {
+         // examine all the points in the polygon
+         Block* block = *it;
+         
+         for( unsigned int p=0; p < block->pt_count; p++ )
+               {
+                 stg_point_t* pt = &block->pts[p];
+                 if( pt->x < minx ) minx = pt->x;
+                 if( pt->y < miny ) miny = pt->y;
+                 if( pt->x > maxx ) maxx = pt->x;
+                 if( pt->y > maxy ) maxy = pt->y;
+               }
+         
+         size.z = MAX( block->local_z.max, size.z );
+       }
   
   // store these bounds for normalization purposes
   size.x = maxx-minx;
@@ -105,13 +104,13 @@
 void BlockGroup::Map()
 {
   FOR_EACH( it, blocks )
-        (*it)->Map();
+       (*it)->Map();
 }
 
 void BlockGroup::UnMap()
 {
   FOR_EACH( it, blocks )
-        (*it)->UnMap();
+       (*it)->UnMap();
 }
 
 void BlockGroup::DrawSolid( const Geom & geom )
@@ -121,13 +120,13 @@
   Gl::pose_shift( geom.pose );
 
   glScalef( geom.size.x / size.x,
-                               geom.size.y / size.y,                           
-                               geom.size.z / size.z );
+                       geom.size.y / size.y,                           
+                       geom.size.z / size.z );
   
   glTranslatef( -offset.x, -offset.y, -offset.z );
   
   FOR_EACH( it, blocks )
-        (*it)->DrawSolid();
+       (*it)->DrawSolid();
 
   glPopMatrix();
 }
@@ -137,13 +136,13 @@
   glPushMatrix();
   
   glScalef( geom.size.x / size.x,
-                               geom.size.y / size.y,                           
-                               geom.size.z / size.z );
+                       geom.size.y / size.y,                           
+                       geom.size.z / size.z );
   
   glTranslatef( -offset.x, -offset.y, -offset.z );
   
   FOR_EACH( it, blocks )
-        (*it)->DrawFootPrint();
+       (*it)->DrawFootPrint();
 
   glPopMatrix();
 }
@@ -151,15 +150,15 @@
 void BlockGroup::BuildDisplayList( Model* mod )
 {
   if( ! mod->world->IsGUI() )
-        return;
+       return;
 
   //printf( "display list for model %s\n", mod->token );
 
   if( displaylist == 0 )
-        {
-               displaylist = glGenLists(1);
-               CalcSize();
-        }
+       {
+         displaylist = glGenLists(1);
+         CalcSize();
+       }
 
   glNewList( displaylist, GL_COMPILE );        
     
@@ -169,8 +168,8 @@
   Gl::pose_shift( geom.pose );
 
   glScalef( geom.size.x / size.x,
-                               geom.size.y / size.y,                           
-                               geom.size.z / size.z );
+                       geom.size.y / size.y,                           
+                       geom.size.z / size.z );
   
   glTranslatef( -offset.x, -offset.y, -offset.z );
   
@@ -181,18 +180,18 @@
   mod->PushColor( mod->color );
   
   FOR_EACH( it, blocks )
-        {
-               Block* blk = (*it);
+       {
+         Block* blk = (*it);
                
-               if( (!blk->inherit_color) && (blk->color != mod->color) )
-                 {
-                        mod->PushColor( blk->color );          
-                        blk->DrawSolid();
-                        mod->PopColor();
-                 }
-               else
+         if( (!blk->inherit_color) && (blk->color != mod->color) )
+               {
+                 mod->PushColor( blk->color );         
                  blk->DrawSolid();
-        }
+                 mod->PopColor();
+               }
+         else
+               blk->DrawSolid();
+       }
   
   mod->PopColor();
   
@@ -206,19 +205,19 @@
   mod->PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a ));
   
   FOR_EACH( it, blocks )
-        {
-               Block* blk = *it;
+       {
+         Block* blk = *it;
                
-               if( (!blk->inherit_color) && (blk->color != mod->color) )
-                 {
-                        stg_color_unpack( blk->color, &r, &g, &b, &a );
-                        mod->PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a 
));
-                        blk->DrawSolid();
-                        mod->PopColor();
-                 }
-               else
+         if( (!blk->inherit_color) && (blk->color != mod->color) )
+               {
+                 stg_color_unpack( blk->color, &r, &g, &b, &a );
+                 mod->PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a ));
                  blk->DrawSolid();
-        }
+                 mod->PopColor();
+               }
+         else
+               blk->DrawSolid();
+       }
 
   glDepthMask(GL_TRUE);
   glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
@@ -231,7 +230,7 @@
 void BlockGroup::CallDisplayList( Model* mod )
 {
   if( displaylist == 0 )
-        BuildDisplayList( mod );
+       BuildDisplayList( mod );
   
   glCallList( displaylist );
 }
@@ -248,14 +247,14 @@
   char full[_POSIX_PATH_MAX];
   
   if( bitmapfile[0] == '/' )
-        strcpy( full, bitmapfile );
+       strcpy( full, bitmapfile );
   else
-        {
-               char *tmp = strdup(wf->filename);
-               snprintf( full, _POSIX_PATH_MAX,
-                                        "%s/%s",  dirname(tmp), bitmapfile );
-               free(tmp);
-        }
+       {
+         char *tmp = strdup(wf->filename);
+         snprintf( full, _POSIX_PATH_MAX,
+                               "%s/%s",  dirname(tmp), bitmapfile );
+         free(tmp);
+       }
   
   PRINT_DEBUG1( "attempting to load image %s", full );
   
@@ -263,60 +262,60 @@
   unsigned int rect_count = 0;
   unsigned int width, height;
   if( stg_rotrects_from_image_file( full,
-                                                                               
                &rects,
-                                                                               
                &rect_count,
-                                                                               
                &width, &height ) )
-        {
-               PRINT_ERR1( "failed to load rects from image file \"%s\"",
-                                               full );
-               return;
-        }
+                                                                       &rects,
+                                                                       
&rect_count,
+                                                                       &width, 
&height ) )
+       {
+         PRINT_ERR1( "failed to load rects from image file \"%s\"",
+                                 full );
+         return;
+       }
   
   //printf( "found %d rects in \"%s\" at %p\n", 
   //     rect_count, full, rects );
                         
   if( rects && (rect_count > 0) )
-        {
-               // TODO fix this
-               stg_color_t col = stg_color_pack( 1.0, 0,0,1.0 ); 
+       {
+         // TODO fix this
+         stg_color_t col = stg_color_pack( 1.0, 0,0,1.0 ); 
                
-               for( unsigned int r=0; r<rect_count; r++ )
-                 {
-                        stg_point_t pts[4];
+         for( unsigned int r=0; r<rect_count; r++ )
+               {
+                 stg_point_t pts[4];
                         
-                        double x = rects[r].pose.x;
-                        double y = rects[r].pose.y;
-                        double w = rects[r].size.x;
-                        double h = rects[r].size.y;
+                 double x = rects[r].pose.x;
+                 double y = rects[r].pose.y;
+                 double w = rects[r].size.x;
+                 double h = rects[r].size.y;
                         
-                        pts[0].x = x;
-                        pts[0].y = y;
-                        pts[1].x = x + w;
-                        pts[1].y = y;
-                        pts[2].x = x + w;
-                        pts[2].y = y + h;
-                        pts[3].x = x;
-                        pts[3].y = y + h;                                      
                 
+                 pts[0].x = x;
+                 pts[0].y = y;
+                 pts[1].x = x + w;
+                 pts[1].y = y;
+                 pts[2].x = x + w;
+                 pts[2].y = y + h;
+                 pts[3].x = x;
+                 pts[3].y = y + h;                                             
         
                         
-                        AppendBlock( new Block( mod,
-                                                                               
         pts,4,
-                                                                               
         0,1,
-                                                                               
         col,
-                                                                               
         true ) );               
-                 }                      
-               free( rects );
-        }  
+                 AppendBlock( new Block( mod,
+                                                                 pts,4,
+                                                                 0,1,
+                                                                 col,
+                                                                 true ) );     
         
+               }                        
+         free( rects );
+       }  
   
   CalcSize();
 }
 
 
 void BlockGroup::Rasterize( uint8_t* data, 
-                                                                        
unsigned int width, 
-                                                                        
unsigned int height,
-                                                                        
stg_meters_t cellwidth,
-                                                                        
stg_meters_t cellheight )
+                                                       unsigned int width, 
+                                                       unsigned int height,
+                                                       stg_meters_t cellwidth,
+                                                       stg_meters_t cellheight 
)
 {  
   FOR_EACH( it, blocks )
-        (*it)->Rasterize( data, width, height, cellwidth, cellheight );
+       (*it)->Rasterize( data, width, height, cellwidth, cellheight );
 }

Modified: code/stage/trunk/libstage/canvas.cc
===================================================================
--- code/stage/trunk/libstage/canvas.cc 2009-07-01 15:37:14 UTC (rev 7934)
+++ code/stage/trunk/libstage/canvas.cc 2009-07-01 17:50:43 UTC (rev 7935)
@@ -901,9 +901,6 @@
   if( showBBoxes )
         DrawBoundingBoxes();
 
-
-  //LISTMETHOD( world->puck_list, Puck*, Draw );
-
   // TODO - finish this properly
   //LISTMETHOD( models_sorted, Model*, DrawWaypoints );
   

Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc  2009-07-01 15:37:14 UTC (rev 7934)
+++ code/stage/trunk/libstage/model.cc  2009-07-01 17:50:43 UTC (rev 7935)
@@ -278,7 +278,7 @@
   
   // remove myself from my parent's child list, or the world's child
   // list if I have no parent
-  std::vector<Model*>& vec  = parent ? parent->children : world->children;
+  ModelPtrVec& vec  = parent ? parent->children : world->children;
   vec.erase( std::remove( vec.begin(), vec.end(), this ));
 
   if( callbacks ) g_hash_table_destroy( callbacks );
@@ -767,13 +767,11 @@
     SetPose( Pose::Random( xmin,xmax, ymin, ymax ));           
 }
 
-
-GList* Model::AppendTouchingModels( GList* list )
+void Model::AppendTouchingModels( ModelPtrSet& touchers )
 {
-  return blockgroup.AppendTouchingModels( list );
+  blockgroup.AppendTouchingModels( touchers );
 }
 
-
 Model* Model::TestCollision()
 {
   //printf( "mod %s test collision...\n", token );  
@@ -810,11 +808,18 @@
   pps_charging = NULL;
   
   // run through and update all appropriate touchers
-  for( GList* touchers = AppendTouchingModels( NULL );
-                touchers;
-                touchers = touchers->next )
+  ModelPtrSet touchers;
+  AppendTouchingModels( touchers );
+
+  //  for( GList* touchers = AppendTouchingModels( NULL );
+  //    touchers;
+  //    touchers = touchers->next )
+  FOR_EACH( it, touchers )
+//   for( GList* touchers = AppendTouchingModels( NULL );
+//              touchers;
+//              touchers = touchers->next )
         {
-               Model* toucher = (Model*)touchers->data;                
+          Model* toucher = (*it); //(Model*)touchers->data;            
                PowerPack* hispp =toucher->FindPowerPack();             
                
                if( hispp && toucher->watts_take > 0.0) 
@@ -892,18 +897,19 @@
        
   // convert usec to sec
   double interval( (double)world->interval_sim / 1e6 );
-       
+  
   // find the change of pose due to our velocity vector
   Pose p( velocity.x * interval,
-                                       velocity.y * interval,
-                                       velocity.z * interval,
-                                       normalize( velocity.a * interval ));
-       
+                 velocity.y * interval,
+                 velocity.z * interval,
+                 normalize( velocity.a * interval ));
+  
   // attempts to move to the new pose. If the move fails because we'd
   // hit another model, that model is returned.        
        // ConditionalMove() returns a pointer to the model we hit, or
        // NULL. We use this as a boolean for SetStall()
-  SetStall( ConditionalMove( pose_sum( pose, p ) ) );
+  //SetStall( ConditionalMove( pose_sum( pose, p ) ) );
+  SetStall( ConditionalMove( pose + p ) );
 }
 
 

Modified: code/stage/trunk/libstage/model_getset.cc
===================================================================
--- code/stage/trunk/libstage/model_getset.cc   2009-07-01 15:37:14 UTC (rev 
7934)
+++ code/stage/trunk/libstage/model_getset.cc   2009-07-01 17:50:43 UTC (rev 
7935)
@@ -192,7 +192,7 @@
     return pose;
   
   // otherwise    
-  Pose global_pose = pose_sum( parent->GetGlobalPose(), pose );                
+  Pose global_pose = parent->GetGlobalPose() + pose;           
   
   // we are on top of our parent
   global_pose.z += parent->geom.size.z;

Modified: code/stage/trunk/libstage/stage.cc
===================================================================
--- code/stage/trunk/libstage/stage.cc  2009-07-01 15:37:14 UTC (rev 7934)
+++ code/stage/trunk/libstage/stage.cc  2009-07-01 17:50:43 UTC (rev 7935)
@@ -127,9 +127,7 @@
 
                        // map the name to the color in the table
                        g_hash_table_insert( table, (gpointer)colorname, 
(gpointer)col );
-
                }
-
                fclose(file);
        }
 
@@ -142,19 +140,6 @@
                return (stg_color_t)0;
 }
 
-// returns the resultant of vector [p1] and [p2] 
-Pose Stg::pose_scale( const Pose& p1, const double sx, const double sy, const 
double sz )
-{
-  Pose scaled;
-  scaled.x = p1.x * sx;
-  scaled.y = p1.y * sy;
-  scaled.z = p1.z * sz;
-  scaled.a = p1.a;
-  
-  return scaled;
-}
-
-
 static guchar* pb_get_pixel( Fl_Shared_Image* img, int x, int y )
 {
   guchar* pixels = (guchar*)(img->data()[0]);
@@ -215,12 +200,10 @@
        if( widthp ) *widthp = img_width;
        if( heightp ) *heightp = img_height;
 
-
-       int y, x;
-       for(y = 0; y < img_height; y++)
-       {
-               for(x = 0; x < img_width; x++)
-               {
+       for(int y = 0; y < img_height; y++)
+         {
+               for(int x = 0; x < img_width; x++)
+                 {
                        // skip blank (white) pixels
                        if(  pb_pixel_is_set( img,x,y, threshold) )
                                continue;

Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh  2009-07-01 15:37:14 UTC (rev 7934)
+++ code/stage/trunk/libstage/stage.hh  2009-07-01 17:50:43 UTC (rev 7935)
@@ -74,6 +74,7 @@
 namespace Stg 
 {
   // forward declare
+  class Block;
   class Canvas;
   class Cell;
   class Worldfile;
@@ -84,6 +85,15 @@
   class Camera;
   class FileManager;
   class Option;
+    
+  /** Set of pointers to Models. */
+  typedef std::set<Model*> ModelPtrSet;
+  /** Vector of pointers to Models. */
+  typedef std::vector<Model*> ModelPtrVec;
+  /** Vector of pointers to Blocks. */
+  typedef std::vector<Block*> BlockPtrVec;
+  /** Vector of pointers to Cells.*/
+  typedef std::vector<Cell*> CellPtrVec;
 
   /** Initialize the Stage library */
   void Init( int* argc, char** argv[] );
@@ -106,10 +116,10 @@
     MODEL_TYPE_BLOBFINDER,
     MODEL_TYPE_BLINKENLIGHT,
     MODEL_TYPE_CAMERA,
-        MODEL_TYPE_GRIPPER,
-        MODEL_TYPE_ACTUATOR,
-        MODEL_TYPE_LOADCELL,
-        MODEL_TYPE_LIGHTINDICATOR,
+       MODEL_TYPE_GRIPPER,
+       MODEL_TYPE_ACTUATOR,
+       MODEL_TYPE_LOADCELL,
+       MODEL_TYPE_LIGHTINDICATOR,
     MODEL_TYPE_COUNT // must be the last entry, to count the number of
     // types
   } stg_model_type_t;
@@ -184,13 +194,12 @@
   /** take binary sign of a, either -1, or 1 if >= 0. */
   inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); }
 
-
   /** Describe the image format used for saving screenshots. */
   typedef enum {
     STG_IMAGE_FORMAT_PNG,
     STG_IMAGE_FORMAT_JPG
   } stg_image_format_t;
-
+  
   /** any integer value other than this is a valid fiducial ID */
   enum { FiducialNone = 0 };
 
@@ -234,27 +243,24 @@
   void stg_color_unpack( stg_color_t col, 
                                                                 double* r, 
double* g, double* b, double* a );
   
-  //typedef std::vector<Model*> ModelPtrVec;
-  //typedef std::vector<Model&> ModelRefVec;
-
   /** specify a rectangular size */
   class Size
   {
   public:
     stg_meters_t x, y, z;
-        
+       
     Size( stg_meters_t x, 
-                        stg_meters_t y, 
-                        stg_meters_t z )
+                 stg_meters_t y, 
+                 stg_meters_t z )
       : x(x), y(y), z(z)
     {/*empty*/}
-
+       
     /** default constructor uses default non-zero values */
     Size() : x( 0.4 ), y( 0.4 ), z( 1.0 )
     {/*empty*/}        
-
-        void Load( Worldfile* wf, int section, const char* keyword );
-        void Save( Worldfile* wf, int section, const char* keyword );
+       
+       void Load( Worldfile* wf, int section, const char* keyword );
+       void Save( Worldfile* wf, int section, const char* keyword );
   };
   
   /** Specify a 3 axis position, in x, y and heading. */
@@ -265,9 +271,9 @@
     stg_radians_t a;///< rotation about the z axis. 
     
     Pose( stg_meters_t x, 
-                        stg_meters_t y, 
-                        stg_meters_t z,
-                        stg_radians_t a ) 
+                 stg_meters_t y, 
+                 stg_meters_t z,
+                 stg_radians_t a ) 
       : x(x), y(y), z(z), a(a)
     { /*empty*/ }
     
@@ -279,39 +285,50 @@
     /** return a random pose within the bounding rectangle, with z=0 and
                  angle random */
     static Pose Random( stg_meters_t xmin, stg_meters_t xmax, 
-                                                               stg_meters_t 
ymin, stg_meters_t ymax )
+                                               stg_meters_t ymin, stg_meters_t 
ymax )
     {           
       return Pose( xmin + drand48() * (xmax-xmin),
-                                                ymin + drand48() * (ymax-ymin),
-                                                0, 
-                                                normalize( drand48() * (2.0 * 
M_PI) ));
+                                  ymin + drand48() * (ymax-ymin),
+                                  0, 
+                                  normalize( drand48() * (2.0 * M_PI) ));
     }
     
-        /** Print pose in human-readable format on stdout
-                 @param prefix Character string to prepend to pose output 
-        */
+       /** Print pose in human-readable format on stdout
+               @param prefix Character string to prepend to pose output 
+       */
     virtual void Print( const char* prefix )
     {
       printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
-                                 prefix, x,y,z,a );
+                         prefix, x,y,z,a );
     }
-        
-        std::string String()
-        {
-               char buf[256];
-               snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]",
-                                        x,y,z,a );
-               return std::string(buf);
-        }
-
-        /* returns true iff all components of the velocity are zero. */
-        bool IsZero() const { return( !(x || y || z || a )); };
-
-        /** Set the pose to zero [0,0,0,0] */
-        void Zero(){ x=y=z=a=0.0; }
-
-        void Load( Worldfile* wf, int section, const char* keyword );
-        void Save( Worldfile* wf, int section, const char* keyword );
+       
+       std::string String()
+       {
+         char buf[256];
+         snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]",
+                               x,y,z,a );
+         return std::string(buf);
+       }
+       
+       /* returns true iff all components of the velocity are zero. */
+       bool IsZero() const { return( !(x || y || z || a )); };
+       
+       /** Set the pose to zero [0,0,0,0] */
+       void Zero(){ x=y=z=a=0.0; }
+       
+       void Load( Worldfile* wf, int section, const char* keyword );
+       void Save( Worldfile* wf, int section, const char* keyword );
+       
+       inline Pose operator+( const Pose& p )
+       {
+         const double cosa = cos(a);
+         const double sina = sin(a);
+         
+         return Pose( x + p.x * cosa - p.y * sina,
+                                  y + p.x * sina + p.y * cosa,
+                                  z + p.z,
+                                  normalize(a + p.a) );         
+       }       
   };
   
   
@@ -424,24 +441,24 @@
   {
   public:
     stg_meters_t x, y;
-        stg_point_t( stg_meters_t x, stg_meters_t y ) : x(x), y(y){}    
-        stg_point_t() : x(0.0), y(0.0){}
-
-        bool operator+=( const stg_point_t& other ) 
-        { return ((x += other.x) && (y += other.y) ); }  
+       stg_point_t( stg_meters_t x, stg_meters_t y ) : x(x), y(y){}     
+       stg_point_t() : x(0.0), y(0.0){}
+       
+       bool operator+=( const stg_point_t& other ) 
+       { return ((x += other.x) && (y += other.y) ); }  
   };
-    
+  
   /** Define a point in 3d space */
   class stg_point3_t
   {
   public:
     stg_meters_t x,y,z;
-        stg_point3_t( stg_meters_t x, stg_meters_t y, stg_meters_t z ) 
-               : x(x), y(y), z(z) {}    
-        //stg_point3_t( int x, int y ) : x(x), y(y), z(0.0) {}  
-        stg_point3_t() : x(0.0), y(0.0), z(0.0) {}
+       stg_point3_t( stg_meters_t x, stg_meters_t y, stg_meters_t z ) 
+         : x(x), y(y), z(z) {}  
+  
+       stg_point3_t() : x(0.0), y(0.0), z(0.0) {}
   };
-
+  
   /** Define an integer point on the 2d plane */
   class stg_point_int_t
   {
@@ -458,6 +475,7 @@
         { return ((x == other.x) && (y == other.y) ); }
   };
   
+  typedef std::vector<stg_point_int_t> PointIntVec;
 
   /** create an array of 4 points containing the corners of a unit
       square.  */
@@ -588,42 +606,20 @@
 
     /** Create a draw_t object of specified type from a vertex array */
     draw_t* create( type_t type,  
-                                                 vertex_t* verts,
-                                                 size_t vert_count );
+                                       vertex_t* verts,
+                                       size_t vert_count );
 
     /** Delete the draw_t object, deallocting its memory */
     void destroy( draw_t* d );
   } // end namespace draw
 
-
-  // MACROS ------------------------------------------------------
-  // Some useful macros
-
+  
   /** Look up the color in the X11 database.  (i.e. transform color
       name to color value).  If the color is not found in the
       database, a bright red color (0xF00) will be returned instead.
   */
   stg_color_t stg_lookup_color(const char *name);
-  
-  /** returns the sum of [p1] + [p2], in [p1]'s coordinate system */
-  inline Pose pose_sum( const Pose& p1, const Pose& p2 )
-  {
-        double cosa = cos(p1.a);
-        double sina = sin(p1.a);
-        
-        Pose result;
-        result.x = p1.x + p2.x * cosa - p2.y * sina;
-        result.y = p1.y + p2.x * sina + p2.y * cosa;
-        result.z = p1.z + p2.z;
-        result.a = normalize(p1.a + p2.a);
-        
-        return result;
-  }
-
-  /** returns a new pose, with each axis scaled */
-  inline Pose pose_scale( const Pose& p1, const double x, const double y, 
const double z );
-
-
+    
   // PRETTY PRINTING -------------------------------------------------
 
   /** Report an error, with a standard, friendly message header */
@@ -826,9 +822,8 @@
     friend class Canvas; // allow Canvas access to our private members
         
   protected:
-       std::vector<Model*> children;
+       ModelPtrVec children;
     bool debug;
-       GList* puck_list;
     char* token;
        
         void Load( Worldfile* wf, int section );
@@ -837,7 +832,7 @@
   public:
        
     /** get the children of the this element */
-       std::vector<Model*>& GetChildren(){ return children;}
+       ModelPtrVec& GetChildren(){ return children;}
     
     /** recursively call func( model, arg ) for each descendant */
     void ForEachDescendant( stg_model_callback_t func, void* arg );
@@ -940,7 +935,7 @@
     static void UpdateCb( World* world);
     static unsigned int next_id; ///<initially zero, used to allocate unique 
sequential world ids
         
-       std::set<Model*> charge_list; ///< Models which receive charge are 
listed here
+       ModelPtrSet charge_list; ///< Models which receive charge are listed 
here
     bool destroy;
     bool dirty; ///< iff true, a gui redraw would be required
         GList* event_list; //< 
@@ -956,7 +951,7 @@
         unsigned int show_clock_interval; ///< updates between clock xoutputs
     GMutex* thread_mutex; ///< protect the worker thread management stuff
     int total_subs; ///< the total number of subscriptions to all models
-       std::set<Model*> velocity_list; ///< Models with non-zero velocity and 
should have their poses updated
+       ModelPtrSet velocity_list; ///< Models with non-zero velocity and 
should have their poses updated
        
        unsigned int worker_threads; ///< the number of worker threads to use
        unsigned int threads_working; ///< the number of worker threads not yet 
finished
@@ -965,23 +960,23 @@
        
        /** Keep a list of all models with detectable fiducials. This
                avoids searching the whole world for fiducials. */
-       std::set<Model*> models_with_fiducials;
+       ModelPtrSet models_with_fiducials;
        
   protected:    
 
-        std::list<std::pair<stg_world_callback_t,void*> > cb_list; ///< List 
of callback functions and arguments
+       std::list<std::pair<stg_world_callback_t,void*> > cb_list; ///< List of 
callback functions and arguments
     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 
-        GHashTable* option_table; ///< GUI options (toggles) registered by 
models
-        GList* powerpack_list; ///< List of all the powerpacks attached to 
models in the world
+       GHashTable* option_table; ///< GUI options (toggles) registered by 
models
+       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 
microseconds
-        std::map<stg_point_int_t,SuperRegion*> superregions;
+       std::map<stg_point_int_t,SuperRegion*> superregions;
     SuperRegion* sr_cached; ///< The last superregion looked up by this world
        
        // todo - test performance of std::set
-       std::vector<std::vector<Model*> > update_lists;  
+       std::vector<ModelPtrVec > update_lists;  
         
     long unsigned int updates; ///< the number of simulated time steps 
executed so far
     Worldfile* wf; ///< If set, points to the worldfile used to create this 
world
@@ -997,8 +992,8 @@
     void TogglePause(){ paused = !paused; };
        bool Paused(){ return( paused ); };
        
-        std::vector<stg_point_int_t> rt_cells;
-        std::vector<stg_point_int_t> rt_candidate_cells;
+        PointIntVec rt_cells;
+        PointIntVec rt_candidate_cells;
 
     static const int DEFAULT_PPM = 50;  // default resolution in pixels per 
meter
     static const stg_msec_t DEFAULT_INTERVAL_SIM = 100;  ///< duration of sim 
timestep
@@ -1027,7 +1022,7 @@
     void LoadModel( Worldfile* wf, int entity, GHashTable* entitytable );
     void LoadBlock( Worldfile* wf, int entity, GHashTable* entitytable );
     void LoadBlockGroup( Worldfile* wf, int entity, GHashTable* entitytable );
-    void LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable );
+       //    void LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable 
);
 
         virtual Model* RecentlySelectedModel(){ return NULL; }
 
@@ -1043,7 +1038,7 @@
                  pt1 to pt2 inclusive */
     void ForEachCellInLine( const stg_point_int_t& pt1,
                                                                         const 
stg_point_int_t& pt2, 
-                                                                        
std::vector<Cell*>& cells );
+                                                                        
CellPtrVec& cells );
 
     /** convert a distance in meters to a distance in world occupancy
                  grid pixels */
@@ -1225,10 +1220,11 @@
                std::vector<stg_point_t>& Points()
     { return pts; };            
 
-    void AddToCellArray( std::vector<Cell*>* blocks );
-    void RemoveFromCellArray( std::vector<Cell*>* blocks );
+    void AddToCellArray( CellPtrVec* blocks );
+    void RemoveFromCellArray( CellPtrVec* blocks );
     void GenerateCandidateCells();  
-        GList* AppendTouchingModels( GList* list );
+
+       void AppendTouchingModels( ModelPtrSet& touchers );
         
                /** Returns the first model that shares a bitmap cell with this 
model */
     Model* TestCollision(); 
@@ -1266,7 +1262,7 @@
         
     /** record the cells into which this block has been rendered to
                  UnMapping them very quickly. */  
-        std::vector<Cell*> * rendered_cells;
+        CellPtrVec * rendered_cells;
 
     /** When moving a model, we test for collisions by generating, for
                  each block, a list of the cells in which it would be rendered 
if the
@@ -1274,19 +1270,19 @@
                  allowed - the rendered cells are cleared, the potential cells 
are
                  written, and the pointers to the rendered and potential cells 
are
                  switched for next time (avoiding a memory copy).*/
-        std::vector<Cell*> * candidate_cells;
-
-               std::vector<stg_point_int_t> gpts;
-
-        /** find the position of a block's point in model coordinates
-                 (m) */
-        stg_point_t BlockPointToModelMeters( const stg_point_t& bpt );
-
-        /** Update the cache of block points converted to model coordinates */
-        //stg_point_t* GetPointsInModelCoords();
-
-        /** invalidate the cache of points in model coordinates */
-        void InvalidateModelPointCache();
+       CellPtrVec * candidate_cells;
+       
+       PointIntVec gpts;
+       
+       /** find the position of a block's point in model coordinates
+               (m) */
+       stg_point_t BlockPointToModelMeters( const stg_point_t& bpt );
+       
+       /** Update the cache of block points converted to model coordinates */
+       //stg_point_t* GetPointsInModelCoords();
+       
+       /** invalidate the cache of points in model coordinates */
+       void InvalidateModelPointCache();
   };
 
 
@@ -1300,7 +1296,7 @@
 
     void BuildDisplayList( Model* mod );
         
-        std::vector<Block*> blocks;
+        BlockPtrVec blocks;
     Size size;
     stg_point3_t offset;
     stg_meters_t minx, maxx, miny, maxy;
@@ -1320,7 +1316,7 @@
     void CallDisplayList( Model* mod );
     void Clear() ; /** deletes all blocks from the group */
         
-        GList* AppendTouchingModels( GList* list );
+       void AppendTouchingModels( ModelPtrSet& touchers );
        
     /** Returns a pointer to the first model detected to be colliding
                  with a block in this group, or NULL, if none are detected. */
@@ -1346,7 +1342,7 @@
         
         void InvalidateModelPointCache()
         {
-               for( std::vector<Block*>::iterator it( blocks.begin() );
+               for( BlockPtrVec::iterator it( blocks.begin() );
                          it != blocks.end();
                          ++it )
                  (*it)->InvalidateModelPointCache();
@@ -1939,7 +1935,7 @@
         /// Register an Option for pickup by the GUI
         void RegisterOption( Option* opt );
 
-        GList* AppendTouchingModels( GList* list );
+        void AppendTouchingModels( ModelPtrSet& touchers );
 
         /** Check to see if the current pose will yield a collision with
                  obstacles.  Returns a pointer to the first entity we are in
@@ -2393,10 +2389,10 @@
         Pose GlobalToLocal( const Pose& pose ) const;
         
         /** Return the global pose (i.e. pose in world coordinates) of a
-                 pose specified in the model's local coordinate system */
-        Pose LocalToGlobal( const Pose& pose ) const
-               {  
-                       return pose_sum( pose_sum( GetGlobalPose(), geom.pose 
), pose );
+                pose specified in the model's local coordinate system */
+       Pose LocalToGlobal( const Pose& pose ) const
+       {  
+         return( ( GetGlobalPose() + geom.pose ) + pose );
     }
                
 //      /** Return the 3d point in world coordinates of a 3d point

Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc  2009-07-01 15:37:14 UTC (rev 7934)
+++ code/stage/trunk/libstage/world.cc  2009-07-01 17:50:43 UTC (rev 7935)
@@ -227,15 +227,6 @@
   mod->LoadBlock( wf, entity );
 }
 
-// void World::LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable )
-// { 
-//   Puck* puck = new Puck();
-//   puck->Load( wf, entity );  
-//   puck_list = g_list_prepend( puck_list, puck );
-// }
-
-
-
 Model* World::CreateModel( Model* parent, const char* typestr )
 {
   Model* mod = NULL; // new model to return
@@ -370,8 +361,6 @@
                }
       else if( strcmp( typestr, "block" ) == 0 )
                LoadBlock( wf, entity, entitytable );
-         //            else if( strcmp( typestr, "puck" ) == 0 )
-         //              LoadPuck( wf, entity, entitytable );
          else
                LoadModel( wf, entity, entitytable );
     }
@@ -744,7 +733,7 @@
                                 (cy>=0) && (cy<REGIONWIDTH) && 
                                 n > 0 )
                        {                        
-                         for( std::vector<Block*>::iterator it( 
c->blocks.begin() );
+                         for( BlockPtrVec::iterator it( c->blocks.begin() );
                                   it != c->blocks.end();
                                   ++it )                                       
 
                                {                     
@@ -1085,7 +1074,7 @@
 void World::UpdateListRemove( Model* mod )
 { 
   // choose the right update list
-  std::vector<Model*>& vec = update_lists[ mod->update_list_num ];
+  ModelPtrVec& vec = update_lists[ mod->update_list_num ];
   // and erase the model from it
   vec.erase( remove( vec.begin(), vec.end(), mod ));
 }


This was sent by the SourceForge.net collaborative development platform, the 
world's largest Open Source development site.

------------------------------------------------------------------------------
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit

Reply via email to