From: <rt...@us...> - 2009-04-11 00:39:46
|
Revision: 7594 http://playerstage.svn.sourceforge.net/playerstage/?rev=7594&view=rev Author: rtv Date: 2009-04-11 00:39:13 +0000 (Sat, 11 Apr 2009) Log Message: ----------- fixed raserizer scaling bug 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/libstage/worldgui.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-04-11 00:39:13 UTC (rev 7594) @@ -505,10 +505,11 @@ if( strcmp( mod->Token(), "r0" ) == 0 ) { const unsigned int dw = 64, dh = 32; + 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 ); + mod->GetWorld()->GetModel( "cave" )->Rasterize( data, dw, dh, 0.25, 0.5 ); putchar( '\n' ); for( unsigned int y=0; y<dh; y++ ) Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/libstage/block.cc 2009-04-11 00:39:13 UTC (rev 7594) @@ -50,7 +50,6 @@ Load( wf, entity ); } - Block::~Block() { if( mapped ) UnMap(); @@ -72,7 +71,6 @@ mod->blockgroup.BuildDisplayList( mod ); } - double Block::CenterY() { double min = billion; @@ -254,6 +252,25 @@ mapped = true; } +stg_point_t Block::BlockPointToModelMeters( const stg_point_t& bpt ) +{ + Pose gpose = mod->GetGlobalPose(); + gpose = pose_sum( gpose, mod->geom.pose ); // add local offset + + Size bgsize = mod->blockgroup.GetSize(); + stg_point3_t bgoffset = mod->blockgroup.GetOffset(); + + stg_point3_t scale; + scale.x = mod->geom.size.x / bgsize.x; + scale.y = mod->geom.size.y / bgsize.y; + scale.z = mod->geom.size.z / bgsize.z; + + stg_point_t mpt; + mpt.x = (bpt.x - bgoffset.x) * scale.x; + mpt.y = (bpt.y - bgoffset.y) * scale.y; + return mpt; +} + void Block::GenerateCandidateCells() { @@ -313,131 +330,168 @@ mapped = true; } -void Block::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) -{ - // add local offset - // pose = pose_sum( pose, mod->geom.pose ); +// void Block::Rasterize( uint8_t* data, +// unsigned int width, +// unsigned int height, +// stg_meters_t cellwidth, +// stg_meters_t cellheight ) +// { +// // add local offset +// // pose = pose_sum( pose, mod->geom.pose ); - Size bgsize = mod->blockgroup.GetSize(); - - double scalex = (double)(width) / (double)bgsize.x; - double scaley = (double)(height) / (double)bgsize.y; - //double scalex = (width) / bgsize.x; - //double scaley = (height) / bgsize.y; +// Size bgsize = mod->blockgroup.GetSize(); + +// double scalex = (width*cellwidth) / bgsize.x; +// double scaley = (height*cellheight) / bgsize.y; - Rasterize( data, width, height, scalex, scaley, 0,0 ); -} - -// void swap( int& a, int& b ) -// { -// int tmp = a; -// a = b; -// b = tmp; +// Rasterize( data, width, height, scalex, scaley, 0,0 ); // } -void swap( int* a, int* b ) +void swap( int& a, int& b ) { - int foo = *a; - *a = *b; - *b = foo; + 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 ); +// 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 ); - unsigned int W=0; +// for( unsigned int i=0; i<pt_count; i++ ) +// { +// double px = pts[i].x; +// double py = pts[i].y; -// W+=20; -// W /= 2; - -// printf( "W is %u", W ); +// //unsigned int keep_i = i; - for( W=0; W<pt_count; W++ ) - { - double px = pts[W].x; - double py = pts[W].y; +// int xa = floor( (pts[i ].x + offsetx) * scalex ); +// int ya = floor( (pts[i ].y + offsety) * scaley ); +// int xb = floor( (pts[(i+1)%pt_count].x + offsetx) * scalex ); +// int yb = floor( (pts[(i+1)%pt_count].y + offsety) * scaley ); - //unsigned int keep_W = W; +// mod->rastervis.AddPoint( px, py ); - int xa = floor( (pts[W ].x + offsetx) * scalex ); - int ya = floor( (pts[W ].y + offsety) * scaley ); - int xb = floor( (pts[(W+1)%pt_count].x + offsetx) * scalex ); - int yb = floor( (pts[(W+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 ); +// } + +// double dydx = (double) (yb - ya) / (double) (xb - xa); +// double y = ya; +// for(int x=xa; x<=xb; 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; +// } +// } +// } - mod->rastervis.AddPoint( px, py ); +void Block::Rasterize( uint8_t* data, + 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 ); - //int keep_xa = xa; - //int keep_xb = xb; + 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 + int xa = floor( mpt1.x / cellwidth ); + int ya = floor( mpt1.y / cellheight ); + int xb = floor( mpt2.x / cellwidth ); + int yb = floor( mpt2.y / cellheight ); //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 ); + swap( xa, ya ); + swap( xb, yb ); } if( xa > xb ) { - swap( &xa, &xb ); - swap( &ya, &yb ); + swap( xa, xb ); + swap( ya, yb ); } double dydx = (double) (yb - ya) / (double) (xb - xa); double y = ya; for(int x=xa; x<=xb; 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 ) + { + 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; - -// if( (floor(y) == 75) && -// x == 119 ) -// { -// puts( "foo" ); -// // while(1) {} - -// printf( "W: %u keep_W: %u px: %.4f\npy: %.4f\n", -// W, keep_W, -// px, py ); - -// printf( "XA: %.4f\nXB: %.4f\n", -// (pts[W ].x + offsetx) * scalex, -// (pts[(W+1)%pt_count].x + offsetx) * scalex ); - -// printf( "KEEP: %d %d\n", keep_xa, keep_xb ); -// printf( "NOW: %d %d\n", xa, xb ); -// } - } } } - 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-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/libstage/blockgroup.cc 2009-04-11 00:39:13 UTC (rev 7594) @@ -310,8 +310,12 @@ } -void BlockGroup::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) +void BlockGroup::Rasterize( uint8_t* data, + unsigned int width, + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ) { for( GList* it = blocks; it; it=it->next ) - ((Block*)it->data)->Rasterize( data, width, height ); + ((Block*)it->data)->Rasterize( data, width, height, cellwidth, cellheight ); } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/libstage/model.cc 2009-04-11 00:39:13 UTC (rev 7594) @@ -1025,11 +1025,15 @@ } -void Model::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) +void Model::Rasterize( uint8_t* data, + unsigned int width, + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ) { rastervis.ClearPts(); - blockgroup.Rasterize( data, width, height ); - rastervis.SetData( data, width, height ); + blockgroup.Rasterize( data, width, height, cellwidth, cellheight ); + rastervis.SetData( data, width, height, cellwidth, cellheight ); } //*************************************************************** @@ -1040,6 +1044,8 @@ data(NULL), width(0), height(0), + cellwidth(0), + cellheight(0), pts(NULL) { } @@ -1060,8 +1066,8 @@ { glPushMatrix(); Size sz = mod->blockgroup.GetSize(); - glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); - glScalef( mod->geom.size.x / sz.x, mod->geom.size.y / sz.y, 1 ); + //glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); + //glScalef( mod->geom.size.x / sz.x, mod->geom.size.y / sz.y, 1 ); // now we're in world meters coordinates glPointSize( 4 ); @@ -1085,8 +1091,10 @@ // go into bitmap pixel coords glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); - glScalef( mod->geom.size.x / width, mod->geom.size.y / height, 1 ); + //glScalef( mod->geom.size.x / width, mod->geom.size.y / height, 1 ); + glScalef( cellwidth, cellheight, 1 ); + mod->PushColor( 0,0,0,0.5 ); glPolygonMode( GL_FRONT, GL_FILL ); for( unsigned int y=0; y<height; y++ ) @@ -1131,7 +1139,9 @@ void Model::RasterVis::SetData( uint8_t* data, unsigned int width, - unsigned int height ) + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ) { // copy the raster for test visualization if( this->data ) @@ -1142,6 +1152,8 @@ memcpy( this->data, data, len ); this->width = width; this->height = height; + this->cellwidth = cellwidth; + this->cellheight = cellheight; } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/libstage/stage.hh 2009-04-11 00:39:13 UTC (rev 7594) @@ -1128,14 +1128,15 @@ Model* GetModel(){ return mod; }; 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, +// double scalex, double scaley, +// double offsetx, double offsety ); + void Rasterize( uint8_t* data, - unsigned int width, unsigned int height ); + unsigned int width, unsigned int height, + stg_meters_t cellwidth, stg_meters_t cellheight ); private: Model* mod; ///< model to which this block belongs @@ -1169,6 +1170,11 @@ written, and the pointers to the rendered and potential cells are switched for next time (avoiding a memory copy).*/ GPtrArray* candidate_cells; + + + // find the position of a block's internal point in meters + // relative to the model + stg_point_t BlockPointToModelMeters( const stg_point_t& bpt ); }; @@ -1222,15 +1228,16 @@ 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 ); + + void Rasterize( uint8_t* data, + unsigned int width, unsigned int height, + stg_meters_t cellwidth, stg_meters_t cellheight ); }; typedef int ctrlinit_t( Model* mod ); //typedef void ctrlupdate_t( Model* mod ); - - + // BLOCKS class Camera @@ -1642,6 +1649,7 @@ private: uint8_t* data; unsigned int width, height; + stg_meters_t cellwidth, cellheight; GList* pts; public: @@ -1651,7 +1659,9 @@ void SetData( uint8_t* data, unsigned int width, - unsigned int height ); + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ); void AddPoint( stg_meters_t x, stg_meters_t y ); void ClearPts(); @@ -1764,7 +1774,9 @@ /** 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 Rasterize( uint8_t* data, + unsigned int width, unsigned int height, + stg_meters_t cellwidth, stg_meters_t cellheight ); void Lock() { Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/libstage/worldgui.cc 2009-04-11 00:39:13 UTC (rev 7594) @@ -388,7 +388,7 @@ char buf[32]; snprintf( buf, 32, " [%.2f]", localratio ); - str + buf; + str += buf; if( paused == true ) str += " [ PAUSED ]"; Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/worlds/fasr.world 2009-04-11 00:39:13 UTC (rev 7594) @@ -40,8 +40,11 @@ floorplan ( name "cave" - size [16.000 16.000 0.600] pose [0 0 0 0] + #size [1000.000 750.000 0.600] + #bitmap "bitmaps/SFU_medium_1.png" + + size [16 16 0.600] bitmap "bitmaps/cave.png" ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |