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.
------------------------------------------------------------------------------
This SF.net email is sponsored by:
High Quality Requirements in a Collaborative Environment.
Download a free trial of Rational Requirements Composer Now!
http://p.sf.net/sfu/www-ibm-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
