Skip to content
Browse files

added model rasterization

  • Loading branch information...
1 parent c1cde49 commit e96a997c809706fdc31587208f6fc476ad539e7a rtv committed Apr 4, 2009
Showing with 132 additions and 4 deletions.
  1. +18 −0 examples/ctrl/fasr.cc
  2. +81 −0 libstage/block.cc
  3. +12 −0 libstage/blockgroup.cc
  4. +4 −0 libstage/model.cc
  5. +16 −3 libstage/stage.hh
  6. +1 −1 worlds/fasr.world
View
18 examples/ctrl/fasr.cc
@@ -502,6 +502,24 @@ class Robot
// Stage calls this when the model starts up
extern "C" int Init( Model* mod )
{
+ if( strcmp( mod->Token(), "r0" ) == 0 )
+ {
+ const unsigned int dw = 60, dh = 30;
+ uint8_t* data = new uint8_t[dw*dh*2];
+ memset( data, 0, sizeof(uint8_t) * dw * dh );
+
+ mod->GetWorld()->GetModel( "cave" )->Rasterize( data, dw, dh );
+
+ putchar( '\n' );
+ for( unsigned int y=0; y<dh; y++ )
+ {
+ for( unsigned int x=0; x<dw; x++ )
+ putchar( data[x + ((dh-y-1)*dw)] ? 'O' : '.' );
+ putchar( '\n' );
+ }
+ delete data;
+ }
+
new Robot( (ModelPosition*)mod,
mod->GetWorld()->GetModel( "source" ),
mod->GetWorld()->GetModel( "sink" ) );
View
81 libstage/block.cc
@@ -314,6 +314,87 @@ void Block::GenerateCandidateCells()
mapped = true;
}
+void Block::Rasterize( uint8_t* data, unsigned int width, unsigned int height )
+{
+ Pose pose;// = mod->GetPose();
+
+ // add local offset
+ pose = pose_sum( pose, mod->geom.pose );
+
+ Size bgsize = mod->blockgroup.GetSize();
+
+ double scalex = (width-1) / bgsize.x;
+ double scaley = (height-1) / bgsize.y;
+
+ Rasterize( data, width, height, scalex, scaley, 0,0 );
+}
+
+void swap( int& a, int& b )
+{
+ int tmp = a;
+ a = b;
+ b = tmp;
+}
+
+void Block::Rasterize( uint8_t* data,
+ unsigned int width, unsigned int height,
+ double scalex, double scaley,
+ double offsetx, double offsety )
+{
+ //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 p=0; p<pt_count; p++ )
+ {
+ int xa = round( (pts[p ].x + offsetx) * scalex );
+ int ya = round( (pts[p ].y + offsety) * scaley );
+ int xb = round( (pts[(p+1)%pt_count].x + offsetx) * scalex );
+ int yb = round( (pts[(p+1)%pt_count].y + offsety) * scaley );
+
+ //printf( " line (%d,%d) to (%d,%d)\n", xa,ya,xb,yb );
+
+ bool steep = abs( yb-ya ) > abs( xb-xa );
+ if( steep )
+ {
+ swap( xa, ya );
+ swap( xb, yb );
+ }
+
+ if( xa > xb )
+ {
+ swap( xa, xb );
+ swap( ya, yb );
+ }
+
+ int x;
+ float dydx = (float) (yb - ya) / (float) (xb - xa);
+ float y = ya;
+ for (x=xa; x<=xb; x++)
+ {
+ if( steep )
+ {
+ if( ! (round(y) >= 0) ) continue;
+ if( ! (round(y) < (int)width) ) continue;
+ if( ! (x >= 0) ) continue;
+ if( ! (x < height) ) continue;
+ }
+ else
+ {
+ if( ! (x >= 0) ) continue;
+ if( ! (x < (int)width) ) continue;
+ if( ! (round(y) >= 0) ) continue;
+ if( ! (round(y) < height) ) continue;
+ }
+
+ if( steep )
+ data[ (int)round(y) + (x * width)] = 1;
+ else
+ data[ x + ((int)round(y) * width)] = 1;
+ y = y + dydx;
+ }
+ }
+}
+
void Block::DrawTop()
{
View
12 libstage/blockgroup.cc
@@ -305,3 +305,15 @@ void BlockGroup::LoadBitmap( Model* mod, const char* bitmapfile, Worldfile* wf )
CalcSize();
}
+
+
+#include <math.h> /* for round() */
+extern void output (int x, int y); /* forward declaration for user-defined output */
+
+
+
+void BlockGroup::Rasterize( uint8_t* data, unsigned int width, unsigned int height )
+{
+ for( GList* it = blocks; it; it=it->next )
+ ((Block*)it->data)->Rasterize( data, width, height );
+}
View
4 libstage/model.cc
@@ -1020,3 +1020,7 @@ void Model::RegisterOption( Option* opt )
}
+void Model::Rasterize( uint8_t* data, unsigned int width, unsigned int height )
+{
+ blockgroup.Rasterize( data, width, height );
+}
View
19 libstage/stage.hh
@@ -1121,6 +1121,14 @@ namespace Stg
stg_color_t GetColor();
+ void Rasterize( uint8_t* data,
+ unsigned int width, unsigned int height,
+ double scalex, double scaley,
+ double offsetx, double offsety );
+
+ void Rasterize( uint8_t* data,
+ unsigned int width, unsigned int height );
+
private:
Model* mod; ///< model to which this block belongs
@@ -1187,9 +1195,8 @@ namespace Stg
void CallDisplayList( Model* mod );
void Clear() ; /** deletes all blocks from the group */
- GList* AppendTouchingModels( GList* list );
- //void AddTouchingModelsToList( GList* list );
-
+ GList* AppendTouchingModels( GList* list );
+
/** Returns a pointer to the first model detected to be colliding
with a block in this group, or NULL, if none are detected. */
Model* TestCollision();
@@ -1207,6 +1214,8 @@ namespace Stg
void LoadBitmap( Model* mod, const char* bitmapfile, Worldfile *wf );
void LoadBlock( Model* mod, Worldfile* wf, int entity );
+
+ void Rasterize( uint8_t* data, unsigned int width, unsigned int height );
};
@@ -1722,6 +1731,10 @@ namespace Stg
Visibility vis;
stg_usec_t GetSimInterval(){ return world->interval_sim; }
+
+ /** Render the model's blocks as an occupancy grid into the
+ preallocated array of width by height pixels */
+ void Rasterize( uint8_t* data, unsigned int width, unsigned int height );
void Lock()
{
View
2 worlds/fasr.world
@@ -153,7 +153,7 @@ define puck model (
#puck( pose [ 1.067 3.367 0 0 ] )
#puck( pose [ 1.412 3.604 0 0 ] )
-autorob( pose [5.488 5.149 0 35.947] joules 300000 )
+autorob( pose [5.488 5.149 0 35.947] joules 300000 name "r0" )
autorob( pose [6.431 5.593 0 -111.715] joules 100000 )
autorob( pose [5.615 6.185 0 107.666] joules 200000 )
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.