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.
------------------------------------------------------------------------------
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
