From: <rt...@us...> - 2009-04-11 05:32:40
|
Revision: 7595 http://playerstage.svn.sourceforge.net/playerstage/?rev=7595&view=rev Author: rtv Date: 2009-04-11 05:32:07 +0000 (Sat, 11 Apr 2009) Log Message: ----------- cleaning up code Modified Paths: -------------- 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/world.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-04-11 00:39:13 UTC (rev 7594) +++ code/stage/trunk/libstage/block.cc 2009-04-11 05:32:07 UTC (rev 7595) @@ -8,14 +8,15 @@ blocks. The point data is copied, so pts can safely be freed after calling this.*/ Block::Block( Model* mod, - stg_point_t* pts, - size_t pt_count, - stg_meters_t zmin, - stg_meters_t zmax, - stg_color_t color, - bool inherit_color - ) : + stg_point_t* pts, + size_t pt_count, + stg_meters_t zmin, + stg_meters_t zmax, + stg_color_t color, + bool inherit_color + ) : mod( mod ), + mpts(NULL), pt_count( pt_count ), pts( (stg_point_t*)g_memdup( pts, pt_count * sizeof(stg_point_t)) ), color( color ), @@ -36,6 +37,7 @@ Worldfile* wf, int entity) : mod( mod ), + mpts(NULL), pt_count(0), pts(NULL), color(0), @@ -252,101 +254,69 @@ mapped = true; } -stg_point_t Block::BlockPointToModelMeters( const stg_point_t& bpt ) +inline 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; + return stg_point_t( (bpt.x - bgoffset.x) * (mod->geom.size.x/bgsize.x), + (bpt.y - bgoffset.y) * (mod->geom.size.y/bgsize.y)); +} - stg_point_t mpt; - mpt.x = (bpt.x - bgoffset.x) * scale.x; - mpt.y = (bpt.y - bgoffset.y) * scale.y; - return mpt; +stg_point_t* Block::GetPointsInModelCoords() +{ + if( ! mpts ) + { + // no valid cache of model coord points, so generate them + mpts = new stg_point_t[pt_count]; + + for( unsigned int i=0; i<pt_count; i++ ) + mpts[i] = BlockPointToModelMeters( pts[i] ); + } + + return mpts; } -void Block::GenerateCandidateCells() +void Block::InvalidateModelPointCache() +{ + // this doesn't happen often, so this simple strategy isn't too wasteful + if( mpts ) + { + delete[] mpts; + mpts = NULL; + } +} +void Block::GenerateCandidateCells() { - Pose gpose = mod->GetGlobalPose(); + stg_point_t* mpts = GetPointsInModelCoords(); + + // convert the mpts in model coords into global coords + stg_point_t* gpts = new stg_point_t[pt_count]; + for( unsigned int i=0; i<pt_count; i++ ) + gpts[i] = mod->LocalToGlobal( mpts[i] ); - // add local offset - gpose = pose_sum( gpose, mod->geom.pose ); + g_ptr_array_set_size( candidate_cells, 0 ); - Size bgsize = mod->blockgroup.GetSize(); - stg_point3_t bgoffset = mod->blockgroup.GetOffset(); + mod->world-> + ForEachCellInPolygon( gpts, pt_count, + (stg_cell_callback_t)AppendCellToPtrArray, + candidate_cells ); + delete[] gpts; - 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; - - - g_ptr_array_set_size( candidate_cells, 0 ); + // set global Z + Pose gpose = mod->GetGlobalPose(); + gpose.z += mod->geom.pose.z; + double scalez = mod->geom.size.z / mod->blockgroup.GetSize().z; + stg_meters_t z = gpose.z - mod->blockgroup.GetOffset().z; - // compute the global location of the first point - Pose local( (pts[0].x - bgoffset.x) * scale.x , - (pts[0].y - bgoffset.y) * scale.y, - -bgoffset.z, - 0 ); - - Pose first_gpose, last_gpose; - first_gpose = last_gpose = pose_sum( gpose, local ); - // store the block's absolute z bounds at this rendering - global_z.min = (scale.z * local_z.min) + last_gpose.z; - global_z.max = (scale.z * local_z.max) + last_gpose.z; + global_z.min = (scalez * local_z.min) + z; + global_z.max = (scalez * local_z.max) + z; - // now loop from the the second to the last - for( unsigned int p=1; p<pt_count; p++ ) - { - Pose local( (pts[p].x - bgoffset.x) * scale.x , - (pts[p].y - bgoffset.y) * scale.y, - -bgoffset.z, - 0 ); - - Pose gpose2 = pose_sum( gpose, local ); - - // and render the shape of the block into the global cells - mod->world->ForEachCellInLine( last_gpose.x, last_gpose.y, - gpose2.x, gpose2.y, - (stg_cell_callback_t)AppendCellToPtrArray, - candidate_cells ); - last_gpose = gpose2; - } - - // close the polygon - mod->world->ForEachCellInLine( last_gpose.x, last_gpose.y, - first_gpose.x, first_gpose.y, - (stg_cell_callback_t)AppendCellToPtrArray, - candidate_cells ); - mapped = true; } -// 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 = (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; @@ -354,71 +324,6 @@ 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 i=0; i<pt_count; i++ ) -// { -// double px = pts[i].x; -// double py = pts[i].y; - -// //unsigned int keep_i = i; - -// 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 ); - -// mod->rastervis.AddPoint( px, py ); - -// //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; -// } -// } -// } - void Block::Rasterize( uint8_t* data, unsigned int width, unsigned int height, Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-04-11 00:39:13 UTC (rev 7594) +++ code/stage/trunk/libstage/blockgroup.cc 2009-04-11 05:32:07 UTC (rev 7595) @@ -102,6 +102,8 @@ offset.x = minx + size.x/2.0; offset.y = miny + size.y/2.0; offset.z = 0; // todo? + + InvalidateModelPointCache(); } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-04-11 00:39:13 UTC (rev 7594) +++ code/stage/trunk/libstage/model.cc 2009-04-11 05:32:07 UTC (rev 7595) @@ -561,6 +561,12 @@ return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); } +stg_point_t Model::LocalToGlobal( const stg_point_t& pt) const +{ + Pose gpose = LocalToGlobal( Pose( pt.x, pt.y, 0, 0 ) ); + return stg_point_t( gpose.x, gpose.y ); +} + void Model::MapWithChildren() { UnMap(); @@ -1159,10 +1165,7 @@ void Model::RasterVis::AddPoint( stg_meters_t x, stg_meters_t y ) { - stg_point_t* pt = new stg_point_t; - pt->x = x; - pt->y = y; - pts = g_list_prepend( pts, pt ); + pts = g_list_prepend( pts, new stg_point_t( x, y ) ); } void Model::RasterVis::ClearPts() Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-11 00:39:13 UTC (rev 7594) +++ code/stage/trunk/libstage/stage.hh 2009-04-11 05:32:07 UTC (rev 7595) @@ -387,10 +387,21 @@ } stg_fov_t; /** Define a point on a 2d plane */ - typedef struct + class stg_point_t { + public: stg_meters_t x, y; - } stg_point_t; + + // init + stg_point_t( stg_meters_t x, stg_meters_t y ) + : x(x), y(y){} + + // init + stg_point_t() : x(0), y(0){} + + // copy + stg_point_t( const stg_point_t& pt) : x(pt.x), y(pt.y){} + }; /** Define a point in 3d space */ typedef struct @@ -919,19 +930,14 @@ inline Cell* GetCell( const int32_t x, const int32_t y ); void ForEachCellInPolygon( const stg_point_t pts[], - const uint32_t pt_count, + const unsigned int pt_count, stg_cell_callback_t cb, void* cb_arg ); - void ForEachCellInLine( const stg_point_t pt1, - const stg_point_t pt2, + void ForEachCellInLine( const stg_point_t& pt1, + const stg_point_t& pt2, stg_cell_callback_t cb, void* cb_arg ); - - void ForEachCellInLine( stg_meters_t x1, stg_meters_t y1, - stg_meters_t x2, stg_meters_t y2, - stg_cell_callback_t cb, - void* cb_arg ); /** convert a distance in meters to a distance in world occupancy grid pixels */ @@ -1141,6 +1147,7 @@ private: Model* mod; ///< model to which this block belongs + stg_point_t* mpts; ///< cache of this->pts in model coordindates size_t pt_count; ///< the number of points stg_point_t* pts; ///< points defining a polygon @@ -1175,6 +1182,12 @@ // find the position of a block's internal point in meters // relative to the model stg_point_t BlockPointToModelMeters( const stg_point_t& bpt ); + + /** Update the cache of block points converted to model coordinates */ + stg_point_t* GetPointsInModelCoords(); + + /** invalidate the cache of points in model coordinates */ + void InvalidateModelPointCache(); }; @@ -1232,6 +1245,9 @@ void Rasterize( uint8_t* data, unsigned int width, unsigned int height, stg_meters_t cellwidth, stg_meters_t cellheight ); + + void InvalidateModelPointCache() + { LISTMETHOD( blocks, Block*, InvalidateModelPointCache ); } }; @@ -2258,10 +2274,14 @@ pose specified in the model's local coordinate system */ Pose LocalToGlobal( const Pose& pose ) const; - /** Return the 3d point in world coordinates of a 3d point +// /** Return the 3d point in world coordinates of a 3d point +// specified in the model's local coordinate system */ +// stg_point3_t LocalToGlobal( const stg_point3_t local ) const; + + /** Return the 2d point in world coordinates of a 2d point specified in the model's local coordinate system */ - stg_point3_t LocalToGlobal( const stg_point3_t local ) const; - + stg_point_t LocalToGlobal( const stg_point_t& pt) const; + /** returns the first descendent of this model that is unsubscribed and has the type indicated by the string */ Model* GetUnsubscribedModelOfType( const stg_model_type_t type ) const; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-04-11 00:39:13 UTC (rev 7594) +++ code/stage/trunk/libstage/world.cc 2009-04-11 05:32:07 UTC (rev 7595) @@ -914,16 +914,26 @@ ->GetCell( CELL(x), CELL(y) )) ; } -void World::ForEachCellInLine( stg_meters_t x1, stg_meters_t y1, - stg_meters_t x2, stg_meters_t y2, - stg_cell_callback_t cb, - void* cb_arg ) +void World::ForEachCellInPolygon( const stg_point_t pts[], + const unsigned int pt_count, + stg_cell_callback_t cb, + void* cb_arg ) +{ + for( unsigned int i=0; i<pt_count; i++ ) + ForEachCellInLine( pts[i], pts[(i+1)%pt_count], cb, cb_arg ); + +} + +void World::ForEachCellInLine( const stg_point_t& pt1, + const stg_point_t& pt2, + stg_cell_callback_t cb, + void* cb_arg ) { - int32_t x = MetersToPixels( x1 ); // global pixel coords - int32_t y = MetersToPixels( y1 ); + int32_t x = MetersToPixels( pt1.x ); // global pixel coords + int32_t y = MetersToPixels( pt1.y ); - int32_t dx = MetersToPixels( x2 - x1 ); - int32_t dy = MetersToPixels( y2 - y1 ); + int32_t dx = MetersToPixels( pt2.x - pt1.x ); + int32_t dy = MetersToPixels( pt2.y - pt1.y ); // line rasterization adapted from Cohen's 3D version in // Graphics Gems II. Should be very fast. This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |