Menu
▾
▴
[Playerstage-commit] code/stage/libstage gl.cc, NONE, 1.1 Makefile.manual, 1.4, 1.5 block.cc, 1.3, 1.4 canvas.cc, 1.6, 1.7 model.cc, 1.3, 1.4 model_fiducial.cc, 1.2, 1.3 model_laser.cc, 1.2, 1.3 model_load.cc, 1.2, 1.3 model_position.cc, 1.2, 1.3 model_ranger.cc, 1.2, 1.3 stage.cc, 1.2, 1.3 stage.hh, 1.5, 1.6 world.cc, 1.6, 1.7 worldgui.cc, 1.4, 1.5
|
From: Richard V. <rt...@us...> - 2008-02-17 19:52:27
|
Update of /cvsroot/playerstage/code/stage/libstage In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv12579 Modified Files: Makefile.manual block.cc canvas.cc model.cc model_fiducial.cc model_laser.cc model_load.cc model_position.cc model_ranger.cc stage.cc stage.hh world.cc worldgui.cc Added Files: gl.cc Log Message: fixed geometry bug where origin was ignored by blocks. cleaned up some API and added global grid drawing that tracks world extent Index: worldgui.cc =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/worldgui.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** worldgui.cc 1 Feb 2008 03:11:02 -0000 1.4 --- worldgui.cc 18 Feb 2008 03:52:30 -0000 1.5 *************** *** 93,96 **** --- 93,98 ---- #include "stage_internal.hh" + #include "region.hh" + #include <FL/Fl_Image.H> #include <FL/Fl_Shared_Image.H> *************** *** 114,140 **** static const char* MITEM_VIEW_ARROWS = "View/Trails/Arrows"; - // transform the current coordinate frame by the given pose - void Stg::gl_coord_shift( double x, double y, double z, double a ) - { - glTranslatef( x,y,z ); - glRotatef( rtod(a), 0,0,1 ); - } - - // transform the current coordinate frame by the given pose - void Stg::gl_pose_shift( stg_pose_t* pose ) - { - gl_coord_shift( pose->x, pose->y, pose->z, pose->a ); - } - - // TODO - this could be faster, but we don't draw a lot of text - void Stg::gl_draw_string( float x, float y, float z, char *str ) - { - char *c; - glRasterPos3f(x, y,z); - for (c=str; *c != '\0'; c++) - glutBitmapCharacter( GLUT_BITMAP_HELVETICA_12, *c); - } - - ///// void dummy_cb(Fl_Widget*, void* v) --- 116,119 ---- *************** *** 199,202 **** --- 178,183 ---- : Fl_Window(0,0,W,H,L) { + //size_range( 100,100 ); // set minimum window size + graphics = true; *************** *** 259,264 **** int width = (int)wf->ReadTupleFloat(wf_section, "size", 0, w() ); int height = (int)wf->ReadTupleFloat(wf_section, "size", 1, h() ); ! size( width,height ); // resize this window canvas->panx = wf->ReadTupleFloat(wf_section, "center", 0, canvas->panx ); canvas->pany = wf->ReadTupleFloat(wf_section, "center", 1, canvas->pany ); --- 240,248 ---- int width = (int)wf->ReadTupleFloat(wf_section, "size", 0, w() ); int height = (int)wf->ReadTupleFloat(wf_section, "size", 1, h() ); ! // on OS X this behaves badly - prevents the Window manager resizing ! //larger than this size. ! //size( width,height ); + canvas->panx = wf->ReadTupleFloat(wf_section, "center", 0, canvas->panx ); canvas->pany = wf->ReadTupleFloat(wf_section, "center", 1, canvas->pany ); *************** *** 341,347 **** bool StgWorldGui::RealTimeUpdate() { ! // puts( "RTU" ); ! bool updated = StgWorld::RealTimeUpdate(); ! Fl::check(); // may redraw the window return updated; } --- 325,329 ---- bool StgWorldGui::RealTimeUpdate() { ! bool updated = StgWorld::RealTimeUpdateWithIdler( Fl::check); return updated; } *************** *** 349,355 **** bool StgWorldGui::Update() { - // puts( "RTU" ); bool updated = StgWorld::Update(); Fl::check(); // may redraw the window return updated; } --- 331,347 ---- bool StgWorldGui::Update() { bool updated = StgWorld::Update(); Fl::check(); // may redraw the window return updated; } + + void StgWorldGui::DrawTree( bool drawall ) + { + g_hash_table_foreach( superregions, (GHFunc)SuperRegion::Draw_cb, NULL ); + } + + void StgWorldGui::DrawFloor() + { + g_hash_table_foreach( superregions, (GHFunc)SuperRegion::Floor_cb, NULL ); + } + Index: stage.cc =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/stage.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** stage.cc 15 Jan 2008 01:16:49 -0000 1.2 --- stage.cc 18 Feb 2008 03:52:30 -0000 1.3 *************** *** 312,315 **** --- 312,329 ---- } + // returns the resultant of vector [p1] and [p2] + stg_pose_t Stg::pose_sum( stg_pose_t p1, stg_pose_t p2 ) + { + double cosa = cos(p1.a); + double sina = sin(p1.a); + + stg_pose_t result; + result.x = p1.x + p2.x * cosa - p2.y * sina; + result.y = p1.y + p2.x * sina + p2.y * cosa; + result.z = p1.z + p2.z; + result.a = normalize(p1.a + p2.a); + + return result; + } // pb_* functions are only used inside this file Index: model_laser.cc =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/model_laser.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** model_laser.cc 15 Jan 2008 01:16:49 -0000 1.2 --- model_laser.cc 18 Feb 2008 03:52:30 -0000 1.3 *************** *** 92,96 **** geom.size.y = DEFAULT_LASER_SIZEY; geom.size.z = DEFAULT_LASER_SIZEZ; ! SetGeom( &geom ); // set default color --- 92,96 ---- geom.size.y = DEFAULT_LASER_SIZEY; geom.size.z = DEFAULT_LASER_SIZEZ; ! SetGeom( geom ); // set default color Index: model.cc =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/model.cc,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** model.cc 12 Feb 2008 02:53:53 -0000 1.3 --- model.cc 18 Feb 2008 03:52:30 -0000 1.4 *************** *** 169,172 **** --- 169,175 ---- bzero( &pose, sizeof(pose)); + pose.x = 5; // prevent creating uncessary superregions + pose.y = 5; + bzero( &global_pose, sizeof(global_pose)); *************** *** 280,285 **** stg_raytrace_sample_t* sample ) { ! LocalToGlobal( &pose ); ! world->Raytrace( pose, range, func, --- 283,288 ---- stg_raytrace_sample_t* sample ) { ! //LocalToGlobal( &pose ); ! world->Raytrace( LocalToGlobal(pose), range, func, *************** *** 310,315 **** uint32_t sample_count ) { ! LocalToGlobal( &pose ); ! world->Raytrace( pose, range, fov, --- 313,318 ---- uint32_t sample_count ) { ! //LocalToGlobal( &pose ); ! world->Raytrace( LocalToGlobal(pose), range, fov, *************** *** 444,448 **** // set the model's velocity in the global frame ! void StgModel::SetGlobalVelocity( stg_velocity_t* gv ) { stg_pose_t gpose = GetGlobalPose(); --- 447,451 ---- // set the model's velocity in the global frame ! void StgModel::SetGlobalVelocity( stg_velocity_t gv ) { stg_pose_t gpose = GetGlobalPose(); *************** *** 450,460 **** double cosa = cos( gpose.a ); double sina = sin( gpose.a ); ! stg_velocity_t lv; ! lv.x = gv->x * cosa + gv->y * sina; ! lv.y = -gv->x * sina + gv->y * cosa; ! lv.a = gv->a; ! this->SetVelocity( &lv ); } --- 453,463 ---- double cosa = cos( gpose.a ); double sina = sin( gpose.a ); ! stg_velocity_t lv; ! lv.x = gv.x * cosa + gv.y * sina; ! lv.y = -gv.x * sina + gv.y * cosa; ! lv.a = gv.a; ! this->SetVelocity( lv ); } *************** *** 496,513 **** } ! // convert a pose in this model's local coordinates into global // coordinates // should one day do all this with affine transforms for neatness? ! void StgModel::LocalToGlobal( stg_pose_t* pose ) { ! stg_pose_t pz; ! memcpy( &pz, pose, sizeof(stg_pose_t)); ! ! stg_pose_t origin = GetGlobalPose(); ! ! stg_pose_sum( pose, &origin, &pz ); } void StgModel::MapWithChildren() --- 499,527 ---- } ! // convert a pose in this model's local coordinates into global // coordinates // should one day do all this with affine transforms for neatness? ! stg_pose_t StgModel::LocalToGlobal( stg_pose_t pose ) { ! return pose_sum( pose_sum( geom.pose, GetGlobalPose()), pose ); } + stg_point3_t StgModel::LocalToGlobal( stg_point3_t point ) + { + stg_pose_t pose; + pose.x = point.x; + pose.y = point.y; + pose.z = point.z; + pose.a = 0; + + pose = LocalToGlobal( pose ); + + point.x = pose.x; + point.y = pose.y; + point.z = pose.z; + + return point; + } void StgModel::MapWithChildren() *************** *** 867,929 **** void StgModel::DrawGrid( void ) { ! PushColor( 0,0,0,0.1 ); ! ! double dx = geom.size.x; ! double dy = geom.size.y; ! double sp = 1.0; ! ! int nx = (int) ceil((dx/2.0) / sp ); ! int ny = (int) ceil((dy/2.0) / sp ); ! ! if( nx == 0 ) nx = 1.0; ! if( ny == 0 ) ny = 1.0; ! ! glBegin(GL_LINES); ! ! // draw the bounding box first ! glVertex2f( -nx, -ny ); ! glVertex2f( nx, -ny ); ! ! glVertex2f( -nx, ny ); ! glVertex2f( nx, ny ); ! ! glVertex2f( nx, -ny ); ! glVertex2f( nx, ny ); ! ! glVertex2f( -nx,-ny ); ! glVertex2f( -nx, ny ); ! ! ! int i; ! for (i = -nx+1; i < nx; i++) ! { ! glVertex2f( i * sp, - dy/2 ); ! glVertex2f( i * sp, + dy/2 ); ! } ! for (i = -ny+1; i < ny; i++) ! { ! glVertex2f( - dx/2, i * sp ); ! glVertex2f( + dx/2, i * sp ); ! } ! ! glEnd(); ! ! PushColor( 0,0,0,0.3); ! ! char str[16]; ! ! for (i = -nx+1; i < nx; i++) ! { ! snprintf( str, 16, "%d", (int)i ); ! gl_draw_string( -dx/2.0 + -0.2 + (nx + i * sp), -0.2 , 0.01, str ); ! } ! for (i = -ny+1; i < ny; i++) ! { ! snprintf( str, 16, "%d", (int)i ); ! gl_draw_string( -0.2, -dy/2.0 + -0.2 + (ny + i * sp) , 0.01, str ); ! } ! ! PopColor(); PopColor(); } --- 881,894 ---- void StgModel::DrawGrid( void ) { ! stg_bounds3d_t vol; ! vol.x.min = -geom.size.x/2.0; ! vol.x.max = geom.size.x/2.0; ! vol.y.min = -geom.size.y/2.0; ! vol.y.max = geom.size.y/2.0; ! vol.z.min = 0; ! vol.z.max = geom.size.z; ! PushColor( 0,0,1,0.4 ); ! gl_draw_grid(vol); PopColor(); } *************** *** 934,941 **** } ! void StgModel::SetVelocity( stg_velocity_t* vel ) { ! assert(vel); ! memcpy( &this->velocity, vel, sizeof(stg_velocity_t)); if( ! this->on_velocity_list && velocity_is_nonzero( & this->velocity ) ) --- 899,905 ---- } ! void StgModel::SetVelocity( stg_velocity_t vel ) { ! this->velocity = vel; if( ! this->on_velocity_list && velocity_is_nonzero( & this->velocity ) ) *************** *** 967,985 **** } ! void StgModel::SetPose( stg_pose_t* pose ) { //PRINT_DEBUG5( "%s.SetPose(%.2f %.2f %.2f %.2f)", // this->token, pose->x, pose->y, pose->z, pose->a ); - assert(pose); - // if the pose has changed, we need to do some work ! if( memcmp( &this->pose, pose, sizeof(stg_pose_t) ) != 0 ) { UnMapWithChildren(); ! memcpy( &this->pose, pose, sizeof(stg_pose_t)); ! this->pose.a = normalize(this->pose.a); //double hitx, hity; --- 931,950 ---- } ! void StgModel::SetPose( stg_pose_t pose ) { //PRINT_DEBUG5( "%s.SetPose(%.2f %.2f %.2f %.2f)", // this->token, pose->x, pose->y, pose->z, pose->a ); // if the pose has changed, we need to do some work ! if( memcmp( &this->pose, &pose, sizeof(stg_pose_t) ) != 0 ) { UnMapWithChildren(); + + pose.a = normalize( pose.a ); + this->pose = pose; ! //memcpy( &this->pose, &pose, sizeof(stg_pose_t)); ! //this->pose.a = normalize(this->pose.a); //double hitx, hity; *************** *** 1000,1005 **** if( dx || dy || dz || da ) { ! stg_pose_t pose; ! GetPose( &pose ); pose.x += dx; pose.y += dy; --- 965,969 ---- if( dx || dy || dz || da ) { ! stg_pose_t pose = GetPose(); pose.x += dx; pose.y += dy; *************** *** 1007,1022 **** pose.a += da; ! SetPose( &pose ); } } ! void StgModel::AddToPose( stg_pose_t* pose ) { ! assert( pose ); ! this->AddToPose( pose->x, pose->y, pose->z, pose->a ); } ! void StgModel::SetGeom( stg_geom_t* geom ) { //printf( "MODEL \"%s\" SET GEOM (%.2f %.2f %.2f)[%.2f %.2f]\n", --- 971,985 ---- pose.a += da; ! SetPose( pose ); } } ! void StgModel::AddToPose( stg_pose_t pose ) { ! this->AddToPose( pose.x, pose.y, pose.z, pose.a ); } ! void StgModel::SetGeom( stg_geom_t geom ) { //printf( "MODEL \"%s\" SET GEOM (%.2f %.2f %.2f)[%.2f %.2f]\n", *************** *** 1025,1035 **** // geom->size.x, geom->size.y ); - assert(geom); - UnMap(); ! memcpy( &this->geom, geom, sizeof(stg_geom_t)); ! StgBlock::ScaleList( blocks, &geom->size ); body_dirty = true; --- 988,996 ---- // geom->size.x, geom->size.y ); UnMap(); ! this->geom = geom; ! StgBlock::ScaleList( blocks, &geom.size ); body_dirty = true; *************** *** 1143,1154 **** } - void StgModel::GetPose( stg_pose_t* dest ) - { - assert(dest); - memcpy( dest, &this->pose, sizeof(stg_pose_t)); - } - // set the pose of model in global coordinates ! void StgModel::SetGlobalPose( stg_pose_t* gpose ) { if( this->parent == NULL ) --- 1104,1109 ---- } // set the pose of model in global coordinates ! void StgModel::SetGlobalPose( stg_pose_t gpose ) { if( this->parent == NULL ) *************** *** 1160,1166 **** { stg_pose_t lpose; ! memcpy( &lpose, gpose, sizeof(lpose) ); this->parent->GlobalToLocal( &lpose ); ! this->SetPose( &lpose ); } --- 1115,1121 ---- { stg_pose_t lpose; ! memcpy( &lpose, &gpose, sizeof(lpose) ); this->parent->GlobalToLocal( &lpose ); ! this->SetPose( lpose ); } *************** *** 1255,1264 **** // shift the edge ray vector by the local change in pose ! stg_pose_t raypose; ! stg_pose_sum( &raypose, posedelta, &edgepose ); // raytrace in local coordinates stg_raytrace_sample_t sample; ! Raytrace( raypose, range, (stg_block_match_func_t)collision_match, --- 1210,1219 ---- // shift the edge ray vector by the local change in pose ! // stg_pose_t raypose; ! // stg_pose_sum( &raypose, posedelta, &edgepose ); // raytrace in local coordinates stg_raytrace_sample_t sample; ! Raytrace( pose_sum( *posedelta, edgepose), range, (stg_block_match_func_t)collision_match, *************** *** 1325,1329 **** stg_pose_t newpose; stg_pose_sum( &newpose, &this->pose, &p ); ! this->SetPose( &newpose ); } --- 1280,1284 ---- stg_pose_t newpose; stg_pose_sum( &newpose, &this->pose, &p ); ! this->SetPose( newpose ); } Index: block.cc =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/block.cc,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** block.cc 1 Feb 2008 03:11:01 -0000 1.3 --- block.cc 18 Feb 2008 03:52:30 -0000 1.4 *************** *** 141,145 **** glDepthMask(GL_FALSE); DrawTop(); ! //DrawSides(); glDepthMask(GL_TRUE); --- 141,145 ---- glDepthMask(GL_FALSE); DrawTop(); ! //DrawSides(); // skip this in 2d mode - faster! glDepthMask(GL_TRUE); *************** *** 165,183 **** // update the global coordinate list ! stg_pose_t gpose; ! bzero(&gpose,sizeof(gpose)); for( unsigned int p=0; p<pt_count; p++ ) { ! gpose.x = pts[p].x; ! gpose.y = pts[p].y; ! gpose.z = zmin; ! 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].x = (int32_t)floor(gpose.x*ppm); ! pts_global[p].y = (int32_t)floor(gpose.y*ppm); PRINT_DEBUG2("loc [%.2f %.2f]", --- 165,182 ---- // update the global coordinate list ! ! stg_point3_t global; for( unsigned int p=0; p<pt_count; p++ ) { ! stg_point3_t local; ! local.x = pts[p].x; ! local.y = pts[p].y; ! local.z = zmin; ! global = mod->LocalToGlobal( local ); ! ! pts_global[p].x = (int32_t)floor(global.x*ppm); ! pts_global[p].y = (int32_t)floor(global.y*ppm); PRINT_DEBUG2("loc [%.2f %.2f]", *************** *** 192,197 **** // store the block's global vertical bounds for inspection by the // raytracer ! global_zmin = gpose.z; ! global_zmax = gpose.z + (zmax-zmin); stg_render_info_t render_info; --- 191,196 ---- // store the block's global vertical bounds for inspection by the // raytracer ! global_zmin = global.z; ! global_zmax = global.z + (zmax-zmin); stg_render_info_t render_info; Index: world.cc =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/world.cc,v retrieving revision 1.6 retrieving revision 1.7 diff -C2 -d -r1.6 -r1.7 *** world.cc 12 Feb 2008 02:53:53 -0000 1.6 --- world.cc 18 Feb 2008 03:52:30 -0000 1.7 *************** *** 51,61 **** #include "stage_internal.hh" - const double STG_DEFAULT_WORLD_PPM = 50; // 2cm pixels - const stg_msec_t STG_DEFAULT_WORLD_INTERVAL_REAL = 100; ///< real time between updates - const stg_msec_t STG_DEFAULT_WORLD_INTERVAL_SIM = 100; ///< duration of sim timestep - const uint32_t RBITS = 6; // regions contain (2^RBITS)^2 pixels - const uint32_t SBITS = 5; // superregions contain (2^SBITS)^2 regions - const uint32_t SRBITS = RBITS+SBITS; // static data members --- 51,56 ---- #include "stage_internal.hh" + #include "region.hh" // static data members *************** *** 63,67 **** bool StgWorld::quit_all = false; - static guint PointIntHash( stg_point_int_t* pt ) { --- 58,61 ---- *************** *** 74,226 **** } - typedef struct - { - GSList* list; - } stg_cell_t; - - class Stg::Region - { - friend class SuperRegion; - - private: - static const uint32_t REGIONWIDTH = 1<<RBITS; - static const uint32_t REGIONSIZE = REGIONWIDTH*REGIONWIDTH; - - stg_cell_t cells[REGIONSIZE]; - - public: - uint32_t count; // number of blocks rendered into these cells - - Region() - { - bzero( cells, REGIONSIZE * sizeof(stg_cell_t)); - count = 0; - } - - stg_cell_t* GetCell( int32_t x, int32_t y ) - { - uint32_t index = x + (y*REGIONWIDTH); - assert( index >=0 ); - assert( index < REGIONSIZE ); - return &cells[index]; - } - - // add a block to a region cell specified in REGION coordinates - void AddBlock( StgBlock* block, int32_t x, int32_t y, unsigned int* count2 ) - { - stg_cell_t* cell = GetCell( x, y ); - cell->list = g_slist_prepend( cell->list, block ); - block->RecordRenderPoint( &cell->list, cell->list, &this->count, count2 ); - count++; - } - }; - - - class Stg::SuperRegion - { - friend class StgWorld; - - private: - static const uint32_t SUPERREGIONWIDTH = 1<<SBITS; - static const uint32_t SUPERREGIONSIZE = SUPERREGIONWIDTH*SUPERREGIONWIDTH; - - Region regions[SUPERREGIONSIZE]; - uint32_t count; // number of blocks rendered into these regions - - stg_point_int_t origin; - - public: - - SuperRegion( int32_t x, int32_t y ) - { - count = 0; - origin.x = x; - origin.y = y; - //regions = new Region[SUPERREGIONSIZE]; - } - - ~SuperRegion() - { - //delete [] regions; - } - - // get the region x,y from the region array - Region* GetRegion( int32_t x, int32_t y ) - { - int32_t index = x + (y*SUPERREGIONWIDTH); - assert( index >=0 ); - assert( index < (int)SUPERREGIONSIZE ); - return ®ions[ index ]; - } - - // add a block to a cell specified in superregion coordinates - void AddBlock( StgBlock* block, int32_t x, int32_t y ) - { - GetRegion( x>>RBITS, y>>RBITS ) - ->AddBlock( block, - x - ((x>>RBITS)<<RBITS), - y - ((y>>RBITS)<<RBITS), - &count); - count++; - } - - // callback wrapper for Draw() - static void Draw_cb( gpointer dummykey, - SuperRegion* sr, - gpointer dummyval ) - { - sr->Draw(); - } - - void Draw() - { - glPushMatrix(); - glTranslatef( origin.x<<SRBITS, origin.y<<SRBITS,0 ); - - glColor3f( 1,0,0 ); - - for( unsigned int x=0; x<SUPERREGIONWIDTH; x++ ) - for( unsigned int y=0; y<SUPERREGIONWIDTH; y++ ) - { - Region* r = GetRegion(x,y); - - for( unsigned int p=0; p<Region::REGIONWIDTH; p++ ) - for( unsigned int q=0; q<Region::REGIONWIDTH; q++ ) - if( r->cells[p+(q*Region::REGIONWIDTH)].list ) - glRecti( p+(x<<RBITS), q+(y<<RBITS), - 1+p+(x<<RBITS), 1+q+(y<<RBITS) ); - } - - // outline regions - glColor3f( 0,1,0 ); - for( unsigned int x=0; x<SUPERREGIONWIDTH; x++ ) - for( unsigned int y=0; y<SUPERREGIONWIDTH; y++ ) - glRecti( x<<RBITS, y<<RBITS, - (x+1)<<RBITS, (y+1)<<RBITS ); - - // outline superregion - glColor3f( 0,0,1 ); - glRecti( 0,0, 1<<SRBITS, 1<<SRBITS ); - - glPopMatrix(); - } - - static void Floor_cb( gpointer dummykey, - SuperRegion* sr, - gpointer dummyval ) - { - sr->Floor(); - } - - void Floor() - { - glPushMatrix(); - glTranslatef( origin.x<<SRBITS, origin.y<<SRBITS,0 ); - glRecti( 0,0, 1<<SRBITS, 1<<SRBITS ); - glPopMatrix(); - } - - - }; SuperRegion* StgWorld::CreateSuperRegion( int32_t x, int32_t y ) --- 68,71 ---- *************** *** 250,254 **** double ppm ) { ! Initialize( token, interval_sim, interval_real, ppm );//, width, height ); } --- 95,99 ---- double ppm ) { ! Initialize( token, interval_sim, interval_real, ppm ); } *************** *** 294,303 **** (GEqualFunc)PointIntEqual ); - //this->CreateSuperRegion( -2, -2 ); - this->total_subs = 0; this->paused = true; this->destroy = false; ! for( unsigned int i=0; i<INTERVAL_LOG_LEN; i++ ) this->interval_log[i] = this->interval_real; --- 139,148 ---- (GEqualFunc)PointIntEqual ); this->total_subs = 0; this->paused = true; this->destroy = false; ! ! bzero( &this->extent, sizeof(this->extent)); ! for( unsigned int i=0; i<INTERVAL_LOG_LEN; i++ ) this->interval_log[i] = this->interval_real; *************** *** 498,501 **** --- 343,372 ---- } + void StgWorld::IdleUntilNextUpdateTime( int (*idler)(void) ) + { + // sleep until it's time to update + stg_usec_t timenow = RealTimeSinceStart(); + + /* printf( "\ntimesincestart %llu interval_real %llu interval_sim %llu real_time_next_update %llu\n", + timenow, + interval_real, + interval_sim, + real_time_next_update ); + */ + + while( timenow < real_time_next_update ) + { + (*idler)(); + usleep( 10000 ); + timenow = RealTimeSinceStart(); + } + + interval_log[updates%INTERVAL_LOG_LEN] = timenow - real_time_now; + + real_time_now = timenow; + real_time_next_update += interval_real; + } + + bool StgWorld::Update() { *************** *** 533,536 **** --- 404,417 ---- } + bool StgWorld::RealTimeUpdateWithIdler( int (*idler)(void) ) + + { + //PRINT_DEBUG( "StageWorld::RealTimeUpdate()" ); + bool updated = Update(); + IdleUntilNextUpdateTime( idler ); + + return updated; + } + void StgWorld::AddModel( StgModel* mod ) { *************** *** 781,790 **** void StgWorld::Reload( void ) { - // can't reload the file yet - need to hack on the worldfile class. - //wf->load( NULL ); - - // ask every model to load itself from the file database - //g_hash_table_foreach( this->models_by_id, stg_model_reload_cb, NULL ); - ForEachModel( _reload_cb, NULL ); } --- 662,665 ---- *************** *** 826,829 **** --- 701,716 ---- //printf( "Creating super region [ %d %d ]\n", sup.x, sup.y ); sr = rinfo->world->CreateSuperRegion( sup.x, sup.y ); + + // the bounds of the world have changed + stg_point3_t pt; + pt.x = (sup.x << SRBITS) / rinfo->world->ppm; + pt.y = (sup.y << SRBITS) / rinfo->world->ppm; + pt.z = 0; + rinfo->world->Extend( pt ); // lower left corner of the new superregion + + pt.x = ((sup.x+1) << SRBITS) / rinfo->world->ppm; + pt.y = ((sup.y+1) << SRBITS) / rinfo->world->ppm; + pt.z = 0; + rinfo->world->Extend( pt ); // top right corner of the new superregion } *************** *** 842,852 **** } ! void StgWorld::DrawTree( bool drawall ) ! { ! g_hash_table_foreach( superregions, (GHFunc)SuperRegion::Draw_cb, NULL ); ! } ! ! void StgWorld::DrawFloor() { ! g_hash_table_foreach( superregions, (GHFunc)SuperRegion::Floor_cb, NULL ); } --- 729,739 ---- } ! void StgWorld::Extend( stg_point3_t pt ) { ! extent.x.min = MIN( extent.x.min, pt.x); ! extent.x.max = MAX( extent.x.max, pt.x ); ! extent.y.min = MIN( extent.y.min, pt.y ); ! extent.y.max = MAX( extent.y.max, pt.y ); ! extent.z.min = MIN( extent.z.min, pt.z ); ! extent.z.max = MAX( extent.z.max, pt.z ); } --- NEW FILE: gl.cc --- #include "stage_internal.hh" // transform the current coordinate frame by the given pose void Stg::gl_coord_shift( double x, double y, double z, double a ) { glTranslatef( x,y,z ); glRotatef( rtod(a), 0,0,1 ); } // transform the current coordinate frame by the given pose void Stg::gl_pose_shift( stg_pose_t* pose ) { gl_coord_shift( pose->x, pose->y, pose->z, pose->a ); } // TODO - this could be faster, but we don't draw a lot of text void Stg::gl_draw_string( float x, float y, float z, char *str ) { char *c; glRasterPos3f(x, y,z); for (c=str; *c != '\0'; c++) glutBitmapCharacter( GLUT_BITMAP_HELVETICA_12, *c); } void Stg::gl_draw_grid( stg_bounds3d_t vol ) { glBegin(GL_LINES); for( int i = floor(vol.x.min); i < vol.x.max; i++) { glVertex2f( i, vol.y.min ); glVertex2f( i, vol.y.max ); } for( int i = floor(vol.y.min); i < vol.y.max; i++) { glVertex2f( vol.x.min, i ); glVertex2f( vol.x.max, i ); } glEnd(); char str[16]; for( int i = floor(vol.x.min); i < vol.x.max; i++) { snprintf( str, 16, "%d", (int)i ); gl_draw_string( i, 0, 0.01, str ); } for( int i = floor(vol.y.min); i < vol.y.max; i++) { snprintf( str, 16, "%d", (int)i ); gl_draw_string( 0, i, 0.01, str ); } } Index: model_load.cc =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/model_load.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** model_load.cc 15 Jan 2008 01:16:49 -0000 1.2 --- model_load.cc 18 Feb 2008 03:52:30 -0000 1.3 *************** *** 44,48 **** geom.pose.y = wf->ReadTupleLength(this->id, "origin", 1, geom.pose.y ); geom.pose.a = wf->ReadTupleAngle(this->id, "origin", 2, geom.pose.a ); ! this->SetGeom( &geom ); } --- 44,48 ---- geom.pose.y = wf->ReadTupleLength(this->id, "origin", 1, geom.pose.y ); geom.pose.a = wf->ReadTupleAngle(this->id, "origin", 2, geom.pose.a ); ! this->SetGeom( geom ); } *************** *** 54,58 **** geom.pose.z = wf->ReadTupleLength(this->id, "origin4", 2, geom.pose.z ); geom.pose.a = wf->ReadTupleAngle(this->id, "origin4", 3, geom.pose.a ); ! this->SetGeom( &geom ); } --- 54,58 ---- geom.pose.z = wf->ReadTupleLength(this->id, "origin4", 2, geom.pose.z ); geom.pose.a = wf->ReadTupleAngle(this->id, "origin4", 3, geom.pose.a ); ! this->SetGeom( geom ); } *************** *** 62,66 **** geom.size.x = wf->ReadTupleLength(this->id, "size", 0, geom.size.x ); geom.size.y = wf->ReadTupleLength(this->id, "size", 1, geom.size.y ); ! this->SetGeom( &geom ); } --- 62,66 ---- geom.size.x = wf->ReadTupleLength(this->id, "size", 0, geom.size.x ); geom.size.y = wf->ReadTupleLength(this->id, "size", 1, geom.size.y ); ! this->SetGeom( geom ); } *************** *** 71,75 **** geom.size.y = wf->ReadTupleLength(this->id, "size3", 1, geom.size.y ); geom.size.z = wf->ReadTupleLength(this->id, "size3", 2, geom.size.z ); ! this->SetGeom( &geom ); } --- 71,75 ---- geom.size.y = wf->ReadTupleLength(this->id, "size3", 1, geom.size.y ); geom.size.z = wf->ReadTupleLength(this->id, "size3", 2, geom.size.z ); ! this->SetGeom( geom ); } *************** *** 80,84 **** pose.y = wf->ReadTupleLength(this->id, "pose", 1, pose.y ); pose.a = wf->ReadTupleAngle(this->id, "pose", 2, pose.a ); ! this->SetPose( &pose ); } --- 80,84 ---- pose.y = wf->ReadTupleLength(this->id, "pose", 1, pose.y ); pose.a = wf->ReadTupleAngle(this->id, "pose", 2, pose.a ); ! this->SetPose( pose ); } *************** *** 91,95 **** pose.a = wf->ReadTupleAngle( this->id, "pose4", 3, pose.a ); ! this->SetPose( &pose ); } --- 91,95 ---- pose.a = wf->ReadTupleAngle( this->id, "pose4", 3, pose.a ); ! this->SetPose( pose ); } *************** *** 100,104 **** vel.y = wf->ReadTupleLength(this->id, "velocity", 1, vel.y ); vel.a = wf->ReadTupleAngle(this->id, "velocity", 3, vel.a ); ! this->SetVelocity( &vel ); } --- 100,104 ---- vel.y = wf->ReadTupleLength(this->id, "velocity", 1, vel.y ); vel.a = wf->ReadTupleAngle(this->id, "velocity", 3, vel.a ); ! this->SetVelocity( vel ); } *************** *** 110,114 **** vel.z = wf->ReadTupleLength(this->id, "velocity4", 2, vel.z ); vel.a = wf->ReadTupleAngle(this->id, "velocity4", 3, vel.a ); ! this->SetVelocity( &vel ); } --- 110,114 ---- vel.z = wf->ReadTupleLength(this->id, "velocity4", 2, vel.z ); vel.a = wf->ReadTupleAngle(this->id, "velocity4", 3, vel.a ); ! this->SetVelocity( vel ); } Index: model_ranger.cc =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/model_ranger.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** model_ranger.cc 15 Jan 2008 01:16:49 -0000 1.2 --- model_ranger.cc 18 Feb 2008 03:52:30 -0000 1.3 *************** *** 111,115 **** stg_geom_t geom; memset( &geom, 0, sizeof(geom)); // no size ! this->SetGeom( &geom ); samples = NULL; --- 111,115 ---- stg_geom_t geom; memset( &geom, 0, sizeof(geom)); // no size ! this->SetGeom( geom ); samples = NULL; Index: model_position.cc =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/model_position.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** model_position.cc 15 Jan 2008 01:16:49 -0000 1.2 --- model_position.cc 18 Feb 2008 03:52:30 -0000 1.3 *************** *** 96,100 **** stg_velocity_t vel; memset( &vel, 0, sizeof(vel)); ! this->SetVelocity( &vel ); this->SetBlobReturn( TRUE ); --- 96,100 ---- stg_velocity_t vel; memset( &vel, 0, sizeof(vel)); ! this->SetVelocity( vel ); this->SetBlobReturn( TRUE ); *************** *** 391,395 **** // this->velocity.a ); ! this->SetVelocity( &vel ); } --- 391,395 ---- // this->velocity.a ); ! this->SetVelocity( vel ); } Index: model_fiducial.cc =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/model_fiducial.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** model_fiducial.cc 15 Jan 2008 01:16:49 -0000 1.2 --- model_fiducial.cc 18 Feb 2008 03:52:30 -0000 1.3 *************** *** 76,80 **** stg_geom_t geom; memset( &geom, 0, sizeof(geom)); ! SetGeom( &geom ); // default parameters --- 76,80 ---- stg_geom_t geom; memset( &geom, 0, sizeof(geom)); ! SetGeom( geom ); // default parameters Index: Makefile.manual =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/Makefile.manual,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** Makefile.manual 1 Feb 2008 03:11:01 -0000 1.4 --- Makefile.manual 18 Feb 2008 03:52:30 -0000 1.5 *************** *** 11,14 **** --- 11,15 ---- block.cc \ canvas.cc \ + gl.cc \ glcolorstack.cc \ model.cc \ Index: stage.hh =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/stage.hh,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** stage.hh 1 Feb 2008 03:11:01 -0000 1.5 --- stage.hh 18 Feb 2008 03:52:30 -0000 1.6 *************** *** 276,279 **** --- 276,287 ---- double max; //< largest value in range } stg_bounds_t; + + /** bound a volume along the x,y,z axes */ + typedef struct + { + stg_bounds_t x; //< volume extent along x axis + stg_bounds_t y; //< volume extent along y axis + stg_bounds_t z; //< volume extent along z axis + } stg_bounds3d_t; /** bound a range of range values, from min to max *************** *** 298,308 **** } stg_fov_t; ! ! /** define a point on the plane */ typedef struct { stg_meters_t x, y; } stg_point_t; ! /** define an integer point on the plane --- 306,321 ---- } stg_fov_t; ! ! /** define a point on a 2d plane */ typedef struct { stg_meters_t x, y; } stg_point_t; ! ! /** define a point in 3d space */ ! typedef struct ! { ! stg_meters_t x, y, z; ! } stg_point3_t; /** define an integer point on the plane *************** *** 470,473 **** --- 483,489 ---- copy the result into result. */ void stg_pose_sum( stg_pose_t* result, stg_pose_t* p1, stg_pose_t* p2 ); + + /** returns the sum of [p1] + [p2], in [p1]'s coordinate system */ + stg_pose_t pose_sum( stg_pose_t p1, stg_pose_t p2 ); // PRETTY PRINTING ------------------------------------------------- *************** *** 511,514 **** --- 527,531 ---- + void gl_draw_grid( stg_bounds3d_t vol ); void gl_pose_shift( stg_pose_t* pose ); void gl_coord_shift( double x, double y, double z, double a ); *************** *** 979,983 **** Worldfile* wf; ///< If set, points to the worldfile used to create this world bool graphics; ! //StgBlockGrid* bgrid; public: --- 996,1003 ---- Worldfile* wf; ///< If set, points to the worldfile used to create this world bool graphics; ! stg_bounds3d_t extent; ! ! /** Enlarge the bounding volume to include this point */ ! void Extend( stg_point3_t pt ); public: *************** *** 998,1001 **** --- 1018,1022 ---- stg_usec_t RealTimeSinceStart(void); void PauseUntilNextUpdateTime(void); + void IdleUntilNextUpdateTime( int (*idler)(void) ); stg_usec_t GetSimInterval(){ return interval_sim; }; *************** *** 1008,1011 **** --- 1029,1034 ---- virtual bool Update(void); virtual bool RealTimeUpdate(void); + virtual bool RealTimeUpdateWithIdler( int (*idler)(void) ); + virtual void AddModel( StgModel* mod ); virtual void RemoveModel( StgModel* mod ); *************** *** 1026,1038 **** StgModel* GetModel( const stg_id_t id ); StgModel* GetModel( const char* name ); ! GList* GetRayList(){ return ray_list; }; void ClearRays(); - void DrawTree( bool leaves ); - void DrawFloor(); void ClockString( char* str, size_t maxlen ); void ForEachModel( GHFunc func, void* arg ) { g_hash_table_foreach( models_by_id, func, arg ); }; --- 1049,1061 ---- StgModel* GetModel( const stg_id_t id ); StgModel* GetModel( const char* name ); ! GList* GetRayList(){ return ray_list; }; void ClearRays(); void ClockString( char* str, size_t maxlen ); + stg_bounds3d_t GetExtent(){ return extent; }; + void ForEachModel( GHFunc func, void* arg ) { g_hash_table_foreach( models_by_id, func, arg ); }; *************** *** 1273,1277 **** /* set the velocity of a model in the global coordinate system */ ! void SetGlobalVelocity( stg_velocity_t* gvel ); /** subscribe to a model's data */ --- 1296,1300 ---- /* set the velocity of a model in the global coordinate system */ ! void SetGlobalVelocity( stg_velocity_t gvel ); /** subscribe to a model's data */ *************** *** 1282,1295 **** /** set the pose of model in global coordinates */ ! void SetGlobalPose( stg_pose_t* gpose ); /** set a model's velocity in its parent's coordinate system */ ! void SetVelocity( stg_velocity_t* vel ); /** set a model's pose in its parent's coordinate system */ ! void SetPose( stg_pose_t* pose ); /** add values to a model's pose in its parent's coordinate system */ ! void AddToPose( stg_pose_t* pose ); /** add values to a model's pose in its parent's coordinate system */ --- 1305,1318 ---- /** set the pose of model in global coordinates */ ! void SetGlobalPose( stg_pose_t gpose ); /** set a model's velocity in its parent's coordinate system */ ! void SetVelocity( stg_velocity_t vel ); /** set a model's pose in its parent's coordinate system */ ! void SetPose( stg_pose_t pose ); /** add values to a model's pose in its parent's coordinate system */ ! void AddToPose( stg_pose_t pose ); /** add values to a model's pose in its parent's coordinate system */ *************** *** 1297,1301 **** /** set a model's geometry (size and center offsets) */ ! void SetGeom( stg_geom_t* src ); /** set a model's geometry (size and center offsets) */ --- 1320,1324 ---- /** set a model's geometry (size and center offsets) */ ! void SetGeom( stg_geom_t src ); /** set a model's geometry (size and center offsets) */ *************** *** 1318,1324 **** /** Get the pose of a model in its parent's coordinate system */ - void GetPose( stg_pose_t* dest ); - - /** Get the pose of a model in its parent's coordinate system */ stg_pose_t GetPose(){ return pose; } --- 1341,1344 ---- *************** *** 1448,1452 **** world coordinate system. Overwrites [pose] with the new coordinate. */ ! void LocalToGlobal( stg_pose_t* pose ); /** returns the first descendent of this model that is unsubscribed --- 1468,1480 ---- world coordinate system. Overwrites [pose] with the new coordinate. */ ! //void LocalToGlobal( stg_pose_t* pose ); ! ! /** Return the global pose (i.e. pose in world coordinates) of a ! pose specified in the model's local coordinate system */ ! stg_pose_t LocalToGlobal( stg_pose_t pose ); ! ! /** Return the 3d point in world coordinates of a 3d point ! specified in the model's local coordinate system */ ! stg_point3_t LocalToGlobal( stg_point3_t local ); /** returns the first descendent of this model that is unsubscribed *************** *** 1586,1589 **** --- 1614,1618 ---- void DrawRays(); void ClearRays(); + void DrawGlobalGrid(); public: *************** *** 1654,1657 **** --- 1683,1689 ---- virtual void PopColor() { canvas->PopColor(); } + + void DrawTree( bool leaves ); + void DrawFloor(); }; Index: canvas.cc =================================================================== RCS file: /cvsroot/playerstage/code/stage/libstage/canvas.cc,v retrieving revision 1.6 retrieving revision 1.7 diff -C2 -d -r1.6 -r1.7 *** canvas.cc 12 Feb 2008 02:53:53 -0000 1.6 --- canvas.cc 18 Feb 2008 03:52:30 -0000 1.7 *************** *** 97,101 **** glEnable(GL_DITHER); ! glClearColor ( 0.7, 0.7, 0.8, 1.0); if( mod ) // we clicked on a root model --- 97,102 ---- glEnable(GL_DITHER); ! //glClearColor ( 0.7, 0.7, 0.8, 1.0); ! glClearColor ( 1,1,1,1 ); if( mod ) // we clicked on a root model *************** *** 322,325 **** --- 323,333 ---- } + void StgCanvas::DrawGlobalGrid() + { + PushColor( 0,0,0,0.2 ); + gl_draw_grid( world->GetExtent() ); + PopColor(); + } + void StgCanvas::draw() { *************** *** 332,336 **** // set gl state that won't change every redraw ! glClearColor ( 0.7, 0.7, 0.8, 1.0); glDisable(GL_LIGHTING); glEnable (GL_DEPTH_TEST); --- 340,345 ---- // set gl state that won't change every redraw ! //glClearColor ( 0.7, 0.7, 0.8, 1.0); ! glClearColor ( 1,1,1,1 ); glDisable(GL_LIGHTING); glEnable (GL_DEPTH_TEST); *************** *** 347,351 **** gl_font( FL_HELVETICA, 12 ); ! double zclip = 20 * scale; //hypot(world->Width(), world->Height()) * scale; double pixels_width = w(); double pixels_height = h(); --- 356,360 ---- gl_font( FL_HELVETICA, 12 ); ! //double zclip = 20 * scale; //hypot(world->Width(), world->Height()) * scale; double pixels_width = w(); double pixels_height = h(); *************** *** 356,362 **** glOrtho( -pixels_width/2.0, pixels_width/2.0, -pixels_height/2.0, pixels_height/2.0, ! -zclip, zclip ); // set the modelview matrix --- 365,373 ---- + stg_bounds3d_t extent = world->GetExtent(); + glOrtho( -pixels_width/2.0, pixels_width/2.0, -pixels_height/2.0, pixels_height/2.0, ! extent.y.min*scale, extent.y.max*scale ); // set the modelview matrix *************** *** 367,371 **** glScalef( scale, scale, scale ); glTranslatef( -panx/scale, -pany/scale, 0 ); ! glRotatef( rtod(-stheta), fabs(cos(sphi)), 0, 0 ); glRotatef( rtod(sphi), 0,0,1 ); // rotate about z - yaw --- 378,383 ---- glScalef( scale, scale, scale ); glTranslatef( -panx/scale, -pany/scale, 0 ); ! // .. from here ! glRotatef( rtod(-stheta), fabs(cos(sphi)), 0, 0 ); glRotatef( rtod(sphi), 0,0,1 ); // rotate about z - yaw *************** *** 413,426 **** // draw the world size rectangle in white, using the polygon offset // so it doesn't z-fight with the models ! glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); ! glEnable(GL_POLYGON_OFFSET_FILL); ! glPolygonOffset(1.0, 1.0); ! glColor3f( 1,1,1 ); ! glPushMatrix(); glScalef( 1.0/world->Resolution(), 1.0/world->Resolution(), 0 ); ! ((StgWorldGui*)world)->DrawFloor(); ! glDisable(GL_POLYGON_OFFSET_FILL); if( (showflags & STG_SHOW_QUADTREE) || (showflags & STG_SHOW_OCCUPANCY) ) --- 425,440 ---- // draw the world size rectangle in white, using the polygon offset // so it doesn't z-fight with the models ! //glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); ! //glEnable(GL_POLYGON_OFFSET_FILL); ! //glPolygonOffset(1.0, 1.0); ! //glColor3f( 1,1,1 ); ! //colorstack.Push(1,1,1); ! glPushMatrix(); glScalef( 1.0/world->Resolution(), 1.0/world->Resolution(), 0 ); ! //((StgWorldGui*)world)->DrawFloor(); ! ! //glDisable(GL_POLYGON_OFFSET_FILL); if( (showflags & STG_SHOW_QUADTREE) || (showflags & STG_SHOW_OCCUPANCY) ) *************** *** 444,447 **** --- 458,464 ---- glPopMatrix(); + if( showflags & STG_SHOW_GRID ) + DrawGlobalGrid(); + for( GList* it=selected_models; it; it=it->next ) ((StgModel*)it->data)->DrawSelected(); |