Skip to content
Browse files

API changes! removed the Stg prefix from all class names, and renamed…

… some key classes, eg. Stg::stg_pose_t is now Stg::Pose
  • Loading branch information...
1 parent 2b31d43 commit 3680786ba7d9c1fff520f358493c1cddecd49b0c rtv committed Jan 5, 2009
Showing with 3,319 additions and 3,367 deletions.
  1. +15 −18 examples/ctrl/fasr.cc
  2. +2 −2 examples/ctrl/lasernoise.cc
  3. +4 −4 examples/ctrl/sink.cc
  4. +5 −5 examples/ctrl/source.cc
  5. +10 −10 examples/ctrl/wander.cc
  6. +11 −11 examples/libstage/stest.cc
  7. +9 −9 libstage/ancestor.cc
  8. +29 −29 libstage/block.cc
  9. +7 −7 libstage/blockgrid.cc
  10. +24 −24 libstage/blockgroup.cc
  11. +86 −83 libstage/canvas.cc
  12. +2 −2 libstage/gl.cc
  13. +6 −6 libstage/main.cc
  14. +533 −552 libstage/model.cc
  15. +702 −646 libstage/model.hh
  16. +14 −14 libstage/model_blinkenlight.cc
  17. +22 −22 libstage/model_blobfinder.cc
  18. +4 −4 libstage/model_callbacks.cc
  19. +15 −15 libstage/model_camera.cc
  20. +27 −27 libstage/model_fiducial.cc
  21. +31 −32 libstage/model_laser.cc
  22. +62 −90 libstage/model_load.cc
  23. +35 −35 libstage/model_position.cc
  24. +11 −11 libstage/model_props.cc
  25. +315 −315 libstage/model_ranger.cc
  26. +3 −3 libstage/model_wifi.cc
  27. +2 −2 libstage/region.cc
  28. +5 −5 libstage/region.hh
  29. +4 −4 libstage/resource.cc
  30. +5 −5 libstage/stage.cc
  31. +695 −752 libstage/stage.hh
  32. +25 −25 libstage/stage_internal.hh
  33. +20 −20 libstage/test.cc
  34. +16 −16 libstage/typetable.cc
  35. +1 −1 libstage/waypoint.cc
  36. +485 −484 libstage/world.cc
  37. +49 −49 libstage/worldgui.cc
  38. +2 −2 libstageplugin/p_fiducial.cc
  39. +1 −1 libstageplugin/p_graphics3d.cc
  40. +2 −2 libstageplugin/p_gripper.cc
  41. +2 −2 libstageplugin/p_laser.cc
  42. +1 −1 libstageplugin/p_localize.cc
  43. +3 −3 libstageplugin/p_map.cc
  44. +2 −2 libstageplugin/p_position.cc
  45. +1 −1 libstageplugin/p_ptz.cc
  46. +4 −4 libstageplugin/p_simulation.cc
  47. +10 −10 worlds/benchmark/expand.cc
