Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

moved trail recording to worldgui from world, and made recording trai…

…ls faster using a vector ring buffer instead of a list
  • Loading branch information...
commit 3ea9a043edb76188eef9913557e5c82f8a627e27 1 parent be502c5
rtv authored
View
21 libstage/model.cc
@@ -287,10 +287,11 @@ Model::Model( World* world,
rebuild_displaylist(true),
say_string(),
stall(false),
- subs(0),
- thread_safe( false ),
- trail(),
- type(type),
+ subs(0),
+ thread_safe( false ),
+ trail(trail_length),
+ trail_index(0),
+ type(type),
event_queue_num( 0 ),
used(false),
velocity(),
@@ -907,10 +908,16 @@ void Model::UpdatePose( void )
void Model::UpdateTrail()
{
- trail.push_back( TrailItem( world->sim_time, GetGlobalPose(), color ) );
+ // get the current item and increment the counter
+ TrailItem* item = &trail[trail_index++];
- if( trail.size() > trail_length )
- trail.pop_front();
+ // record the current info
+ item->time = world->sim_time;
+ item->pose = GetGlobalPose();
+ item->color = color;
+
+ // wrap around ring buffer
+ trail_index %= trail_length;
}
Model* Model::GetUnsubscribedModelOfType( const std::string& type ) const
View
43 libstage/model_draw.cc
@@ -55,24 +55,31 @@ void Model::DrawTrailFootprint()
PushColor( 0,0,0,1 ); // dummy pushL just saving the color
- FOR_EACH( it, trail )
- {
- TrailItem& checkpoint = *it;
-
- glPushMatrix();
- Pose pz = checkpoint.pose;
-
- Gl::pose_shift( pz );
- Gl::pose_shift( geom.pose );
-
- darkness += fade;
- Color c = checkpoint.color;
- c.a = darkness;
- glColor4f( c.r, c.g, c.b, c.a );
-
- blockgroup.DrawFootPrint( geom );
-
- glPopMatrix();
+ // this loop could be faster, but optimzing vis is not a priority
+ for( unsigned int i=0; i<trail_length; i++ )
+ {
+ // find correct offset inside ring buffer
+ TrailItem& checkpoint =
+ trail[ (i + trail_index) % trail_length ];
+
+ // ignore invalid items
+ if( checkpoint.time == 0 )
+ continue;
+
+ glPushMatrix();
+ Pose pz = checkpoint.pose;
+
+ Gl::pose_shift( pz );
+ Gl::pose_shift( geom.pose );
+
+ darkness += fade;
+ Color c = checkpoint.color;
+ c.a = darkness;
+ glColor4f( c.r, c.g, c.b, c.a );
+
+ blockgroup.DrawFootPrint( geom );
+
+ glPopMatrix();
}
PopColor();
View
2  libstage/model_load.cc
@@ -221,6 +221,8 @@ void Model::Load()
this->Say( wf->ReadString(wf_entity, "say", "" ));
trail_length = wf->ReadInt( wf_entity, "trail_length", trail_length );
+ trail.resize( trail_length );
+
trail_interval = wf->ReadInt( wf_entity, "trail_interval", trail_interval );
this->alwayson = wf->ReadInt( wf_entity, "alwayson", alwayson );
View
70 libstage/stage.hh
@@ -60,13 +60,11 @@
#include <FL/Fl_Window.H>
#include <FL/fl_draw.H>
#include <FL/gl.h> // FLTK takes care of platform-specific GL stuff
-// except GLU & GLUT
+// except GLU
#ifdef __APPLE__
#include <OpenGL/glu.h>
-//#include <GLUT/glut.h>
#else
#include <GL/glu.h>
-//#include <GL/glut.h>
#endif
/** @brief The Stage library uses its own namespace */
@@ -109,22 +107,22 @@ namespace Stg
number. */
const char* Version();
- /// Copyright string
+ /** Copyright string */
const char COPYRIGHT[] =
"Copyright Richard Vaughan and contributors 2000-2009";
- /// Author string
+ /** Author string */
const char AUTHORS[] =
"Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and contributors.";
- /// Project website string
+ /** Project website string */
const char WEBSITE[] = "http://playerstage.org";
- /// Project description string
+ /** Project description string */
const char DESCRIPTION[] =
"Robot simulation library\nPart of the Player Project";
- /// Project distribution license string
+ /** Project distribution license string */
const char LICENSE[] =
"Stage robot simulation library\n" \
"Copyright (C) 2000-2009 Richard Vaughan and contributors\n" \
@@ -169,11 +167,11 @@ namespace Stg
while( a > M_PI ) a -= 2.0*M_PI;
return a;
};
-
+
/** take binary sign of a, either -1, or 1 if >= 0 */
inline int sgn( int a){ return( a<0 ? -1 : 1); }
-
- /** take binary sign of a, either -1, or 1 if >= 0. */
+
+ /** take binary sign of a, either -1.0, or 1.0 if >= 0. */
inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); }
/** any integer value other than this is a valid fiducial ID */
@@ -446,15 +444,19 @@ namespace Stg
{
public:
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) ); }
-
+ 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
+ {
+ if( x < other.x ) return true;
+ if( other.x < x ) return false;
+ return y < other.y;
+ }
+
bool operator==( const stg_point_int_t& other ) const
- { return ((x == other.x) && (y == other.y) ); }
+ { return ((x == other.x) && (y == other.y) ); }
};
typedef std::vector<stg_point_int_t> PointIntVec;
@@ -463,20 +465,7 @@ namespace Stg
square. */
stg_point_t* stg_unit_square_points_create();
- const char MP_PREFIX[] = "_mp_";
- const char MP_POSE[] = "_mp_pose";
- const char MP_VELOCITY[] = "_mp_velocity";
- const char MP_GEOM[] = "_mp_geom";
- const char MP_COLOR[] = "_mp_color";
- const char MP_WATTS[] = "_mp_watts";
- const char MP_FIDUCIAL_RETURN[] = "_mp_fiducial_return";
- const char MP_LASER_RETURN[] = "_mp_laser_return";
- const char MP_OBSTACLE_RETURN[] = "_mp_obstacle_return";
- const char MP_RANGER_RETURN[] = "_mp_ranger_return";
- const char MP_GRIPPER_RETURN[] = "_mp_gripper_return";
- const char MP_MASS[] = "_mp_mass";
-
- /// laser return value
+ /** laser return value */
typedef enum
{
LaserTransparent=0, ///<not detected by laser model
@@ -1818,12 +1807,19 @@ namespace Stg
Pose pose;
Color color;
- TrailItem( stg_usec_t time, Pose pose, Color color )
- : time(time), pose(pose), color(color){}
+ TrailItem()
+ : time(0), pose(), color(){}
+
+ //TrailItem( stg_usec_t time, Pose pose, Color color )
+ //: time(time), pose(pose), color(color){}
};
- std::list<TrailItem> trail;
-
+ /** a ring buffer for storing recent poses */
+ std::vector<TrailItem> trail;
+
+ /** current position in the ring buffer */
+ unsigned int trail_index;
+
/** The maxiumum length of the trail drawn. Default is 20, but can
be set in the world file using the tail_length model
property. */
View
6 libstage/world.cc
@@ -557,10 +557,6 @@ bool World::Update()
FOR_EACH( it, active_energy )
(*it)->UpdateCharge();
- if( Model::trail_length > 0 && updates % Model::trail_interval == 0 )
- FOR_EACH( it, active_velocity )
- (*it)->UpdateTrail();
-
if( show_clock && ((this->updates % show_clock_interval) == 0) )
{
printf( "\r[Stage: %s]", ClockString().c_str() );
@@ -679,7 +675,7 @@ stg_raytrace_result_t World::Raytrace( const Ray& r )
// fast integer line 3d algorithm adapted from Cohen's code from
// Graphics Gems IV
- const int32_t sx(sgn(dx)); // sgn() is a fast macro
+ const int32_t sx(sgn(dx));
const int32_t sy(sgn(dy));
const int32_t ax(abs(dx));
const int32_t ay(abs(dy));
View
6 libstage/worldgui.cc
@@ -362,7 +362,11 @@ bool WorldGui::Update()
// inherit
bool done = World::Update();
-
+
+ if( Model::trail_length > 0 && updates % Model::trail_interval == 0 )
+ FOR_EACH( it, active_velocity )
+ (*it)->UpdateTrail();
+
if( done )
{
quit_time = 0; // allows us to continue by un-pausing
View
3  todo.txt
@@ -18,7 +18,8 @@ OS X 16.20 (rev 7949)
14.5 (rev 8295) threads: 6: 13.7 3: 13.4
MBA 17.3
-MBA 23.2
+MBA 23.1 (1 thread)
+MBA 19.3 (2 threads)
** 3.2.0 RELEASE *
View
2  worlds/fasr.world
@@ -15,7 +15,7 @@ quit_time 3600 # 1 hour of simulated time
resolution 0.02
-threads 3
+threads 2
# configure the GUI window
window
Please sign in to comment.
Something went wrong with that request. Please try again.