blockgroup.cc
Go to the documentation of this file.
00001 
00002 #include "stage.hh"
00003 #include "worldfile.hh"
00004 
00005 #include <libgen.h> // for dirname(3)
00006 #include <limits.h> // for _POSIX_PATH_MAX
00007 
00008 #undef DEBUG
00009 
00010 using namespace Stg;
00011 using namespace std;
00012 
00013 BlockGroup::BlockGroup() 
00014   : displaylist(0),
00015     blocks(), 
00016     minx(0),
00017     maxx(0),
00018     miny(0),
00019     maxy(0)
00020 { /* empty */ }
00021 
00022 BlockGroup::~BlockGroup()
00023 {        
00024   Clear();
00025 }
00026 
00027 void BlockGroup::AppendBlock( Block* block )
00028 {
00029   blocks.push_back( block );
00030 }
00031 
00032 void BlockGroup::Clear()
00033 {
00034   FOR_EACH( it, blocks )
00035     delete *it;
00036   
00037   blocks.clear();
00038 }
00039 
00040 void BlockGroup::AppendTouchingModels( std::set<Model*>& v )
00041 {
00042   FOR_EACH( it, blocks )
00043     (*it)->AppendTouchingModels( v );  
00044 }
00045 
00046 Model* BlockGroup::TestCollision()
00047 {
00048   Model* hitmod = NULL;
00049    
00050   FOR_EACH( it, blocks )
00051     if( (hitmod = (*it)->TestCollision()))
00052       break; // bail on the earliest collision
00053 
00054   return hitmod; // NULL if no collision
00055 }
00056 
00057 
00058 // establish the min and max of all the blocks, so we can scale this
00059 // group later
00060 void BlockGroup::CalcSize()
00061 {  
00062   // assuming the blocks currently fit in a square +/- one billion units
00063   //double minx, miny, maxx, maxy, minz, maxz;
00064   minx = miny =  billion;
00065   maxx = maxy = -billion;
00066   
00067   size.z = 0.0; // grow to largest z we see
00068   
00069   FOR_EACH( it, blocks )
00070     {
00071       // examine all the points in the polygon
00072       Block* block = *it;
00073       
00074       FOR_EACH( it, block->pts )
00075         {
00076           if( it->x < minx ) minx = it->x;
00077           if( it->y < miny ) miny = it->y;
00078           if( it->x > maxx ) maxx = it->x;
00079           if( it->y > maxy ) maxy = it->y;
00080         }
00081       
00082       size.z = std::max( block->local_z.max, size.z );
00083     }
00084   
00085   // store these bounds for normalization purposes
00086   size.x = maxx-minx;
00087   size.y = maxy-miny;
00088   
00089   offset.x = minx + size.x/2.0;
00090   offset.y = miny + size.y/2.0;
00091   offset.z = 0; // todo?
00092 
00093   InvalidateModelPointCache();
00094 }
00095 
00096 
00097 void BlockGroup::Map( unsigned int layer )
00098 {
00099   FOR_EACH( it, blocks )
00100     (*it)->Map(layer);
00101 }
00102 
00103 // defined in world.cc
00104 //void SwitchSuperRegionLock( SuperRegion* current, SuperRegion* next );
00105 
00106 void BlockGroup::UnMap( unsigned int layer )
00107 {
00108   FOR_EACH( it, blocks )
00109     (*it)->UnMap(layer);
00110 }
00111 
00112 void BlockGroup::DrawSolid( const Geom & geom )
00113 {
00114   glPushMatrix();
00115   
00116   Gl::pose_shift( geom.pose );
00117   
00118   glScalef( geom.size.x / size.x,
00119             geom.size.y / size.y,                               
00120             geom.size.z / size.z );
00121   
00122   glTranslatef( -offset.x, -offset.y, -offset.z );
00123   
00124   FOR_EACH( it, blocks )
00125     (*it)->DrawSolid(false);
00126   
00127   glPopMatrix();
00128 }
00129 
00130 void BlockGroup::DrawFootPrint( const Geom & geom )
00131 {
00132   glPushMatrix();
00133   
00134   glScalef( geom.size.x / size.x,
00135             geom.size.y / size.y,                               
00136             geom.size.z / size.z );
00137   
00138   glTranslatef( -offset.x, -offset.y, -offset.z );
00139   
00140   FOR_EACH( it, blocks )
00141     (*it)->DrawFootPrint();
00142   
00143   glPopMatrix();
00144 }
00145 
00146 void BlockGroup::BuildDisplayList( Model* mod )
00147 {
00148   //puts( "build" );
00149   
00150   if( ! mod->world->IsGUI() )
00151     return;
00152   
00153   //printf( "display list for model %s\n", mod->token );
00154   if( displaylist == 0 )
00155     {
00156       displaylist = glGenLists(1);
00157       CalcSize();
00158     }
00159   
00160   
00161   glNewList( displaylist, GL_COMPILE ); 
00162   
00163   
00164   // render each block as a polygon extruded into Z
00165   
00166   Geom geom = mod->GetGeom();
00167   
00168   Gl::pose_shift( geom.pose );
00169   
00170   glScalef( geom.size.x / size.x,
00171             geom.size.y / size.y,                               
00172             geom.size.z / size.z );
00173   
00174   glTranslatef( -offset.x, -offset.y, -offset.z );
00175   
00176   glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
00177   glEnable(GL_POLYGON_OFFSET_FILL);
00178   glPolygonOffset(1.0, 1.0);
00179   
00180   mod->PushColor( mod->color );
00181   
00182   //const bool topview =  dynamic_cast<WorldGui*>(mod->world)->IsTopView();
00183   
00184   FOR_EACH( it, blocks )
00185     {
00186       Block* blk = (*it);
00187       
00188       if( (!blk->inherit_color) && (blk->color != mod->color) )
00189         {
00190           mod->PushColor( blk->color );                                  
00191           blk->DrawSolid(false);                        
00192           mod->PopColor();
00193         }
00194       else
00195         blk->DrawSolid(false);
00196     }
00197   
00198   mod->PopColor();
00199   
00200   // outline each poly in a darker version of the same color
00201 
00202   glDisable(GL_POLYGON_OFFSET_FILL);
00203   
00204   glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
00205   glDepthMask(GL_FALSE);
00206   
00207   Color c = mod->color;
00208   c.r /= 2.0;
00209   c.g /= 2.0;
00210   c.b /= 2.0;
00211   mod->PushColor( c );
00212   
00213   FOR_EACH( it, blocks )
00214     {
00215       Block* blk = *it;
00216                 
00217       if( (!blk->inherit_color) && (blk->color != mod->color) )
00218         {
00219           Color c = blk->color;
00220           c.r /= 2.0;
00221           c.g /= 2.0;
00222           c.b /= 2.0;
00223           mod->PushColor( c );
00224           blk->DrawSolid(false);
00225           mod->PopColor();
00226         }
00227       else
00228         blk->DrawSolid(false);
00229     }
00230   
00231   glDepthMask(GL_TRUE);
00232   glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
00233   
00234   mod->PopColor();
00235 
00236   glEndList();
00237 }
00238 
00239 void BlockGroup::CallDisplayList( Model* mod )
00240 {
00241   if( displaylist == 0 || mod->rebuild_displaylist )
00242     {
00243       BuildDisplayList( mod );
00244       mod->rebuild_displaylist = 0;
00245     }
00246   
00247   glCallList( displaylist );
00248 }
00249 
00250 void BlockGroup::LoadBlock( Model* mod, Worldfile* wf, int entity )
00251 {
00252   AppendBlock( new Block( mod, wf, entity ));
00253   CalcSize();
00254 }                               
00255   
00256 void BlockGroup::LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile* wf )
00257 {
00258   PRINT_DEBUG1( "attempting to load bitmap \"%s\n", bitmapfile );
00259         
00260   std::string full;
00261   
00262   if( bitmapfile[0] == '/' )
00263     full = bitmapfile;
00264   else
00265     {
00266       char* workaround_const = strdup(wf->filename.c_str());
00267       full = std::string(dirname(workaround_const)) + "/" + bitmapfile;                 
00268       free( workaround_const );
00269     }
00270   
00271   PRINT_DEBUG1( "attempting to load image %s", full );
00272   
00273   std::vector<rotrect_t> rects;
00274   if( rotrects_from_image_file( full,
00275                                 rects ) )
00276     {
00277       PRINT_ERR1( "failed to load rects from image file \"%s\"",
00278                   full.c_str() );
00279       return;
00280     }
00281   
00282   //printf( "found %d rects in \"%s\" at %p\n", 
00283   //      rect_count, full, rects );
00284                          
00285   // TODO fix this
00286   Color col( 1.0, 0.0, 1.0, 1.0 );
00287         
00288   FOR_EACH( rect, rects )
00289     {
00290       std::vector<point_t> pts(4);
00291                         
00292       const double x = rect->pose.x;
00293       const double y = rect->pose.y;
00294       const double w = rect->size.x;
00295       const double h = rect->size.y;
00296                         
00297       pts[0].x = x;
00298       pts[0].y = y;
00299       pts[1].x = x + w;
00300       pts[1].y = y;
00301       pts[2].x = x + w;
00302       pts[2].y = y + h;
00303       pts[3].x = x;
00304       pts[3].y = y + h;                                                  
00305       
00306       AppendBlock( new Block( mod,
00307                               pts,
00308                               0,1,
00309                               col,
00310                               true,
00311                               false) );          
00312     }                    
00313   
00314   CalcSize();
00315 }
00316 
00317 
00318 void BlockGroup::Rasterize( uint8_t* data, 
00319                             unsigned int width, 
00320                             unsigned int height,
00321                             meters_t cellwidth,
00322                             meters_t cellheight )
00323 {  
00324   FOR_EACH( it, blocks )
00325     (*it)->Rasterize( data, width, height, cellwidth, cellheight );
00326 }


stage
Author(s): Richard Vaughan , Brian Gerkey , Reed Hedges , Andrew Howard , Toby Collett , Pooya Karimian , Jeremy Asher , Alex Couture-Beil , Geoff Biggs , Rich Mattes , Abbas Sadat
autogenerated on Thu Aug 27 2015 15:20:57