Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

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
21 libstage/model.cc
View
@@ -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
43 libstage/model_draw.cc
View
@@ -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();
2  libstage/model_load.cc
View
@@ -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 );
70 libstage/stage.hh
View
@@ -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. */
6 libstage/world.cc
View
@@ -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));
6 libstage/worldgui.cc
View
@@ -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
3  todo.txt
View
@@ -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 *
2  worlds/fasr.world
View
@@ -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.