42 #include <sys/types.h>
58 #include <FL/Fl_Box.H>
59 #include <FL/Fl_Gl_Window.H>
60 #include <FL/Fl_Menu_Bar.H>
61 #include <FL/Fl_Window.H>
62 #include <FL/fl_draw.H>
66 #include <OpenGL/glu.h>
102 "Copyright Richard Vaughan and contributors 2000-2009";
106 "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and contributors.";
109 const char WEBSITE[] =
"http://playerstage.org";
113 "Robot simulation library\nPart of the Player Project";
117 "Stage robot simulation library\n" \
118 "Copyright (C) 2000-2009 Richard Vaughan and contributors\n" \
119 "Part of the Player Project [http://playerstage.org]\n" \
121 "This program is free software; you can redistribute it and/or\n" \
122 "modify it under the terms of the GNU General Public License\n" \
123 "as published by the Free Software Foundation; either version 2\n" \
124 "of the License, or (at your option) any later version.\n" \
126 "This program is distributed in the hope that it will be useful,\n" \
127 "but WITHOUT ANY WARRANTY; without even the implied warranty of\n" \
128 "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" \
129 "GNU General Public License for more details.\n" \
131 "You should have received a copy of the GNU General Public License\n" \
132 "along with this program; if not, write to the Free Software\n" \
133 "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.\n" \
135 "The text of the license may also be available online at\n" \
136 "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";
148 inline double rtod(
double r ){
return( r*180.0/M_PI ); }
151 inline double dtor(
double d ){
return( d*M_PI/180.0 ); }
156 while( a < -M_PI ) a += 2.0*M_PI;
157 while( a > M_PI ) a -= 2.0*M_PI;
162 inline int sgn(
int a){
return( a<0 ? -1 : 1); }
165 inline double sgn(
double a){
return( a<0 ? -1.0 : 1.0); }
202 Color(
double r,
double g,
double b,
double a=1.0 );
207 Color(
const std::string& name );
214 void Print(
const char* prefix )
const;
241 void Save(
Worldfile* wf,
int section,
const char* keyword )
const;
258 : x(x), y(y), z(z), a(a)
271 return Pose( xmin + drand48() * (xmax-xmin),
272 ymin + drand48() * (ymax-ymin),
280 virtual void Print(
const char* prefix )
const
282 printf(
"%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n",
289 snprintf( buf, 256,
"[ %.3f %.3f %.3f %.3f ]",
291 return std::string(buf);
296 {
return( !(
x ||
y ||
z ||
a )); };
303 void Save(
Worldfile* wf,
int section,
const char* keyword );
307 const double cosa = cos(
a);
308 const double sina = sin(
a);
310 return Pose(
x + p.
x * cosa - p.
y * sina,
311 y + p.
x * sina + p.
y * cosa,
319 return( hypot(
y,
x ) < hypot( other.
y, other.
x ));
324 return(
x==other.
x &&
332 return(
x!=other.
x ||
340 return hypot(
x-other.
x,
y-other.
y );
378 virtual void Print(
const char* prefix )
const
381 printf(
"%s", prefix );
383 printf(
"velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
402 void Print(
const char* prefix )
const
405 printf(
"%s", prefix );
407 printf(
"geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
459 : x(x), y(y), z(z) {}
478 {
return ((
x += other.
x) && (
y += other.
y) ); }
487 : x(x), y(y), z(z) {}
503 if(
x < other.
x )
return true;
504 if( other.
x <
x )
return false;
509 {
return ((
x == other.
x) && (
y == other.
y) ); }
522 void coord_shift(
double x,
double y,
double z,
double a );
525 void draw_string(
float x,
float y,
float z,
const char *
string);
527 const char *
string, Fl_Align align );
530 void draw_octagon(
float x,
float y,
float w,
float h,
float m );
533 void draw_array(
float x,
float y,
float w,
float h,
534 float* data,
size_t len,
size_t offset,
535 float min,
float max );
536 void draw_array(
float x,
float y,
float w,
float h,
537 float* data,
size_t len,
size_t offset );
548 const std::string menu_name;
549 const std::string worldfile_name;
553 const std::string& worldfile_name )
554 : menu_name( menu_name ),
555 worldfile_name( worldfile_name )
573 double constrain(
double val,
double minval,
double maxval );
598 std::vector<rotrect_t>& rects );
609 #define VAR(V,init) __typeof(init) V=(init)
616 #define FOR_EACH(I,C) for(VAR(I,(C).begin()),ite=(C).end();(I)!=ite;++(I))
620 template <
class T,
class C>
622 { cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); }
625 #define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
626 #define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
627 #define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
628 #define PRINT_ERR3(m,a,b,c) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
629 #define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
630 #define PRINT_ERR5(m,a,b,c,d,e) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
633 #define PRINT_WARN(m) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
634 #define PRINT_WARN1(m,a) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
635 #define PRINT_WARN2(m,a,b) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
636 #define PRINT_WARN3(m,a,b,c) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
637 #define PRINT_WARN4(m,a,b,c,d) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
638 #define PRINT_WARN5(m,a,b,c,d,e) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
642 #define PRINT_MSG(m) printf( "Stage: "m" (%s %s)\n", __FILE__, __FUNCTION__)
643 #define PRINT_MSG1(m,a) printf( "Stage: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
644 #define PRINT_MSG2(m,a,b) printf( "Stage: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
645 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
646 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
647 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m" (%s %s)\n", a, b, c, d, e,__FILE__, __FUNCTION__)
649 #define PRINT_MSG(m) printf( "Stage: "m"\n" )
650 #define PRINT_MSG1(m,a) printf( "Stage: "m"\n", a)
651 #define PRINT_MSG2(m,a,b) printf( "Stage: "m"\n,", a, b )
652 #define PRINT_MSG3(m,a,b,c) printf( "Stage: "m"\n", a, b, c )
653 #define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m"\n", a, b, c, d )
654 #define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m"\n", a, b, c, d, e )
659 #define PRINT_DEBUG(m) printf( "debug: "m" (%s %s)\n", __FILE__, __FUNCTION__)
660 #define PRINT_DEBUG1(m,a) printf( "debug: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
661 #define PRINT_DEBUG2(m,a,b) printf( "debug: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
662 #define PRINT_DEBUG3(m,a,b,c) printf( "debug: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
663 #define PRINT_DEBUG4(m,a,b,c,d) printf( "debug: "m" (%s %s)\n", a, b, c ,d, __FILE__, __FUNCTION__)
664 #define PRINT_DEBUG5(m,a,b,c,d,e) printf( "debug: "m" (%s %s)\n", a, b, c ,d, e, __FILE__, __FUNCTION__)
666 #define PRINT_DEBUG(m)
667 #define PRINT_DEBUG1(m,a)
668 #define PRINT_DEBUG2(m,a,b)
669 #define PRINT_DEBUG3(m,a,b,c)
670 #define PRINT_DEBUG4(m,a,b,c,d)
671 #define PRINT_DEBUG5(m,a,b,c,d,e)
725 PRINT_ERR(
"Ancestor::SetToken() called with zero length string. Ignored." );
734 std::map<std::string,void*>::iterator it =
props.find( key );
735 return( it ==
props.end() ? NULL : it->second );
752 : pose(pose), range(range),
mod(NULL),
color() {}
759 mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest)
790 static std::vector<LogEntry>
log;
826 static std::vector<std::string>
args;
831 static std::set<World*> world_set;
832 static bool quit_all;
833 static void UpdateCb(
World* world);
834 static unsigned int next_id;
840 std::set<Model*> models;
843 std::map<std::string, Model*> models_by_name;
846 std::map<int,Model*> models_by_wfentity;
850 std::vector<Model*> models_with_fiducials;
854 bool operator()(
const Model* a,
const Model* b)
const;
859 bool operator()(
const Model* a,
const Model* b)
const;
864 std::set<Model*,ltx> models_with_fiducials_byx;
868 std::set<Model*,lty> models_with_fiducials_byy;
871 void FiducialInsert(
Model* mod )
873 FiducialErase( mod );
874 models_with_fiducials.push_back( mod );
878 void FiducialErase(
Model* mod )
880 EraseAll( mod, models_with_fiducials );
887 unsigned int show_clock_interval;
889 pthread_mutex_t sync_mutex;
890 unsigned int threads_working;
891 pthread_cond_t threads_start_cond;
892 pthread_cond_t threads_done_cond;
894 unsigned int worker_threads;
898 std::list<std::pair<world_callback_t,void*> >
cb_list;
970 void MapPoly(
const std::vector<point_int_t>& poly,
972 unsigned int layer );
982 {
return (int32_t)floor(x * ppm); };
990 virtual void PushColor(
double r,
double g,
double b,
double a )
991 { (void)r; (void)g; (void)b; (void)a; };
1004 const Model* finder,
1012 const Model* finder,
1015 const uint32_t sample_count,
1033 void RecordRay(
double x1,
double y1,
double x2,
double y2 );
1046 : time(time), mod(mod), cb(cb), arg(arg) {}
1113 World(
const std::string& name =
"MyWorld",
1128 virtual bool IsGUI()
const {
return false; }
1135 virtual void Load(
const std::string& worldfile_path );
1143 virtual bool Save(
const char* filename );
1148 virtual bool Update(
void);
1211 const std::vector<point_t>& pts,
1224 void Map(
unsigned int layer );
1227 void UnMap(
unsigned int layer );
1254 void SetZ(
double min,
double max );
1265 unsigned int width,
unsigned int height,
1270 std::vector<point_t> mpts;
1272 std::vector<point_t> pts;
1287 std::vector< std::list<Block*>::iterator > list_entries;
1291 std::vector<Cell*> rendered_cells[2];
1298 void InvalidateModelPointCache();
1311 void BuildDisplayList(
Model* mod );
1313 std::vector<Block*> blocks;
1340 void Map(
unsigned int layer );
1341 void UnMap(
unsigned int layer );
1353 unsigned int width,
unsigned int height,
1359 (*it)->InvalidateModelPointCache();
1375 virtual void Draw(
void )
const = 0;
1381 double x(
void )
const {
return _x; }
1382 double y(
void )
const {
return _y; }
1383 double z(
void )
const {
return _z; }
1385 virtual void reset() = 0;
1404 virtual void Draw(
void )
const;
1409 void strafe(
double amount );
1410 void forward(
double amount );
1414 void move(
double x,
double y,
double z );
1415 void setFov(
double horiz_fov,
double vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
1420 double vertFov(
void )
const {
return _vert_fov; }
1426 return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) );
1431 void setClip(
double near,
double far ) { _z_far = far; _z_near = near; }
1443 double _pixels_width;
1444 double _pixels_height;
1457 virtual void Draw()
const;
1460 double pixels_height,
1466 void move(
double x,
double y );
1485 void scale(
double scale,
double shift_x = 0,
double h = 0,
double shift_y = 0,
double w = 0 );
1508 std::vector<Option*> drawOptions;
1510 std::vector<usec_t> interval_log;
1522 usec_t real_time_interval;
1529 usec_t real_time_recorded;
1532 uint64_t timing_interval;
1535 static void windowCb( Fl_Widget* w,
WorldGui* wg );
1536 static void fileLoadCb( Fl_Widget* w,
WorldGui* wg );
1537 static void fileSaveCb( Fl_Widget* w,
WorldGui* wg );
1538 static void fileSaveAsCb( Fl_Widget* w,
WorldGui* wg );
1539 static void fileExitCb( Fl_Widget* w,
WorldGui* wg );
1542 static void helpAboutCb( Fl_Widget* w,
WorldGui* wg );
1543 static void pauseCb( Fl_Widget* w,
WorldGui* wg );
1544 static void onceCb( Fl_Widget* w,
WorldGui* wg );
1545 static void fasterCb( Fl_Widget* w,
WorldGui* wg );
1546 static void slowerCb( Fl_Widget* w,
WorldGui* wg );
1547 static void realtimeCb( Fl_Widget* w,
WorldGui* wg );
1548 static void fasttimeCb( Fl_Widget* w,
WorldGui* wg );
1549 static void resetViewCb( Fl_Widget* w,
WorldGui* wg );
1550 static void moreHelptCb( Fl_Widget* w,
WorldGui* wg );
1553 bool saveAsDialog();
1554 bool closeWindowQuery();
1556 virtual void AddModel(
Model* mod );
1563 virtual void PushColor(
double r,
double g,
double b,
double a );
1571 WorldGui(
int W,
int H,
const char*L=0);
1575 virtual void Redraw(
void );
1579 virtual void Load(
const std::string& filename );
1581 virtual bool Save(
const char* filename );
1582 virtual bool IsGUI()
const {
return true; };
1585 virtual void Start();
1586 virtual void Stop();
1614 float x,y,w,h,min,max;
1615 Color fgcolor, bgcolor;
1621 const char* name,
const char* wfname );
1638 unsigned int columns, rows;
1641 std::vector<joules_t> cells;
1716 printf(
"%s", prefix );
1758 static uint32_t count;
1759 static std::map<id_t,Model*> modelsbyid;
1764 std::vector<Option*> drawOptions;
1765 const std::vector<Option*>& getOptions()
const {
return drawOptions; }
1795 :
callback(NULL), arg(arg) { (void)cb; }
1803 return(
arg < other.
arg );
1827 Flag(
const Color& color,
double size );
1832 void Draw( GLUquadric* quadric );
1930 unsigned int width, height;
1932 std::vector<point_t> pts;
1941 unsigned int height,
2026 if( str.size() > 0 )
2032 PRINT_ERR(
"Model::SetToken() called with zero length string. Ignored." );
2069 unsigned int width,
unsigned int height,
2096 void Map(
unsigned int layer );
2101 void UnMap(
unsigned int layer );
2119 const bool ztest =
true );
2129 const uint32_t sample_count,
2130 const bool ztest =
true );
2136 const bool ztest =
true );
2144 const uint32_t sample_count,
2145 const bool ztest =
true );
2173 void DrawImage( uint32_t texture_id,
Camera* cam,
float alpha,
double width=1.0,
double height=1.0 );
2207 {
return modelsbyid[
id]; }
2212 const std::string&
type =
"model",
2213 const std::string& name =
"" );
2221 boundary(false),
data_fresh(false),
disabled(true),
friction(0),
has_default_block(false),
log_state(false),
map_resolution(0),
mass(0),
parent(NULL),
rebuild_displaylist(false),
stack_children(true),
stall(false),
subs(0),
thread_safe(false),
trail_index(0),
event_queue_num(0),
used(false),
watts(0),
watts_give(0),
watts_take(0),
wf(NULL),
wf_entity(0),
world(NULL)
2224 void Say(
const std::string& str );
2247 virtual void Load();
2250 virtual void Save();
2346 void AddToPose(
double dx,
double dy,
double dz,
double da );
2426 virtual void Print(
char* prefix )
const;
2441 std::vector<point_int_t>
LocalToPixels(
const std::vector<point_t>& local )
const;
2506 std::vector<Blob> blobs;
2507 std::vector<Color> colors;
2510 static bool BlockMatcher(
Block* testblock,
Model* finder );
2522 const std::string&
type );
2529 virtual void Load();
2533 if( count ) *count = blobs.size();
2560 const std::string&
type );
2621 virtual void Update();
2622 virtual void DataVisualize(
Camera* cam );
2625 void PositionPaddles();
2626 void UpdateBreakBeams();
2627 void UpdateContacts();
2633 Block* paddle_right;
2643 const std::string&
type );
2647 virtual void Load();
2648 virtual void Save();
2705 void AddModelIfVisible(
Model* him );
2707 virtual void Update();
2708 virtual void DataVisualize(
Camera* cam );
2713 std::vector<Fiducial> fiducials;
2718 const std::string&
type );
2721 virtual void Load();
2738 if( count ) *count = fiducials.size();
2739 return &fiducials[0];
2752 const std::string&
type );
2755 virtual void Load();
2756 virtual void Print(
char* prefix )
const;
2787 size( 0.02, 0.02, 0.02 ),
2799 std::string
String()
const;
2814 std::vector<Sensor> sensors;
2832 static Option showBlinkenData;
2836 const std::string&
type );
2840 virtual void Load();
2861 GLfloat* _frame_data;
2862 GLubyte* _frame_color_data;
2864 bool _valid_vertexbuf_cache;
2869 static const int _depth = 4;
2871 int _camera_quads_size;
2872 GLfloat* _camera_quads;
2873 GLubyte* _camera_colors;
2875 static Option showCameraData;
2879 double _pitch_offset;
2887 const std::string&
type );
2891 virtual void Load();
2918 void setPitch(
double pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache =
false; }
2921 void setYaw(
double yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache =
false; }
2972 const std::string&
type );
2976 virtual void Move();
2980 virtual void Load();
3024 void SetSpeed(
double x,
double y,
double a );
3035 void GoTo(
double x,
double y,
double a );
3072 double min_position;
3073 double max_position;
3074 double start_position;
3086 const std::string&
type );
3093 virtual void Load();