Revision: 7645
http://playerstage.svn.sourceforge.net/playerstage/?rev=7645&view=rev
Author: rtv
Date: 2009-05-13 17:57:36 +0000 (Wed, 13 May 2009)
Log Message:
-----------
replaced hash table with stl::map in inner RT loop, increased superregion size
Modified Paths:
--------------
code/stage/trunk/libstage/CMakeLists.txt
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/region.hh
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/libstage/worldgui.cc
code/stage/trunk/libstageplugin/p_laser.cc
code/stage/trunk/worlds/simple.world
Modified: code/stage/trunk/libstage/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstage/CMakeLists.txt 2009-05-13 15:41:40 UTC (rev
7644)
+++ code/stage/trunk/libstage/CMakeLists.txt 2009-05-13 17:57:36 UTC (rev
7645)
@@ -97,6 +97,6 @@
)
INSTALL(FILES stage.hh
- DESTINATION include/${PROJECT_NAME}-${V_MAJOR}.${V_MINOR})
+ DESTINATION include/${PROJECT_NAME}-${APIVERSION})
ADD_TEST( test1 ${EXECUTABLE_OUTPUT_PATH}stagetest )
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2009-05-13 15:41:40 UTC (rev
7644)
+++ code/stage/trunk/libstage/model_laser.cc 2009-05-13 17:57:36 UTC (rev
7645)
@@ -181,7 +181,7 @@
glPopMatrix();
}
-
+
ModelLaser::ModelLaser( World* world,
Model* parent )
: Model( world, parent, MODEL_TYPE_LASER ),
@@ -208,16 +208,16 @@
geom.size = DEFAULT_SIZE;
SetGeom( geom );
- rays.resize( sample_count );
- samples.resize( sample_count );
-
// assert that Update() is reentrant for this derived model
thread_safe = true;
// set default color
SetColor( stg_lookup_color(DEFAULT_COLOR));
- AddVisualizer( &vis, true );
+ // set up our data buffers and raytracing
+ SampleConfig();
+
+ AddVisualizer( &vis, true );
}
@@ -242,9 +242,8 @@
}
Model::Load();
-
- rays.resize(sample_count);
- samples.resize(sample_count);
+
+ SampleConfig();
}
stg_laser_cfg_t ModelLaser::GetConfig()
@@ -266,8 +265,7 @@
interval = cfg.interval;
sample_count = cfg.sample_count;
- samples.resize( sample_count );
- rays.resize( sample_count );
+ SampleConfig();
}
static bool laser_raytrace_match( Model* hit,
@@ -279,40 +277,40 @@
return( (hit != finder) && (hit->vis.laser_return > 0 ) );
}
-// void ModelLaser::SetSampleCount( unsigned int count )
-// {
-// sample_count = count;
+void ModelLaser::SampleConfig()
+{
+ samples.resize( sample_count );
+ rays.resize( sample_count );
-// samples.resize( count );
-// rays.resize( count );
+ for( unsigned int t=0; t<sample_count; ++t )
+ {
+ // configure a normal ray with our laser-specific settings
+ rays[t].func = laser_raytrace_match;
+ rays[t].arg = NULL;
+ rays[t].ztest = true;
+ rays[t].range = range_max;
+ rays[t].mod = this;
+ }
+}
void ModelLaser::Update( void )
{
+ assert( samples.size() == sample_count );
+ assert( rays.size() == sample_count );
+
double bearing = -fov/2.0;
- double sample_incr = fov / (double)(sample_count-1);
+ double sample_incr = fov / (double)sample_count;
Pose rayorg = geom.pose;
- bzero( &rayorg, sizeof(rayorg));
- rayorg.z += geom.size.z/2;
+ rayorg.z += geom.size.z/2.0;
+ rayorg.a = bearing;
+ rayorg = LocalToGlobal(rayorg);
- assert( samples.size() == sample_count );
-
+ // set up the ray origins in global coords
for( unsigned int t=0; t<sample_count; t += resolution )
{
- rayorg.a = bearing;
-
- // set up the ray
- rays[t].origin = LocalToGlobal(rayorg);
-
- // todo - do this constant stuff in advance
- rays[t].func = laser_raytrace_match;
- rays[t].arg = NULL;
- rays[t].range = range_max;
- rays[t].arg = NULL;
- rays[t].ztest = true;
- rays[t].mod = this;
-
- bearing += sample_incr;
+ rays[t].origin = rayorg;
+ rayorg.a += sample_incr;
}
// do the raytracing of all rays in one go (allows parallel implementation)
@@ -375,11 +373,7 @@
void ModelLaser::Shutdown( void )
{
PRINT_DEBUG( "laser shutdown" );
-
- // stop consuming power
- SetWatts( 0 );
-
-
+ SetWatts( 0 ); // stop consuming power
Model::Shutdown();
}
@@ -401,6 +395,12 @@
stg_laser_sample_t* ModelLaser::GetSamples( uint32_t* count )
{
+ // get a C style array from our vector
if( count ) *count = sample_count;
return &samples[0];
}
+
+const std::vector<stg_laser_sample_t>& ModelLaser::GetSamples()
+{
+ return samples;
+}
Modified: code/stage/trunk/libstage/region.hh
===================================================================
--- code/stage/trunk/libstage/region.hh 2009-05-13 15:41:40 UTC (rev 7644)
+++ code/stage/trunk/libstage/region.hh 2009-05-13 17:57:36 UTC (rev 7645)
@@ -15,7 +15,7 @@
// a bit of experimenting suggests that these values are fast. YMMV.
#define RBITS 4 // regions contain (2^RBITS)^2 pixels
-#define SBITS 6 // superregions contain (2^SBITS)^2 regions
+#define SBITS 5 // superregions contain (2^SBITS)^2 regions
#define SRBITS (RBITS+SBITS)
#define REGIONWIDTH (1<<RBITS)
@@ -107,8 +107,16 @@
void DecrementOccupancy();
void IncrementOccupancy();
-};
+ bool Raytrace( int32_t x, int32_t y,
+ int32_t dx, int32_t dy,
+ const stg_ray_test_func_t func,
+ const Model* mod,
+ const void* arg,
+ const bool ztest,
+ stg_raytrace_result_t* res );
+ };
+
class SuperRegion
{
friend class World;
@@ -141,7 +149,6 @@
void IncrementOccupancy(){ ++count; };
};
-
inline void Region::DecrementOccupancy()
{
assert( superregion );
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-05-13 15:41:40 UTC (rev 7644)
+++ code/stage/trunk/libstage/stage.hh 2009-05-13 17:57:36 UTC (rev 7645)
@@ -42,6 +42,7 @@
#include <iostream>
#include <vector>
#include <list>
+#include <map>
//#include <pair>
// we use GLib's data structures extensively. Perhaps we'll move to
@@ -419,6 +420,10 @@
int x,y;
stg_point_int_t( int x, int y ) : x(x), y(y){}
stg_point_int_t() : x(0), y(0){}
+
+ /** required to put these in sorted containers like std::map */
+ bool operator<( const stg_point_int_t& other ) const
+ { return ((x < other.x) || (y < other.y) ); }
};
@@ -816,15 +821,19 @@
/** raytrace sample
*/
- typedef struct
+ class RaytraceResult
{
+ public:
Pose pose; ///< location and direction of the ray origin
stg_meters_t range; ///< range to beam hit in meters
Model* mod; ///< the model struck by this beam
stg_color_t color; ///< the color struck by this beam
- } stg_raytrace_result_t;
+ RaytraceResult() : pose(), range(0), mod(NULL), color() {}
+ };
+ typedef RaytraceResult stg_raytrace_result_t;
+
class Ray
{
public:
@@ -842,7 +851,7 @@
void* arg;
bool ztest;
- stg_raytrace_result_t result;
+ RaytraceResult result;
};
@@ -923,9 +932,8 @@
GList* powerpack_list; ///< List of all the powerpacks attached to
models in the world
GList* ray_list;///< List of rays traced for debug visualization
stg_usec_t sim_time; ///< the current sim time in this world in
microseconds
- GHashTable* superregions;
+ std::map<stg_point_int_t,SuperRegion*> superregions;
SuperRegion* sr_cached; ///< The last superregion looked up by this world
- // GList* update_list; ///< Models that have a subscriber or controller,
and need to be updated
GList* reentrant_update_list; ///< It is safe to call these model's
Update() in parallel
GList* nonreentrant_update_list; ///< It is NOT safe to call these model's
Update() in parallel
@@ -996,8 +1004,11 @@
SuperRegion* CreateSuperRegion( stg_point_int_t origin );
void DestroySuperRegion( SuperRegion* sr );
- // trace a vector of rays all in one go
+ /** trace a vector of rays all in one go. */
void Raytrace( std::vector<Ray>& rays );
+
+ /** trace a ray. */
+ void Raytrace( Ray& ray );
stg_raytrace_result_t Raytrace( const Pose& pose,
const stg_meters_t range,
@@ -1162,6 +1173,9 @@
void AddToCellArray( GPtrArray* ptrarray );
void RemoveFromCellArray( GPtrArray* ptrarray );
+
+ //void AddToCellArray( std::vector<Cell*>& blocks );
+ //void RemoveFromCellArray( std::vector<Cell*>& blocks );
void GenerateCandidateCells();
@@ -2455,7 +2469,7 @@
};
Vis vis;
-
+
//class LaserRay : public Ray
/** OpenGL displaylist for laser data */
@@ -2473,6 +2487,9 @@
std::vector<Ray> rays;
+ // set up data buffers after the config changes
+ void SampleConfig();
+
public:
static const char* typestr;
// constructor
@@ -2490,15 +2507,18 @@
uint32_t GetSampleCount(){ return sample_count; }
- stg_laser_sample_t* GetSamples( uint32_t* count=NULL);
-
- //void SetSamples( stg_laser_sample_t* samples, uint32_t count);
-
+ /** returns an array of samples */
+ stg_laser_sample_t* GetSamples( uint32_t* count );
+
+ /** returns a const reference to a vector of samples */
+ const std::vector<stg_laser_sample_t>& GetSamples();
+
// Get the user-tweakable configuration of the laser
stg_laser_cfg_t GetConfig( );
// Set the user-tweakable configuration of the laser
void SetConfig( stg_laser_cfg_t cfg );
+
};
// \todo GRIPPER MODEL
--------------------------------------------------------
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-05-13 15:41:40 UTC (rev 7644)
+++ code/stage/trunk/libstage/world.cc 2009-05-13 17:57:36 UTC (rev 7645)
@@ -59,17 +59,7 @@
bool World::quit_all = false;
GList* World::world_list = NULL;
-static guint PointIntHash( stg_point_int_t* pt )
-{
- return( pt->x + (pt->y<<16 ));
-}
-static gboolean PointIntEqual( stg_point_int_t* p1, stg_point_int_t* p2 )
-{
- return( memcmp( p1, p2, sizeof(stg_point_int_t) ) == 0 );
-}
-
-
World::World( const char* token,
stg_msec_t interval_sim,
double ppm )
@@ -103,8 +93,9 @@
powerpack_list( NULL ),
ray_list( NULL ),
sim_time( 0 ),
- superregions( g_hash_table_new( (GHashFunc)PointIntHash,
-
(GEqualFunc)PointIntEqual ) ),
+ superregions(),
+ // g_hash_table_new( (GHashFunc)PointIntHash,
+ // (GEqualFunc)PointIntEqual ) ),
sr_cached(NULL),
reentrant_update_list( NULL ),
nonreentrant_update_list( NULL ),
@@ -137,15 +128,14 @@
SuperRegion* World::CreateSuperRegion( stg_point_int_t origin )
{
SuperRegion* sr = new SuperRegion( this, origin );
- g_hash_table_insert( superregions, &sr->origin, sr );
-
+ superregions[ sr->origin ] = sr;
dirty = true; // force redraw
return sr;
}
void World::DestroySuperRegion( SuperRegion* sr )
{
- g_hash_table_remove( superregions, &sr->origin );
+ superregions.erase( sr->origin );
delete sr;
}
@@ -397,10 +387,11 @@
g_list_free( ray_list );
ray_list = NULL;
-
- g_hash_table_foreach( superregions, (GHFunc)destroy_sregion, NULL );
- g_hash_table_remove_all( superregions );
+ // todo
+ //g_hash_table_foreach( superregions, (GHFunc)destroy_sregion, NULL );
+ //g_hash_table_remove_all( superregions );
+
token = NULL;
}
@@ -614,15 +605,15 @@
void World::Raytrace( std::vector<Ray>& rays )
{
- for( std::vector<Ray>::iterator it = rays.begin();
- it != rays.end();
- ++it )
- {
- Ray& r = *it;
- r.result = Raytrace( r.origin, r.range, r.func, r.mod, r.arg,
r.ztest );
- }
+ for( unsigned int i=0; i<rays.size(); i++ )
+ Raytrace( rays[i] );
}
+inline void World::Raytrace( Ray& r )
+{
+ r.result = Raytrace( r.origin, r.range, r.func, r.mod, r.arg, r.ztest );
+}
+
void World::Raytrace( const Pose &gpose, // global pose
const stg_meters_t
range,
const stg_radians_t
fov,
@@ -707,7 +698,7 @@
// initialize the sample
sample.pose = gpose;
- sample.pose.a = normalize( sample.pose.a );
+ //sample.pose.a = normalize( sample.pose.a );
sample.range = range; // we might change this below
sample.mod = NULL; // we might change this below
@@ -722,10 +713,6 @@
// and the x and y offsets of the ray
int32_t dx = (int32_t)(ppm*range * cos(gpose.a));
int32_t dy = (int32_t)(ppm*range * sin(gpose.a));
-
-// // the number of regions we will travel through
-// int32_t rdx = dx / Region::WIDTH;
-// int32_t rdy = dy / Region::WIDTH;
// if( finder->debug )
// RecordRay( pose.x,
@@ -746,16 +733,6 @@
int exy = ay-ax;
int n = ax+ay;
-// // region units
-// int rsx = sgn(rdx); // sgn() is a fast macro
-// int rsy = sgn(rdy);
-// int rax = abs(rdx);
-// int ray = abs(rdy);
-// int rbx = 2*rax;
-// int rby = 2*ray;
-// int rexy = ray-rax;
-// int rn = rax+ray;
-
// printf( "Raytracing from (%d,%d) steps (%d,%d) %d\n",
// x,y, dx,dy, n );
@@ -820,13 +797,6 @@
Block* block = *it;
assert( block );
- //printf( "ENT %p mod %p z
[%.2f %.2f] color %X\n",
- // ent,
- // ent->mod,
- //
ent->zbounds.min,
- //
ent->zbounds.max,
- // ent->color );
-
// skip if not in the right z
range
if( ztest &&
( gpose.z <
block->global_z.min ||
@@ -851,6 +821,7 @@
sample.range = hypot(
(glob.x-start.x)/ppm, (glob.y-start.y)/ppm );
return sample;
}
+
}
}
}
@@ -868,7 +839,7 @@
sup.x = SUPERREGION( glob.x );
if( sup.x != lastsup.x )
{
- sr = (SuperRegion*)g_hash_table_lookup(
superregions, (void*)&sup );
+ sr = superregions[ sup ];
lastsup = sup; // remember these coords
}
@@ -886,7 +857,7 @@
sup.y = SUPERREGION( glob.y );
if( sup.y != lastsup.y )
{
- sr = (SuperRegion*)g_hash_table_lookup(
superregions, (void*)&sup );
+ sr = superregions[ sup ];
lastsup = sup; // remember these coords
}
@@ -965,10 +936,7 @@
inline SuperRegion* World::GetSuperRegion( const stg_point_int_t& sup )
{
//printf( "SUP[ %d %d ] ", sup.x, sup.y );
-
- // no, so we try to fetch it out of the hash table
- SuperRegion* sr = (SuperRegion*)
- g_hash_table_lookup( superregions, (gpointer)&sup );
+ SuperRegion* sr = superregions[ sup ];
if( sr == NULL ) // no superregion exists! make a new one
sr = AddSuperRegion( sup );
Modified: code/stage/trunk/libstage/worldgui.cc
===================================================================
--- code/stage/trunk/libstage/worldgui.cc 2009-05-13 15:41:40 UTC (rev
7644)
+++ code/stage/trunk/libstage/worldgui.cc 2009-05-13 17:57:36 UTC (rev
7645)
@@ -441,7 +441,12 @@
void WorldGui::DrawTree( bool drawall )
{
- g_hash_table_foreach( superregions, (GHFunc)Draw_cb, (void*)drawall );
+ //g_hash_table_foreach( superregions, (GHFunc)Draw_cb, (void*)drawall );
+
+ for( std::map<stg_point_int_t,SuperRegion*>::iterator it =
superregions.begin();
+ it != superregions.end();
+ it++ )
+ (*it).second->Draw( drawall );
}
// callback wrapper for SuperRegion::Floor()
@@ -456,7 +461,13 @@
void WorldGui::DrawFloor()
{
PushColor( 1,1,1,1 );
- g_hash_table_foreach( superregions, (GHFunc)Floor_cb, NULL );
+ //g_hash_table_foreach( superregions, (GHFunc)Floor_cb, NULL );
+
+ for( std::map<stg_point_int_t,SuperRegion*>::iterator it =
superregions.begin();
+ it != superregions.end();
+ it++ )
+ (*it).second->Floor();
+
PopColor();
}
Modified: code/stage/trunk/libstageplugin/p_laser.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_laser.cc 2009-05-13 15:41:40 UTC (rev
7644)
+++ code/stage/trunk/libstageplugin/p_laser.cc 2009-05-13 17:57:36 UTC (rev
7645)
@@ -53,7 +53,7 @@
void InterfaceLaser::Publish( void )
{
ModelLaser* mod = (ModelLaser*)this->mod;
- stg_laser_sample_t* samples = mod->GetSamples();
+ stg_laser_sample_t* samples = mod->GetSamples(NULL);
// don't publish anything until we have some real data
if( samples == NULL )
Modified: code/stage/trunk/worlds/simple.world
===================================================================
--- code/stage/trunk/worlds/simple.world 2009-05-13 15:41:40 UTC (rev
7644)
+++ code/stage/trunk/worlds/simple.world 2009-05-13 17:57:36 UTC (rev
7645)
@@ -9,7 +9,7 @@
interval_sim 100 # simulation timestep in milliseconds
interval_real 20 # real-time interval between simulation updates in
milliseconds
-quit_time 60
+quit_time 600
paused 0
@@ -43,7 +43,7 @@
name "r0"
pose [ -7 -7 0 45 ]
- sicklaser()
+ sicklaser( samples 361 )
ctrl "wander"
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
The NEW KODAK i700 Series Scanners deliver under ANY circumstances! Your
production scanning environment may not be a perfect world - but thanks to
Kodak, there's a perfect scanner to get the job done! With the NEW KODAK i700
Series Scanner you'll get full speed at 300 dpi even with all image
processing features enabled. http://p.sf.net/sfu/kodak-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
