Skip to content
Browse files

fixed raserizer scaling bug

  • Loading branch information...
1 parent 3290093 commit 750e0489346eded82e67e6b36301b87b525edec2 rtv committed
Showing with 196 additions and 110 deletions.
  1. +2 −1 examples/ctrl/fasr.cc
  2. +140 −86 libstage/block.cc
  3. +6 −2 libstage/blockgroup.cc
  4. +19 −7 libstage/model.cc
  5. +24 −12 libstage/stage.hh
  6. +1 −1 libstage/worldgui.cc
  7. +4 −1 worlds/fasr.world
View
3 examples/ctrl/fasr.cc
@@ -505,10 +505,11 @@ extern "C" int Init( Model* mod )
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++ )
View
226 libstage/block.cc
@@ -50,7 +50,6 @@ Block::Block( Model* mod,
Load( wf, entity );
}
-
Block::~Block()
{
if( mapped ) UnMap();
@@ -72,7 +71,6 @@ void Block::Translate( double x, double y )
mod->blockgroup.BuildDisplayList( mod );
}
-
double Block::CenterY()
{
double min = billion;
@@ -254,6 +252,25 @@ void Block::SwitchToTestedCells()
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 @@ void Block::GenerateCandidateCells()
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
View
8 libstage/blockgroup.cc
@@ -310,8 +310,12 @@ void BlockGroup::LoadBitmap( Model* mod, const char* bitmapfile, Worldfile* wf )
}
-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 );
}
View
26 libstage/model.cc
@@ -1025,11 +1025,15 @@ void Model::RegisterOption( Option* opt )
}
-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 @@ Model::RasterVis::RasterVis()
data(NULL),
width(0),
height(0),
+ cellwidth(0),
+ cellheight(0),
pts(NULL)
{
}
@@ -1060,8 +1066,8 @@ void Model::RasterVis::Visualize( Model* mod, Camera* cam )
{
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,7 +1091,9 @@ void Model::RasterVis::Visualize( Model* mod, Camera* cam )
// 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 );
@@ -1131,7 +1139,9 @@ void Model::RasterVis::Visualize( Model* mod, Camera* cam )
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 @@ void Model::RasterVis::SetData( uint8_t* data,
memcpy( this->data, data, len );
this->width = width;
this->height = height;
+ this->cellwidth = cellwidth;
+ this->cellheight = cellheight;
}
View
36 libstage/stage.hh
@@ -1128,14 +1128,15 @@ namespace Stg
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 @@ namespace Stg
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 @@ 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 );
+
+ 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 @@ namespace Stg
private:
uint8_t* data;
unsigned int width, height;
+ stg_meters_t cellwidth, cellheight;
GList* pts;
public:
@@ -1651,7 +1659,9 @@ namespace Stg
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 @@ namespace Stg
/** 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()
{
View
2 libstage/worldgui.cc
@@ -388,7 +388,7 @@ std::string WorldGui::ClockString()
char buf[32];
snprintf( buf, 32, " [%.2f]", localratio );
- str + buf;
+ str += buf;
if( paused == true )
str += " [ PAUSED ]";
View
5 worlds/fasr.world
@@ -40,8 +40,11 @@ window
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"
)

0 comments on commit 750e048

Please sign in to comment.
Something went wrong with that request. Please try again.