Revision: 6777
http://playerstage.svn.sourceforge.net/playerstage/?rev=6777&view=rev
Author: rtv
Date: 2008-07-05 23:32:02 -0700 (Sat, 05 Jul 2008)
Log Message:
-----------
elegant fix for height, finally.
Modified Paths:
--------------
code/stage/trunk/libstage/canvas.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_camera.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/worlds/autolab.world
code/stage/trunk/worlds/chatterbox.inc
code/stage/trunk/worlds/everything.world
code/stage/trunk/worlds/fasr.world
code/stage/trunk/worlds/simple.world
code/stage/trunk/worlds/swarmbenchmark/cave.world
Added Paths:
-----------
code/stage/trunk/worlds/irobot.inc
Removed Paths:
-------------
code/stage/trunk/worlds/roomba.inc
Modified: code/stage/trunk/libstage/canvas.cc
===================================================================
--- code/stage/trunk/libstage/canvas.cc 2008-07-06 03:49:04 UTC (rev 6776)
+++ code/stage/trunk/libstage/canvas.cc 2008-07-06 06:32:02 UTC (rev 6777)
@@ -551,6 +551,10 @@
x += -sin( sphi ) * 100;
y += -cos( sphi ) * 100;
+ // TODO - keep this map around in between frames, because the order
+ // changes slowly, and sorting an already-sorted list is
+ // usually very fast (rtv)
+
//store all models in a sorted multimap
std::multimap< float, StgModel* > ordered;
for( GList* it=world->StgWorld::children; it; it=it->next ) {
@@ -564,8 +568,6 @@
//now the models can be iterated over with:
// for( std::multimap< float, StgModel* >::reverse_iterator i =
ordered.rbegin(); i != ordered.rend(); i++ )
-
-
if( ! showTrails )
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2008-07-06 03:49:04 UTC (rev 6776)
+++ code/stage/trunk/libstage/model.cc 2008-07-06 06:32:02 UTC (rev 6777)
@@ -146,17 +146,17 @@
// constructor
StgModel::StgModel( StgWorld* world,
- StgModel* parent,
- const stg_model_type_t type )
+ StgModel* parent,
+ const stg_model_type_t type )
: StgAncestor()
{
assert( modelsbyid );
assert( world );
PRINT_DEBUG3( "Constructing model world: %s parent: %s type: %d ",
- world->Token(),
- parent ? parent->Token() : "(null)",
- type );
+ world->Token(),
+ parent ? parent->Token() : "(null)",
+ type );
this->parent = parent;
this->world = world;
@@ -176,7 +176,6 @@
world->AddModel( this );
bzero( &pose, sizeof(pose));
- if( parent) pose.z += parent->geom.size.z;
bzero( &global_pose, sizeof(global_pose));
this->gpose_dirty = true;
@@ -194,8 +193,8 @@
this->blocks_dl = glGenLists( 1 );
this->geom.size.x = DEFAULT_GEOM_SIZEX;
- this->geom.size.y = DEFAULT_GEOM_SIZEX;
- this->geom.size.z = DEFAULT_GEOM_SIZEX;
+ this->geom.size.y = DEFAULT_GEOM_SIZEY;
+ this->geom.size.z = DEFAULT_GEOM_SIZEZ;
memset( &this->geom.pose, 0, sizeof(this->geom.pose));
this->obstacle_return = DEFAULT_OBSTACLERETURN;
@@ -235,7 +234,7 @@
this->AddBlockRect( -0.5,-0.5,1,1 );
PRINT_DEBUG2( "finished model %s @ %p",
- this->token, this );
+ this->token, this );
}
StgModel::~StgModel( void )
@@ -297,16 +296,16 @@
void StgModel::AddBlock( stg_point_t* pts,
- size_t pt_count,
- stg_meters_t zmin,
- stg_meters_t zmax,
- stg_color_t col,
- bool inherit_color )
+ size_t
pt_count,
+ stg_meters_t
zmin,
+ stg_meters_t
zmax,
+ stg_color_t
col,
+ bool
inherit_color )
{
blocks =
g_list_prepend( blocks, new StgBlock( this, pts, pt_count,
- zmin, zmax,
- col, inherit_color ));
+
zmin, zmax,
+
col, inherit_color ));
// force recreation of display lists before drawing
NeedRedraw();
@@ -321,7 +320,7 @@
}
void StgModel::AddBlockRect( double x, double y,
- double width, double height )
+
double width, double height )
{
stg_point_t pts[4];
pts[0].x = x;
@@ -339,64 +338,64 @@
void StgModel::Raytrace( stg_pose_t pose,
- stg_meters_t range,
- stg_block_match_func_t func,
- const void* arg,
- stg_raytrace_sample_t* sample,
- bool ztest )
+ stg_meters_t
range,
+
stg_block_match_func_t func,
+ const void*
arg,
+
stg_raytrace_sample_t* sample,
+ bool ztest )
{
world->Raytrace( LocalToGlobal(pose),
- range,
- func,
- this,
- arg,
- sample,
- ztest );
+ range,
+ func,
+ this,
+ arg,
+ sample,
+ ztest );
}
void StgModel::Raytrace( stg_radians_t bearing,
- stg_meters_t range,
- stg_block_match_func_t func,
- const void* arg,
- stg_raytrace_sample_t* sample,
- bool ztest )
+ stg_meters_t
range,
+
stg_block_match_func_t func,
+ const void*
arg,
+
stg_raytrace_sample_t* sample,
+ bool ztest )
{
stg_pose_t raystart;
bzero( &raystart, sizeof(raystart));
raystart.a = bearing;
world->Raytrace( LocalToGlobal(raystart),
- range,
- func,
- this,
- arg,
- sample,
- ztest );
+ range,
+ func,
+ this,
+ arg,
+ sample,
+ ztest );
}
void StgModel::Raytrace( stg_radians_t bearing,
- stg_meters_t range,
- stg_radians_t fov,
- stg_block_match_func_t func,
- const void* arg,
- stg_raytrace_sample_t* samples,
- uint32_t sample_count,
- bool ztest )
+ stg_meters_t
range,
+ stg_radians_t
fov,
+
stg_block_match_func_t func,
+ const void*
arg,
+
stg_raytrace_sample_t* samples,
+ uint32_t
sample_count,
+ bool ztest )
{
stg_pose_t raystart;
bzero( &raystart, sizeof(raystart));
raystart.a = bearing;
world->Raytrace( LocalToGlobal(raystart),
- range,
- fov,
- func,
- this,
- arg,
- samples,
- sample_count,
- ztest );
+ range,
+ fov,
+ func,
+ this,
+ arg,
+ samples,
+ sample_count,
+ ztest );
}
// utility for g_free()ing everything in a list
@@ -463,7 +462,7 @@
{
StgModel* child = (StgModel*)it->data;
if( child->IsDescendent( testmod ) )
- return true;
+ return true;
}
// neither mod nor a child of mod matches testmod
@@ -526,26 +525,25 @@
{
//printf( "model %s global pose ", token );
- if( this->gpose_dirty )
- {
- stg_pose_t parent_pose;
+ // if( this->gpose_dirty )
+ {
+ stg_pose_t parent_pose;
- // find my parent's pose
- if( this->parent )
- {
- parent_pose = parent->GetGlobalPose();
- stg_pose_sum( &global_pose, &parent_pose, &pose );
-
- // we are on top of our parent
- global_pose.z += parent->geom.size.z;
- }
- else
- memcpy( &global_pose, &pose, sizeof(stg_pose_t));
-
- this->gpose_dirty = false;
- //printf( " WORK " );
-
- }
+ // find my parent's pose
+ if( this->parent )
+ {
+ parent_pose = parent->GetGlobalPose();
+ stg_pose_sum( &global_pose, &parent_pose, &pose );
+
+ // we are on top of our parent
+ global_pose.z += parent->geom.size.z;
+ }
+ else
+ memcpy( &global_pose, &pose, sizeof(stg_pose_t));
+
+ this->gpose_dirty = false;
+ //printf( " WORK " );
+ }
//else
//printf( " CACHED " );
@@ -671,9 +669,9 @@
printf( "Model ");
printf( "%s:%s\n",
- // id,
- world->Token(),
- token );
+ // id,
+ world->Token(),
+ token );
for( GList* it=children; it; it=it->next )
((StgModel*)it->data)->Print( prefix );
@@ -685,8 +683,8 @@
static char txt[256];
snprintf(txt, sizeof(txt), "%s @ [%.2f,%.2f,%.2f,%.2f]",
- token,
- gpose.x, gpose.y, gpose.z, gpose.a );
+ token,
+ gpose.x, gpose.y, gpose.z, gpose.a );
return txt;
}
@@ -811,8 +809,6 @@
void StgModel::DrawTrailArrows()
{
- // PushColor( color );
-
double r,g,b,a;
double dx = 0.2;
@@ -838,9 +834,9 @@
glPolygonOffset(1.0, 1.0);
glBegin( GL_TRIANGLES );
- glVertex3f( 0, -dy, 0);
- glVertex3f( dx, 0, 0 );
- glVertex3f( 0, +dy, 0 );
+ glVertex3f( 0, -dy, 0);
+ glVertex3f( dx, 0, 0 );
+ glVertex3f( 0, +dy, 0 );
glEnd();
glDisable(GL_POLYGON_OFFSET_FILL);
@@ -851,9 +847,9 @@
glDepthMask(GL_FALSE);
glBegin( GL_TRIANGLES );
- glVertex3f( 0, -dy, 0);
- glVertex3f( dx, 0, 0 );
- glVertex3f( 0, +dy, 0 );
+ glVertex3f( 0, -dy, 0);
+ glVertex3f( dx, 0, 0 );
+ glVertex3f( 0, +dy, 0 );
glEnd();
glDepthMask(GL_TRUE);
@@ -876,25 +872,58 @@
void StgModel::DrawBlocks( )
{
+ // testing - draw bounding box
+// PushColor( color );
+
+// // bottom
+// glBegin( GL_LINE_LOOP );
+// glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 );
+// glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, 0 );
+// glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 );
+// glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, 0 );
+// glEnd();
+
+// // top
+// glBegin( GL_LINE_LOOP );
+// glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
+// glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, geom.size.z );
+// glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
+// glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, geom.size.z );
+// glEnd();
+
+// PopColor();
+
// TODO - fix this!
//if( rebuild_displaylist )
- {
- rebuild_displaylist = false;
+ {
+ //rebuild_displaylist = false;
- glNewList( blocks_dl, GL_COMPILE );
- LISTMETHOD( this->blocks, StgBlock*, Draw );
- glEndList();
- }
+ //glNewList( blocks_dl, GL_COMPILE );
+ LISTMETHOD( this->blocks, StgBlock*, Draw );
+ //glEndList();
+ }
- glCallList( blocks_dl );
+ //glCallList( blocks_dl );
}
// move into this model's local coordinate frame
void StgModel::PushLocalCoords()
{
glPushMatrix();
+
+ if( parent )
+ glTranslatef( 0,0, parent->geom.size.z );
+
gl_pose_shift( &pose );
gl_pose_shift( &geom.pose );
+
+ // useful debug - draw a point at the local origin
+ // PushColor( color );
+// glPointSize( 5.0 );
+// glBegin( GL_POINTS );
+// glVertex2i( 0, 0 );
+// glEnd();
+// PopColor();
}
void StgModel::PopCoords()
@@ -912,8 +941,8 @@
void StgModel::DrawStatus( StgCanvas* canvas )
{
- if( say_string )
- {
+ if( say_string )
+ {
float yaw, pitch, scale;
@@ -925,14 +954,14 @@
printf("x: %f, y: %f, z: %f, dist: %f, scale: %f\n", x, y, z,
dist, scale);
if ( canvas->perspectiveCam ) {
- pitch = -canvas->current_camera->pitch();
- yaw = -canvas->current_camera->yaw();
- scale = 500/dist;
+ pitch = -canvas->current_camera->pitch();
+ yaw = -canvas->current_camera->yaw();
+ scale = 500/dist;
}
else {
- pitch = canvas->current_camera->pitch();
- yaw = canvas->current_camera->yaw();
- scale = canvas->camera.getScale();
+ pitch = canvas->current_camera->pitch();
+ yaw = canvas->current_camera->yaw();
+ scale = canvas->camera.getScale();
}
float robotAngle = -rtod(pose.a);
@@ -990,14 +1019,14 @@
{
float stheta = -dtor( canvas->current_camera->pitch() );
float sphi = dtor( canvas->current_camera->yaw() );
- if( canvas->perspectiveCam == true ) {
- sphi = atan2(
- ( pose.x - canvas->current_camera->x() )
- ,
- ( pose.y - canvas->current_camera->y() )
- );
- stheta = -stheta;
- }
+ if( canvas->perspectiveCam == true ) {
+ sphi = atan2(
+ ( pose.x -
canvas->current_camera->x() )
+ ,
+ ( pose.y -
canvas->current_camera->y() )
+ );
+ stheta = -stheta;
+ }
glEnable(GL_TEXTURE_2D);
glBindTexture( GL_TEXTURE_2D, texture_id );
@@ -1029,104 +1058,61 @@
glDisable(GL_TEXTURE_2D);
}
-
-void StgModel::Draw( uint32_t flags, Stg::StgCanvas* canvas )
-{
- //PRINT_DEBUG1( "Drawing %s", token );
-
- glPushMatrix();
-
- // move into this model's local coordinate frame
- gl_pose_shift( &this->pose );
- gl_pose_shift( &this->geom.pose );
-
- // if ( ShowVisData )
- // DataVisualize();
- //
- // if( gui_grid && (flags & STG_SHOW_GRID) )
- // DrawGrid();
- //
- // if( flag_list && ShowFlags )
- // DrawFlagList();
- //
- // if( ShowBlinken )
- // DrawBlinkenlights();
- //
- // if ( ShowStatus ) {
- // DrawStatus( canvas );
- // }
-
- // shift up the CS to the top of this model
- //gl_coord_shift( 0,0, this->geom.size.z, 0 );
-
- // recursively draw the tree below this model
- for( GList* it=children; it; it=it->next )
- ((StgModel*)it->data)->Draw( flags, canvas );
-
- glPopMatrix(); // drop out of local coords
-}
-
void StgModel::DrawFlagList( void )
{
- if ( flag_list ) {
- glPushMatrix();
+ if( flag_list == NULL )
+ return;
+
+ PushLocalCoords();
+
+ GLUquadric* quadric = gluNewQuadric();
+ glTranslatef(0,0,1); // jump up
+ stg_pose_t gpose = GetGlobalPose();
+ glRotatef( 180 + rtod(-gpose.a),0,0,1 );
+
+
+ GList* list = g_list_copy( flag_list );
+ list = g_list_reverse(list);
+
+ for( GList* item = list; item; item = item->next )
+ {
- // move into this model's local coordinate frame
- gl_pose_shift( &this->pose );
- gl_pose_shift( &this->geom.pose );
-
- GLUquadric* quadric = gluNewQuadric();
- glTranslatef(0,0,1); // jump up
- stg_pose_t gpose = GetGlobalPose();
- glRotatef( 180 + rtod(-gpose.a),0,0,1 );
-
-
- GList* list = g_list_copy( flag_list );
- list = g_list_reverse(list);
-
- for( GList* item = list; item; item = item->next )
- {
-
- StgFlag* flag = (StgFlag*)item->data;
-
- glTranslatef( 0, 0, flag->size/2.0 );
-
-
- PushColor( flag->color );
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
-
- gluQuadricDrawStyle( quadric, GLU_FILL );
- gluSphere( quadric, flag->size/2.0, 4,2 );
-
- // draw the edges darker version of the same color
- double r,g,b,a;
- stg_color_unpack( flag->color, &r, &g, &b, &a );
- PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a ));
-
- gluQuadricDrawStyle( quadric, GLU_LINE );
- gluSphere( quadric, flag->size/2.0, 4,2 );
-
- PopColor();
- PopColor();
-
- glTranslatef( 0, 0, flag->size/2.0 );
- }
-
- g_list_free( list );
-
- gluDeleteQuadric( quadric );
+ StgFlag* flag = (StgFlag*)item->data;
- glPopMatrix();
- }
+ glTranslatef( 0, 0, flag->size/2.0 );
+
+
+ PushColor( flag->color );
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+
+ gluQuadricDrawStyle( quadric, GLU_FILL );
+ gluSphere( quadric, flag->size/2.0, 4,2 );
+
+ // draw the edges darker version of the same color
+ double r,g,b,a;
+ stg_color_unpack( flag->color, &r, &g, &b, &a );
+ PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a ));
+
+ gluQuadricDrawStyle( quadric, GLU_LINE );
+ gluSphere( quadric, flag->size/2.0, 4,2 );
+
+ PopColor();
+ PopColor();
+
+ glTranslatef( 0, 0, flag->size/2.0 );
+ }
+
+ g_list_free( list );
+
+ gluDeleteQuadric( quadric );
+
+ PopCoords();
}
+
void StgModel::DrawBlinkenlights()
{
- glPushMatrix();
-
- // move into this model's local coordinate frame
- gl_pose_shift( &this->pose );
- gl_pose_shift( &this->geom.pose );
+ PushLocalCoords();
GLUquadric* quadric = gluNewQuadric();
//glTranslatef(0,0,1); // jump up
@@ -1138,7 +1124,7 @@
for( unsigned int i=0; i<blinkenlights->len; i++ )
{
stg_blinkenlight_t* b =
- (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
+ (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
assert(b);
glTranslatef( b->pose.x, b->pose.y, b->pose.z );
@@ -1146,9 +1132,9 @@
PushColor( b->color );
if( b->enabled )
- gluQuadricDrawStyle( quadric, GLU_FILL );
+ gluQuadricDrawStyle( quadric, GLU_FILL );
else
- gluQuadricDrawStyle( quadric, GLU_LINE );
+ gluQuadricDrawStyle( quadric, GLU_LINE );
gluSphere( quadric, b->size/2.0, 8,8 );
@@ -1156,30 +1142,26 @@
}
gluDeleteQuadric( quadric );
- glPopMatrix();
+
+ PopCoords();
}
void StgModel::DrawPicker( void )
{
//PRINT_DEBUG1( "Drawing %s", token );
+ PushLocalCoords();
- glPushMatrix();
-
- // move into this model's local coordinate frame
- gl_pose_shift( &this->pose );
- gl_pose_shift( &this->geom.pose );
-
// draw the boxes
for( GList* it=blocks; it; it=it->next )
((StgBlock*)it->data)->DrawSolid() ;
// shift up the CS to the top of this model
- gl_coord_shift( 0,0, this->geom.size.z, 0 );
+ //gl_coord_shift( 0,0, this->geom.size.z, 0 );
// recursively draw the tree below this model
LISTMETHOD( this->children, StgModel*, DrawPicker );
- glPopMatrix(); // drop out of local coords
+ PopCoords();
}
void StgModel::DataVisualize( void )
@@ -1192,9 +1174,6 @@
PushLocalCoords();
DataVisualize(); // virtual function overridden by most model types
- // shift to top of this model - why is this not necessary?
- glTranslatef( 0,0, geom.size.z );
-
// and draw the children
LISTMETHOD( children, StgModel*, DataVisualizeTree );
@@ -1207,7 +1186,7 @@
if ( gui_grid )
{
PushLocalCoords();
-
+
stg_bounds3d_t vol;
vol.x.min = -geom.size.x/2.0;
vol.x.max = geom.size.x/2.0;
@@ -1263,14 +1242,22 @@
((StgModel*)it->data)->GPoseDirtyTree();
}
+// find the Z-axis offset of this model, by recursively summing the
+// height of its parents and any Z poses shifts
+// stg_meters_t StgModel::BaseHeight()
+// {
+// if( ! parent )
+// return pose.z;
+
+// return pose.z + parent->geom.size.z + parent->BaseHeight();
+// }
+
+
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( parent )
- pose.z += parent->geom.size.z;
-
// if the pose has changed, we need to do some work
if( memcmp( &this->pose, &pose, sizeof(stg_pose_t) ) != 0 )
{
@@ -1309,7 +1296,6 @@
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",
@@ -1317,6 +1303,8 @@
// geom->pose.x, geom->pose.y, geom->pose.a,
// geom->size.x, geom->size.y );
+ this->gpose_dirty = true;
+
UnMap();
this->geom = geom;
@@ -1471,14 +1459,14 @@
static bool collision_match( StgBlock* testblock,
- StgModel* finder )
+
StgModel* finder )
{
return( (testblock->Model() != finder) &&
testblock->Model()->ObstacleReturn() );
}
void StgModel::PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax,
- stg_meters_t ymin, stg_meters_t ymax )
+
stg_meters_t ymin, stg_meters_t ymax )
{
while( TestCollision( NULL, NULL ) )
SetPose( random_pose( xmin,xmax, ymin, ymax ));
@@ -1491,8 +1479,8 @@
}
StgModel* StgModel::TestCollision( stg_pose_t posedelta,
- stg_meters_t* hitx,
- stg_meters_t* hity )
+
stg_meters_t* hitx,
+
stg_meters_t* hity )
{
/* stg_model_t* child_hit = NULL; */
@@ -1530,39 +1518,39 @@
// loop over all edges of the block
for( unsigned int p=0; p < pt_count; p++ )
- {
- // find the local poses of the ends of this block edge
- stg_point_t* pt1 = &pts[p];
- stg_point_t* pt2 = &pts[(p+1) % pt_count];
- double dx = pt2->x - pt1->x;
- double dy = pt2->y - pt1->y;
+ {
+ // find the local poses of the ends of this block edge
+ stg_point_t* pt1 = &pts[p];
+ stg_point_t* pt2 = &pts[(p+1) % pt_count];
+ double dx = pt2->x - pt1->x;
+ double dy = pt2->y - pt1->y;
- // find the range and bearing to raytrace along the block edge
- double range = hypot( dx, dy );
- double bearing = atan2( dy,dx );
+ // find the range and bearing to raytrace along the
block edge
+ double range = hypot( dx, dy );
+ double bearing = atan2( dy,dx );
- stg_pose_t edgepose;
- bzero(&edgepose,sizeof(edgepose));
- edgepose.x = pt1->x;
- edgepose.y = pt1->y;
- edgepose.a = bearing;
+ stg_pose_t edgepose;
+ bzero(&edgepose,sizeof(edgepose));
+ edgepose.x = pt1->x;
+ edgepose.y = pt1->y;
+ edgepose.a = bearing;
- // shift the edge ray vector by the local change in pose
- // stg_pose_t raypose;
- // stg_pose_sum( &raypose, posedelta, &edgepose );
+ // 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,
- NULL,
- &sample,
- true );
+ // raytrace in local coordinates
+ stg_raytrace_sample_t sample;
+ Raytrace( pose_sum( posedelta, edgepose),
+ range,
+
(stg_block_match_func_t)collision_match,
+ NULL,
+ &sample,
+ true );
- if( sample.block )
- hitmod = sample.block->Model();
- }
+ if( sample.block )
+ hitmod = sample.block->Model();
+ }
}
// re-render myself
@@ -1586,7 +1574,7 @@
checkpoint.time = world->sim_time;
if( trail->len > 100 )
- g_array_remove_index( trail, 0 );
+ g_array_remove_index( trail, 0 );
g_array_append_val( this->trail, checkpoint );
}
@@ -1767,7 +1755,7 @@
StgModel* found = child->GetUnsubscribedModelOfType( type );
if( found )
- return found;
+ return found;
}
// nothing matching below this model
Modified: code/stage/trunk/libstage/model_camera.cc
===================================================================
--- code/stage/trunk/libstage/model_camera.cc 2008-07-06 03:49:04 UTC (rev
6776)
+++ code/stage/trunk/libstage/model_camera.cc 2008-07-06 06:32:02 UTC (rev
6777)
@@ -341,7 +341,3 @@
}
-void StgModelCamera::Draw( uint32_t flags, StgCanvas* canvas )
-{
- StgModel::Draw( flags, canvas );
-}
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2008-07-06 03:49:04 UTC (rev 6776)
+++ code/stage/trunk/libstage/stage.hh 2008-07-06 06:32:02 UTC (rev 6777)
@@ -1324,7 +1324,6 @@
virtual void Shutdown();
virtual void Update();
virtual void UpdatePose();
- virtual void Draw( uint32_t flags, StgCanvas* canvas );
void DrawBlocksTree();
virtual void DrawBlocks();
@@ -1738,7 +1737,6 @@
StgModel* mod; //< model to which this block belongs
stg_point_int_t* pts_global_pixels; //< points defining a
polygon in global coords
- //GLubyte* edge_indices;
stg_color_t color;
bool inherit_color;
@@ -2540,7 +2538,7 @@
virtual void Update();
///Draw Camera Model - TODO
- virtual void Draw( uint32_t flags, StgCanvas* canvas );
+ //virtual void Draw( uint32_t flags, StgCanvas* canvas );
///Draw camera visualization
virtual void DataVisualize();
Modified: code/stage/trunk/worlds/autolab.world
===================================================================
--- code/stage/trunk/worlds/autolab.world 2008-07-06 03:49:04 UTC (rev
6776)
+++ code/stage/trunk/worlds/autolab.world 2008-07-06 06:32:02 UTC (rev
6777)
@@ -12,8 +12,6 @@
include "chatterbox.inc"
include "map.inc"
-size [10 10]
-
# configure the GUI window
window
(
@@ -26,12 +24,12 @@
map
(
#bitmap "bitmaps/autolab.png"
- size [10 10]
+ size [10 10 0.5]
boundary 1
gui_grid 1
name "lab"
- polygon[0].fill 0
+ # block[0].fill 0
)
# define and place some grippable objects
@@ -40,8 +38,8 @@
define mother pioneer2dx
(
laser()
- ptz( blobfinder() )
- gripper( pose [0.200 0.000 0.000] color "gray" )
+ #ptz( blobfinder() )
+ #gripper( pose [0.200 0.000 0 0.000] color "gray" )
fiducialfinder(
# set to match the fiducial_key of objects you want to
@@ -53,14 +51,14 @@
mother
(
name "mother0"
- pose [-4.029 -3.369 -325.994]
+ pose [-4.029 -3.369 0 -325.994]
color "red"
)
mother
(
name "mother1"
- pose [-3.220 -4.179 -313.105]
+ pose [-3.220 -4.179 0 -313.105]
color "blue"
)
@@ -69,34 +67,34 @@
define bluechatterbox chatterbox ( color "blue" )
define redchatterbox chatterbox ( color "red" )
-bluechatterbox( name "cbb0" pose [3.321 3.012 0.000] )
-bluechatterbox( name "cbb1" pose [3.022 4.003 -171.141] )
-bluechatterbox( name "cbb2" pose [3.144 2.468 -159.523] )
-bluechatterbox( name "cbb3" pose [1.477 3.816 -28.297] )
-bluechatterbox( name "cbb4" pose [2.323 3.390 -108.654] )
-bluechatterbox( name "cbb5" pose [2.443 3.750 -108.654] )
-bluechatterbox( name "cbb6" pose [2.638 3.435 -108.654] )
-bluechatterbox( name "cbb7" pose [2.668 3.660 -108.654] )
-bluechatterbox( name "cbb8" pose [2.263 4.020 -108.654] )
-bluechatterbox( name "cbb9" pose [2.188 3.630 -108.654] )
+bluechatterbox( name "cbb0" pose [3.321 3.012 0 0.000] )
+bluechatterbox( name "cbb1" pose [3.022 4.003 0 -171.141] )
+bluechatterbox( name "cbb2" pose [3.144 2.468 0 -159.523] )
+bluechatterbox( name "cbb3" pose [1.477 3.816 0 -28.297] )
+bluechatterbox( name "cbb4" pose [2.323 3.390 0 -108.654] )
+bluechatterbox( name "cbb5" pose [2.443 3.750 0 -108.654] )
+bluechatterbox( name "cbb6" pose [2.638 3.435 0 -108.654] )
+bluechatterbox( name "cbb7" pose [2.668 3.660 0 -108.654] )
+bluechatterbox( name "cbb8" pose [2.263 4.020 0 -108.654] )
+bluechatterbox( name "cbb9" pose [2.188 3.630 0 -108.654] )
-redchatterbox( name "cbr0" pose [2.653 4.152 0.000] )
-redchatterbox( name "cbr1" pose [2.659 3.053 0.000] )
-redchatterbox( name "cbr2" pose [3.326 3.593 -53.892] )
-redchatterbox( name "cbr3" pose [3.691 3.061 -28.297] )
-redchatterbox( name "cbr4" pose [1.632 3.232 -54.883] )
-redchatterbox( name "cbr5" pose [2.157 2.902 -54.883] )
-redchatterbox( name "cbr6" pose [2.382 2.452 -54.883] )
-redchatterbox( name "cbr7" pose [2.112 2.137 -54.883] )
-redchatterbox( name "cbr8" pose [1.827 2.347 -54.883] )
-redchatterbox( name "cbr9" pose [1.647 2.737 -54.883] )
+redchatterbox( name "cbr0" pose [2.653 4.152 0 0.000] )
+redchatterbox( name "cbr1" pose [2.659 3.053 0 0.000] )
+redchatterbox( name "cbr2" pose [3.326 3.593 0 -53.892] )
+redchatterbox( name "cbr3" pose [3.691 3.061 0 -28.297] )
+redchatterbox( name "cbr4" pose [1.632 3.232 0 -54.883] )
+redchatterbox( name "cbr5" pose [2.157 2.902 0 -54.883] )
+redchatterbox( name "cbr6" pose [2.382 2.452 0 -54.883] )
+redchatterbox( name "cbr7" pose [2.112 2.137 0 -54.883] )
+redchatterbox( name "cbr8" pose [1.827 2.347 0 -54.883] )
+redchatterbox( name "cbr9" pose [1.647 2.737 0 -54.883] )
# define and place some charging stations
define charger model
(
- size [ 0.5 0.5 ]
+ size [ 0.5 0.5 0.01]
color "green"
#energy.capacity -1.0 # infinite
@@ -108,4 +106,4 @@
fiducial_key 1
)
-charger( pose [ -4.522 -4.546 0.000 ] fiducial_return 1 )
+charger( pose [ -4.522 -4.546 0 0.000 ] fiducial_return 1 )
Modified: code/stage/trunk/worlds/chatterbox.inc
===================================================================
--- code/stage/trunk/worlds/chatterbox.inc 2008-07-06 03:49:04 UTC (rev
6776)
+++ code/stage/trunk/worlds/chatterbox.inc 2008-07-06 06:32:02 UTC (rev
6777)
@@ -3,23 +3,24 @@
# Author: Richard T Vaughan (rtv)
# $Id: chatterbox.inc,v 1.5 2008-01-15 01:25:42 rtv Exp $
-define chatterbox position
+include "irobot.inc"
+
+define chatterbox create
(
# long range IRs
- ranger(
- base -0.1
+ ranger
+ (
scount 3
spose[0] [ 0.050 0.0 0 ]
spose[1] [ 0.0 0.050 90 ]
spose[2] [ 0.0 -0.050 270 ]
-
sview [ 0 1.0 20 ]
ssize [0.01 0.03 ]
)
# short rage IRs
- ranger(
- base -0.1
+ ranger
+ (
scount 4
spose[0] [ 0.050 0.0 0 ]
spose[1] [ 0.035 0.035 30 ]
@@ -29,12 +30,4 @@
sview [ 0 0.5 30 ]
ssize [0.01 0.03 ]
)
-
-
- mass 0.5
- size [0.12 0.12]
- height 0.12
- gui_nose 1
-
- polygon[0].fill 0
)
\ No newline at end of file
Modified: code/stage/trunk/worlds/everything.world
===================================================================
--- code/stage/trunk/worlds/everything.world 2008-07-06 03:49:04 UTC (rev
6776)
+++ code/stage/trunk/worlds/everything.world 2008-07-06 06:32:02 UTC (rev
6777)
@@ -18,8 +18,8 @@
window
(
size [ 1059.000 541.000 ]
- center [-6.333 2.011]
- scale 80.217 # pixels per meter
+ center [0 0.042]
+ scale 23.827 # pixels per meter
show_data 1
)
@@ -56,7 +56,7 @@
(
ranger( alwayson 1 )
- sicklaser( pose [0.030 0 0.220 0 ] )
+ sicklaser( pose [0.030 0 0 0 ] )
fiducial( range_max 8 range_max_id 5 )
@@ -88,7 +88,7 @@
color "red"
name "redrobot_w_camera"
pose [-5.645 3.034 0 -162.098]
- camera()
+ camera( pose [0.060 0 0.190 0 ] )
)
trickedoutpioneer
@@ -149,12 +149,27 @@
silly(
name "ghost"
- pose [-6.060 0.794 0 0]
+ pose [-5.583 -0.684 0 0]
color "blue"
bitmap "bitmaps/ghost.png"
)
-roomba(
- name "roomba1"
+create(
+ name "create0"
+ pose [-8.844 0.019 0 0]
+)
+
+create(
+ name "create1"
+ pose [-8.625 -0.527 0 0]
+)
+
+create(
+ name "create2"
+ pose [-8.501 -1.044 0 0]
+)
+
+create(
+ name "create3"
pose [-9.000 0.500 0 0]
)
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2008-07-06 03:49:04 UTC (rev 6776)
+++ code/stage/trunk/worlds/fasr.world 2008-07-06 06:32:02 UTC (rev 6777)
@@ -57,14 +57,13 @@
define autorob pioneer2dx
(
- sicklaser( pose [ 0.040 0 0.220 0 ] samples 30 range_max 5 laser_return 2 )
+ sicklaser( pose [ 0.040 0 0 0 ] samples 180 range_max 5 laser_return 2 )
ctrl "fasr"
#camera( pose [ 0 0 0 0 ] width 100 height 100 horizfov 70 vertfov 40 yaw 0 )
#say "Autolab"
)
-
autorob( pose [3.273 4.908 0 1.930] )
autorob( pose [6.635 6.458 0 -52.629] )
autorob( pose [6.385 5.805 0 -87.082] )
Copied: code/stage/trunk/worlds/irobot.inc (from rev 6776,
code/stage/trunk/worlds/roomba.inc)
===================================================================
--- code/stage/trunk/worlds/irobot.inc (rev 0)
+++ code/stage/trunk/worlds/irobot.inc 2008-07-06 06:32:02 UTC (rev 6777)
@@ -0,0 +1,36 @@
+
+define roomba position
+(
+ size [0.33 0.33 0.1]
+
+ # this block approximates the circular shape of a Roomba
+ blocks 1
+ block[0].points 16
+ block[0].point[0] [ 0.225 0.000 ]
+ block[0].point[1] [ 0.208 0.086 ]
+ block[0].point[2] [ 0.159 0.159 ]
+ block[0].point[3] [ 0.086 0.208 ]
+ block[0].point[4] [ 0.000 0.225 ]
+ block[0].point[5] [ -0.086 0.208 ]
+ block[0].point[6] [ -0.159 0.159 ]
+ block[0].point[7] [ -0.208 0.086 ]
+ block[0].point[8] [ -0.225 0.000 ]
+ block[0].point[9] [ -0.208 -0.086 ]
+ block[0].point[10] [ -0.159 -0.159 ]
+ block[0].point[11] [ -0.086 -0.208 ]
+ block[0].point[12] [ -0.000 -0.225 ]
+ block[0].point[13] [ 0.086 -0.208 ]
+ block[0].point[14] [ 0.159 -0.159 ]
+ block[0].point[15] [ 0.208 -0.086 ]
+
+ # this bumper array VERY crudely approximates the Roomba's bumpers
+# bumper( bcount 2
+# blength 0.33
+# bpose[0] [0.12 0.12 45]
+# bpose[1] [0.12 -0.12 -45]
+# )
+
+ color "gray50"
+)
+
+define create roomba( color "gray90" )
\ No newline at end of file
Deleted: code/stage/trunk/worlds/roomba.inc
===================================================================
--- code/stage/trunk/worlds/roomba.inc 2008-07-06 03:49:04 UTC (rev 6776)
+++ code/stage/trunk/worlds/roomba.inc 2008-07-06 06:32:02 UTC (rev 6777)
@@ -1,36 +0,0 @@
-
-define roomba position
-(
- size [0.33 0.33 0.1]
-
- # this block approximates the circular shape of a Roomba
- blocks 1
- block[0].points 16
- block[0].point[0] [ 0.225 0.000 ]
- block[0].point[1] [ 0.208 0.086 ]
- block[0].point[2] [ 0.159 0.159 ]
- block[0].point[3] [ 0.086 0.208 ]
- block[0].point[4] [ 0.000 0.225 ]
- block[0].point[5] [ -0.086 0.208 ]
- block[0].point[6] [ -0.159 0.159 ]
- block[0].point[7] [ -0.208 0.086 ]
- block[0].point[8] [ -0.225 0.000 ]
- block[0].point[9] [ -0.208 -0.086 ]
- block[0].point[10] [ -0.159 -0.159 ]
- block[0].point[11] [ -0.086 -0.208 ]
- block[0].point[12] [ -0.000 -0.225 ]
- block[0].point[13] [ 0.086 -0.208 ]
- block[0].point[14] [ 0.159 -0.159 ]
- block[0].point[15] [ 0.208 -0.086 ]
-
- # this bumper array VERY crudely approximates the Roomba's bumpers
-# bumper( bcount 2
-# blength 0.33
-# bpose[0] [0.12 0.12 45]
-# bpose[1] [0.12 -0.12 -45]
-# )
-
- color "gray50"
-)
-
-define create roomba( color "gray 90" )
\ No newline at end of file
Modified: code/stage/trunk/worlds/simple.world
===================================================================
--- code/stage/trunk/worlds/simple.world 2008-07-06 03:49:04 UTC (rev
6776)
+++ code/stage/trunk/worlds/simple.world 2008-07-06 06:32:02 UTC (rev
6777)
@@ -28,19 +28,32 @@
floorplan
(
name "cave"
- size [16 16 1.5]
+ size [16 16 0.6]
bitmap "bitmaps/cave.png"
)
pioneer2dx
-(
+(
pose [ -6.5 -6.5 0 45 ]
- sicklaser( pose [ 0.04 0 0 0 ] )
+ sicklaser( pose [0.04 0 0 0]
+# model( size [0.1 0.1 0.1 ] color "red"
+# model ( size [0.1 0.1 0.1 ] color "green"
+# model (size [0.1 0.1 0.1 ] color "blue" pose [0 0 0.03 0 ]
+# model (size [0.1 0.1 0.1 ] color "magenta"
+# )
+# )
+# )
+# )
+ )
+
+ #pose [ 0.04 0 0 0 ] )
+
# can refer to the robot by this name
name "r0"
# remove this if you're using Player to control the robot
ctrl "wander"
+
)
Modified: code/stage/trunk/worlds/swarmbenchmark/cave.world
===================================================================
--- code/stage/trunk/worlds/swarmbenchmark/cave.world 2008-07-06 03:49:04 UTC
(rev 6776)
+++ code/stage/trunk/worlds/swarmbenchmark/cave.world 2008-07-06 06:32:02 UTC
(rev 6777)
@@ -24,7 +24,7 @@
floorplan
(
name "cave"
- size [16.000 16.000 1.500]
+ size [16.000 16.000 0.600]
pose [0 0 0 0]
bitmap "../bitmaps/cave.png"
)
@@ -33,11 +33,11 @@
#define rob fancypioneer2dx # alternative with more complex polygons
(
- sicklaser( )#pose4 [ 0.040 0.000 0.01 0.000 ] samples 180 )
+ sicklaser( pose [ 0.040 0 0 0 ] samples 180 )
# alternative laser with more complex polygons
- # fancysicklaser( pose [ 0.040 0.000 0.000 ] samples 180 )
+ # fancysicklaser( pose [ 0.040 0 0 0 ] samples 180 )
- ranger( pose [ 0 0 0.170 0 ] )
+ ranger( pose [ 0 0 -0.050 0 ] )
ctrl "expand"
)
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
Sponsored by: SourceForge.net Community Choice Awards: VOTE NOW!
Studies have shown that voting for your favorite open source project,
along with a healthy diet, reduces your potential for chronic lameness
and boredom. Vote Now at http://www.sourceforge.net/community/cca08
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
