Skip to content
This repository
Browse code

added model rasterization

  • Loading branch information...
commit e96a997c809706fdc31587208f6fc476ad539e7a 1 parent c1cde49
rtv authored
18 examples/ctrl/fasr.cc
@@ -502,6 +502,24 @@ class Robot
502 502 // Stage calls this when the model starts up
503 503 extern "C" int Init( Model* mod )
504 504 {
  505 + if( strcmp( mod->Token(), "r0" ) == 0 )
  506 + {
  507 + const unsigned int dw = 60, dh = 30;
  508 + uint8_t* data = new uint8_t[dw*dh*2];
  509 + memset( data, 0, sizeof(uint8_t) * dw * dh );
  510 +
  511 + mod->GetWorld()->GetModel( "cave" )->Rasterize( data, dw, dh );
  512 +
  513 + putchar( '\n' );
  514 + for( unsigned int y=0; y<dh; y++ )
  515 + {
  516 + for( unsigned int x=0; x<dw; x++ )
  517 + putchar( data[x + ((dh-y-1)*dw)] ? 'O' : '.' );
  518 + putchar( '\n' );
  519 + }
  520 + delete data;
  521 + }
  522 +
505 523 new Robot( (ModelPosition*)mod,
506 524 mod->GetWorld()->GetModel( "source" ),
507 525 mod->GetWorld()->GetModel( "sink" ) );
81 libstage/block.cc
@@ -314,6 +314,87 @@ void Block::GenerateCandidateCells()
314 314 mapped = true;
315 315 }
316 316
  317 +void Block::Rasterize( uint8_t* data, unsigned int width, unsigned int height )
  318 +{
  319 + Pose pose;// = mod->GetPose();
  320 +
  321 + // add local offset
  322 + pose = pose_sum( pose, mod->geom.pose );
  323 +
  324 + Size bgsize = mod->blockgroup.GetSize();
  325 +
  326 + double scalex = (width-1) / bgsize.x;
  327 + double scaley = (height-1) / bgsize.y;
  328 +
  329 + Rasterize( data, width, height, scalex, scaley, 0,0 );
  330 +}
  331 +
  332 +void swap( int& a, int& b )
  333 +{
  334 + int tmp = a;
  335 + a = b;
  336 + b = tmp;
  337 +}
  338 +
  339 +void Block::Rasterize( uint8_t* data,
  340 + unsigned int width, unsigned int height,
  341 + double scalex, double scaley,
  342 + double offsetx, double offsety )
  343 +{
  344 + //printf( "rasterize block %p : w: %u h: %u scale %.2f %.2f offset %.2f %.2f\n",
  345 + // this, width, height, scalex, scaley, offsetx, offsety );
  346 +
  347 + for( unsigned int p=0; p<pt_count; p++ )
  348 + {
  349 + int xa = round( (pts[p ].x + offsetx) * scalex );
  350 + int ya = round( (pts[p ].y + offsety) * scaley );
  351 + int xb = round( (pts[(p+1)%pt_count].x + offsetx) * scalex );
  352 + int yb = round( (pts[(p+1)%pt_count].y + offsety) * scaley );
  353 +
  354 + //printf( " line (%d,%d) to (%d,%d)\n", xa,ya,xb,yb );
  355 +
  356 + bool steep = abs( yb-ya ) > abs( xb-xa );
  357 + if( steep )
  358 + {
  359 + swap( xa, ya );
  360 + swap( xb, yb );
  361 + }
  362 +
  363 + if( xa > xb )
  364 + {
  365 + swap( xa, xb );
  366 + swap( ya, yb );
  367 + }
  368 +
  369 + int x;
  370 + float dydx = (float) (yb - ya) / (float) (xb - xa);
  371 + float y = ya;
  372 + for (x=xa; x<=xb; x++)
  373 + {
  374 + if( steep )
  375 + {
  376 + if( ! (round(y) >= 0) ) continue;
  377 + if( ! (round(y) < (int)width) ) continue;
  378 + if( ! (x >= 0) ) continue;
  379 + if( ! (x < height) ) continue;
  380 + }
  381 + else
  382 + {
  383 + if( ! (x >= 0) ) continue;
  384 + if( ! (x < (int)width) ) continue;
  385 + if( ! (round(y) >= 0) ) continue;
  386 + if( ! (round(y) < height) ) continue;
  387 + }
  388 +
  389 + if( steep )
  390 + data[ (int)round(y) + (x * width)] = 1;
  391 + else
  392 + data[ x + ((int)round(y) * width)] = 1;
  393 + y = y + dydx;
  394 + }
  395 + }
  396 +}
  397 +
