Revision: 7618
http://playerstage.svn.sourceforge.net/playerstage/?rev=7618&view=rev
Author: rtv
Date: 2009-05-05 00:12:13 +0000 (Tue, 05 May 2009)
Log Message:
-----------
working on core raytracing code
Modified Paths:
--------------
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/worlds/fasr.world
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-05-04 20:49:10 UTC (rev 7617)
+++ code/stage/trunk/libstage/model.cc 2009-05-05 00:12:13 UTC (rev 7618)
@@ -557,16 +557,16 @@
return candidate->IsDescendent( that );
}
-inline Pose Model::LocalToGlobal( const Pose& pose ) const
-{
- return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose );
-}
+// Pose Model::LocalToGlobal( const Pose& pose ) const
+// {
+// return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose );
+// }
stg_point_t Model::LocalToGlobal( const stg_point_t& pt) const
-{
- Pose gpose = LocalToGlobal( Pose( pt.x, pt.y, 0, 0 ) );
- return stg_point_t( gpose.x, gpose.y );
-}
+ {
+ Pose gpose = LocalToGlobal( Pose( pt.x, pt.y, 0, 0 ) );
+ return stg_point_t( gpose.x, gpose.y );
+ }
void Model::MapWithChildren()
{
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2009-05-04 20:49:10 UTC (rev
7617)
+++ code/stage/trunk/libstage/model_laser.cc 2009-05-05 00:12:13 UTC (rev
7618)
@@ -1,6 +1,4 @@
-///////////////////////////////////////////////////////////////////////////
-//
-// File: model_laser.c
+// file: model_laser.c
// Author: Richard Vaughan
// Date: 10 June 2004
//
@@ -91,10 +89,7 @@
ModelLaser* laser = dynamic_cast<ModelLaser*>(mod);
unsigned int sample_count = 0;
stg_laser_sample_t* samples = laser->GetSamples( &sample_count );
-
- if( ! (samples && sample_count) )
- return;
-
+
glPushMatrix();
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
@@ -193,11 +188,12 @@
vis( world ),
data_dl(0),
data_dirty( true ),
- samples( NULL ), // don't allocate sample buffer memory until Update()
is called
sample_count( DEFAULT_SAMPLES ),
+ samples(),
range_max( DEFAULT_MAXRANGE ),
fov( DEFAULT_FOV ),
- resolution( DEFAULT_RESOLUTION )
+ resolution( DEFAULT_RESOLUTION ),
+ rays()
{
PRINT_DEBUG2( "Constructing ModelLaser %d (%s)\n",
@@ -212,6 +208,9 @@
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;
@@ -224,11 +223,6 @@
ModelLaser::~ModelLaser( void )
{
- if(samples)
- {
- g_free( samples );
- samples = NULL;
- }
}
void ModelLaser::Load( void )
@@ -248,6 +242,9 @@
}
Model::Load();
+
+ rays.resize(sample_count);
+ samples.resize(sample_count);
}
stg_laser_cfg_t ModelLaser::GetConfig()
@@ -267,6 +264,10 @@
fov = cfg.fov;
resolution = cfg.resolution;
interval = cfg.interval;
+ sample_count = cfg.sample_count;
+
+ samples.resize( sample_count );
+ rays.resize( sample_count );
}
static bool laser_raytrace_match( Model* hit,
@@ -278,62 +279,82 @@
return( (hit != finder) && (hit->vis.laser_return > 0 ) );
}
+// void ModelLaser::SetSampleCount( unsigned int count )
+// {
+// sample_count = count;
+
+// samples.resize( count );
+// rays.resize( count );
+
void ModelLaser::Update( void )
{
double bearing = -fov/2.0;
double sample_incr = fov / (double)(sample_count-1);
- samples = g_renew( stg_laser_sample_t, samples, sample_count );
-
Pose rayorg = geom.pose;
bzero( &rayorg, sizeof(rayorg));
rayorg.z += geom.size.z/2;
+ assert( samples.size() == sample_count );
+
for( unsigned int t=0; t<sample_count; t += resolution )
{
rayorg.a = bearing;
+
+ // set up the ray
+ rays[t].origin = LocalToGlobal(rayorg);
- stg_raytrace_result_t sample =
- Raytrace( rayorg,
- range_max,
- laser_raytrace_match,
- NULL,
- true ); // z testing enabled
+ // 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;
- samples[t].range = sample.range;
+ bearing += sample_incr;
+ }
+
+ // do the raytracing of all rays in one go (allows parallel implementation)
+ world->Raytrace( rays );
+
+ // now process the results and fill in our samples
+ for( unsigned int t=0; t<sample_count; t += resolution )
+ {
+ stg_raytrace_result_t* r = &rays[t].result;
+ assert( r );
+ samples[t].range = r->range;
+
// if we hit a model and it reflects brightly, we set
// reflectance high, else low
- if( sample.mod && ( sample.mod->vis.laser_return >= LaserBright ) )
- samples[t].reflectance = 1;
+ if( r->mod && ( r->mod->vis.laser_return >= LaserBright ) )
+ samples[t].reflectance = 1;
else
- samples[t].reflectance = 0;
-
- // todo - lower bound on range
- bearing += sample_incr;
+ samples[t].reflectance = 0;
}
-
+
// we may need to interpolate the samples we skipped
if( resolution > 1 )
{
for( unsigned int t=resolution; t<sample_count; t+=resolution )
- for( unsigned int g=1; g<resolution; g++ )
- {
- if( t >= sample_count )
- break;
+ for( unsigned int g=1; g<resolution; g++ )
+ {
+ if( t >= sample_count )
+ break;
- // copy the rightmost sample data into this point
- memcpy( &samples[t-g],
- &samples[t-resolution],
- sizeof(stg_laser_sample_t));
+ // copy the rightmost sample data into this
point
+ memcpy( &samples[t-g],
+ &samples[t-resolution],
+ sizeof(stg_laser_sample_t));
- double left = samples[t].range;
- double right = samples[t-resolution].range;
+ double left = samples[t].range;
+ double right = samples[t-resolution].range;
- // linear range interpolation between the left and right samples
- samples[t-g].range = (left-g*(left-right)/resolution);
- }
- }
+ // linear range interpolation between the left
and right samples
+ samples[t-g].range =
(left-g*(left-right)/resolution);
+ }
+ }
data_dirty = true;
@@ -358,13 +379,7 @@
// stop consuming power
SetWatts( 0 );
- // clear the data
- if(samples)
- {
- g_free( samples );
- samples = NULL;
- }
-
+
Model::Shutdown();
}
@@ -373,22 +388,13 @@
Model::Print( prefix );
printf( "\tRanges[ " );
-
- if( samples )
- for( unsigned int i=0; i<sample_count; i++ )
+ for( unsigned int i=0; i<sample_count; i++ )
printf( "%.2f ", samples[i].range );
- else
- printf( "<none until subscribed>" );
puts( " ]" );
printf( "\tReflectance[ " );
-
- if( samples )
- for( unsigned int i=0; i<sample_count; i++ )
- printf( "%.2f ", samples[i].reflectance );
- else
- printf( "<none until subscribed>" );
-
+ for( unsigned int i=0; i<sample_count; i++ )
+ printf( "%.2f ", samples[i].reflectance );
puts( " ]" );
}
@@ -396,17 +402,5 @@
stg_laser_sample_t* ModelLaser::GetSamples( uint32_t* count )
{
if( count ) *count = sample_count;
- return samples;
+ return &samples[0];
}
-
-void ModelLaser::SetSamples( stg_laser_sample_t* samples, uint32_t count)
-{
- this->samples = g_renew( stg_laser_sample_t, this->samples, sample_count );
- memcpy( this->samples, samples, sample_count * sizeof(stg_laser_sample_t));
- this->sample_count = count;
- this->data_dirty = true;
-}
-
-
-
-
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-05-04 20:49:10 UTC (rev 7617)
+++ code/stage/trunk/libstage/stage.hh 2009-05-05 00:12:13 UTC (rev 7618)
@@ -825,6 +825,27 @@
} stg_raytrace_result_t;
+ class Ray
+ {
+ public:
+ //SuperRegion& sup;
+ //Regiion& reg;
+ //Cell& cell;
+ //stg_point_int_t glob;
+ //stg_point_int_t origin;
+ //stg_point_int_t dest;
+
+ Model* mod;
+ Pose origin;
+ stg_meters_t range;
+ stg_ray_test_func_t func;
+ void* arg;
+ bool ztest;
+
+ stg_raytrace_result_t result;
+ };
+
+
const uint32_t INTERVAL_LOG_LEN = 32;
// defined in stage_internal.hh
@@ -975,6 +996,9 @@
SuperRegion* CreateSuperRegion( stg_point_int_t origin );
void DestroySuperRegion( SuperRegion* sr );
+ // trace a vector of rays all in one go
+ void Raytrace( std::vector<Ray>& rays );
+
stg_raytrace_result_t Raytrace( const Pose& pose,
const stg_meters_t range,
const stg_ray_test_func_t func,
@@ -2298,7 +2322,10 @@
/** Return the global pose (i.e. pose in world coordinates) of a
pose specified in the model's local coordinate system */
- Pose LocalToGlobal( const Pose& pose ) const;
+ Pose LocalToGlobal( const Pose& pose ) const
+ {
+ return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose );
+ }
// /** Return the 3d point in world coordinates of a 3d point
// specified in the model's local coordinate system */
@@ -2420,24 +2447,31 @@
static Option showStrikes;
static Option showFov;
static Option showBeams;
-
+
public:
Vis( World* world );
virtual ~Vis( void ){}
virtual void Visualize( Model* mod, Camera* cam );
};
-
+
Vis vis;
+ //class LaserRay : public Ray
+
/** OpenGL displaylist for laser data */
int data_dl;
bool data_dirty;
- stg_laser_sample_t* samples;
+ //stg_laser_sample_t* samples;
uint32_t sample_count;
+
+ std::vector<stg_laser_sample_t> samples;
+
stg_meters_t range_max;
stg_radians_t fov;
uint32_t resolution;
+
+ std::vector<Ray> rays;
public:
static const char* typestr;
@@ -2458,7 +2492,7 @@
stg_laser_sample_t* GetSamples( uint32_t* count=NULL);
- void SetSamples( stg_laser_sample_t* samples, uint32_t count);
+ //void SetSamples( stg_laser_sample_t* samples, uint32_t count);
// Get the user-tweakable configuration of the laser
stg_laser_cfg_t GetConfig( );
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-05-04 20:49:10 UTC (rev 7617)
+++ code/stage/trunk/libstage/world.cc 2009-05-05 00:12:13 UTC (rev 7618)
@@ -612,7 +612,18 @@
}
-void World::Raytrace( const Pose &pose, // global pose
+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 );
+ }
+}
+
+void World::Raytrace( const Pose &gpose, // global pose
const stg_meters_t
range,
const stg_radians_t
fov,
const
stg_ray_test_func_t func,
@@ -623,7 +634,7 @@
const bool ztest )
{
// find the direction of the first ray
- Pose raypose = pose;
+ Pose raypose = gpose;
double starta = fov/2.0 - raypose.a;
for( uint32_t s=0; s < sample_count; s++ )
@@ -711,6 +722,10 @@
// 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,
@@ -721,6 +736,7 @@
// fast integer line 3d algorithm adapted from Cohen's code from
// Graphics Gems IV
+ // cell unti
int sx = sgn(dx); // sgn() is a fast macro
int sy = sgn(dy);
int ax = abs(dx);
@@ -729,7 +745,17 @@
int by = 2*ay;
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 );
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2009-05-04 20:49:10 UTC (rev 7617)
+++ code/stage/trunk/worlds/fasr.world 2009-05-05 00:12:13 UTC (rev 7618)
@@ -72,7 +72,7 @@
define autorob pioneer2dx
(
- sicklaser( samples 16 range_max 5 laser_return 2 watts 30 )
+ sicklaser( samples 180 range_max 5 laser_return 2 watts 30 )
ctrl "fasr"
joules 100000
joules_capacity 400000
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Register Now & Save for Velocity, the Web Performance & Operations
Conference from O'Reilly Media. Velocity features a full day of
expert-led, hands-on workshops and two days of sessions from industry
leaders in dedicated Performance & Operations tracks. Use code vel09scf
and Save an extra 15% before 5/3. http://p.sf.net/sfu/velocityconf
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