View
33 examples/ctrl/fasr.cc
@@ -29,37 +29,34 @@ double need[4][4] = {
typedef struct
{
- StgModelPosition* pos;
- StgModelLaser* laser;
- StgModelRanger* ranger;
- StgModelBlobfinder* blobfinder;
-
- StgModel *source, *sink;
-
+ ModelPosition* pos;
+ ModelLaser* laser;
+ ModelRanger* ranger;
+ ModelBlobfinder* blobfinder;
+ Model *source, *sink;
int avoidcount, randcount;
-
int work_get, work_put;
} robot_t;
-int LaserUpdate( StgModel* mod, robot_t* robot );
-int PositionUpdate( StgModel* mod, robot_t* robot );
+int LaserUpdate( Model* mod, robot_t* robot );
+int PositionUpdate( Model* mod, robot_t* robot );
// Stage calls this when the model starts up
-extern "C" int Init( StgModel* mod )
+extern "C" int Init( Model* mod )
{
robot_t* robot = new robot_t;
robot->work_get = 0;
robot->work_put = 0;
- robot->pos = (StgModelPosition*)mod;
+ robot->pos = (ModelPosition*)mod;
- robot->laser = (StgModelLaser*)mod->GetModel( "laser:0" );
+ robot->laser = (ModelLaser*)mod->GetModel( "laser:0" );
assert( robot->laser );
robot->laser->Subscribe();
- robot->ranger = (StgModelRanger*)mod->GetModel( "ranger:0" );
+ robot->ranger = (ModelRanger*)mod->GetModel( "ranger:0" );
assert( robot->ranger );
//robot->ranger->Subscribe();
@@ -95,7 +92,7 @@ extern "C" int Init( StgModel* mod )
}
// inspect the laser data and decide what to do
-int LaserUpdate( StgModel* mod, robot_t* robot )
+int LaserUpdate( Model* mod, robot_t* robot )
{
// get the data
uint32_t sample_count=0;
@@ -178,7 +175,7 @@ int LaserUpdate( StgModel* mod, robot_t* robot )
robot->avoidcount = 0;
robot->pos->SetXSpeed( cruisespeed );
- stg_pose_t pose = robot->pos->GetPose();
+ Pose pose = robot->pos->GetPose();
int x = (pose.x + 8) / 4;
int y = (pose.y + 8) / 4;
@@ -208,9 +205,9 @@ int LaserUpdate( StgModel* mod, robot_t* robot )
return 0;
}
-int PositionUpdate( StgModel* mod, robot_t* robot )
+int PositionUpdate( Model* mod, robot_t* robot )
{
- stg_pose_t pose = robot->pos->GetPose();
+ Pose pose = robot->pos->GetPose();
//printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
// pose.x, pose.y, pose.z, pose.a );
View
4 examples/ctrl/lasernoise.cc
@@ -21,7 +21,7 @@ double simple_normal_deviate( double mean, double stddev )
}
// process the laser data
-int LaserUpdate( StgModelLaser* mod, void* dummy )
+int LaserUpdate( ModelLaser* mod, void* dummy )
{
// get the data
uint32_t sample_count=0;
@@ -36,7 +36,7 @@ int LaserUpdate( StgModelLaser* mod, void* dummy )
// Stage calls this when the model starts up. we just add a callback to
// the model that gets called just after the sensor update is done.
-extern "C" int Init( StgModel* mod )
+extern "C" int Init( Model* mod )
{
mod->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, NULL );
return 0; // ok
View
8 examples/ctrl/sink.cc
@@ -3,22 +3,22 @@ using namespace Stg;
const int INTERVAL = 50;
-int Update( StgModel* mod, void* dummy );
+int Update( Model* mod, void* dummy );
// Stage calls this when the model starts up
-extern "C" int Init( StgModel* mod )
+extern "C" int Init( Model* mod )
{
for( int i=0; i<5; i++ )
- mod->PushFlag( new StgFlag( stg_color_pack( 1,1,0,0), 0.5 ) );
+ mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0), 0.5 ) );
mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL );
return 0; //ok
}
// inspect the laser data and decide what to do
-int Update( StgModel* mod, void* dummy )
+int Update( Model* mod, void* dummy )
{
// protect access to this model from other controllers
mod->Lock();
View
10 examples/ctrl/source.cc
@@ -3,29 +3,29 @@ using namespace Stg;
const int INTERVAL = 50;
-int Update( StgModel* mod, void* dummy );
+int Update( Model* mod, void* dummy );
const double flagsz = 0.4;
// Stage calls this when the model starts up
-extern "C" int Init( StgModel* mod )
+extern "C" int Init( Model* mod )
{
for( int i=0; i<5; i++ )
- mod->PushFlag( new StgFlag( stg_color_pack( 1,1,0,0 ), flagsz ) );
+ mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0 ), flagsz ) );
mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL );
return 0; //ok
}
// inspect the laser data and decide what to do
-int Update( StgModel* mod, void* dummy )
+int Update( Model* mod, void* dummy )
{
// protect access to this model from other controllers
mod->Lock();
if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 )
- mod->PushFlag( new StgFlag( stg_color_pack( 1,1,0,0), flagsz ) );
+ mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0), flagsz ) );
mod->Unlock();
View
20 examples/ctrl/wander.cc
@@ -11,24 +11,24 @@ const int avoidduration = 10;
typedef struct
{
- StgModelPosition* pos;
- StgModelLaser* laser;
+ ModelPosition* pos;
+ ModelLaser* laser;
int avoidcount, randcount;
} robot_t;
-int LaserUpdate( StgModel* mod, robot_t* robot );
-int PositionUpdate( StgModel* mod, robot_t* robot );
+int LaserUpdate( Model* mod, robot_t* robot );
+int PositionUpdate( Model* mod, robot_t* robot );
// Stage calls this when the model starts up
-extern "C" int Init( StgModel* mod )
+extern "C" int Init( Model* mod )
{
robot_t* robot = new robot_t;
robot->avoidcount = 0;
robot->randcount = 0;
- robot->pos = (StgModelPosition*)mod;
- robot->laser = (StgModelLaser*)mod->GetModel( "laser:0" );
+ robot->pos = (ModelPosition*)mod;
+ robot->laser = (ModelLaser*)mod->GetModel( "laser:0" );
robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot );
robot->laser->Subscribe(); // starts the laser
@@ -37,7 +37,7 @@ extern "C" int Init( StgModel* mod )
// inspect the laser data and decide what to do
-int LaserUpdate( StgModel* mod, robot_t* robot )
+int LaserUpdate( Model* mod, robot_t* robot )
{
// get the data
uint32_t sample_count=0;
@@ -123,9 +123,9 @@ int LaserUpdate( StgModel* mod, robot_t* robot )
return 0;
}
-int PositionUpdate( StgModel* mod, robot_t* robot )
+int PositionUpdate( Model* mod, robot_t* robot )
{
- stg_pose_t pose = robot->pos->GetPose();
+ Pose pose = robot->pos->GetPose();
printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
pose.x, pose.y, pose.z, pose.a );
View
22 examples/libstage/stest.cc
@@ -21,11 +21,11 @@ double turnrate = M_PI/3.0;
typedef struct
{
- StgModelLaser* laser;
- StgModelPosition* position;
- StgModelRanger* ranger;
- StgModelFiducial* fiducial;
- StgModelBlobfinder* blobfinder;
+ ModelLaser* laser;
+ ModelPosition* position;
+ ModelRanger* ranger;
+ ModelFiducial* fiducial;
+ ModelBlobfinder* blobfinder;
} robot_t;
#define VSPEED 0.4 // meters per second
@@ -59,26 +59,26 @@ int main( int argc, char* argv[] )
{
char* base = "r";
sprintf( namebuf, "%s%d", base, i );
- robots[i].position = (StgModelPosition*)world.GetModel( namebuf );
+ robots[i].position = (ModelPosition*)world.GetModel( namebuf );
assert(robots[i].position);
robots[i].position->Subscribe();
-// robots[i].laser = (StgModelLaser*)
+// robots[i].laser = (ModelLaser*)
// robots[i].position->GetUnsubscribedModelOfType( "laser" );
// assert(robots[i].laser);
// robots[i].laser->Subscribe();
-// robots[i].fiducial = (StgModelFiducial*)
+// robots[i].fiducial = (ModelFiducial*)
// robots[i].position->GetUnsubscribedModelOfType( "fiducial" );
// assert(robots[i].fiducial);
// robots[i].fiducial->Subscribe();
- robots[i].ranger = (StgModelRanger*)
+ robots[i].ranger = (ModelRanger*)
robots[i].position->GetUnsubscribedModelOfType( "ranger" );
assert(robots[i].ranger);
robots[i].ranger->Subscribe();
-// robots[i].blobfinder = (StgModelBlobfinder*)
+// robots[i].blobfinder = (ModelBlobfinder*)
// robots[i].position->GetUnsubscribedModelOfType( "blobfinder" );
// assert(robots[i].blobfinder);
// robots[i].blobfinder->Subscribe();
@@ -96,7 +96,7 @@ int main( int argc, char* argv[] )
//continue;
- StgModelRanger* rgr = robots[i].ranger;
+ ModelRanger* rgr = robots[i].ranger;
if( rgr->samples == NULL )
continue;
View
18 libstage/ancestor.cc
@@ -1,6 +1,6 @@
#include "stage_internal.hh"
-StgAncestor::StgAncestor()
+Ancestor::Ancestor()
{
token = NULL;
children = NULL;
@@ -11,19 +11,19 @@ StgAncestor::StgAncestor()
debug = false;
}
-StgAncestor::~StgAncestor()
+Ancestor::~Ancestor()
{
if( children )
{
for( GList* it = children; it; it=it->next )
- delete (StgModel*)it->data;
+ delete (Model*)it->data;
g_list_free( children );
}
}
-void StgAncestor::AddChild( StgModel* mod )
+void Ancestor::AddChild( Model* mod )
{
// poke a name into the child
@@ -50,23 +50,23 @@ void StgAncestor::AddChild( StgModel* mod )
delete[] buf;
}
-void StgAncestor::RemoveChild( StgModel* mod )
+void Ancestor::RemoveChild( Model* mod )
{
child_type_counts[mod->type]--;
children = g_list_remove( children, mod );
}
-stg_pose_t StgAncestor::GetGlobalPose()
+Pose Ancestor::GetGlobalPose()
{
- stg_pose_t pose;
+ Pose pose;
bzero( &pose, sizeof(pose));
return pose;
}
-void StgAncestor::ForEachDescendant( stg_model_callback_t func, void* arg )
+void Ancestor::ForEachDescendant( stg_model_callback_t func, void* arg )
{
for( GList* it=children; it; it=it->next ) {
- StgModel* mod = (StgModel*)it->data;
+ Model* mod = (Model*)it->data;
func( mod, arg );
mod->ForEachDescendant( func, arg );
}
View
58 libstage/block.cc
@@ -1,12 +1,12 @@
#include "stage_internal.hh"
-//GPtrArray* StgBlock::global_verts = g_ptr_array_sized_new( 1024 );
+//GPtrArray* Block::global_verts = g_ptr_array_sized_new( 1024 );
/** Create a new block. A model's body is a list of these
blocks. The point data is copied, so pts can safely be freed
after calling this.*/
-StgBlock::StgBlock( StgModel* mod,
+Block::Block( Model* mod,
stg_point_t* pts,
size_t pt_count,
stg_meters_t zmin,
@@ -35,7 +35,7 @@ StgBlock::StgBlock( StgModel* mod,
}
/** A from-file constructor */
-StgBlock::StgBlock( StgModel* mod,
+Block::Block( Model* mod,
Worldfile* wf,
int entity)
: mod( mod ),
@@ -58,7 +58,7 @@ StgBlock::StgBlock( StgModel* mod,
}
-StgBlock::~StgBlock()
+Block::~Block()
{
if( mapped ) UnMap();
@@ -71,14 +71,14 @@ StgBlock::~StgBlock()
//g_ptr_array_remove( global_verts, this );
}
-stg_color_t StgBlock::GetColor()
+stg_color_t Block::GetColor()
{
return( inherit_color ? mod->color : color );
}
-StgModel* StgBlock::TestCollision()
+Model* Block::TestCollision()
{
//printf( "model %s block %p test collision...\n", mod->Token(), this );
@@ -93,14 +93,14 @@ StgModel* StgBlock::TestCollision()
// for every rendered into that cell
for( GSList* it = cell->list; it; it=it->next )
{
- StgBlock* testblock = (StgBlock*)it->data;
- StgModel* testmod = testblock->mod;
+ Block* testblock = (Block*)it->data;
+ Model* testmod = testblock->mod;
//printf( " testing block %p of model %s\n", testblock, testmod->Token() );
// if the tested model is an obstacle and it's not attached to this model
if( (testmod != this->mod) &&
- testmod->obstacle_return &&
+ testmod->vis.obstacle_return &&
!mod->IsRelated( testmod ))
{
//puts( "HIT");
@@ -114,13 +114,13 @@ StgModel* StgBlock::TestCollision()
}
-void StgBlock::RemoveFromCellArray( GPtrArray* ptrarray )
+void Block::RemoveFromCellArray( GPtrArray* ptrarray )
{
for( unsigned int i=0; i<ptrarray->len; i++ )
((Cell*)g_ptr_array_index(ptrarray, i))->RemoveBlock( this );
}
-void StgBlock::AddToCellArray( GPtrArray* ptrarray )
+void Block::AddToCellArray( GPtrArray* ptrarray )
{
for( unsigned int i=0; i<ptrarray->len; i++ )
((Cell*)g_ptr_array_index(ptrarray, i))->AddBlock( this );
@@ -134,12 +134,12 @@ void AppendCellToPtrArray( Cell* c, GPtrArray* a )
}
// used as a callback to gather an array of cells in a polygon
-void AddBlockToCell( Cell* c, StgBlock* block )
+void AddBlockToCell( Cell* c, Block* block )
{
c->AddBlock( block );
}
-void StgBlock::Map()
+void Block::Map()
{
// TODO - if called often, we may not need to generate each time
GenerateCandidateCells();
@@ -149,15 +149,15 @@ void StgBlock::Map()
mapped = true;
}
-void StgBlock::UnMap()
+void Block::UnMap()
{
RemoveFromCellArray( rendered_cells );
g_ptr_array_set_size( rendered_cells, 0 );
mapped = false;
}
-void StgBlock::SwitchToTestedCells()
+void Block::SwitchToTestedCells()
{
RemoveFromCellArray( rendered_cells );
AddToCellArray( candidate_cells );
@@ -170,15 +170,15 @@ void StgBlock::SwitchToTestedCells()
mapped = true;
}
-void StgBlock::GenerateCandidateCells()
+void Block::GenerateCandidateCells()
{
- stg_pose_t gpose = mod->GetGlobalPose();
+ Pose gpose = mod->GetGlobalPose();
// add local offset
gpose = pose_sum( gpose, mod->geom.pose );
- stg_size_t bgsize = mod->blockgroup.GetSize();
+ Size bgsize = mod->blockgroup.GetSize();
stg_point3_t bgoffset = mod->blockgroup.GetOffset();
stg_point3_t scale;
@@ -190,12 +190,12 @@ void StgBlock::GenerateCandidateCells()
g_ptr_array_set_size( candidate_cells, 0 );
// compute the global location of the first point
- stg_pose_t local( (pts[0].x - bgoffset.x) * scale.x ,
+ Pose local( (pts[0].x - bgoffset.x) * scale.x ,
(pts[0].y - bgoffset.y) * scale.y,
-bgoffset.z,
0 );
- stg_pose_t first_gpose, last_gpose;
+ Pose first_gpose, last_gpose;
first_gpose = last_gpose = pose_sum( gpose, local );
// store the block's absolute z bounds at this rendering
@@ -205,12 +205,12 @@ void StgBlock::GenerateCandidateCells()
// now loop from the the second to the last
for( unsigned int p=1; p<pt_count; p++ )
{
- stg_pose_t local( (pts[p].x - bgoffset.x) * scale.x ,
+ Pose local( (pts[p].x - bgoffset.x) * scale.x ,
(pts[p].y - bgoffset.y) * scale.y,
-bgoffset.z,
0 );
- stg_pose_t gpose2 = pose_sum( gpose, local );
+ Pose gpose2 = pose_sum( gpose, local );
// and render the shape of the block into the global cells
mod->world->ForEachCellInLine( last_gpose.x, last_gpose.y,
@@ -230,7 +230,7 @@ void StgBlock::GenerateCandidateCells()
}
-void StgBlock::DrawTop()
+void Block::DrawTop()
{
// draw the top of the block - a polygon at the highest vertical
// extent
@@ -240,7 +240,7 @@ void StgBlock::DrawTop()
glEnd();
}
-void StgBlock::DrawSides()
+void Block::DrawSides()
{
// construct a strip that wraps around the polygon
glBegin(GL_QUAD_STRIP);
@@ -255,15 +255,15 @@ void StgBlock::DrawSides()
glEnd();
}
-void StgBlock::DrawFootPrint()
+void Block::DrawFootPrint()
{
glBegin(GL_POLYGON);
for( unsigned int p=0; p<pt_count; p++ )
glVertex2f( pts[p].x, pts[p].y );
glEnd();
}
-void StgBlock::Draw( StgModel* mod )
+void Block::Draw( Model* mod )
{
// draw filled color polygons
stg_color_t col = inherit_color ? mod->color : color;
@@ -291,7 +291,7 @@ void StgBlock::Draw( StgModel* mod )
mod->PopColor();
}
-void StgBlock::DrawSolid()
+void Block::DrawSolid()
{
DrawSides();
DrawTop();
@@ -301,9 +301,9 @@ void StgBlock::DrawSolid()
//#define DEBUG 1
-void StgBlock::Load( Worldfile* wf, int entity )
+void Block::Load( Worldfile* wf, int entity )
{
- //printf( "StgBlock::Load entity %d\n", entity );
+ //printf( "Block::Load entity %d\n", entity );
if( pts )
stg_points_destroy( pts );
View
14 libstage/blockgrid.cc
@@ -7,7 +7,7 @@ static const uint32_t NSIZE = 1<<NBITS;
static const uint32_t NSQR = NSIZE*NSIZE;
static const uint32_t MASK = NSIZE-1;
-StgBlockGrid::StgBlockGrid( uint32_t width, uint32_t height )
+BlockGrid::BlockGrid( uint32_t width, uint32_t height )
{
this->width = width;
this->height = height;
@@ -16,7 +16,7 @@ StgBlockGrid::StgBlockGrid( uint32_t width, uint32_t height )
assert(this->cells);
}
-StgBlockGrid::~StgBlockGrid()
+BlockGrid::~BlockGrid()
{
for( uint32_t x=0; x<width; x++ )
for( uint32_t y=0; y<height; y++ )
@@ -29,7 +29,7 @@ StgBlockGrid::~StgBlockGrid()
//delete[] map;
}
-void StgBlockGrid::AddBlock( uint32_t x, uint32_t y, StgBlock* block )
+void BlockGrid::AddBlock( uint32_t x, uint32_t y, Block* block )
{
//printf( "add block %u %u\n", x, y );
@@ -40,7 +40,7 @@ void StgBlockGrid::AddBlock( uint32_t x, uint32_t y, StgBlock* block )
}
}
-void StgBlockGrid::RemoveBlock( uint32_t x, uint32_t y, StgBlock* block )
+void BlockGrid::RemoveBlock( uint32_t x, uint32_t y, Block* block )
{
//printf( "remove block %u %u\n", x, y );
@@ -50,22 +50,22 @@ void StgBlockGrid::RemoveBlock( uint32_t x, uint32_t y, StgBlock* block )
}
}
-GSList* StgBlockGrid::GetList( uint32_t x, uint32_t y )
+GSList* BlockGrid::GetList( uint32_t x, uint32_t y )
{
if( x < width && y < height )
return cells[ x+y*width ];
else
return NULL;
}
-void StgBlockGrid::GlobalRemoveBlock( StgBlock* block )
+void BlockGrid::GlobalRemoveBlock( Block* block )
{
for( uint32_t x=0; x<width; x++ )
for( uint32_t y=0; y<height; y++ )
RemoveBlock(x,y,block );
}
-void StgBlockGrid::Draw( bool drawall )
+void BlockGrid::Draw( bool drawall )
{
for( uint32_t x=0; x<width; x++ )
for( uint32_t y=0; y<height; y++ )
View
48 libstage/blockgroup.cc
@@ -21,7 +21,7 @@ BlockGroup::~BlockGroup()
Clear();
}
-void BlockGroup::AppendBlock( StgBlock* block )
+void BlockGroup::AppendBlock( Block* block )
{
blocks = g_list_append( blocks, block );
++count;
@@ -33,7 +33,7 @@ void BlockGroup::Clear()
{
while( blocks )
{
- delete (StgBlock*)blocks->data;
+ delete (Block*)blocks->data;
blocks = blocks->next;
}
@@ -45,17 +45,17 @@ void BlockGroup::Clear()
void BlockGroup::SwitchToTestedCells()
{
// confirm the tentative pose for all blocks
- LISTMETHOD( blocks, StgBlock*, SwitchToTestedCells );
+ LISTMETHOD( blocks, Block*, SwitchToTestedCells );
}
-StgModel* BlockGroup::TestCollision()
+Model* BlockGroup::TestCollision()
{
//printf( "blockgroup %p test collision...\n", this );
- StgModel* hitmod = NULL;
+ Model* hitmod = NULL;
for( GList* it=blocks; it; it = it->next )
- if( (hitmod = ((StgBlock*)it->data)->TestCollision()))
+ if( (hitmod = ((Block*)it->data)->TestCollision()))
break; // bail on the earliest collision
//printf( "blockgroup %p test collision done.\n", this );
@@ -76,12 +76,12 @@ void BlockGroup::CalcSize()
size.z = 0.0; // grow to largest z we see
if( blocks )
- ((StgBlock*)blocks->data)->mod->map_caches_are_invalid = true;
+ ((Block*)blocks->data)->mod->map_caches_are_invalid = true;
for( GList* it=blocks; it; it=it->next ) // examine all the blocks
{
// examine all the points in the polygon
- StgBlock* block = (StgBlock*)it->data;
+ Block* block = (Block*)it->data;
for( unsigned int p=0; p < block->pt_count; p++ )
{
@@ -105,21 +105,21 @@ void BlockGroup::CalcSize()
// normalize blocks
// for( GList* it = blocks; itl it=it->next )
- //((StgBlock*)it->data)->Normalize( size.x, size.y, size.z, offset.x
+ //((Block*)it->data)->Normalize( size.x, size.y, size.z, offset.x
}
void BlockGroup::Map()
{
- LISTMETHOD( blocks, StgBlock*, Map );
+ LISTMETHOD( blocks, Block*, Map );
}
void BlockGroup::UnMap()
{
- LISTMETHOD( blocks, StgBlock*, UnMap );
+ LISTMETHOD( blocks, Block*, UnMap );
}
-void BlockGroup::DrawSolid( const stg_geom_t & geom )
+void BlockGroup::DrawSolid( const Geom & geom )
{
glPushMatrix();
@@ -131,12 +131,12 @@ void BlockGroup::DrawSolid( const stg_geom_t & geom )
glTranslatef( -offset.x, -offset.y, -offset.z );
- LISTMETHOD( blocks, StgBlock*, DrawSolid );
+ LISTMETHOD( blocks, Block*, DrawSolid );
glPopMatrix();
}
-void BlockGroup::DrawFootPrint( const stg_geom_t & geom )
+void BlockGroup::DrawFootPrint( const Geom & geom )
{
glPushMatrix();
@@ -146,12 +146,12 @@ void BlockGroup::DrawFootPrint( const stg_geom_t & geom )
glTranslatef( -offset.x, -offset.y, -offset.z );
- LISTMETHOD( blocks, StgBlock*, DrawFootPrint);
+ LISTMETHOD( blocks, Block*, DrawFootPrint);
glPopMatrix();
}
-void BlockGroup::BuildDisplayList( StgModel* mod )
+void BlockGroup::BuildDisplayList( Model* mod )
{
//printf( "display list for model %s\n", mod->token );
@@ -164,7 +164,7 @@ void BlockGroup::BuildDisplayList( StgModel* mod )
glNewList( displaylist, GL_COMPILE );
- stg_geom_t geom = mod->GetGeom();
+ Geom geom = mod->GetGeom();
gl_pose_shift( geom.pose );
@@ -182,7 +182,7 @@ void BlockGroup::BuildDisplayList( StgModel* mod )
for( GList* it=blocks; it; it=it->next )
{
- StgBlock* blk = (StgBlock*)it->data;
+ Block* blk = (Block*)it->data;
if( (!blk->inherit_color) && (blk->color != mod->color) )
{
@@ -207,7 +207,7 @@ void BlockGroup::BuildDisplayList( StgModel* mod )
for( GList* it=blocks; it; it=it->next )
{
- StgBlock* blk = (StgBlock*)it->data;
+ Block* blk = (Block*)it->data;
if( (!blk->inherit_color) && (blk->color != mod->color) )
{
@@ -228,20 +228,20 @@ void BlockGroup::BuildDisplayList( StgModel* mod )
glEndList();
}
-void BlockGroup::CallDisplayList( StgModel* mod )
+void BlockGroup::CallDisplayList( Model* mod )
{
if( displaylist == 0 )
BuildDisplayList( mod );
glCallList( displaylist );
}
-void BlockGroup::LoadBlock( StgModel* mod, Worldfile* wf, int entity )
+void BlockGroup::LoadBlock( Model* mod, Worldfile* wf, int entity )
{
- AppendBlock( new StgBlock( mod, wf, entity ));
+ AppendBlock( new Block( mod, wf, entity ));
}
-void BlockGroup::LoadBitmap( StgModel* mod, const char* bitmapfile, Worldfile* wf )
+void BlockGroup::LoadBitmap( Model* mod, const char* bitmapfile, Worldfile* wf )
{
PRINT_DEBUG1( "attempting to load bitmap \"%s\n", bitmapfile );
@@ -312,7 +312,7 @@ void BlockGroup::LoadBitmap( StgModel* mod, const char* bitmapfile, Worldfile* w
// TODO fix this
stg_color_t col = stg_color_pack( 1.0, 0,0,1.0 );
- AppendBlock( new StgBlock( mod,
+ AppendBlock( new Block( mod,
pts,4,
0,1,
col,
View
169 libstage/canvas.cc
@@ -31,7 +31,7 @@ static bool blur = true;
static bool init_done = false;
-void StgCanvas::TimerCallback( StgCanvas* c )
+void Canvas::TimerCallback( Canvas* c )
{
if( c->world->dirty )
{
@@ -41,11 +41,11 @@ void StgCanvas::TimerCallback( StgCanvas* c )
}
Fl::repeat_timeout(((double)c->interval/1000),
- (Fl_Timeout_Handler)StgCanvas::TimerCallback,
+ (Fl_Timeout_Handler)Canvas::TimerCallback,
c);
}
-StgCanvas::StgCanvas( StgWorldGui* world,
+Canvas::Canvas( WorldGui* world,
int x, int y,
int width, int height) :
Fl_Gl_Window( x, y, width, height ),
@@ -95,7 +95,7 @@ StgCanvas::StgCanvas( StgWorldGui* world,
assert( can_do( FL_ACCUM ) );
}
-void StgCanvas::InitGl()
+void Canvas::InitGl()
{
valid(1);
FixViewport(w(), h());
@@ -163,12 +163,12 @@ void StgCanvas::InitGl()
}
-StgCanvas::~StgCanvas()
+Canvas::~Canvas()
{
// nothing to do
}
-StgModel* StgCanvas::getModel( int x, int y )
+Model* Canvas::getModel( int x, int y )
{
// render all models in a unique color
make_current(); // make sure the GL context is current
@@ -185,11 +185,11 @@ StgModel* StgCanvas::getModel( int x, int y )
// render all top-level, draggable models in a color that is their
// id
- for( GList* it= world->StgWorld::children; it; it=it->next )
+ for( GList* it= world->World::children; it; it=it->next )
{
- StgModel* mod = (StgModel*)it->data;
+ Model* mod = (Model*)it->data;
- if( mod->GuiMask() & (STG_MOVE_TRANS | STG_MOVE_ROT ))
+ if( mod->gui.mask & (STG_MOVE_TRANS | STG_MOVE_ROT ))
{
uint8_t rByte, gByte, bByte, aByte;
uint32_t modelId = mod->id;
@@ -230,7 +230,7 @@ StgModel* StgCanvas::getModel( int x, int y )
// printf("Clicked rByte: 0x%X, gByte: 0x%X, bByte: 0x%X, aByte: 0x%X\n", rByte, gByte, bByte, aByte);
// printf("-->model Id = 0x%X\n", modelId);
- StgModel* mod = StgModel::LookupId( modelId );
+ Model* mod = Model::LookupId( modelId );
//printf("%p %s %d %x\n", mod, mod ? mod->Token() : "(none)", id, id );
@@ -245,14 +245,14 @@ StgModel* StgCanvas::getModel( int x, int y )
return mod;
}
-bool StgCanvas::selected( StgModel* mod ) {
+bool Canvas::selected( Model* mod ) {
if( g_list_find( selected_models, mod ) )
return true;
else
return false;
}
-void StgCanvas::select( StgModel* mod ) {
+void Canvas::select( Model* mod ) {
if( mod )
{
last_selection = mod;
@@ -263,7 +263,7 @@ void StgCanvas::select( StgModel* mod ) {
}
}
-void StgCanvas::unSelect( StgModel* mod ) {
+void Canvas::unSelect( Model* mod ) {
if( mod )
{
if ( GList* link = g_list_find( selected_models, mod ) )
@@ -277,16 +277,16 @@ void StgCanvas::unSelect( StgModel* mod ) {
}
}
-void StgCanvas::unSelectAll() {
+void Canvas::unSelectAll() {
// for( GList* it=selected_models; it; it=it->next )
- // ((StgModel*)it->data)->Enable();
+ // ((Model*)it->data)->Enable();
g_list_free( selected_models );
selected_models = NULL;
}
// convert from 2d window pixel to 3d world coordinates
-void StgCanvas::CanvasToWorld( int px, int py,
+void Canvas::CanvasToWorld( int px, int py,
double *wx, double *wy, double* wz )
{
if( px <= 0 )
@@ -324,7 +324,7 @@ void StgCanvas::CanvasToWorld( int px, int py,
}
-int StgCanvas::handle(int event)
+int Canvas::handle(int event)
{
switch(event)
{
@@ -379,7 +379,7 @@ int StgCanvas::handle(int event)
case FL_PUSH: // button pressed
{
- StgModel* mod = getModel( startx, starty );
+ Model* mod = getModel( startx, starty );
startx = Fl::event_x();
starty = Fl::event_y();
selectedModel = false;
@@ -443,7 +443,7 @@ int StgCanvas::handle(int event)
// move all selected models to the mouse pointer
for( GList* it = selected_models; it; it=it->next )
{
- StgModel* mod = (StgModel*)it->data;
+ Model* mod = (Model*)it->data;
mod->AddToPose( x-sx, y-sy, 0, 0 );
}
}
@@ -463,7 +463,7 @@ int StgCanvas::handle(int event)
// rotate all selected models
for( GList* it = selected_models; it; it=it->next )
{
- StgModel* mod = (StgModel*)it->data;
+ Model* mod = (Model*)it->data;
mod->AddToPose( 0,0,0, 0.05*(dx+dy) );
}
}
@@ -497,12 +497,12 @@ int StgCanvas::handle(int event)
{
// // start the timer that causes regular redraws
Fl::add_timeout( ((double)interval/1000),
- (Fl_Timeout_Handler)StgCanvas::TimerCallback,
+ (Fl_Timeout_Handler)Canvas::TimerCallback,
this);
}
else
{ // remove the timeout
- Fl::remove_timeout( (Fl_Timeout_Handler)StgCanvas::TimerCallback );
+ Fl::remove_timeout( (Fl_Timeout_Handler)Canvas::TimerCallback );
}
redraw(); // in case something happened that will never be
@@ -579,41 +579,44 @@ int StgCanvas::handle(int event)
} // end switch( event )
}
-void StgCanvas::FixViewport(int W,int H)
+void Canvas::FixViewport(int W,int H)
{
glLoadIdentity();
glViewport(0,0,W,H);
}
-void StgCanvas::AddModel( StgModel* mod )
+void Canvas::AddModel( Model* mod )
{
models_sorted = g_list_append( models_sorted, mod );
}
-void StgCanvas::DrawGlobalGrid()
+void Canvas::DrawGlobalGrid()
{
- stg_bounds3d_t bounds = world->GetExtent();
-// printf( "bounds [%.2f %.2f] [%.2f %.2f] [%.2f %.2f]\n",
-// bounds.x.min, bounds.x.max,
-// bounds.y.min, bounds.y.max,
-// bounds.z.min, bounds.z.max );
+ stg_bounds3d_t bounds = world->GetExtent();
+ /* printf( "bounds [%.2f %.2f] [%.2f %.2f] [%.2f %.2f]\n",
+ bounds.x.min, bounds.x.max,
+ bounds.y.min, bounds.y.max,
+ bounds.z.min, bounds.z.max );
+
+ */
+
char str[64];
- PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray
- for( double i = floor(bounds.x.min); i < bounds.x.max; i++)
- {
- snprintf( str, 16, "%d", (int)i );
- gl_draw_string( i, 0, 0.00, str );
- }
-
- for( double i = floor(bounds.y.min); i < bounds.y.max; i++)
- {
- snprintf( str, 16, "%d", (int)i );
- gl_draw_string( 0, i, 0.00, str );
- }
- PopColor();
-
+ PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray
+ for( double i = floor(bounds.x.min); i < bounds.x.max; i++)
+ {
+ snprintf( str, 16, "%d", (int)i );
+ gl_draw_string( i, 0, 0.00, str );
+ }
+
+ for( double i = floor(bounds.y.min); i < bounds.y.max; i++)
+ {
+ snprintf( str, 16, "%d", (int)i );
+ gl_draw_string( 0, i, 0.00, str );
+ }
+ PopColor();
+
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
glEnable(GL_POLYGON_OFFSET_FILL);
@@ -642,7 +645,7 @@ void StgCanvas::DrawGlobalGrid()
}
//draw the floor without any grid ( for robot's perspective camera model )
-void StgCanvas::DrawFloor()
+void Canvas::DrawFloor()
{
stg_bounds3d_t bounds = world->GetExtent();
@@ -661,12 +664,12 @@ void StgCanvas::DrawFloor()
glEnd();
}
-void StgCanvas::DrawBlocks()
+void Canvas::DrawBlocks()
{
- LISTMETHOD( models_sorted, StgModel*, DrawBlocksTree );
+ LISTMETHOD( models_sorted, Model*, DrawBlocksTree );
}
-void StgCanvas::DrawBoundingBoxes()
+void Canvas::DrawBoundingBoxes()
{
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
glLineWidth( 2.0 );
@@ -680,15 +683,15 @@ void StgCanvas::DrawBoundingBoxes()
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
}
-inline void StgCanvas::resetCamera()
+inline void Canvas::resetCamera()
{
float max_x = 0, max_y = 0, min_x = 0, min_y = 0;
//TODO take orrientation ( `a' ) and geom.pose offset into consideration
- for( GList* it=world->StgWorld::children; it; it=it->next ) {
- StgModel* ptr = (StgModel*) it->data;
- stg_pose_t pose = ptr->GetPose();
- stg_geom_t geom = ptr->GetGeom();
+ for( GList* it=world->World::children; it; it=it->next ) {
+ Model* ptr = (Model*) it->data;
+ Pose pose = ptr->GetPose();
+ Geom geom = ptr->GetGeom();
float tmp_min_x = pose.x - geom.size.x / 2.0;
float tmp_max_x = pose.x + geom.size.x / 2.0;
@@ -713,10 +716,10 @@ inline void StgCanvas::resetCamera()
}
// used to sort a list of models by inverse distance from the x,y pose in [coords]
-gint compare_distance( StgModel* a, StgModel* b, double coords[2] )
+gint compare_distance( Model* a, Model* b, double coords[2] )
{
- stg_pose_t a_pose = a->GetGlobalPose();
- stg_pose_t b_pose = b->GetGlobalPose();
+ Pose a_pose = a->GetGlobalPose();
+ Pose b_pose = b->GetGlobalPose();
double a_dist = hypot( coords[1] - a_pose.y,
coords[0] - a_pose.x );
@@ -733,7 +736,7 @@ gint compare_distance( StgModel* a, StgModel* b, double coords[2] )
return 0; // must be the same
}
-void StgCanvas::renderFrame()
+void Canvas::renderFrame()
{
//before drawing, order all models based on distance from camera
float x = current_camera->x();
@@ -767,18 +770,18 @@ void StgCanvas::renderFrame()
// out for Z. look into it.
if( showOccupancy )
- ((StgWorldGui*)world)->DrawTree( false );
+ ((WorldGui*)world)->DrawTree( false );
if( showTree )
- ((StgWorldGui*)world)->DrawTree( true );
+ ((WorldGui*)world)->DrawTree( true );
glPopMatrix();
}
if( showFootprints )
{
glDisable( GL_DEPTH_TEST );
- LISTMETHOD( models_sorted, StgModel*, DrawTrailFootprint );
+ LISTMETHOD( models_sorted, Model*, DrawTrailFootprint );
glEnable( GL_DEPTH_TEST );
}
@@ -794,7 +797,7 @@ void StgCanvas::renderFrame()
DrawBoundingBoxes();
// TODO - finish this properly
- //LISTMETHOD( models_sorted, StgModel*, DrawWaypoints );
+ //LISTMETHOD( models_sorted, Model*, DrawWaypoints );
// MOTION BLUR
if( 0 )//showBlur )
@@ -877,52 +880,52 @@ void StgCanvas::renderFrame()
// if( showTrailRise )
// {
-// for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) {
+// for( std::multimap< float, Model* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) {
// i->second->DrawTrailBlocks();
// }
// }
// if( showTrailArrows )
// {
// glEnable( GL_DEPTH_TEST );
-// for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) {
+// for( std::multimap< float, Model* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) {
// i->second->DrawTrailArrows();
// }
// }
for( GList* it=selected_models; it; it=it->next )
- ((StgModel*)it->data)->DrawSelected();
+ ((Model*)it->data)->DrawSelected();
// useful debug - puts a point at the origin of each model
- //for( GList* it = world->StgWorld::children; it; it=it->next )
- // ((StgModel*)it->data)->DrawOriginTree();
+ //for( GList* it = world->World::children; it; it=it->next )
+ // ((Model*)it->data)->DrawOriginTree();
// draw the model-specific visualizations
if( showData ) {
if ( ! visualizeAll ) {
- for( GList* it = world->StgWorld::children; it; it=it->next )
- ((StgModel*)it->data)->DataVisualizeTree( current_camera );
+ for( GList* it = world->World::children; it; it=it->next )
+ ((Model*)it->data)->DataVisualizeTree( current_camera );
}
else if ( selected_models ) {
for( GList* it = selected_models; it; it=it->next )
- ((StgModel*)it->data)->DataVisualizeTree( current_camera );
+ ((Model*)it->data)->DataVisualizeTree( current_camera );
}
else if ( last_selection ) {
last_selection->DataVisualizeTree( current_camera );
}
}
if( showGrid )
- LISTMETHOD( models_sorted, StgModel*, DrawGrid );
+ LISTMETHOD( models_sorted, Model*, DrawGrid );
if( showFlags )
- LISTMETHOD( models_sorted, StgModel*, DrawFlagList );
+ LISTMETHOD( models_sorted, Model*, DrawFlagList );
if( showBlinken )
- LISTMETHOD( models_sorted, StgModel*, DrawBlinkenlights );
+ LISTMETHOD( models_sorted, Model*, DrawBlinkenlights );
if( showStatus )
{
@@ -933,7 +936,7 @@ void StgCanvas::renderFrame()
if( camera.pitch() == 0 && !pCamOn )
glTranslatef( 0, 0, 0.1 );
- LISTMETHODARG( models_sorted, StgModel*, DrawStatusTree, &camera );
+ LISTMETHODARG( models_sorted, Model*, DrawStatusTree, &camera );
glEnable( GL_DEPTH_TEST );
glPopMatrix();
@@ -1009,7 +1012,7 @@ void StgCanvas::renderFrame()
}
-void StgCanvas::Screenshot()
+void Canvas::Screenshot()
{
int width = w();
@@ -1071,9 +1074,9 @@ void StgCanvas::Screenshot()
printf( "Saved %s\n", filename );
}
-void StgCanvas::perspectiveCb( Fl_Widget* w, void* p )
+void Canvas::perspectiveCb( Fl_Widget* w, void* p )
{
- StgCanvas* canvas = static_cast<StgCanvas*>( w );
+ Canvas* canvas = static_cast<Canvas*>( w );
Option* opt = static_cast<Option*>( p ); // pCamOn
if ( opt ) {
// Perspective mode is on, change camera
@@ -1086,7 +1089,7 @@ void StgCanvas::perspectiveCb( Fl_Widget* w, void* p )
canvas->invalidate();
}
-void StgCanvas::createMenuItems( Fl_Menu_Bar* menu, std::string path )
+void Canvas::createMenuItems( Fl_Menu_Bar* menu, std::string path )
{
showData.createMenuItem( menu, path );
// visualizeAll.createMenuItem( menu, path );
@@ -1111,7 +1114,7 @@ void StgCanvas::createMenuItems( Fl_Menu_Bar* menu, std::string path )
}
-void StgCanvas::Load( Worldfile* wf, int sec )
+void Canvas::Load( Worldfile* wf, int sec )
{
this->wf = wf;
camera.Load( wf, sec );
@@ -1143,13 +1146,13 @@ void StgCanvas::Load( Worldfile* wf, int sec )
if( ! world->paused )
// // start the timer that causes regular redraws
Fl::add_timeout( ((double)interval/1000),
- (Fl_Timeout_Handler)StgCanvas::TimerCallback,
+ (Fl_Timeout_Handler)Canvas::TimerCallback,
this);
invalidate(); // we probably changed something
}
-void StgCanvas::Save( Worldfile* wf, int sec )
+void Canvas::Save( Worldfile* wf, int sec )
{
camera.Save( wf, sec );
perspective_camera.Save( wf, sec );
@@ -1175,7 +1178,7 @@ void StgCanvas::Save( Worldfile* wf, int sec )
}
-void StgCanvas::draw()
+void Canvas::draw()
{
// static unsigned long calls=0;
// printf( "Draw calls %lu\n", ++calls );
@@ -1209,7 +1212,7 @@ void StgCanvas::draw()
//Follow the selected robot
if( showFollow && last_selection )
{
- stg_pose_t gpose = last_selection->GetGlobalPose();
+ Pose gpose = last_selection->GetGlobalPose();
if( pCamOn == true )
{
perspective_camera.setPose( gpose.x, gpose.y, 0.2 );
@@ -1225,7 +1228,7 @@ void StgCanvas::draw()
renderFrame();
}
-void StgCanvas::resize(int X,int Y,int W,int H)
+void Canvas::resize(int X,int Y,int W,int H)
{
Fl_Gl_Window::resize(X,Y,W,H);
FixViewport(W,H);
View
4 libstage/gl.cc
@@ -9,12 +9,12 @@ void Stg::gl_coord_shift( double x, double y, double z, double a )
}
// transform the current coordinate frame by the given pose
-void Stg::gl_pose_shift( const stg_pose_t &pose )
+void Stg::gl_pose_shift( const Pose &pose )
{
gl_coord_shift( pose.x, pose.y, pose.z, pose.a );
}
-void Stg::gl_pose_inverse_shift( const stg_pose_t &pose )
+void Stg::gl_pose_inverse_shift( const Pose &pose )
{
gl_coord_shift( 0,0,0, -pose.a );
gl_coord_shift( -pose.x, -pose.y, -pose.z, 0 );
View
12 libstage/main.cc
@@ -57,9 +57,9 @@ int main( int argc, char* argv[] )
if( optindex > 0 )
{
const char* worldfilename = argv[optindex];
- StgWorld* world = ( usegui ?
- new StgWorldGui( 400, 300, worldfilename ) :
- new StgWorld( worldfilename ) );
+ World* world = ( usegui ?
+ new WorldGui( 400, 300, worldfilename ) :
+ new World( worldfilename ) );
world->Load( worldfilename );
loaded_world_file = true;
}
@@ -69,20 +69,20 @@ int main( int argc, char* argv[] )
if( loaded_world_file == false )
{
// TODO: special window/loading dialog for this case
- new StgWorldGui( 400, 300 );
+ new WorldGui( 400, 300 );
}
if( usegui == true )
{
//don't close the window once time has finished
while( true )
- StgWorld::UpdateAll();
+ World::UpdateAll();
}
else
{
//close program once time has completed
bool quit = false;
while( quit == false )
- quit = StgWorld::UpdateAll();
+ quit = World::UpdateAll();
}
}
View
1,085 libstage/model.cc
533 additions, 552 deletions not shown because the diff is too large. Please use a local Git client to view these changes.
View
1,348 libstage/model.hh
702 additions, 646 deletions not shown because the diff is too large. Please use a local Git client to view these changes.
View
28 libstage/model_blinkenlight.cc
@@ -16,7 +16,7 @@
@defgroup model_blinkenlight Blinkenlight model
Simulates a blinking light.
-API: Stg::StgModelBlinkenlight
+API: Stg::ModelBlinkenlight
<h2>Worldfile properties</h2>
@@ -51,26 +51,26 @@ enabled 1
#include "stage_internal.hh"
#include "option.hh"
-Option StgModelBlinkenlight::showBlinkenData( "Show Blink", "show_blinken", "", true );
+Option ModelBlinkenlight::showBlinkenData( "Show Blink", "show_blinken", "", true );
-StgModelBlinkenlight::StgModelBlinkenlight( StgWorld* world,
- StgModel* parent )
- : StgModel( world, parent, MODEL_TYPE_BLINKENLIGHT ),
+ModelBlinkenlight::ModelBlinkenlight( World* world,
+ Model* parent )
+ : Model( world, parent, MODEL_TYPE_BLINKENLIGHT ),
dutycycle( 1.0 ),
enabled( true ),
period( 1000 ),
on( true )
{
- PRINT_DEBUG2( "Constructing StgModelBlinkenlight %d (%s)\n",
+ PRINT_DEBUG2( "Constructing ModelBlinkenlight %d (%s)\n",
id, typestr );
// Set up sensible defaults
this->SetColor( stg_lookup_color( "green" ) );
- stg_geom_t geom;
+ Geom geom;
memset( &geom, 0, sizeof(geom)); // no size
geom.size.x = 0.02;
geom.size.y = 0.02;
@@ -83,35 +83,35 @@ StgModelBlinkenlight::StgModelBlinkenlight( StgWorld* world,
registerOption( &showBlinkenData );
}
-StgModelBlinkenlight::~StgModelBlinkenlight()
+ModelBlinkenlight::~ModelBlinkenlight()
{
}
-void StgModelBlinkenlight::Load( void )
+void ModelBlinkenlight::Load( void )
{
- StgModel::Load();
+ Model::Load();
this->dutycycle = wf->ReadFloat( wf_entity, "dutycycle", this->dutycycle );
this->period = wf->ReadInt( wf_entity, "period", this->period );
this->enabled = wf->ReadInt( wf_entity, "dutycycle", this->enabled );
}
-void StgModelBlinkenlight::Update( void )
+void ModelBlinkenlight::Update( void )
{
- StgModel::Update();
+ Model::Update();
// invert
this->on = ! this->on;
}
-void StgModelBlinkenlight::DataVisualize( Camera* cam )
+void ModelBlinkenlight::DataVisualize( Camera* cam )
{
// TODO XX
if( on && showBlinkenData )
{
- //LISTMETHOD( this->blocks, StgBlock*, Draw );
+ //LISTMETHOD( this->blocks, Block*, Draw );
}
}
View
44 libstage/model_blobfinder.cc
@@ -25,7 +25,7 @@ static const unsigned int DEFAULT_BLOBFINDERRESOLUTION = 1;
static const unsigned int DEFAULT_BLOBFINDERSCANWIDTH = 80;
static const unsigned int DEFAULT_BLOBFINDERSCANHEIGHT = 60;
-Option StgModelBlobfinder::showBlobData( "Show Blobfinder", "show_blob", "", true );
+Option ModelBlobfinder::showBlobData( "Show Blobfinder", "show_blob", "", true );
/**
@ingroup model
@@ -39,7 +39,7 @@ Option StgModelBlobfinder::showBlobData( "Show Blobfinder", "show_blob", "", tru
channel one, blue objects in channel two, etc. The color associated
with each channel is configurable.
-API: Stg::StgModelBlobfinder
+API: Stg::ModelBlobfinder
<h2>Worldfile properties</h2>
@@ -76,11 +76,11 @@ blobfinder
*/
-StgModelBlobfinder::StgModelBlobfinder( StgWorld* world,
- StgModel* parent )
- : StgModel( world, parent, MODEL_TYPE_BLOBFINDER )
+ModelBlobfinder::ModelBlobfinder( World* world,
+ Model* parent )
+ : Model( world, parent, MODEL_TYPE_BLOBFINDER )
{
- PRINT_DEBUG2( "Constructing StgModelBlobfinder %d (%s)\n",
+ PRINT_DEBUG2( "Constructing ModelBlobfinder %d (%s)\n",
id, typestr );
scan_width = DEFAULT_BLOBFINDERSCANWIDTH;
@@ -99,7 +99,7 @@ StgModelBlobfinder::StgModelBlobfinder( StgWorld* world,
}
-StgModelBlobfinder::~StgModelBlobfinder( void )
+ModelBlobfinder::~ModelBlobfinder( void )
{
if( blobs )
g_array_free( blobs, true );
@@ -108,8 +108,8 @@ StgModelBlobfinder::~StgModelBlobfinder( void )
g_array_free( colors, true );
}
-static bool blob_match( StgModel* candidate,
- StgModel* finder,
+static bool blob_match( Model* candidate,
+ Model* finder,
const void* dummy )
{
return( ! finder->IsRelated( candidate ));
@@ -121,13 +121,13 @@ static bool ColorMatchIgnoreAlpha( stg_color_t a, stg_color_t b )
return( (a & 0x00FFFFFF) == (b & 0x00FFFFFF ) );
}
-void StgModelBlobfinder::StgModelBlobfinder::AddColor( stg_color_t col )
+void ModelBlobfinder::ModelBlobfinder::AddColor( stg_color_t col )
{
g_array_append_val( colors, col );
}
/** Stop tracking blobs with this color */
-void StgModelBlobfinder::RemoveColor( stg_color_t col )
+void ModelBlobfinder::RemoveColor( stg_color_t col )
{
for( unsigned int i=0; i<colors->len; i++ )
if( col == g_array_index( colors, stg_color_t, i ) )
@@ -136,14 +136,14 @@ void StgModelBlobfinder::RemoveColor( stg_color_t col )
/** Stop tracking all colors. Call this to clear the defaults, then
add colors individually with AddColor(); */
-void StgModelBlobfinder::RemoveAllColors()
+void ModelBlobfinder::RemoveAllColors()
{
g_array_set_size( colors, 0 );
}
-void StgModelBlobfinder::Load( void )
+void ModelBlobfinder::Load( void )
{
- StgModel::Load();
+ Model::Load();
Worldfile* wf = world->GetWorldFile();
@@ -173,9 +173,9 @@ void StgModelBlobfinder::Load( void )
}
-void StgModelBlobfinder::Update( void )
+void ModelBlobfinder::Update( void )
{
- StgModel::Update();
+ Model::Update();
// generate a scan for post-processing into a blob image
@@ -270,17 +270,17 @@ void StgModelBlobfinder::Update( void )
}
-void StgModelBlobfinder::Startup( void )
+void ModelBlobfinder::Startup( void )
{
- StgModel::Startup();
+ Model::Startup();
PRINT_DEBUG( "blobfinder startup" );
// start consuming power
SetWatts( DEFAULT_BLOBFINDERWATTS );
}
-void StgModelBlobfinder::Shutdown( void )
+void ModelBlobfinder::Shutdown( void )
{
PRINT_DEBUG( "blobfinder shutdown" );
@@ -292,10 +292,10 @@ void StgModelBlobfinder::Shutdown( void )
if( blobs )
g_array_set_size( blobs, 0 );
- StgModel::Shutdown();
+ Model::Shutdown();
}
-void StgModelBlobfinder::DataVisualize( Camera* cam )
+void ModelBlobfinder::DataVisualize( Camera* cam )
{
if ( !showBlobData )
return;
@@ -326,7 +326,7 @@ void StgModelBlobfinder::DataVisualize( Camera* cam )
glPushMatrix();
// return to global rotation frame
- stg_pose_t gpose = GetGlobalPose();
+ Pose gpose = GetGlobalPose();
glRotatef( rtod(-gpose.a),0,0,1 );
// place the "screen" a little away from the robot
View
8 libstage/model_callbacks.cc
@@ -1,11 +1,11 @@
#include "stage_internal.hh"
-int key_gen( StgModel* mod, void* address )
+int key_gen( Model* mod, void* address )
{
return ((int*)address) - ((int*)mod);
}
-void StgModel::AddCallback( void* address,
+void Model::AddCallback( void* address,
stg_model_callback_t cb,
void* user )
{
@@ -31,7 +31,7 @@ void StgModel::AddCallback( void* address,
}
-int StgModel::RemoveCallback( void* member,
+int Model::RemoveCallback( void* member,
stg_model_callback_t callback )
{
int key = key_gen( this, member );
@@ -77,7 +77,7 @@ int StgModel::RemoveCallback( void* member,
}
-void StgModel::CallCallbacks( void* address )
+void Model::CallCallbacks( void* address )
{
assert( address );
View
30 libstage/model_camera.cc
@@ -17,9 +17,9 @@
#include <sstream>
#include <iomanip>
-Option StgModelCamera::showCameraData( "Show Camera Data", "show_camera", "", true );
+Option ModelCamera::showCameraData( "Show Camera Data", "show_camera", "", true );
-static const stg_size_t DEFAULT_SIZE( 0.1, 0.07, 0.05 );
+static const Size DEFAULT_SIZE( 0.1, 0.07, 0.05 );
static const char DEFAULT_GEOM_COLOR[] = "black";
static const float DEFAULT_HFOV = 70;
static const float DEFAULT_VFOV = 40;
@@ -29,7 +29,7 @@ static const float DEFAULT_VFOV = 40;
@defgroup model_camera Camera model
The camera model simulates a camera
-API: Stg::StgModelCamera
+API: Stg::ModelCamera
<h2>Worldfile properties</h2>
@@ -79,8 +79,8 @@ void cross( float& x1, float& y1, float& z1, float x2, float y2, float z2 )
}
-StgModelCamera::StgModelCamera( StgWorld* world, StgModel* parent )
- : StgModel( world, parent, MODEL_TYPE_CAMERA ),
+ModelCamera::ModelCamera( World* world, Model* parent )
+ : Model( world, parent, MODEL_TYPE_CAMERA ),
_canvas( NULL ),
_frame_data( NULL ),
_frame_color_data( NULL ),
@@ -96,10 +96,10 @@ _yaw_offset( 0.0 ),
_pitch_offset( 0.0 )
{
- PRINT_DEBUG2( "Constructing StgModelCamera %d (%s)\n",
+ PRINT_DEBUG2( "Constructing ModelCamera %d (%s)\n",
id, typestr );
- StgWorldGui* world_gui = dynamic_cast< StgWorldGui* >( world );
+ WorldGui* world_gui = dynamic_cast< WorldGui* >( world );
if( world_gui == NULL ) {
printf( "Unable to use Camera Model - it must be run with a GUI world\n" );
@@ -109,7 +109,7 @@ _pitch_offset( 0.0 )
_camera.setPitch( 90.0 );
- stg_geom_t geom;
+ Geom geom;
memset( &geom, 0, sizeof(geom));
geom.size = DEFAULT_SIZE;
SetGeom( geom );
@@ -122,7 +122,7 @@ _pitch_offset( 0.0 )
Startup();
}
-StgModelCamera::~StgModelCamera()
+ModelCamera::~ModelCamera()
{
if( _frame_data != NULL ) {
//dont forget about GetFrame() //TODO merge these together
@@ -134,9 +134,9 @@ StgModelCamera::~StgModelCamera()
}
}
-void StgModelCamera::Load( void )
+void ModelCamera::Load( void )
{
- StgModel::Load();
+ Model::Load();
float horizFov = wf->ReadTupleFloat( wf_entity, "fov", 0, DEFAULT_HFOV );
float vertFov = wf->ReadTupleFloat( wf_entity, "fov", 1, DEFAULT_VFOV );
@@ -155,13 +155,13 @@ void StgModelCamera::Load( void )
}
-void StgModelCamera::Update( void )
+void ModelCamera::Update( void )
{
GetFrame();
- StgModel::Update();
+ Model::Update();
}
-bool StgModelCamera::GetFrame( void )
+bool ModelCamera::GetFrame( void )
{
if( _width == 0 || _height == 0 )
return false;
@@ -235,7 +235,7 @@ bool StgModelCamera::GetFrame( void )
}
//TODO create lines outlining camera frustrum, then iterate over each depth measurement and create a square
-void StgModelCamera::DataVisualize( Camera* cam )
+void ModelCamera::DataVisualize( Camera* cam )
{
if( _frame_data == NULL || !showCameraData )
View
54 libstage/model_fiducial.cc
@@ -23,15 +23,15 @@ const stg_meters_t DEFAULT_FIDUCIAL_RANGEMAXANON = 8.0;
const stg_radians_t DEFAULT_FIDUCIAL_FOV = M_PI;
const stg_watts_t DEFAULT_FIDUCIAL_WATTS = 10.0;
-Option StgModelFiducial::showFiducialData( "Show Fiducial", "show_fiducial", "", true );
+Option ModelFiducial::showFiducialData( "Show Fiducial", "show_fiducial", "", true );
/**
@ingroup model
@defgroup model_fiducial Fiducial detector model
The fiducial model simulates a fiducial-detecting device.
-API: Stg::StgModelFiducial
+API: Stg::ModelFiducial
<h2>Worldfile properties</h2>
@@ -64,11 +64,11 @@ fiducialfinder
*/
-StgModelFiducial::StgModelFiducial( StgWorld* world,
- StgModel* parent )
- : StgModel( world, parent, MODEL_TYPE_FIDUCIAL )
+ModelFiducial::ModelFiducial( World* world,
+ Model* parent )
+ : Model( world, parent, MODEL_TYPE_FIDUCIAL )
{
- PRINT_DEBUG2( "Constructing StgModelFiducial %d (%s)\n",
+ PRINT_DEBUG2( "Constructing ModelFiducial %d (%s)\n",
id, typestr );
// assert that Update() is reentrant for this derived model
@@ -79,7 +79,7 @@ StgModelFiducial::StgModelFiducial( StgWorld* world,
this->ClearBlocks();
- stg_geom_t geom;
+ Geom geom;
memset( &geom, 0, sizeof(geom));
SetGeom( geom );
fiducials = NULL;
@@ -97,42 +97,42 @@ StgModelFiducial::StgModelFiducial( StgWorld* world,
registerOption( &showFiducialData );
}
-StgModelFiducial::~StgModelFiducial( void )
+ModelFiducial::~ModelFiducial( void )
{
if( data )
g_array_free( data, true );
}
-static bool fiducial_raytrace_match( StgModel* candidate,
- StgModel* finder,
+static bool fiducial_raytrace_match( Model* candidate,
+ Model* finder,
const void* dummy )
{
return( ! finder->IsRelated( candidate ) );
}
-void StgModelFiducial::AddModelIfVisible( StgModel* him )
+void ModelFiducial::AddModelIfVisible( Model* him )
{
//PRINT_DEBUG2( "Fiducial %s is testing model %s", token, him->Token() );
// don't consider models with invalid returns
- if( him->FiducialReturn() == 0 )
+ if( him->vis.fiducial_return == 0 )
{
//PRINT_DEBUG1( " but model %s has a zero fiducial ID", him->Token());
return;
}
// check to see if this neighbor has the right fiducial key
- if( fiducial_key != him->FiducialKey() )
+ if( vis.fiducial_key != him->vis.fiducial_key )
{
//PRINT_DEBUG1( " but model %s doesn't match the fiducial key", him->Token());
return;
}
- stg_pose_t mypose = this->GetGlobalPose();
+ Pose mypose = this->GetGlobalPose();
// are we within range?
- stg_pose_t hispose = him->GetGlobalPose();
+ Pose hispose = him->GetGlobalPose();
double dx = hispose.x - mypose.x;
double dy = hispose.y - mypose.y;
double range = hypot( dy, dx );
@@ -173,7 +173,7 @@ void StgModelFiducial::AddModelIfVisible( StgModel* him )
false );
range = ray.range;
- StgModel* hitmod = ray.mod;
+ Model* hitmod = ray.mod;
// printf( "ray hit %s and was seeking LOS to %s\n",
//hitmod ? hitmod->Token() : "null",
@@ -182,7 +182,7 @@ void StgModelFiducial::AddModelIfVisible( StgModel* him )
// if it was him, we can see him
if( hitmod == him )
{
- stg_geom_t hisgeom = him->GetGeom();
+ Geom hisgeom = him->GetGeom();
// record where we saw him and what he looked like
stg_fiducial_t fid;
@@ -197,10 +197,10 @@ void StgModelFiducial::AddModelIfVisible( StgModel* him )
// if he's within ID range, get his fiducial.return value, else
// we see value 0
- fid.id = range < max_range_id ? hitmod->FiducialReturn() : 0;
+ fid.id = range < max_range_id ? hitmod->vis.fiducial_return : 0;
PRINT_DEBUG2( "adding %s's value %d to my list of fiducials",
- him->Token(), him->FiducialReturn() );
+ him->Token(), him->vis.fiducial_return );
g_array_append_val( data, fid );
}
@@ -213,9 +213,9 @@ void StgModelFiducial::AddModelIfVisible( StgModel* him )
///////////////////////////////////////////////////////////////////////////
// Update the beacon data
//
-void StgModelFiducial::Update( void )
+void ModelFiducial::Update( void )
{
- StgModel::Update();
+ Model::Update();
PRINT_DEBUG( "fiducial update" );
@@ -230,17 +230,17 @@ void StgModelFiducial::Update( void )
// TODO - add a fiducial-only hash table to the world to speed this
// up a lot for large populations
- world->ForEachDescendant( (stg_model_callback_t)(StgModelFiducial::AddModelIfVisibleStatic),
+ world->ForEachDescendant( (stg_model_callback_t)(ModelFiducial::AddModelIfVisibleStatic),
this );
PRINT_DEBUG2( "model %s saw %d fiducials", token, data->len );
}
-void StgModelFiducial::Load( void )
+void ModelFiducial::Load( void )
{
PRINT_DEBUG( "fiducial load" );
- StgModel::Load();
+ Model::Load();
// load fiducial-specific properties
min_range = wf->ReadLength( wf_entity, "range_min", min_range );
@@ -250,7 +250,7 @@ void StgModelFiducial::Load( void )
}
-void StgModelFiducial::DataVisualize( Camera* cam )
+void ModelFiducial::DataVisualize( Camera* cam )
{
if ( !showFiducialData )
return;
@@ -314,7 +314,7 @@ void StgModelFiducial::DataVisualize( Camera* cam )
}
}
-void StgModelFiducial::Shutdown( void )
+void ModelFiducial::Shutdown( void )
{
PRINT_DEBUG( "fiducial shutdown" );