Revision: 7576
http://playerstage.svn.sourceforge.net/playerstage/?rev=7576&view=rev
Author: rtv
Date: 2009-04-04 00:17:35 +0000 (Sat, 04 Apr 2009)
Log Message:
-----------
added model rasterization
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/libstage/block.cc
code/stage/trunk/libstage/blockgroup.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/worlds/fasr.world
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2009-04-03 19:03:02 UTC (rev
7575)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2009-04-04 00:17:35 UTC (rev
7576)
@@ -502,6 +502,24 @@
// 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" ) );
Modified: code/stage/trunk/libstage/block.cc
===================================================================
--- code/stage/trunk/libstage/block.cc 2009-04-03 19:03:02 UTC (rev 7575)
+++ code/stage/trunk/libstage/block.cc 2009-04-04 00:17:35 UTC (rev 7576)
@@ -314,7 +314,88 @@
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()
{
// draw the top of the block - a polygon at the highest vertical
Modified: code/stage/trunk/libstage/blockgroup.cc
===================================================================
--- code/stage/trunk/libstage/blockgroup.cc 2009-04-03 19:03:02 UTC (rev
7575)
+++ code/stage/trunk/libstage/blockgroup.cc 2009-04-04 00:17:35 UTC (rev
7576)
@@ -305,3 +305,15 @@
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 );
+}
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-04-03 19:03:02 UTC (rev 7575)
+++ code/stage/trunk/libstage/model.cc 2009-04-04 00:17:35 UTC (rev 7576)
@@ -1020,3 +1020,7 @@
}
+void Model::Rasterize( uint8_t* data, unsigned int width, unsigned int height )
+{
+ blockgroup.Rasterize( data, width, height );
+}
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-04-03 19:03:02 UTC (rev 7575)
+++ code/stage/trunk/libstage/stage.hh 2009-04-04 00:17:35 UTC (rev 7576)
@@ -1121,6 +1121,14 @@
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 @@
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 @@
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 @@
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()
{
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2009-04-03 19:03:02 UTC (rev 7575)
+++ code/stage/trunk/worlds/fasr.world 2009-04-04 00:17:35 UTC (rev 7576)
@@ -153,7 +153,7 @@
#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 )
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
