Update of /cvsroot/playerstage/code/stage/libstage
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv13048
Modified Files:
Tag: opengl
Makefile.manual block.cc model.cc stage.c stage.hh world.cc
worldgtk.cc
Added Files:
Tag: opengl
blockgrid.cc
Removed Files:
Tag: opengl
cell.cc
Log Message:
replaced kd tree with good old fashioned bitmap. very fast but uses lots of
RAM. plan to use high-order bit hashing trick to implement nested grids for
best of both worlds. code is MUCH simpler than before, so no known bugs
Index: stage.c
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/stage.c,v
retrieving revision 1.1.2.3
retrieving revision 1.1.2.4
diff -C2 -d -r1.1.2.3 -r1.1.2.4
*** stage.c 10 Oct 2007 01:08:19 -0000 1.1.2.3
--- stage.c 30 Oct 2007 01:01:48 -0000 1.1.2.4
***************
*** 39,60 ****
}
- /* const char* stg_model_type_string( stg_model_type_t type ) */
- /* { */
- /* switch( type ) */
- /* { */
- /* case STG_MODEL_BASIC: return "model"; */
- /* case STG_MODEL_LASER: return "laser"; */
- /* case STG_MODEL_POSITION: return "position"; */
- /* case STG_MODEL_BLOB: return "blobfinder"; */
- /* case STG_MODEL_FIDUCIAL: return "fiducial"; */
- /* case STG_MODEL_RANGER: return "ranger"; */
- /* //case STG_MODEL_TEST: return "test"; */
- /* case STG_MODEL_GRIPPER: return "gripper"; */
- /* default: */
- /* break; */
- /* } */
- /* return "<unknown type>"; */
- /* } */
-
void stg_print_err( const char* err )
{
--- 39,42 ----
***************
*** 64,67 ****
--- 46,96 ----
+ /** Visit every voxel along a vector from (x,y,z) to (x+dx, y+dy, z+dz).
+ Call the function for every voxel, passing in the current voxel
+ coordinates followed by the two arguments. Adapted from Graphics
+ Gems IV, algorithm by Cohen & Kaufman 1991 */
+ int stg_line_3d( int32_t x, int32_t y, int32_t z,
+ int32_t dx, int32_t dy, int32_t dz,
+ stg_line3d_func_t visit_voxel,
+ void* arg )
+ {
+ int n, sx, sy, sz, exy, exz, ezy, ax, ay, az, bx, by, bz;
+
+ sx = SGN(dx); sy = SGN(dy); sz = SGN(dz);
+ ax = abs(dx); ay = abs(dy); az = abs(dz);
+ bx = 2*ax; by = 2*ay; bz = 2*az;
+ exy = ay-ax; exz = az-ax; ezy = ay-az;
+ n = ax+ay+az;
+ while ( n-- ) {
+ if((*visit_voxel)( x, y, z, arg ) )
+ {
+ return TRUE; // hit something!
+ }
+
+ if ( exy < 0 ) {
+ if ( exz < 0 ) {
+ x += sx;
+ exy += by; exz += bz;
+ }
+ else {
+ z += sz;
+ exz -= bx; ezy += by;
+ }
+ }
+ else {
+ if ( ezy < 0 ) {
+ z += sz;
+ exz -= bx; ezy += by;
+ }
+ else {
+ y += sy;
+ exy -= bx; ezy -= bz;
+ }
+ }
+ }
+
+ return FALSE; // hit nothing (leave vars not set)
+ }
+
void stg_print_geom( stg_geom_t* geom )
{
Index: model.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/model.cc,v
retrieving revision 1.1.2.7
retrieving revision 1.1.2.8
diff -C2 -d -r1.1.2.7 -r1.1.2.8
*** model.cc 22 Oct 2007 09:06:27 -0000 1.1.2.7
--- model.cc 30 Oct 2007 01:01:48 -0000 1.1.2.8
***************
*** 308,312 ****
gpose.a += angle;
! return world->Raytrace( this,
&gpose,
range,
--- 308,312 ----
gpose.a += angle;
! return world->Raytrace2( this,
&gpose,
range,
***************
*** 327,331 ****
LocalToGlobal( &gpose );
! return world->Raytrace( this,
&gpose,
range,
--- 327,331 ----
LocalToGlobal( &gpose );
! return world->Raytrace2( this,
&gpose,
range,
***************
*** 584,589 ****
--- 584,600 ----
//PRINT_DEBUG1( "%s.Map()", token );
+ if( this->debug )
+ {
+ double scale = 1.0 / world->ppm;
+ glPushMatrix();
+ glTranslatef( 0,0,1 );
+ glScalef( scale,scale,scale );
+ }
+
for( GList* it=blocks; it; it=it->next )
((StgBlock*)it->data)->Map();
+
+ if( this->debug )
+ glPopMatrix();
}
Index: block.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/block.cc,v
retrieving revision 1.1.2.3
retrieving revision 1.1.2.4
diff -C2 -d -r1.1.2.3 -r1.1.2.4
*** block.cc 13 Oct 2007 07:42:54 -0000 1.1.2.3
--- block.cc 30 Oct 2007 01:01:48 -0000 1.1.2.4
***************
*** 20,29 ****
this->pt_count = pt_count;
this->pts = (stg_point_t*)g_memdup( pts, pt_count * sizeof(stg_point_t));
! this->pts_global = (stg_point_t*)g_memdup( pts, pt_count *
sizeof(stg_point_t));
this->zmin = zmin;
this->zmax = zmax;
this->color = color;
this->inherit_color = inherit_color;
! this->rendered_cells = NULL;
}
--- 20,31 ----
this->pt_count = pt_count;
this->pts = (stg_point_t*)g_memdup( pts, pt_count * sizeof(stg_point_t));
! //this->pts_global = (stg_point_t*)g_memdup( pts, pt_count *
sizeof(stg_point_t));
! // allocate space for the integer version of the block vertices
! this->pts_global = new stg_point_int_t[pt_count];
this->zmin = zmin;
this->zmax = zmax;
this->color = color;
this->inherit_color = inherit_color;
! this->rendered_points = g_array_new( FALSE, FALSE, sizeof(stg_point_int_t));
}
***************
*** 32,35 ****
--- 34,38 ----
this->UnMap();
g_free( pts );
+ g_array_free( rendered_points, TRUE );
}
***************
*** 113,116 ****
--- 116,121 ----
(int)pt_count );
+ double ppm = mod->world->ppm;
+
// update the global coordinate list
stg_pose_t gpose;
***************
*** 119,134 ****
gpose.x = pts[p].x;
gpose.y = pts[p].y;
gpose.a = 0.0;
mod->LocalToGlobal( &gpose );
!
! pts_global[p].x = gpose.x;
! pts_global[p].y = gpose.y;
!
PRINT_DEBUG2("loc [%.2f %.2f]",
pts[p].x,
pts[p].y );
! PRINT_DEBUG2("glb [%.2f %.2f]",
pts_global[p].x,
pts_global[p].y );
--- 124,141 ----
gpose.x = pts[p].x;
gpose.y = pts[p].y;
+ //gpose.z = pts[p].z;
gpose.a = 0.0;
mod->LocalToGlobal( &gpose );
!
! pts_global[p].x = (int32_t)floor((gpose.x+mod->world->width/2.0)*ppm);
! pts_global[p].y = (int32_t)floor((gpose.y+mod->world->height/2.0)*ppm);
! //pts_global[p].z = gpose.z;
!
PRINT_DEBUG2("loc [%.2f %.2f]",
pts[p].x,
pts[p].y );
! PRINT_DEBUG2("glb [%d %d]",
pts_global[p].x,
pts_global[p].y );
***************
*** 144,156 ****
(int)pt_count);
! // remove this block from each cell in the list
! for( GList* it = rendered_cells; it; it = it->next )
! //((StgCell*)it->data)->RemoveBlock( this );
! StgCell::RemoveBlock( ((StgCell*)it->data), this );
!
! g_list_free( rendered_cells );
! rendered_cells = NULL;
}
!
void stg_block_list_scale( GList* blocks,
stg_size_t* size )
--- 151,177 ----
(int)pt_count);
! for( unsigned int p=0; p<rendered_points->len; p++ )
! {
! stg_point_int_t* pt =
! &g_array_index( rendered_points, stg_point_int_t, p);
!
! mod->world->bgrid->RemoveBlock( pt->x, pt->y, this );
!
! }
!
! // forget the points we have unrendered (but keep their storage)
! g_array_set_size( rendered_points,0 );
}
!
! void StgBlock::RecordRenderPoint( uint32_t x, uint32_t y )
! {
! // store this index in the block for later fast deletion
! stg_point_int_t pt;
! pt.x = x;
! pt.y = y;
! g_array_append_val( rendered_points, pt );
! }
!
!
void stg_block_list_scale( GList* blocks,
stg_size_t* size )
***************
*** 223,244 ****
}
- void StgBlock::AddCell( StgCell* cell )
- {
- // printf( "adding cell [%.2f %.2f %d]\n",
- // cell->x,
- // cell->y,
- // (int)g_slist_length( cell->list ) );
-
- rendered_cells = g_list_prepend( rendered_cells, cell );
-
- // for( GList* it = rendered_cells; it; it=it->next )
- // {
- // StgCell* c = (StgCell*)it->data;
- // printf( "[%.2f %.2f %d] ",
- // c->x,
- // c->y,
- // (int)g_slist_length( c->list ) );
- // }
-
- // printf( "total %d cells\n", (int)g_list_length( rendered_cells ));
- }
--- 244,245 ----
Index: world.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/world.cc,v
retrieving revision 1.1.2.8
retrieving revision 1.1.2.9
diff -C2 -d -r1.1.2.8 -r1.1.2.9
*** world.cc 22 Oct 2007 19:16:32 -0000 1.1.2.8
--- world.cc 30 Oct 2007 01:01:48 -0000 1.1.2.9
***************
*** 157,165 ****
this->height = height;
this->ppm = ppm; // this is the finest resolution of the matrix
!
! int32_t dx = (int32_t)(width/2.0 * ppm);
! int32_t dy = (int32_t)(height/2.0 * ppm);
! this->root = new StgCell( NULL, -dx, +dx, -dy, +dy );
!
this->total_subs = 0;
this->paused = false;
--- 157,164 ----
this->height = height;
this->ppm = ppm; // this is the finest resolution of the matrix
!
! this->bgrid = new StgBlockGrid( (uint32_t)(width * ppm),
! (uint32_t)(height * ppm) );
!
this->total_subs = 0;
this->paused = false;
***************
*** 171,177 ****
PRINT_DEBUG2( "destroying world %d %s", id, token );
- delete root;
delete wf;
!
g_hash_table_destroy( models_by_id );
g_hash_table_destroy( models_by_name );
--- 170,176 ----
PRINT_DEBUG2( "destroying world %d %s", id, token );
delete wf;
! delete bgrid;
!
g_hash_table_destroy( models_by_id );
g_hash_table_destroy( models_by_name );
***************
*** 248,257 ****
wf->ReadTupleFloat( entity, "size", 1, this->height );
- if( this->root )
- delete root;
! int32_t dx = (int32_t)(width/2.0 * ppm);
! int32_t dy = (int32_t)(height/2.0 * ppm);
! this->root = new StgCell( NULL, -dx, +dx, -dy, +dy );
//_stg_disable_gui = wf->ReadInt( entity, "gui_disable", _stg_disable_gui );
--- 247,257 ----
wf->ReadTupleFloat( entity, "size", 1, this->height );
! if( this->bgrid )
! delete bgrid;
!
! int32_t vwidth = (int32_t)(width * ppm);
! int32_t vheight = (int32_t)(height * ppm);
! this->bgrid = new StgBlockGrid( vwidth, vheight );
//_stg_disable_gui = wf->ReadInt( entity, "gui_disable", _stg_disable_gui );
***************
*** 436,699 ****
}
- // itl->a = atan2( b-y, a-x );
- //itl->max_range = hypot( a-x, b-y );
-
!
! // StgModel* StgWorld::TestCandidateList( StgModel* finder,
! // stg_itl_test_func_t func,
! // stg_meters_t z,
! // GSList* list )
// {
! // assert(list);
!
! // for( GSList* it = list; it; it=it->next )
! // {
! // StgBlock* block = (StgBlock*)it->data;
! // StgModel* candidate = block->mod;
! // assert( candidate );
!
! // //printf( "block of %s\n", candidate->Token() );
!
! // stg_pose_t gpose;
! // candidate->GetGlobalPose( &gpose );
!
! // // test to see if the block exists at height z
! // double block_min = gpose.z + block->zmin;
! // double block_max = gpose.z + block->zmax;
!
! // //printf( "[%.2f %.2f %.2f] %s testing %s (laser return %d)\n",
! // // x,y,z,
! // // finder->Token(), candidate->Token(),
candidate->LaserReturn() );
!
!
! // if( LT(block_min,z) &&
! // GT(block_max,z) &&
! // (*func)( finder, candidate ) )
! // return candidate;
! // }
!
! // return NULL;
// }
! stg_meters_t StgWorld::Raytrace( StgModel* finder,
! stg_pose_t* pose,
! stg_meters_t max_range,
! stg_block_match_func_t func,
! const void* arg,
! StgModel** hit_model )
{
! const double epsilon = 0.0001;
!
! double x = pose->x;
! double y = pose->y;
! double z = pose->z;
! double a = NORMALIZE(pose->a);
!
! double tana = tan( a );
!
! StgCell* cell = root;
! double range = 0.0;
! StgModel* hit = NULL;
! double p2m = 1.0/ppm;
! while( LT(range,max_range) )
{
!
! int xpix = (int)floor(x*ppm);
! int ypix = (int)floor(y*ppm);
!
! //printf( "theta %.2f locating cell at (%.2f %.2f) [%d,%d]\n",a, x, y,
xpix, ypix );
!
! // locate the leaf cell at X,Y
! cell = cell->Locate( xpix, ypix );
!
! //if( cell ) cell->Print( "Found" );
!
! // the cell is null iff the point was outside the root
! if( cell == NULL )
! {
! //printf( "ray at angle %.2f out of bounds at [%.2f, %.2f]",
! // a, x, y );
!
! break;
! }
!
! // printf( "found cell (%.2f,%.2f) (%.2f,%.2f)\n",
! // cell->xmin * p2m,
! // cell->xmax * p2m,
! // cell->ymin * p2m,
! // cell->ymax * p2m );
!
!
! if( finder->debug )
! {
! glPushMatrix();
! glTranslatef( 0,0,1.2 );
! glScalef( 1.0/ppm, 1.0/ppm, 0 );
! PushColor( 1,0,1, 1 );
! glPolygonMode( GL_FRONT, GL_LINE );
! cell->Draw();
! PopColor();
! glPopMatrix();
! }
!
! StgBlock* hitblock = cell->FindBlock( func, arg );
!
! if( hitblock )
! {
! hit = hitblock->mod;
! break;
! }
!
! double c = y - tana * x; // line offset
!
! double xleave = x;
! double yleave = y;
!
! if( GT(a,0) ) // up
! {
! //puts( "up" );
!
!
! int yleave_pixel = cell->ymax; // top edge
!
! // ray could leave through the top edge
! // solve x for known y
! yleave = (double)yleave_pixel * p2m;
! xleave = (yleave - c) / tana;
!
! int xleave_pixel = floor( xleave * ppm );
!
! if( xleave_pixel < cell->xmin ||
! xleave_pixel >= cell->xmax )
! {
! // it must have left the cell on the left or right instead
! // solve y for known x
!
! if( GT(a,M_PI/2.0) ) // left side
! {
! xleave = (cell->xmin*p2m)-epsilon;
! }
! else // right side
! {
! xleave = cell->xmax*p2m +epsilon;
! }
!
! yleave = tana * xleave + c;
! }
! }
! else
! {
! //puts( "down" );
!
! int yleave_pixel = cell->ymin; // bottom edge
!
! // ray could leave through the bottom edge
! // solve x for known y
! yleave = (double)yleave_pixel * p2m;
! xleave = (yleave - c) / tana;
!
! int xleave_pixel = floor( xleave * ppm );
!
!
! // printf( "test leave (%.2f %.2f) %d [%d %d] %d %d %d\n",
! // xleave,
! // yleave,
! // xleave_pixel,
! // cell->xmin,
! // cell->xmax,
! // xleave_pixel < cell->xmin,
! // xleave_pixel >= cell->xmax,
! // xleave_pixel < cell->xmin ||
! // xleave_pixel >= cell->xmax
! // );
!
! // if the edge crossing was not in cell bounds
! //if( !( GTE(xleave,cell->xmin*p2m) && LT(xleave,cell->xmax*p2m)))
! if( xleave_pixel < cell->xmin ||
! xleave_pixel >= cell->xmax )
! {
! // puts( "L or R" );
!
! // it must have left the cell on the left or right instead
! // solve y for known x
! if( LT(a,-M_PI/2.0) ) // left side
! {
! //puts( "L" );
! xleave = (cell->xmin*p2m)-epsilon;
! }
! else
! {
! //puts( "R" );
! xleave = cell->xmax*p2m+epsilon;
! }
!
! yleave = tana * xleave + c;
! }
! else
! {
! yleave-=epsilon;
! //printf( "left through bottom edge\n" );
! }
! }
!
! //cell->Print( NULL );
! //printf( "leave at (%.2f %.2f)\n", hypot( yleave - y, xleave - x ),
range );
! //printf( "leave at (%.2f %.2f)\n", xleave, yleave );
!
! // jump to the leave point
! range += hypot( yleave - y, xleave - x );
!
! if( finder->debug )
! {
! PushColor( 0,0,0, 0.2 );
! glBegin( GL_LINES );
! glVertex3f( x,y, z );
! glVertex3f( xleave, yleave, z );
! glEnd();
! PopColor();
! glPointSize( 4 );
! PushColor( 1,0,0, 1 );
! glBegin( GL_POINTS );
! glVertex3f( x,y, z );
! glEnd();
- PushColor( 0,1,0, 1 );
- glBegin( GL_POINTS );
- glVertex3f( xleave, yleave, z );
- glEnd();
- PopColor();
! glPointSize( 1 );
! }
!
! x = xleave;
! y = yleave;
! }
!
! // if( hit )
! // printf( "HIT %s at [%.2f %.2f %.2f] range %.2f\n",
! // hit->Token(),
! // x,y,z, range );
! // else
! // printf( "no hit at range %.2f\n", range );
!
! // if the caller supplied a place to store the hit model pointer,
! // give it to 'em.
! if( hit_model )
! *hit_model = hit;
!
! // return the range
! return MIN( range, max_range );
}
-
void stg_model_save_cb( gpointer key, gpointer data, gpointer user )
{
--- 436,535 ----
}
! // int raytest( int x, int y, int z,
! // void* arg1, void* arg2 )
// {
! // glRecti( x,y, x+1,y+1 );
! // return FALSE;
// }
! typedef struct
{
! StgWorld* world;
! StgModel* finder;
! StgModel* hit;
! int32_t x, y, z; // leave point
! stg_block_match_func_t func;
! const void* arg;
! } _raytrace_info_t;
! int raytest( int32_t x, int32_t y, int32_t z,
! _raytrace_info_t* rti )
! {
! //glRecti( x,y, x+1,y+1 );
! // for each block recorded at his location
! for( GSList* list = rti->world->bgrid->GetList( x, y );
! list;
! list = list->next )
! {
! StgBlock* block = (StgBlock*)list->data;
! assert( block );
! // // if this block does not belong to the searching model and
! // // matches the predicate
! if( block && (block->mod != rti->finder) )// && (*rti->func)( block,
rti->arg ) )
! {
! // a hit!
! rti->hit = block->mod;
! rti->x = x;
! rti->y = y;
! rti->z = z;
! return TRUE;
! }
! }
!
! return FALSE;
! }
!
!
! stg_meters_t StgWorld::Raytrace2( StgModel* finder,
! stg_pose_t* pose,
! stg_meters_t max_range,
! stg_block_match_func_t func,
! const void* arg,
! StgModel** hit_model )
! {
! // find the global integer bitmap address of the ray
! int32_t x = (int32_t)((pose->x+width/2.0)*ppm);
! int32_t y = (int32_t)((pose->y+height/2.0)*ppm);
!
! // and the x and y offsets of the ray
! int32_t dx = (int32_t)(ppm*max_range * cos(pose->a));
! int32_t dy = (int32_t)(ppm*max_range * sin(pose->a));
!
! //glPushMatrix();
! //glTranslatef( -width/2.0, -height/2.0, 0 );
! //glScalef( 1.0/ppm, 1.0/ppm, 0 );
!
! _raytrace_info_t rinfo;
! rinfo.world = this;
! rinfo.finder = finder;
! rinfo.func = func;
! rinfo.arg = arg;
! rinfo.hit = NULL;
!
! if( stg_line_3d( x, y, 0,
! dx, dy, 0,
! (stg_line3d_func_t)raytest,
! &rinfo ) )
{
! //glPopMatrix();
! *hit_model = rinfo.hit;
! // how far away was that strike?
! return hypot( (rinfo.x-x)/ppm, (rinfo.y-y)/ppm );
! }
! //glPopMatrix();
! // return the range from ray start to object strike
! // hit nothing, so return max range
! return max_range;
}
void stg_model_save_cb( gpointer key, gpointer data, gpointer user )
{
***************
*** 735,839 ****
}
! void StgWorld::MapBlock( StgBlock* block )
{
! stg_point_t* pts = block->pts_global;
! unsigned int pt_count = block->pt_count;
!
! assert( pt_count >=2 );
!
! for( unsigned int p=0; p<pt_count-1; p++ )
! MapBlockLine( block,
! pts[p].x, pts[p].y,
! pts[p+1].x, pts[p+1].y );
! // close the loop
! MapBlockLine( block,
! pts[0].x, pts[0].y,
! pts[pt_count-1].x, pts[pt_count-1].y );
}
! StgCell* myPixel( StgCell* cell, StgBlock* block, long int x, long int y)
{
! StgCell* found = cell->LocateAtom( x, y );
!
! if( found )
! {
! found->AddBlock( block );
! cell = found;
! }
!
! return cell;
}
!
! void StgWorld::MapBlockLine( StgBlock* block,
! double mx1, double my1,
! double mx2, double my2 )
{
! //int32_t x1 = (int32_t)(mx1 * ppm - 0.5);
! //int32_t x2 = (int32_t)(mx2 * ppm - 0.5);
! //int32_t y1 = (int32_t)(my1 * ppm - 0.5);
! //int32_t y2 = (int32_t)(my2 * ppm - 0.5);
! int x1 = (int)floor(mx1 * ppm);
! int x2 = (int)floor(mx2 * ppm);
! int y1 = (int)floor(my1 * ppm);
! int y2 = (int)floor(my2 * ppm);
!
! StgCell* cell = root;
- //cell = myPixel( cell, block, x1, y1 );
! // Extremely Fast Line Algorithm Var A (Division)
! // Copyright 2001-2, By Po-Han Lin
! // Freely useable in non-commercial applications as long as credits
! // to Po-Han Lin and link to http://www.edepot.com is provided in
! // source code and can been seen in compiled executable.
! // Commercial applications please inquire about licensing the algorithms.
! //
! // Lastest version at http://www.edepot.com/phl.html
! // constant string will appear in executable
! const char* EFLA_CREDIT =
! "Contains an implementation of the Extremely Fast Line Algorithm (EFLA)"
! "by Po-Han Lin (http://www.edepot.com/phl.html)";
! char* dummy = (char*)EFLA_CREDIT; // prevents compiler warning about unused
var
! dummy++; // ditto
! bool yLonger=false;
! int incrementVal;
!
! int shortLen = y2-y1;
! int longLen = x2-x1;
! if( abs(shortLen) > abs(longLen) )
{
! int swap=shortLen;
! shortLen=longLen;
! longLen=swap;
! yLonger=true;
! }
!
! if( longLen<0 )
! incrementVal=-1;
! else
! incrementVal=1;
!
! double divDiff;
! if( shortLen == 0 )
! divDiff=longLen;
! else
! divDiff=(double)longLen/(double)shortLen;
!
! if (yLonger)
! for (int i=0;i!=longLen;i+=incrementVal)
! cell = myPixel( cell, block, x1+(int)((double)i/divDiff),y1+i);
! else
! for (int i=0;i!=longLen;i+=incrementVal)
! cell = myPixel( cell, block, x1+i,y1+(int)((double)i/divDiff));
! // END OF EFLA code
}
--- 571,677 ----
}
!
! void draw_voxel( int x, int y, int z )
{
! glTranslatef( x,y,z );
! glBegin(GL_QUADS); // Draw The Cube Using quads
! glColor3f(0.0f,1.0f,0.0f); // Color Blue
! glVertex3f( 1.0f, 1.0f,-1.0f); // Top Right Of The Quad (Top)
! glVertex3f(-1.0f, 1.0f,-1.0f); // Top Left Of The Quad (Top)
! glVertex3f(-1.0f, 1.0f, 1.0f); // Bottom Left Of The Quad (Top)
! glVertex3f( 1.0f, 1.0f, 1.0f); // Bottom Right Of The Quad (Top)
! glColor3f(1.0f,0.5f,0.0f); // Color Orange
! glVertex3f( 1.0f,-1.0f, 1.0f); // Top Right Of The Quad (Bottom)
! glVertex3f(-1.0f,-1.0f, 1.0f); // Top Left Of The Quad (Bottom)
! glVertex3f(-1.0f,-1.0f,-1.0f); // Bottom Left Of The Quad (Bottom)
! glVertex3f( 1.0f,-1.0f,-1.0f); // Bottom Right Of The Quad (Bottom)
! glColor3f(1.0f,0.0f,0.0f); // Color Red
! glVertex3f( 1.0f, 1.0f, 1.0f); // Top Right Of The Quad (Front)
! glVertex3f(-1.0f, 1.0f, 1.0f); // Top Left Of The Quad (Front)
! glVertex3f(-1.0f,-1.0f, 1.0f); // Bottom Left Of The Quad (Front)
! glVertex3f( 1.0f,-1.0f, 1.0f); // Bottom Right Of The Quad (Front)
! glColor3f(1.0f,1.0f,0.0f); // Color Yellow
! glVertex3f( 1.0f,-1.0f,-1.0f); // Top Right Of The Quad (Back)
! glVertex3f(-1.0f,-1.0f,-1.0f); // Top Left Of The Quad (Back)
! glVertex3f(-1.0f, 1.0f,-1.0f); // Bottom Left Of The Quad (Back)
! glVertex3f( 1.0f, 1.0f,-1.0f); // Bottom Right Of The Quad (Back)
! glColor3f(0.0f,0.0f,1.0f); // Color Blue
! glVertex3f(-1.0f, 1.0f, 1.0f); // Top Right Of The Quad (Left)
! glVertex3f(-1.0f, 1.0f,-1.0f); // Top Left Of The Quad (Left)
! glVertex3f(-1.0f,-1.0f,-1.0f); // Bottom Left Of The Quad (Left)
! glVertex3f(-1.0f,-1.0f, 1.0f); // Bottom Right Of The Quad (Left)
! glColor3f(1.0f,0.0f,1.0f); // Color Violet
! glVertex3f( 1.0f, 1.0f,-1.0f); // Top Right Of The Quad (Right)
! glVertex3f( 1.0f, 1.0f, 1.0f); // Top Left Of The Quad (Right)
! glVertex3f( 1.0f,-1.0f, 1.0f); // Bottom Left Of The Quad (Right)
! glVertex3f( 1.0f,-1.0f,-1.0f); // Bottom Right Of The Quad (Right)
! glEnd(); // End Drawing The Cube
!
! glTranslatef( -x, -y, -z );
}
! // int stg_hash3d( int x, int y, int z )
! // {
! // return (x+y+z);
! // }
!
!
! void StgWorld::MetersToPixels( stg_meters_t mx, stg_meters_t my,
! int32_t *px, int32_t *py )
{
! *px = (int32_t)floor((mx+width/2.0) * ppm);
! *py = (int32_t)floor((my+height/2.0) * ppm);
}
! typedef struct
{
! StgBlockGrid* grid;
! StgBlock* block;
! } _render_info_t;
! int stg_add_block_to_grid( int x, int y, int z,
! _render_info_t* rinfo )
! {
! rinfo->grid->AddBlock( x, y, rinfo->block );
! return FALSE;
! }
! void StgWorld::MapBlock( StgBlock* block )
! {
! stg_point_int_t* pts = block->pts_global;
! unsigned int pt_count = block->pt_count;
! assert( pt_count >=2 );
! _render_info_t rinfo;
! rinfo.grid = bgrid;
! rinfo.block - block;
! // TODO - could be a little bit faster - currently considers each
! // vertex twice
! for( unsigned int p=0; p<pt_count; p++ )
{
! int32_t x = pts[p].x; // line start
! int32_t y = pts[p].y; // line end
!
! int32_t dx = pts[(p+1)%pt_count].x - x;
! int32_t dy = pts[(p+1)%pt_count].y - y;
!
! // pass this structure into our per-pixel callback
! _render_info_t rinfo;
! rinfo.grid = bgrid;
! rinfo.block = block;
!
! // ignore return value
! stg_line_3d( x, y, 0,
! dx, dy, 0,
! (stg_line3d_func_t)stg_add_block_to_grid,
! &rinfo );
! }
}
--- NEW FILE: blockgrid.cc ---
#include "stage.hh"
StgBlockGrid::StgBlockGrid( uint32_t width, uint32_t height )
{
printf( "Creating StgBlockGrid of [%d,%d] elements\n",
width, height );
this->width = width;
this->height = height;
this->lists = new GSList*[width * height];
bzero( lists, width*height * sizeof(GSList*));
}
StgBlockGrid::~StgBlockGrid()
{
delete[] lists;
}
void StgBlockGrid::AddBlock( uint32_t x, uint32_t y, StgBlock* block )
{
if( x < width && y < height )
{
unsigned int index = x + width*y;
lists[index] = g_slist_prepend( lists[index], block );
block->RecordRenderPoint( x, y );
}
}
GSList* StgBlockGrid::GetList( uint32_t x, uint32_t y )
{
if( x < width && y < height )
{
unsigned int index = x + width*y;
return lists[index];
}
return NULL;
}
void StgBlockGrid::RemoveBlock( uint32_t x, uint32_t y, StgBlock* block )
{
//printf( "remove block %u %u\n", x, y );
if( x < width && y < height )
{
unsigned int index = x + width*y;
lists[index] = g_slist_remove( lists[index], block );
}
}
void StgBlockGrid::RemoveBlock( StgBlock* block )
{
// todo - speed this up a great deal
for( uint32_t x=0; x<width; x++ )
for( uint32_t y=0; y<height; y++ )
RemoveBlock(x,y,block );
}
void StgBlockGrid::Draw()
{
for( uint32_t x=0; x<width; x++ )
for( uint32_t y=0; y<height; y++ )
if( lists[ x + width*y] )
glRecti( x,y,x+1,y+1 );
//printf( "printing %d %d %d \n", x, y, index );
//glTranslatef( x,y,0 );
// glBegin(GL_QUADS); // Draw The Cube Using quads
// glColor3f(0.0f,1.0f,0.0f); // Color Blue
// glVertex3f( 1.0f, 1.0f,-1.0f); // Top Right Of The Quad (Top)
// glVertex3f(-1.0f, 1.0f,-1.0f); // Top Left Of The Quad (Top)
// glVertex3f(-1.0f, 1.0f, 1.0f); // Bottom Left Of The Quad (Top)
// glVertex3f( 1.0f, 1.0f, 1.0f); // Bottom Right Of The Quad
(Top)
// glColor3f(1.0f,0.5f,0.0f); // Color Orange
// glVertex3f( 1.0f,-1.0f, 1.0f); // Top Right Of The Quad
(Bottom)
// glVertex3f(-1.0f,-1.0f, 1.0f); // Top Left Of The Quad (Bottom)
// glVertex3f(-1.0f,-1.0f,-1.0f); // Bottom Left Of The Quad
(Bottom)
// glVertex3f( 1.0f,-1.0f,-1.0f); // Bottom Right Of The Quad
(Bottom)
// glColor3f(1.0f,0.0f,0.0f); // Color Red
// glVertex3f( 1.0f, 1.0f, 1.0f); // Top Right Of The Quad (Front)
// glVertex3f(-1.0f, 1.0f, 1.0f); // Top Left Of The Quad (Front)
// glVertex3f(-1.0f,-1.0f, 1.0f); // Bottom Left Of The Quad
(Front)
// glVertex3f( 1.0f,-1.0f, 1.0f); // Bottom Right Of The Quad
(Front)
// glColor3f(1.0f,1.0f,0.0f); // Color Yellow
// glVertex3f( 1.0f,-1.0f,-1.0f); // Top Right Of The Quad (Back)
// glVertex3f(-1.0f,-1.0f,-1.0f); // Top Left Of The Quad (Back)
// glVertex3f(-1.0f, 1.0f,-1.0f); // Bottom Left Of The Quad
(Back)
// glVertex3f( 1.0f, 1.0f,-1.0f); // Bottom Right Of The Quad
(Back)
// glColor3f(0.0f,0.0f,1.0f); // Color Blue
// glVertex3f(-1.0f, 1.0f, 1.0f); // Top Right Of The Quad (Left)
// glVertex3f(-1.0f, 1.0f,-1.0f); // Top Left Of The Quad (Left)
// glVertex3f(-1.0f,-1.0f,-1.0f); // Bottom Left Of The Quad
(Left)
// glVertex3f(-1.0f,-1.0f, 1.0f); // Bottom Right Of The Quad
(Left)
// glColor3f(1.0f,0.0f,1.0f); // Color Violet
// glVertex3f( 1.0f, 1.0f,-1.0f); // Top Right Of The Quad (Right)
// glVertex3f( 1.0f, 1.0f, 1.0f); // Top Left Of The Quad (Right)
// glVertex3f( 1.0f,-1.0f, 1.0f); // Bottom Left Of The Quad
(Right)
// glVertex3f( 1.0f,-1.0f,-1.0f); // Bottom Right Of The Quad
(Right)
// glEnd(); // End Drawing The Cube
//glTranslatef( -x, -y, 0 );
}
Index: Makefile.manual
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/Makefile.manual,v
retrieving revision 1.1.2.3
retrieving revision 1.1.2.4
diff -C2 -d -r1.1.2.3 -r1.1.2.4
*** Makefile.manual 22 Oct 2007 09:06:27 -0000 1.1.2.3
--- Makefile.manual 30 Oct 2007 01:01:48 -0000 1.1.2.4
***************
*** 10,14 ****
ancestor.o \
block.o \
! cell.o \
gl.o \
glcolorstack.o \
--- 10,14 ----
ancestor.o \
block.o \
! blockgrid.o \
gl.o \
glcolorstack.o \
Index: stage.hh
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/stage.hh,v
retrieving revision 1.1.2.8
retrieving revision 1.1.2.9
diff -C2 -d -r1.1.2.8 -r1.1.2.9
*** stage.hh 22 Oct 2007 09:06:28 -0000 1.1.2.8
--- stage.hh 30 Oct 2007 01:01:48 -0000 1.1.2.9
***************
*** 276,279 ****
--- 276,297 ----
void stg_quit_request( void );
+
+
+ /** take binary sign of a, either -1, or 1 if >= 0 */
+ #define SGN(a) (((a)<0) ? -1 : 1)
+ /** matching function should return 0 iff the candidate block is
+ acceptable */
+ typedef int(*stg_line3d_func_t)(int32_t x, int32_t y, int32_t z,
+ void* arg );
+
+
+ /** Visit every voxel along a vector from (x,y,z) to (x+dx, y+dy, z+dz
+ ). Call the function for every voxel, passing in the current voxel
+ coordinates followed by the two arguments. Adapted from Graphics
+ Gems IV, algorithm by Cohen & Kaufman 1991 */
+ int stg_line_3d( int32_t x, int32_t y, int32_t z,
+ int32_t dx, int32_t dy, int32_t dz,
+ stg_line3d_func_t visit_voxel,
+ void* arg );
/** Get a string identifying the version of stage. The string is
***************
*** 296,299 ****
--- 314,324 ----
} stg_point_t;
+
+ /** define an integer point on the plane */
+ typedef struct
+ {
+ uint32_t x,y;
+ } stg_point_int_t;
+
/// Define a point in 3D space
typedef struct
***************
*** 301,304 ****
--- 326,336 ----
double x,y,z;
} stg_point_3d_t;
+
+ /** define an integer point in 3D space */
+ // typedef struct
+ // {
+ // uint32_t x,y,z;
+ // } stg_point_3d_int_t;
+
/** Create an array of [count] points. Caller must free the returned
***************
*** 754,757 ****
--- 786,795 ----
/[EMAIL PROTECTED]/
+ // /** matching function should return 0 iff the candidate block is
+ // acceptable */
+ class StgBlock;
+
+ typedef int (*stg_block_match_func_t)(StgBlock* candidate, const void* arg );
+
// MATRIX
-----------------------------------------------------------------------
***************
*** 763,853 ****
*/
! typedef enum {
! STG_SPLIT_NONE=0,
! STG_SPLIT_X,
! STG_SPLIT_Y
! } stg_split_t;
- class StgBlock;
- /** matching function should return 0 iff the candidate block is
- acceptable */
- typedef int(*stg_block_match_func_t)(StgBlock* candidate, const void* arg );
! /** A node in the occupancy quadtree */
! class StgCell
! {
! friend class StgWorld;
! private:
! void Split(); ///< split the cell into children
! /** list of models that have been AddBlock()ed into this
! cell */
! GSList* list;
! /** extent of the cell */
! int32_t width, height;
! /** bounding box */
! int32_t xmin,ymin,xmax,ymax;
! /** direction of split for this cell */
! stg_split_t split;
! /** links for BSP tree */
! StgCell *left, *right;
! /** links for BSP tree */
! StgCell* parent;
! public:
! StgCell( StgCell* parent,
! int32_t xmin, int32_t xmax,
! int32_t ymin, int32_t ymax );
! ~StgCell();
! /** Return the root of the tree that contains this cell */
! StgCell* Root();
! /** Remove block from the cell. This method is static as it may
! destroy the cell. */
! static void RemoveBlock( StgCell* cell, StgBlock* block );
! /** Add a pointer to block to the list of this cells contents */
! void AddBlock( StgBlock* block );
! /** Return the first block in the cell that matches the comparison
! function */
! StgBlock* FindBlock( stg_block_match_func_t, const void* arg );
! /** Draw the cell into the current OpenGL context */
! void Draw();
! /** Draw the cell and all its descendents in the current OpenGL
! context. If leaf_only is true, only occupied leaf nodes are
! drawn */
! void DrawTree( bool leaf_only );
! /** Print a human-readable description of the cell on stdout,
! prefixed by the string prefix. */
! void Print( char* prefix );
! /** returns true iff the cell is a unit square (ie. as small as it
! can be */
! bool Atomic();
! /** Returns true iff the cell contains the point <x,y> */
! bool Contains( int32_t x, int32_t y );
! /** returns the smallest currently existing cell that contains point
! <x,y>. Does not create new cells. */
! StgCell* Locate( int32_t x, int32_t y );
! /** returns a unit cell that contains <x,y>, creating cells if
! necessary */
! StgCell* LocateAtom( int32_t x, int32_t y );
! };
--- 801,887 ----
*/
! // typedef enum {
! // STG_SPLIT_NONE=0,
! // STG_SPLIT_X,
! // STG_SPLIT_Y
! // } stg_split_t;
! // /** A node in the occupancy quadtree */
! // class StgCell
! // {
! // friend class StgWorld;
! // private:
! // void Split(); ///< split the cell into children
! // /** list of models that have been AddBlock()ed into this
! // cell */
! // GSList* list;
! // /** extent of the cell */
! // int32_t width, height;
! // /** bounding box */
! // int32_t xmin,ymin,xmax,ymax;
! // /** direction of split for this cell */
! // stg_split_t split;
! // /** links for BSP tree */
! // StgCell *left, *right;
! // /** links for BSP tree */
! // StgCell* parent;
! // public:
! // StgCell( StgCell* parent,
! // int32_t xmin, int32_t xmax,
! // int32_t ymin, int32_t ymax );
! // ~StgCell();
! // /** Return the root of the tree that contains this cell */
! // StgCell* Root();
! // /** Remove block from the cell. This method is static as it may
! // destroy the cell. */
! // static void RemoveBlock( StgCell* cell, StgBlock* block );
! // /** Add a pointer to block to the list of this cells contents */
! // void AddBlock( StgBlock* block );
! // /** Return the first block in the cell that matches the comparison
! // function */
! // StgBlock* FindBlock( stg_block_match_func_t, const void* arg );
! // /** Draw the cell into the current OpenGL context */
! // void Draw();
! // /** Draw the cell and all its descendents in the current OpenGL
! // context. If leaf_only is true, only occupied leaf nodes are
! // drawn */
! // void DrawTree( bool leaf_only );
! // /** Print a human-readable description of the cell on stdout,
! // prefixed by the string prefix. */
! // void Print( char* prefix );
! // /** returns true iff the cell is a unit square (ie. as small as it
! // can be */
! // bool Atomic();
! // /** Returns true iff the cell contains the point <x,y> */
! // bool Contains( int32_t x, int32_t y );
! // /** returns the smallest currently existing cell that contains point
! // <x,y>. Does not create new cells. */
! // StgCell* Locate( int32_t x, int32_t y );
! // /** returns a unit cell that contains <x,y>, creating cells if
! // necessary */
! // StgCell* LocateAtom( int32_t x, int32_t y );
! // };
***************
*** 1132,1135 ****
--- 1166,1186 ----
+ class StgBlockGrid
+ {
+ private:
+ GSList** lists;
+
+ public:
+ uint32_t width, height;
+ StgBlockGrid( uint32_t width, uint32_t height );
+ ~StgBlockGrid();
+ void AddBlock( uint32_t x, uint32_t y, StgBlock* block );
+ void RemoveBlock( uint32_t x, uint32_t y, StgBlock* block );
+ GSList* GetList( uint32_t x, uint32_t y );
+ void RemoveBlock( StgBlock* block );
+ void Draw();
+ };
+
+
/// WORLD CLASS
class StgWorld : public StgAncestor
***************
*** 1149,1152 ****
--- 1200,1206 ----
static GHashTable* typetable;
+ inline void MetersToPixels( stg_meters_t mx, stg_meters_t my,
+ int32_t *px, int32_t *py );
+
public:
***************
*** 1229,1238 ****
// A quad tree of cells. Each leaf node contains a list of
// pointers to objects located at that cell
! StgCell* root;
!
void MapBlock( StgBlock* block );
! void MapBlockLine( StgBlock* block,
! double x1, double y1,
! double x2, double y2 );
stg_meters_t Raytrace( StgModel* finder,
--- 1283,1296 ----
// A quad tree of cells. Each leaf node contains a list of
// pointers to objects located at that cell
! //StgCell* root;
!
! StgBlockGrid* bgrid;
!
! //uint16_t* sparsetable;
!
void MapBlock( StgBlock* block );
! // void MapBlockLine( StgBlock* block,
! // double x1, double y1, double z1,
! // double x2, double y2, double z2 );
stg_meters_t Raytrace( StgModel* finder,
***************
*** 1242,1245 ****
--- 1300,1310 ----
const void* arg,
StgModel** hit_model );
+
+ stg_meters_t Raytrace2( StgModel* finder,
+ stg_pose_t* pose,
+ stg_meters_t max_range,
+ stg_block_match_func_t func,
+ const void* arg,
+ StgModel** hit_model );
void ClockString( char* str, size_t maxlen );
***************
*** 1648,1659 ****
StgModel* mod; //< model to which this block belongs
! void AddCell( StgCell* cell );
! stg_point_t* pts_global; //< points defining a polygon in global coords
private:
stg_color_t color;
bool inherit_color;
! GList* rendered_cells; //< matrix cells that contain this object, for fast
unrendering
inline void DrawTop();
--- 1713,1727 ----
StgModel* mod; //< model to which this block belongs
! //void AddCell( StgCell* cell );
! stg_point_int_t* pts_global; //< points defining a polygon in global coords
+ void RecordRenderPoint( uint32_t x, uint32_t y );
+
private:
stg_color_t color;
bool inherit_color;
!
! GArray* rendered_points;
inline void DrawTop();
--- cell.cc DELETED ---
Index: worldgtk.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/worldgtk.cc,v
retrieving revision 1.1.2.6
retrieving revision 1.1.2.7
diff -C2 -d -r1.1.2.6 -r1.1.2.7
*** worldgtk.cc 22 Oct 2007 09:06:28 -0000 1.1.2.6
--- worldgtk.cc 30 Oct 2007 01:01:48 -0000 1.1.2.7
***************
*** 1846,1878 ****
glLineWidth( 1 );
- float zm = 1.0/ppm;
-
glPushMatrix();
! glTranslatef( 0,0,1 );
! glScalef( zm, zm, 0 );
!
! StgCell* cell = root;
!
! if( show_quadtree )
! {
! colorstack.Push( 0,1,0 );
! glPolygonMode( GL_FRONT, GL_LINE );
! cell->DrawTree(false);
! }
!
! if( show_occupancy )
! {
! glTranslatef( 0,0,1 );
! glPolygonMode( GL_FRONT, GL_FILL );
! colorstack.Push( 0,0.5,0 );
! cell->DrawTree(true);
! }
!
! glPopMatrix();
colorstack.Pop();
glEnable( GL_LINE_SMOOTH );
}
colorstack.Push( 1, 0, 0 );
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
--- 1846,1862 ----
glLineWidth( 1 );
glPushMatrix();
! glTranslatef( -width/2.0, -height/2.0, 1 );
! glScalef( 1.0/ppm, 1.0/ppm, 0 );
! glPolygonMode( GL_FRONT, GL_LINE );
! colorstack.Push(1,0,0);
! this->bgrid->Draw();
colorstack.Pop();
+ glPopMatrix();
glEnable( GL_LINE_SMOOTH );
}
+
colorstack.Push( 1, 0, 0 );
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
-------------------------------------------------------------------------
This SF.net email is sponsored by: Splunk Inc.
Still grepping through log files to find problems? Stop.
Now Search log events and configuration files using AJAX and a browser.
Download your FREE copy of Splunk now >> http://get.splunk.com/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