317 398
318 399 void Block::DrawTop()
319 400 {
12 libstage/blockgroup.cc
@@ -305,3 +305,15 @@ void BlockGroup::LoadBitmap( Model* mod, const char* bitmapfile, Worldfile* wf )
305 305
306 306 CalcSize();
307 307 }
  308 +
  309 +
  310 +#include <math.h> /* for round() */
  311 +extern void output (int x, int y); /* forward declaration for user-defined output */
  312 +
  313 +
  314 +
  315 +void BlockGroup::Rasterize( uint8_t* data, unsigned int width, unsigned int height )
  316 +{
  317 + for( GList* it = blocks; it; it=it->next )
  318 + ((Block*)it->data)->Rasterize( data, width, height );
  319 +}
4 libstage/model.cc
@@ -1020,3 +1020,7 @@ void Model::RegisterOption( Option* opt )
1020 1020 }
1021 1021
1022 1022
  1023 +void Model::Rasterize( uint8_t* data, unsigned int width, unsigned int height )
  1024 +{
  1025 + blockgroup.Rasterize( data, width, height );
  1026 +}
19 libstage/stage.hh
@@ -1121,6 +1121,14 @@ namespace Stg
1121 1121
1122 1122 stg_color_t GetColor();
1123 1123
  1124 + void Rasterize( uint8_t* data,
  1125 + unsigned int width, unsigned int height,
  1126 + double scalex, double scaley,
  1127 + double offsetx, double offsety );
  1128 +
  1129 + void Rasterize( uint8_t* data,
  1130 + unsigned int width, unsigned int height );
  1131 +
1124 1132 private:
1125 1133 Model* mod; ///< model to which this block belongs
1126 1134
@@ -1187,9 +1195,8 @@ namespace Stg
1187 1195 void CallDisplayList( Model* mod );
1188 1196 void Clear() ; /** deletes all blocks from the group */
1189 1197
1190   - GList* AppendTouchingModels( GList* list );
1191   - //void AddTouchingModelsToList( GList* list );
1192   -
  1198 + GList* AppendTouchingModels( GList* list );
  1199 +
1193 1200 /** Returns a pointer to the first model detected to be colliding
1194 1201 with a block in this group, or NULL, if none are detected. */
1195 1202 Model* TestCollision();
@@ -1207,6 +1214,8 @@ namespace Stg
1207 1214
1208 1215 void LoadBitmap( Model* mod, const char* bitmapfile, Worldfile *wf );
1209 1216 void LoadBlock( Model* mod, Worldfile* wf, int entity );
  1217 +
  1218 + void Rasterize( uint8_t* data, unsigned int width, unsigned int height );
1210 1219 };
1211 1220
1212 1221
@@ -1722,6 +1731,10 @@ namespace Stg
1722 1731 Visibility vis;
1723 1732
1724 1733 stg_usec_t GetSimInterval(){ return world->interval_sim; }
  1734 +
  1735 + /** Render the model's blocks as an occupancy grid into the
  1736 + preallocated array of width by height pixels */
  1737 + void Rasterize( uint8_t* data, unsigned int width, unsigned int height );
1725 1738
1726 1739 void Lock()
1727 1740 {
2  worlds/fasr.world
@@ -153,7 +153,7 @@ define puck model (
153 153 #puck( pose [ 1.067 3.367 0 0 ] )
154 154 #puck( pose [ 1.412 3.604 0 0 ] )
155 155
156   -autorob( pose [5.488 5.149 0 35.947] joules 300000 )
  156 +autorob( pose [5.488 5.149 0 35.947] joules 300000 name "r0" )
157 157 autorob( pose [6.431 5.593 0 -111.715] joules 100000 )
158 158 autorob( pose [5.615 6.185 0 107.666] joules 200000 )
159 159 autorob( pose [7.028 6.502 0 -128.279] joules 400000 )

0 comments on commit e96a997

Please sign in to comment.
Something went wrong with that request. Please try again.