Update of /cvsroot/playerstage/code/stage/libstage
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv14241/libstage
Modified Files:
Tag: opengl
block.cc canvas.cc model.cc model_laser.cc model_ranger.cc
stage.hh stest.cc world.cc worldgui.cc
Log Message:
experimenting with trail visualizations. also fixed 2.5D raytracing. tracking
down disappearing robot bug
Index: worldgui.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/worldgui.cc,v
retrieving revision 1.1.2.4
retrieving revision 1.1.2.5
diff -C2 -d -r1.1.2.4 -r1.1.2.5
*** worldgui.cc 26 Nov 2007 06:28:16 -0000 1.1.2.4
--- worldgui.cc 29 Nov 2007 07:58:59 -0000 1.1.2.5
***************
*** 15,18 ****
--- 15,21 ----
static const char* MITEM_VIEW_FOLLOW = "View/Follow";
static const char* MITEM_VIEW_CLOCK = "View/Clock";
+ static const char* MITEM_VIEW_FOOTPRINTS = "View/Trails/Footprints";
+ static const char* MITEM_VIEW_TRAILS = "View/Trails/Blocks";
+ static const char* MITEM_VIEW_ARROWS = "View/Trails/Arrows";
// transform the current coordinate frame by the given pose
***************
*** 64,67 ****
--- 67,73 ----
else if( strcmp(picked, MITEM_VIEW_OCCUPANCY ) == 0 ) canvas->InvertView(
STG_SHOW_OCCUPANCY );
else if( strcmp(picked, MITEM_VIEW_CLOCK ) == 0 ) canvas->InvertView(
STG_SHOW_CLOCK );
+ else if( strcmp(picked, MITEM_VIEW_FOOTPRINTS ) == 0 ) canvas->InvertView(
STG_SHOW_FOOTPRINT );
+ else if( strcmp(picked, MITEM_VIEW_ARROWS ) == 0 ) canvas->InvertView(
STG_SHOW_ARROWS );
+ else if( strcmp(picked, MITEM_VIEW_TRAILS ) == 0 ) canvas->InvertView(
STG_SHOW_TRAILS );
else PRINT_ERR1( "Unrecognized menu item \"%s\" not handled", picked );
***************
*** 103,106 ****
--- 109,119 ----
mbar->add( MITEM_VIEW_CLOCK, 0, (Fl_Callback*)view_toggle_cb,
(void*)canvas,
FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_CLOCK ?
FL_MENU_VALUE : 0 ));
+
+ mbar->add( MITEM_VIEW_FOOTPRINTS, 0, (Fl_Callback*)view_toggle_cb,
(void*)canvas,
+ FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_FOOTPRINT ?
FL_MENU_VALUE : 0 ));
+ mbar->add( MITEM_VIEW_ARROWS, 0, (Fl_Callback*)view_toggle_cb,
(void*)canvas,
+ FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_ARROWS ?
FL_MENU_VALUE : 0 ));
+ mbar->add( MITEM_VIEW_TRAILS, 0, (Fl_Callback*)view_toggle_cb,
(void*)canvas,
+ FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_TRAILS ?
FL_MENU_VALUE : 0 ));
mbar->add( "Help", 0, 0, 0, FL_SUBMENU );
***************
*** 143,148 ****
uint32_t quadtree = wf->ReadInt(wf_section, "show_tree", flags &
STG_SHOW_QUADTREE ) ? STG_SHOW_QUADTREE : 0;
uint32_t clock = wf->ReadInt(wf_section, "show_clock", flags &
STG_SHOW_CLOCK ) ? STG_SHOW_CLOCK : 0;
! canvas->SetShowFlags( grid | data | follow | blocks | quadtree | clock );
canvas->invalidate(); // we probably changed something
--- 156,165 ----
uint32_t quadtree = wf->ReadInt(wf_section, "show_tree", flags &
STG_SHOW_QUADTREE ) ? STG_SHOW_QUADTREE : 0;
uint32_t clock = wf->ReadInt(wf_section, "show_clock", flags &
STG_SHOW_CLOCK ) ? STG_SHOW_CLOCK : 0;
+ uint32_t trails = wf->ReadInt(wf_section, "show_trails", flags &
STG_SHOW_TRAILS ) ? STG_SHOW_TRAILS : 0;
+ uint32_t arrows = wf->ReadInt(wf_section, "show_arrows", flags &
STG_SHOW_ARROWS ) ? STG_SHOW_ARROWS : 0;
+ uint32_t footprints = wf->ReadInt(wf_section, "show_footprints", flags &
STG_SHOW_FOOTPRINT ) ? STG_SHOW_FOOTPRINT : 0;
! canvas->SetShowFlags( grid | data | follow | blocks | quadtree | clock
! | trails | arrows | footprints );
canvas->invalidate(); // we probably changed something
Index: stest.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/stest.cc,v
retrieving revision 1.1.2.19
retrieving revision 1.1.2.20
diff -C2 -d -r1.1.2.19 -r1.1.2.20
*** stest.cc 27 Nov 2007 05:36:02 -0000 1.1.2.19
--- stest.cc 29 Nov 2007 07:58:59 -0000 1.1.2.20
***************
*** 23,29 ****
StgModelPosition* position;
StgModelRanger* ranger;
! int randcount ;
! int avoidcount;
! bool obs;
} robot_t;
--- 23,29 ----
StgModelPosition* position;
StgModelRanger* ranger;
! //int randcount ;
! //int avoidcount;
! //bool obs;
} robot_t;
***************
*** 78,84 ****
for( int i=0; i<POPSIZE; i++ )
{
! robots[i].randcount = 0;
! robots[i].avoidcount = 0;
! robots[i].obs = false;
char* base = "r";
--- 78,84 ----
for( int i=0; i<POPSIZE; i++ )
{
! //robots[i].randcount = 0;
! //robots[i].avoidcount = 0;
! //robots[i].obs = false;
char* base = "r";
***************
*** 88,97 ****
robots[i].position->Subscribe();
! printf( "returned %s\n", robots[i].position->Token() );
!
! robots[i].laser = (StgModelLaser*)
! robots[i].position->GetUnsubcribedModelOfType( "laser" );
! assert(robots[i].laser);
! robots[i].laser->Subscribe();
robots[i].ranger = (StgModelRanger*)
--- 88,95 ----
robots[i].position->Subscribe();
! // robots[i].laser = (StgModelLaser*)
! // robots[i].position->GetUnsubcribedModelOfType( "laser" );
! // assert(robots[i].laser);
! // robots[i].laser->Subscribe();
robots[i].ranger = (StgModelRanger*)
***************
*** 102,113 ****
// start the clock
! world.Start();
//puts( "done" );
- double newspeed = 0.0;
- double newturnrate = 0.0;
-
- //stg_world_set_interval_real( world, 0 );
-
while( world.RealTimeUpdate() )
// while( world.Update() )
--- 100,106 ----
// start the clock
! //world.Start();
//puts( "done" );
while( world.RealTimeUpdate() )
// while( world.Update() )
***************
*** 120,128 ****
double dx=0, dy=0;
! int num_ranges = rgr->sensor_count;//spp[i]->GetCount();
! for( int s=0; s<num_ranges; s++ )
{
! //player_pose3d_t spose = spp[i]->GetPose(s);
! double srange = rgr->sensors[s].range; //spp[i]->GetScan(s);
dx += srange * cos( rgr->sensors[s].pose.a );
--- 113,119 ----
double dx=0, dy=0;
! for( int s=0; s< rgr->sensor_count; s++ )
{
! double srange = rgr->samples[s];
dx += srange * cos( rgr->sensors[s].pose.a );
***************
*** 137,145 ****
double turn_speed = WGAIN * resultant_angle;
! int forward = num_ranges/2 -1 ;
// if the front is clear, drive forwards
! if( (rgr->sensors[forward-1].range > SAFE_DIST) &&
! (rgr->sensors[forward ].range > SAFE_DIST) &&
! (rgr->sensors[forward+1].range > SAFE_DIST) &&
(fabs( resultant_angle ) < SAFE_ANGLE) )
{
--- 128,136 ----
double turn_speed = WGAIN * resultant_angle;
! int forward = rgr->sensor_count/2 -1 ;
// if the front is clear, drive forwards
! if( (rgr->samples[forward-1] > SAFE_DIST) &&
! (rgr->samples[forward ] > SAFE_DIST) &&
! (rgr->samples[forward+1] > SAFE_DIST) &&
(fabs( resultant_angle ) < SAFE_ANGLE) )
{
Index: model_laser.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/model_laser.cc,v
retrieving revision 1.1.2.13
retrieving revision 1.1.2.14
diff -C2 -d -r1.1.2.13 -r1.1.2.14
*** model_laser.cc 27 Nov 2007 07:03:33 -0000 1.1.2.13
--- model_laser.cc 29 Nov 2007 07:58:59 -0000 1.1.2.14
***************
*** 200,203 ****
--- 200,207 ----
//printf( " [%d] ", (int)t );
+ stg_pose_t pose;
+ bzero( &pose, sizeof(pose) );
+ pose.z =
+
samples[t].range = Raytrace( bearing,
range_max,
***************
*** 205,209 ****
(const void*)this,
&hitmod );
-
// if we hit a model and it reflects brightly, we set
--- 209,212 ----
Index: model.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/model.cc,v
retrieving revision 1.1.2.17
retrieving revision 1.1.2.18
diff -C2 -d -r1.1.2.17 -r1.1.2.18
*** model.cc 27 Nov 2007 07:03:33 -0000 1.1.2.17
--- model.cc 29 Nov 2007 07:58:59 -0000 1.1.2.18
***************
*** 1,4 ****
- //extern GList* dl_list;
-
/** @ingroup stage
@{
--- 1,2 ----
***************
*** 89,93 ****
- iff 1, this model can be detected by a ranger.
- gripper_return [bool]
! - iff 1, this model can be gripped by a gripper and can be pushed around
by collisions with anything that has a non-zero obstacle_return.
*/
--- 87,91 ----
- iff 1, this model can be detected by a ranger.
- gripper_return [bool]
! - iff 1, this model can be gripped by a gripper and can be pushed around
by collisions with anything that has moa non-zero obstacle_return.
*/
***************
*** 185,189 ****
memset( &global_pose, 0, sizeof(global_pose));
!
this->data_fresh = false;
--- 183,188 ----
memset( &global_pose, 0, sizeof(global_pose));
! this->ray_list = NULL;
! this->trail = g_array_new( false, false, sizeof(stg_trail_item_t) );
this->data_fresh = false;
***************
*** 729,732 ****
--- 728,837 ----
}
+
+ void StgModel::DrawTrailFootprint()
+ {
+ double r,g,b,a;
+
+ for( int i=trail->len-1; i>=0; i-- )
+ {
+ stg_trail_item_t* checkpoint = & g_array_index( trail,
stg_trail_item_t, i );
+
+ glPushMatrix();
+ gl_pose_shift( &checkpoint->pose );
+ gl_pose_shift( &geom.pose );
+
+ stg_color_unpack( checkpoint->color, &r, &g, &b, &a );
+ PushColor( r, g, b, 0.1 );
+
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL );
+ LISTMETHOD( this->blocks, StgBlock*, DrawFootPrint );
+
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE );
+ PushColor( r/2, g/2, b/2, 0.1 );
+ LISTMETHOD( this->blocks, StgBlock*, DrawFootPrint );
+
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL );
+
+ PopColor();
+ PopColor();
+ glPopMatrix();
+ }
+ }
+
+ void StgModel::DrawTrailBlocks()
+ {
+ double timescale = 0.0000001;
+
+ for( int i=trail->len-1; i>=0; i-- )
+ {
+ stg_trail_item_t* checkpoint = & g_array_index( trail,
stg_trail_item_t, i );
+
+ stg_pose_t pose;
+ memcpy( &pose, &checkpoint->pose, sizeof(pose));
+ pose.z = (world->sim_time - checkpoint->time) * timescale;
+
+ glPushMatrix();
+ gl_pose_shift( &pose );
+ gl_pose_shift( &geom.pose );
+ LISTMETHOD( this->blocks, StgBlock*, Draw );
+ glPopMatrix();
+ }
+ }
+
+ void StgModel::DrawTrailArrows()
+ {
+ // PushColor( color );
+
+ double r,g,b,a;
+
+ double dx = 0.2;
+ double dy = 0.07;
+ double z;
+
+ double timescale = 0.0000001;
+
+ for( int i=0; i<trail->len; i++ )
+ {
+ stg_trail_item_t* checkpoint = & g_array_index( trail,
stg_trail_item_t, i );
+
+ stg_pose_t pose;
+ memcpy( &pose, &checkpoint->pose, sizeof(pose));
+ pose.z = (world->sim_time - checkpoint->time) * timescale;
+
+ glPushMatrix();
+
+ // set the height proportional to age
+
+ gl_pose_shift( &pose );
+
+ PushColor( checkpoint->color );
+
+ glPolygonMode(GL_FRONT_AND_BACK, GL_FILL );
+
+ glBegin( GL_TRIANGLES );
+ glVertex3f( 0, -dy, 0);
+ glVertex3f( dx, 0, 0 );
+ glVertex3f( 0, +dy, 0 );
+ glEnd();
+
+ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE );
+
+ stg_color_unpack( checkpoint->color, &r, &g, &b, &a );
+ PushColor( r/2, g/2, b/2, 1 ); // darker color
+
+ glDepthMask(GL_FALSE);
+ glBegin( GL_TRIANGLES );
+ glVertex3f( 0, -dy, 0);
+ glVertex3f( dx, 0, 0 );
+ glVertex3f( 0, +dy, 0 );
+ glEnd();
+ glDepthMask(GL_TRUE);
+
+ PopColor();
+ PopColor();
+ glPopMatrix();
+ }
+ }
+
void StgModel::Draw( uint32_t flags )
{
***************
*** 743,758 ****
LISTMETHOD( this->blocks, StgBlock*, Draw );
- if( flags & STG_SHOW_DATA )
- this->DataVisualize();
-
- //if( flags & STG_SHOW_GEOM )
- //this->DataVisualize();
-
- // etc
-
//if( this->say_string )
// gl_speech_bubble( 0,0,0, this->say_string );
!
!
if( gui_grid && (flags & STG_SHOW_GRID) )
DrawGrid();
--- 848,854 ----
LISTMETHOD( this->blocks, StgBlock*, Draw );
//if( this->say_string )
// gl_speech_bubble( 0,0,0, this->say_string );
!
if( gui_grid && (flags & STG_SHOW_GRID) )
DrawGrid();
***************
*** 1132,1149 ****
! // Check to see if the given pose will yield a collision with obstacles.
! // Returns a pointer to the first entity we are in collision with, and stores
! // the location of the hit in hitx,hity (if non-null)
! // Returns NULL if not collisions.
! // This function is useful for writing position devices.
! StgModel* StgModel::TestCollision( stg_pose_t* pose,
double* hitx, double* hity )
{
-
/* stg_model_t* child_hit = NULL; */
! /* GList* it; */
! /* for(it=mod->children; it; it=it->next ) */
/* { */
/* stg_model_t* child = (stg_model_t*)it->data; */
--- 1228,1242 ----
! // Check to see if the given change in pose will yield a collision
! // with obstacles. Returns a pointer to the first entity we are in
! // collision with, and stores the location of the hit in hitx,hity (if
! // non-null) Returns NULL if no collision.
! StgModel* StgModel::TestCollision( stg_pose_t* posedelta,
double* hitx, double* hity )
{
/* stg_model_t* child_hit = NULL; */
! /* for( GList* it=mod->children; it; it=it->next ) */
/* { */
/* stg_model_t* child = (stg_model_t*)it->data; */
***************
*** 1166,1171 ****
// add the local geom offset
! stg_pose_t local;
! stg_pose_sum( &local, pose, &this->geom.pose );
// loop over all blocks
--- 1259,1264 ----
// add the local geom offset
! //stg_pose_t local;
! //stg_pose_sum( &local, pose, &this->geom.pose );
// loop over all blocks
***************
*** 1177,1199 ****
for( unsigned int p=0; p<b->pt_count; p++ )
{
stg_point_t* pt1 = &b->pts[p];
stg_point_t* pt2 = &b->pts[(p+1) % b->pt_count];
-
- //printf( "tracing %.2f %.2f %.2f %.2f\n", pt1->x, pt1->y, pt2->x,
pt2->y );
-
double dx = pt2->x - pt1->x;
double dy = pt2->y - pt1->y;
double range = hypot( dx, dy );
double bearing = atan2( dy,dx );
! stg_pose_t pose;
! bzero(&pose,sizeof(pose));
! pose.x = pt1->x;
! pose.y = pt1->y;
! pose.a = bearing;
! Raytrace( &pose,
range,
(stg_block_match_func_t)collision_match,
--- 1270,1295 ----
for( unsigned int p=0; p<b->pt_count; p++ )
{
+ // find the local poses of the ends of this block edge
stg_point_t* pt1 = &b->pts[p];
stg_point_t* pt2 = &b->pts[(p+1) % b->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 );
! 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 );
! // raytrace in local coordinates
! Raytrace( &raypose,
range,
(stg_block_match_func_t)collision_match,
***************
*** 1201,1207 ****
&hitmod );
- if( hitmod )
- printf( "I hit %s\n", hitmod->Token() );
-
// if( hitmod )
// {
--- 1297,1300 ----
***************
*** 1217,1221 ****
-
void StgModel::UpdatePose( void )
{
--- 1310,1313 ----
***************
*** 1223,1256 ****
return;
! // convert usec to sec
! double interval = (double)world->interval_sim / 1e6;
! //stg_pose_t old_pose;
! //memcpy( &old_pose, &pose, sizeof(old_pose));
!
! stg_velocity_t v;
! this->GetVelocity( &v );
!
! stg_pose_t p;
! this->GetPose( &p );
!
! p.x += (v.x * cos(p.a) - v.y * sin(p.a)) * interval;
! p.y += (v.x * sin(p.a) + v.y * cos(p.a)) * interval;
! p.a += v.a * interval;
!
! // convert the new pose to global coords so we can see what it might hit
! stg_pose_t gp;
! memcpy( &gp, &p, sizeof(stg_pose_t));
! this->LocalToGlobal( &gp );
! // check this model and all it's children at the new pose
! double hitx=0, hity=0;
! StgModel* hitthing = this->TestCollision( &gp, &hitx, &hity );
if( hitthing )
{
! printf( "hit %s at %.2f %.2f\n",
! hitthing->Token(), hitx, hity );
//dd// //memcpy( &mod->pose, &old_pose, sizeof(mod->pose));
--- 1315,1353 ----
return;
! static uint32_t calls = 0;
! if( (world->updates % 10 == 0) )
! {
! stg_trail_item_t checkpoint;
! memcpy( &checkpoint.pose, &pose, sizeof(pose));
! checkpoint.color = color;
! checkpoint.time = world->sim_time;
! if( trail->len > 100 )
! g_array_remove_index( trail, 0 );
!
! g_array_append_val( this->trail, checkpoint );
! }
! // convert usec to sec
! double interval = (double)world->interval_sim / 1e6;
+ // find the change of pose due to our velocity vector
+ stg_pose_t p;
+ bzero(&p,sizeof(p));
+ p.x += velocity.x * interval;
+ p.y += velocity.y * interval;
+ p.a += velocity.a * interval;
+
+ // test to see if this pose change would cause us to crash
+ StgModel* hitthing = this->TestCollision( &p, NULL, NULL );
+
+ //double hitx=0, hity=0;
+ //StgModel* hitthing = this->TestCollision( &p, &hitx, &hity );
+
if( hitthing )
{
! //printf( "hit %s at %.2f %.2f\n",
! // hitthing->Token(), hitx, hity );
//dd// //memcpy( &mod->pose, &old_pose, sizeof(mod->pose));
***************
*** 1258,1268 ****
}
else
! this->SetPose( &p );
!
// int stall = 0;
! // if( hitthing )
! // {
// // grippable objects move when we hit them
// if( hitthing->gripper_return )
--- 1355,1368 ----
}
else
! {
! stg_pose_t newpose;
! stg_pose_sum( &newpose, &this->pose, &p );
! this->SetPose( &newpose );
! }
// int stall = 0;
! // if( hitthing )
! // {
// // grippable objects move when we hit them
// if( hitthing->gripper_return )
***************
*** 1294,1317 ****
// // move back to where we started
// memcpy( &mod->pose, &old_pose, sizeof(mod->pose));
! // interval = 0.2 * interval; // slow down time
!
! // // inch forward until we collide
! // do
! // {
! // memcpy( &old_pose, &mod->pose, sizeof(old_pose));
!
! // mod->pose.x +=
! // (mod->velocity.x * cos(mod->pose.a) - mod->velocity.y *
sin(mod->pose.a)) * interval;
! // mod->pose.y +=
! // (mod->velocity.x * sin(mod->pose.a) + mod->velocity.y *
cos(mod->pose.a)) * interval;
! // mod->pose.a += mod->velocity.a * interval;
!
! // hitthing = stg_model_test_collision( mod, &hitx, &hity );
- // } while( hitthing == NULL );
! // //PRINT_WARN( "HIT something immovable!" );
! // stall = 1;
// // set velocity to zero
--- 1394,1419 ----
// // move back to where we started
// memcpy( &mod->pose, &old_pose, sizeof(mod->pose));
!
! // // compute a move over a smaller period of time
! // interval = 0.1 * interval; // slow down time so we move slowly
! // stg_pose_t p;
! // bzero(&p,sizeof(p));
! // p.x += (v.x * cos(p.a) - v.y * sin(p.a)) * interval;
! // p.y += (v.x * sin(p.a) + v.y * cos(p.a)) * interval;
! // p.a += v.a * interval;
!
! // // inch forward until we collide
! // while( (TestCollision( &p, NULL, NULL ) == NULL) );
! // {
! // puts( "inching" );
! // stg_pose_t newpose;
! // stg_pose_sum( &newpose, &this->pose, &p );
! // this->SetPose( &newpose );
! // }
! // // //PRINT_WARN( "HIT something immovable!" );
! // stall = 1;
// // set velocity to zero
***************
*** 1324,1328 ****
// }
! // }
// // if the pose changed, we need to set it.
--- 1426,1430 ----
// }
! // }
// // if the pose changed, we need to set it.
***************
*** 1387,1391 ****
StgModel* StgModel::GetUnsubcribedModelOfType( char* modelstr )
{
! printf( "searching for %s in model %s with string %s\n", modelstr, token,
typestr );
if( subs == 0 && (strcmp( typestr, modelstr ) == 0) )
--- 1489,1493 ----
StgModel* StgModel::GetUnsubcribedModelOfType( char* modelstr )
{
! // printf( "searching for %s in model %s with string %s\n", modelstr,
token, typestr );
if( subs == 0 && (strcmp( typestr, modelstr ) == 0) )
Index: world.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/world.cc,v
retrieving revision 1.1.2.17
retrieving revision 1.1.2.18
diff -C2 -d -r1.1.2.17 -r1.1.2.18
*** world.cc 27 Nov 2007 05:36:02 -0000 1.1.2.17
--- world.cc 29 Nov 2007 07:58:59 -0000 1.1.2.18
***************
*** 133,136 ****
--- 133,137 ----
this->id = StgWorld::next_id++;
+ this->ray_list = NULL;
assert(token);
***************
*** 325,329 ****
stg_usec_t load_end_time = RealTimeNow();
! printf( "[Load time %.3fsec]\n", (load_end_time - load_start_time) /
1000000.0 );
}
--- 326,330 ----
stg_usec_t load_end_time = RealTimeNow();
! printf( "[Load time %.3fsec]", (load_end_time - load_start_time) /
1000000.0 );
}
***************
*** 452,455 ****
--- 453,495 ----
}
+
+ void StgWorld::RecordRay( double x1, double y1, double x2, double y2 )
+ {
+ float* drawpts = new float[4];
+ drawpts[0] = x1;
+ drawpts[1] = y1;
+ drawpts[2] = x2;
+ drawpts[3] = y2;
+ ray_list = g_list_append( ray_list, drawpts );
+ }
+
+ void StgWorld::DrawRays()
+ {
+ glDisable( GL_DEPTH_TEST );
+ PushColor( 0,0,0,0.5 );
+ for( GList* it = ray_list; it; it=it->next )
+ {
+ float* pts = (float*)it->data;
+ glBegin( GL_LINES );
+ glVertex2f( pts[0], pts[1] );
+ glVertex2f( pts[2], pts[3] );
+ glEnd();
+ }
+ PopColor();
+ glEnable( GL_DEPTH_TEST );
+ }
+
+ void StgWorld::ClearRays()
+ {
+ for( GList* it = ray_list; it; it=it->next )
+ {
+ float* pts = (float*)it->data;
+ delete [] pts;
+ }
+
+ g_list_free(ray_list );
+ ray_list = NULL;
+ }
+
stg_meters_t StgWorld::Raytrace( StgModel* finder,
stg_pose_t* pose,
***************
*** 472,481 ****
int32_t dz = 0;
! // glPushMatrix();
! // glTranslatef( -width/2.0, -height/2.0, 0 );
! // glScalef( 1.0/ppm, 1.0/ppm, 0 );
! // PushColor( 1,0,0,1 );
! // line 3d algorithm adapted from Cohen's code from Graphics Gems IV
int n, sx, sy, sz, exy, exz, ezy, ax, ay, az, bx, by, bz;
sx = SGN(dx); sy = SGN(dy); sz = SGN(dz);
--- 512,522 ----
int32_t dz = 0;
! // RecordRay( pose->x,
! // pose->y,
! // pose->x + max_range * cos(pose->a),
! // pose->y + max_range * sin(pose->a) );
! // fast integer line 3d algorithm adapted from Cohen's code from
! // Graphics Gems IV
int n, sx, sy, sz, exy, exz, ezy, ax, ay, az, bx, by, bz;
sx = SGN(dx); sy = SGN(dy); sz = SGN(dz);
***************
*** 525,536 ****
if( block && (block->mod != finder) &&
(*func)( block, arg ) &&
! pose->z >= block->zmin &&
! pose->z < block->zmax )
{
- //glPopMatrix();
-
// a hit!
if( hit_model )
*hit_model = block->mod;
// how far away was that strike?
return hypot( (x-xstart)/ppm, (y-ystart)/ppm );
--- 566,576 ----
if( block && (block->mod != finder) &&
(*func)( block, arg ) &&
! pose->z >= block->global_zmin &&
! pose->z < block->global_zmax )
{
// a hit!
if( hit_model )
*hit_model = block->mod;
+
// how far away was that strike?
return hypot( (x-xstart)/ppm, (y-ystart)/ppm );
***************
*** 538,545 ****
}
}
- // else
- // {
- // glRecti( x,y, x+1, y+1 );
- // }
if ( exy < 0 ) {
--- 578,581 ----
***************
*** 565,569 ****
}
- //glPopMatrix();
// hit nothing, so return max range
return max_range;
--- 601,604 ----
Index: model_ranger.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/model_ranger.cc,v
retrieving revision 1.1.2.6
retrieving revision 1.1.2.7
diff -C2 -d -r1.1.2.6 -r1.1.2.7
*** model_ranger.cc 26 Nov 2007 06:28:16 -0000 1.1.2.6
--- model_ranger.cc 29 Nov 2007 07:58:59 -0000 1.1.2.7
***************
*** 98,105 ****
this->ClearBlocks();
! //stg_geom_t geom;
! //memset( &geom, 0, sizeof(geom)); // no size
! //this->SetGeom( &geom );
!
sensor_count = 16;
sensors= g_new0( stg_ranger_sensor_t, sensor_count );
--- 98,106 ----
this->ClearBlocks();
! stg_geom_t geom;
! memset( &geom, 0, sizeof(geom)); // no size
! this->SetGeom( &geom );
!
! samples = NULL;
sensor_count = 16;
sensors= g_new0( stg_ranger_sensor_t, sensor_count );
***************
*** 113,119 ****
--- 114,122 ----
sensors[c].pose.x = offset * cos( sensors[c].pose.a );
sensors[c].pose.y = offset * sin( sensors[c].pose.a );
+ sensors[c].pose.z = 0;geom.size.z / 2.0; // half way up
sensors[c].size.x = 0.01; // a small device
sensors[c].size.y = 0.04;
+ sensors[c].size.z = 0.04;
sensors[c].bounds_range.min = 0;
***************
*** 134,137 ****
--- 137,142 ----
PRINT_DEBUG( "ranger startup" );
+ this->samples = new stg_meters_t[sensor_count];
+
this->SetWatts( STG_RANGER_WATTS );
}
***************
*** 143,148 ****
this->SetWatts( 0 );
- //this->SetData( NULL, 0 ); // this will unrender data too.
StgModel::Shutdown();
}
--- 148,158 ----
this->SetWatts( 0 );
+ if( this->samples )
+ {
+ delete[] samples;
+ samples = NULL;
+ }
+
StgModel::Shutdown();
}
***************
*** 191,197 ****
{
snprintf(key, sizeof(key), "spose[%d]", i);
! sensors[i].pose.x = wf->ReadTupleLength( id, key, 0, 0);
! sensors[i].pose.y = wf->ReadTupleLength( id, key, 1, 0);
! sensors[i].pose.a = wf->ReadTupleAngle( id, key, 2, 0);
/* snprintf(key, sizeof(key), "ssize[%d]", i); */
--- 201,214 ----
{
snprintf(key, sizeof(key), "spose[%d]", i);
! sensors[i].pose.x = wf->ReadTupleLength( id, key, 0,
sensors[i].pose.x );
! sensors[i].pose.y = wf->ReadTupleLength( id, key, 1,
sensors[i].pose.y );
! sensors[i].pose.z = 0.0;
! sensors[i].pose.a = wf->ReadTupleAngle( id, key, 2, sensors[i].pose.a
);
!
! snprintf(key, sizeof(key), "spose3[%d]", i);
! sensors[i].pose.x = wf->ReadTupleLength( id, key, 0,
sensors[i].pose.x );
! sensors[i].pose.y = wf->ReadTupleLength( id, key, 1,
sensors[i].pose.y );
! sensors[i].pose.z = wf->ReadTupleLength( id, key, 2,
sensors[i].pose.z );
! sensors[i].pose.a = wf->ReadTupleAngle( id, key, 3, sensors[i].pose.a
);
/* snprintf(key, sizeof(key), "ssize[%d]", i); */
***************
*** 219,222 ****
--- 236,244 ----
bool ranger_raytrace_match( StgBlock* block, StgModel* finder )
{
+ // printf( "ranger match sees %s %p %d zmin %.2f zmax %.2f global_zmin
%.2f global_zmax %.2f\n",
+ // block->mod->Token(), block->mod, block->mod->LaserReturn(),
+ // block->zmin, block->zmax,
+ // block->zmin, block->zmax );
+
// Ignore myself, my children, and my ancestors.
return( block->mod->RangerReturn() && !block->mod->IsRelated( finder ) );
***************
*** 232,236 ****
if( (sensors == NULL) || (sensor_count < 1 ))
return;
!
//PRINT_DEBUG2( "[%d] updating ranger %s", (int)world->sim_time_ms, token );
--- 254,260 ----
if( (sensors == NULL) || (sensor_count < 1 ))
return;
!
! assert( samples ); // make sure we have the range data array
!
//PRINT_DEBUG2( "[%d] updating ranger %s", (int)world->sim_time_ms, token );
***************
*** 244,248 ****
//{
range = Raytrace( &sensors[t].pose,
! sensors[t].bounds_range.max,
(stg_block_match_func_t)ranger_raytrace_match,
this,
--- 268,272 ----
//{
range = Raytrace( &sensors[t].pose,
! sensors[t].bounds_range.max,
(stg_block_match_func_t)ranger_raytrace_match,
this,
***************
*** 254,258 ****
range = sensors[t].bounds_range.min;
! sensors[t].range = range;
//sensors[t].error = TODO;
}
--- 278,282 ----
range = sensors[t].bounds_range.min;
! samples[t] = range;
//sensors[t].error = TODO;
}
***************
*** 279,283 ****
for( unsigned int i=0; i<sensor_count; i++ )
! printf( "%.2f ", sensors[i].range );
puts( " ]" );
}
--- 303,307 ----
for( unsigned int i=0; i<sensor_count; i++ )
! printf( "%.2f ", samples[i] );
puts( " ]" );
}
***************
*** 285,331 ****
void StgModelRanger::DataVisualize( void )
{
! if( sensors && sensor_count )
! {
! // if all models have the same number of sensors, this is fast
! // as it will probably not use a system call or cause a cache
! // miss
! static float* pts = NULL;
! size_t memsize = 6 * sensor_count * sizeof(float);
! pts = (float*)g_realloc( pts, memsize );
! bzero( pts, memsize );
! // calculate a triangle for each non-zero sensor range
! for( unsigned int s=0; s<sensor_count; s++ )
{
! if( sensors[s].range > 0.0 )
! {
! stg_ranger_sensor_t* rngr = &sensors[s];
! // sensor FOV
! double sidelen = sensors[s].range;
! double da = rngr->fov/2.0;
! unsigned int index = s*6;
! pts[index+0] = rngr->pose.x;
! pts[index+1] = rngr->pose.y;
! pts[index+2] = rngr->pose.x + sidelen*cos(rngr->pose.a - da );
! pts[index+3] = rngr->pose.y + sidelen*sin(rngr->pose.a - da );
! pts[index+4] = rngr->pose.x + sidelen*cos(rngr->pose.a + da );
! pts[index+5] = rngr->pose.y + sidelen*sin(rngr->pose.a + da );
! }
}
! // draw the filled triangles in transparent blue
! glDepthMask( GL_FALSE );
! glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
! PushColor( 0, 1, 0, 0.1 ); // transparent pale green
! glEnableClientState( GL_VERTEX_ARRAY );
! glVertexPointer( 2, GL_FLOAT, 0, pts );
! glDrawArrays( GL_TRIANGLES, 0, 3 * sensor_count );
! // restore state
! glDepthMask( GL_TRUE );
! PopColor();
! }
}
--- 309,383 ----
void StgModelRanger::DataVisualize( void )
{
! //if( ! (samples && sensors && sensor_count) )
! //return;
!
! glPushMatrix();
!
! // move into this model's local coordinate frame
! gl_pose_shift( &this->pose );
! gl_pose_shift( &this->geom.pose );
!
! // if all models have the same number of sensors, this is fast
! // as it will probably not use a system call or cause a cache
! // miss
! static float* pts = NULL;
! size_t memsize = 9 * sensor_count * sizeof(float);
! pts = (float*)g_realloc( pts, memsize );
! bzero( pts, memsize );
! //glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
! //PushColor( 0,0,0,1 );
!
! // calculate a triangle for each non-zero sensor range
! for( unsigned int s=0; s<sensor_count; s++ )
! {
! if( samples[s] > 0.0 )
{
! stg_ranger_sensor_t* rngr = &sensors[s];
! double dx = rngr->size.x/2.0;
! double dy = rngr->size.y/2.0;
! double dz = rngr->size.z/2.0;
!
! // DEBUG: draw a point for the sensor pose
! //glPointSize( 6 );
! //glBegin( GL_POINTS );
! //glVertex3f( rngr->pose.x, rngr->pose.y, rngr->pose.z );
! //glEnd();
! // sensor FOV
! double sidelen = samples[s];
! double da = rngr->fov/2.0;
!
! unsigned int index = s*9;
! pts[index+0] = rngr->pose.x;
! pts[index+1] = rngr->pose.y;
! pts[index+2] = rngr->pose.z;
!
! pts[index+3] = rngr->pose.x + sidelen*cos(rngr->pose.a - da );
! pts[index+4] = rngr->pose.y + sidelen*sin(rngr->pose.a - da );
! pts[index+5] = rngr->pose.z;
!
! pts[index+6] = rngr->pose.x + sidelen*cos(rngr->pose.a + da );
! pts[index+7] = rngr->pose.y + sidelen*sin(rngr->pose.a + da );
! pts[index+8] = rngr->pose.z;
}
+ }
! //PopColor();
!
! // draw the filled triangles in transparent blue
! glDepthMask( GL_FALSE );
! glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
! PushColor( 0, 1, 0, 0.1 ); // transparent pale green
! glEnableClientState( GL_VERTEX_ARRAY );
! glVertexPointer( 3, GL_FLOAT, 0, pts );
! glDrawArrays( GL_TRIANGLES, 0, 3 * sensor_count );
! // restore state
! glDepthMask( GL_TRUE );
! PopColor();
!
! glPopMatrix();
}
Index: block.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/block.cc,v
retrieving revision 1.1.2.5
retrieving revision 1.1.2.6
diff -C2 -d -r1.1.2.5 -r1.1.2.6
*** block.cc 17 Nov 2007 23:31:21 -0000 1.1.2.5
--- block.cc 29 Nov 2007 07:58:59 -0000 1.1.2.6
***************
*** 17,21 ****
this->pt_count = pt_count;
this->pts = (stg_point_t*)g_memdup( pts, pt_count * sizeof(stg_point_t));
- //this->pts_global = (stg_point_t*)g_memdup( pts, pt_count *
sizeof(stg_point_t));
// allocate space for the integer version of the block vertices
this->pts_global = new stg_point_int_t[pt_count];
--- 17,20 ----
***************
*** 25,28 ****
--- 24,31 ----
this->inherit_color = inherit_color;
this->rendered_points = g_array_new( FALSE, FALSE, sizeof(stg_point_int_t));
+
+ // flag these as unset until StgBlock::Map() is called.
+ this->global_zmin = -1;
+ this->global_zmax = -1;
}
***************
*** 70,73 ****
--- 73,85 ----
}
+ void StgBlock::DrawFootPrint()
+ {
+ glBegin(GL_POLYGON);
+
+ for( unsigned int p=0; p<pt_count; p++ )
+ glVertex2f( pts[p].x, pts[p].y );
+
+ glEnd();
+ }
void StgBlock::Draw()
***************
*** 121,130 ****
// update the global coordinate list
stg_pose_t gpose;
for( unsigned int p=0; p<pt_count; p++ )
{
gpose.x = pts[p].x;
gpose.y = pts[p].y;
! //gpose.z = pts[p].z;
! gpose.a = 0.0;
mod->LocalToGlobal( &gpose );
--- 133,143 ----
// 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 );
***************
*** 132,136 ****
pts_global[p].x = (int32_t)floor((gpose.x+mod->world->width/2.0)*ppm);
pts_global[p].y = (int32_t)floor((gpose.y+mod->world->height/2.0)*ppm);
- //pts_global[p].z = gpose.z;
PRINT_DEBUG2("loc [%.2f %.2f]",
--- 145,148 ----
***************
*** 143,146 ****
--- 155,163 ----
}
+ // store the block's global vertical bounds for inspection by the
+ // raytracer
+ global_zmin = gpose.z;
+ global_zmax = gpose.z + (zmax-zmin);
+
mod->world->MapBlock( this );
}
Index: stage.hh
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/stage.hh,v
retrieving revision 1.1.2.18
retrieving revision 1.1.2.19
diff -C2 -d -r1.1.2.18 -r1.1.2.19
*** stage.hh 27 Nov 2007 05:36:01 -0000 1.1.2.18
--- stage.hh 29 Nov 2007 07:58:59 -0000 1.1.2.19
***************
*** 584,587 ****
--- 584,589 ----
#define STG_SHOW_CLOCK 128
#define STG_SHOW_QUADTREE 256
+ #define STG_SHOW_ARROWS 512
+ #define STG_SHOW_FOOTPRINT 1024
// STAGE INTERNAL
***************
*** 1088,1091 ****
--- 1090,1099 ----
StgModel** hit_model );
+ // visualize calls to Raytrace() for debugging purposes
+ GList* ray_list;
+ void RecordRay( double x1, double y1, double x2, double y2 );
+ void DrawRays();
+ void ClearRays();
+
void ClockString( char* str, size_t maxlen );
***************
*** 1095,1098 ****
--- 1103,1112 ----
};
+ typedef struct {
+ stg_pose_t pose;
+ stg_color_t color;
+ stg_usec_t time;
+ } stg_trail_item_t;
+
// MODEL CLASS
class StgModel : public StgAncestor
***************
*** 1160,1167 ****
GList* blocks; //< list of stg_block_t structs that comprise a body
! // display lists
! //int dl_body, dl_data, dl_cmd, dl_cfg, dl_grid, dl_debug, dl_raytrace;
! //GList* raytrace_dl_list;
!
bool body_dirty; //< iff true, regenerate block display list before redraw
bool data_dirty; //< iff true, regenerate data display list before redraw
--- 1174,1180 ----
GList* blocks; //< list of stg_block_t structs that comprise a body
! GList* ray_list;
! GArray* trail;
!
bool body_dirty; //< iff true, regenerate block display list before redraw
bool data_dirty; //< iff true, regenerate data display list before redraw
***************
*** 1186,1190 ****
void Say( char* str );
! void UpdateIfDue( void );
/** configure a model by reading from the current world file */
--- 1199,1203 ----
void Say( char* str );
! void UpdateIfDue( void );
/** configure a model by reading from the current world file */
***************
*** 1194,1197 ****
--- 1207,1213 ----
virtual void Draw( uint32_t flags );
virtual void DrawPicker( void );
+ void DrawTrailFootprint();
+ void DrawTrailBlocks();
+ void DrawTrailArrows();
virtual void DataVisualize( void );
void DrawGrid();
***************
*** 1484,1490 ****
void Map();
void UnMap(); // draw the block into the world
!
void Draw(); // draw the block in OpenGL
void DrawSolid(); // draw the block in OpenGL as a solid single color
stg_point_t* pts; //< points defining a polygon
--- 1500,1507 ----
void Map();
void UnMap(); // draw the block into the world
!
void Draw(); // draw the block in OpenGL
void DrawSolid(); // draw the block in OpenGL as a solid single color
+ void DrawFootPrint(); // draw the projection of the block onto the z=0 plane
stg_point_t* pts; //< points defining a polygon
***************
*** 1493,1496 ****
--- 1510,1516 ----
stg_meters_t zmax;
+ stg_meters_t global_zmin;
+ stg_meters_t global_zmax;
+
StgModel* mod; //< model to which this block belongs
stg_point_int_t* pts_global; //< points defining a polygon in global coords
***************
*** 2173,2178 ****
stg_radians_t fov;
int ray_count;
- stg_meters_t range;
- //double error; // TODO
} stg_ranger_sensor_t;
--- 2193,2196 ----
***************
*** 2198,2201 ****
--- 2216,2220 ----
size_t sensor_count;
stg_ranger_sensor_t* sensors;
+ stg_meters_t* samples;
// static wrapper for the constructor - all models must implement
Index: canvas.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/canvas.cc,v
retrieving revision 1.1.2.5
retrieving revision 1.1.2.6
diff -C2 -d -r1.1.2.5 -r1.1.2.6
*** canvas.cc 26 Nov 2007 06:28:16 -0000 1.1.2.5
--- canvas.cc 29 Nov 2007 07:58:59 -0000 1.1.2.6
***************
*** 1,3 ****
!
#include "stage.hh"
#include <FL/fl_draw.H>
--- 1,3 ----
! \
#include "stage.hh"
#include <FL/fl_draw.H>
***************
*** 37,41 ****
showflags = STG_SHOW_CLOCK | STG_SHOW_BLOCKS | STG_SHOW_GRID;
!
// start the timer that causes regular redraws
Fl::add_timeout( ((double)interval/1000),
--- 37,42 ----
showflags = STG_SHOW_CLOCK | STG_SHOW_BLOCKS | STG_SHOW_GRID;
! //showflags = showflags | STG_SHOW_ARROWS | STG_SHOW_FOOTPRINT;
!
// start the timer that causes regular redraws
Fl::add_timeout( ((double)interval/1000),
***************
*** 455,463 ****
((StgModel*)it->data)->DrawSelected();
! // draw the models
! GList* it;
! if( showflags ) // if any bits are set there's something to draw
! for( it=world->children; it; it=it->next )
! ((StgModel*)it->data)->Draw( showflags );
}
--- 456,510 ----
((StgModel*)it->data)->DrawSelected();
! // draw the models
! if( showflags ) // if any bits are set there's something to draw
! {
! if( showflags & STG_SHOW_FOOTPRINT )
! {
! glDisable( GL_DEPTH_TEST );
! glPolygonMode(GL_FRONT_AND_BACK, GL_FILL );
!
! for( GList* it=world->children; it; it=it->next )
! {
! ((StgModel*)it->data)->DrawTrailFootprint();
! }
! glEnable( GL_DEPTH_TEST );
! }
!
! if( showflags & STG_SHOW_TRAILS )
! {
! glPolygonMode(GL_FRONT_AND_BACK, GL_FILL );
!
! for( GList* it=world->children; it; it=it->next )
! {
! ((StgModel*)it->data)->DrawTrailBlocks();
! }
! }
!
! if( showflags & STG_SHOW_ARROWS )
! {
! glEnable( GL_DEPTH_TEST );
! for( GList* it=world->children; it; it=it->next )
! {
! ((StgModel*)it->data)->DrawTrailArrows();
! }
! }
!
! if( showflags & STG_SHOW_DATA )
! {
! for( GList* it=world->children; it; it=it->next )
! ((StgModel*)it->data)->DataVisualize();
! }
!
! for( GList* it=world->children; it; it=it->next )
! ((StgModel*)it->data)->Draw( showflags );
!
!
! }
!
! if( world->ray_list )
! {
! world->DrawRays();
! world->ClearRays();
! }
}
-------------------------------------------------------------------------
SF.Net email is sponsored by: The Future of Linux Business White Paper
from Novell. From the desktop to the data center, Linux is going
mainstream. Let it simplify your IT future.
http://altfarm.mediaplex.com/ad/ck/8857-50307-18918-4
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
