From: <jer...@us...> - 2008-06-03 17:15:12
|
Revision: 6466 http://playerstage.svn.sourceforge.net/playerstage/?rev=6466&view=rev Author: jeremy_asher Date: 2008-06-03 17:15:13 -0700 (Tue, 03 Jun 2008) Log Message: ----------- draw method signature changed to accomodate speech bubble changes Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-04 00:13:25 UTC (rev 6465) +++ code/stage/trunk/libstage/canvas.cc 2008-06-04 00:15:13 UTC (rev 6466) @@ -523,7 +523,7 @@ //mod->Draw( showflags ); // draw the stuff that changes every update // draw everything else for( GList* it=world->children; it; it=it->next ) - ((StgModel*)it->data)->Draw( showflags ); + ((StgModel*)it->data)->Draw( showflags, this ); } if( world->GetRayList() ) Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-06-04 00:13:25 UTC (rev 6465) +++ code/stage/trunk/libstage/model.cc 2008-06-04 00:15:13 UTC (rev 6466) @@ -902,7 +902,7 @@ } -void StgModel::Draw( uint32_t flags ) +void StgModel::Draw( uint32_t flags, StgCanvas* canvas ) { //PRINT_DEBUG1( "Drawing %s", token ); @@ -914,29 +914,36 @@ if( this->say_string ) - { - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + { + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + glPushMatrix(); + + glTranslatef( 0, 0, 0.5 ); + + glRotatef( -rtod(global_pose.a), 0,0,1 ); // out of the robot pose CS + glRotatef( -rtod(canvas->sphi), 0,0,1 ); // out of the world view CS + glRotatef( rtod(canvas->stheta)-90, 1,0,0 ); + glRotatef( 90, 1,0,0 ); // out of the ground plane + + + const float margin = 0.1; + float w = gl_width( this->say_string ) / canvas->scale + 2*margin; + float h = gl_height() / canvas->scale + 2*margin; + + + PushColor( 0.8,0.8,1.0,1.0 ); // pale blue + glRectf( 0,0, w,h ); // draw bubble + PopColor(); + + glTranslatef( 0, 0, 0.1 ); // draw text forwards of bubble + PushColor( color ); + gl_draw_string( margin, margin, 0.0, this->say_string ); + PopColor(); + + glPopMatrix(); + } - glPushMatrix(); - - PushColor( 0.8,0.8,1.0,1.0 ); // pale blue - - - glRotatef( -rtod(global_pose.a), 0,0,1 ); // out of the robot pose CS - // out of the world view CS - glRotatef( 90, 1,0,0 ); // out of the ground plane - - glRectf( 0,0,1,1 ); - PopColor(); - - PushColor( color ); - gl_speech_bubble( 0.3,0.3,0, this->say_string ); - PopColor(); - - glPopMatrix(); - } - - if( flags & STG_SHOW_DATA ) DataVisualize(); @@ -961,7 +968,7 @@ // recursively draw the tree below this model for( GList* it=children; it; it=it->next ) - ((StgModel*)it->data)->Draw( flags ); + ((StgModel*)it->data)->Draw( flags, canvas ); glPopMatrix(); // drop out of local coords } Modified: code/stage/trunk/libstage/model_blinkenlight.cc =================================================================== --- code/stage/trunk/libstage/model_blinkenlight.cc 2008-06-04 00:13:25 UTC (rev 6465) +++ code/stage/trunk/libstage/model_blinkenlight.cc 2008-06-04 00:15:13 UTC (rev 6466) @@ -110,9 +110,9 @@ } -void StgModelBlinkenlight::Draw( uint32_t flags ) +void StgModelBlinkenlight::Draw( uint32_t flags, StgCanvas* canvas ) { - StgModel::Draw( flags ); + StgModel::Draw( flags, canvas ); if( on ) { Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-04 00:13:25 UTC (rev 6465) +++ code/stage/trunk/libstage/stage.hh 2008-06-04 00:15:13 UTC (rev 6466) @@ -1243,7 +1243,7 @@ virtual void Shutdown(); virtual void Update(); virtual void UpdatePose(); - virtual void Draw( uint32_t flags ); + virtual void Draw( uint32_t flags, StgCanvas* canvas ); virtual void DrawBlocks(); @@ -1695,6 +1695,7 @@ class StgCanvas : public Fl_Gl_Window { friend class StgWorldGui; // allow access to private members + friend class StgModel; private: GlColorStack colorstack; @@ -2219,7 +2220,7 @@ virtual void Load(); virtual void Update(); - virtual void Draw( uint32_t flags ); + virtual void Draw( uint32_t flags, StgCanvas* canvas ); // static wrapper for the constructor - all models must implement // this method and add an entry in typetable.cc This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-06-04 11:34:22
|
Revision: 6468 http://playerstage.svn.sourceforge.net/playerstage/?rev=6468&view=rev Author: alexcb Date: 2008-06-04 11:34:30 -0700 (Wed, 04 Jun 2008) Log Message: ----------- added missing texture manager source Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc Added Paths: ----------- code/stage/trunk/libstage/texture_manager.cc code/stage/trunk/libstage/texture_manager.hh Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-04 18:02:22 UTC (rev 6467) +++ code/stage/trunk/libstage/canvas.cc 2008-06-04 18:34:30 UTC (rev 6468) @@ -410,8 +410,8 @@ glLoadIdentity (); // move the next two lines... + glTranslatef( -panx, -pany, 0 ); glScalef( scale, scale, scale ); - glTranslatef( -panx/scale, -pany/scale, 0 ); // .. from here glRotatef( rtod(-stheta), fabs(cos(sphi)), 0, 0 ); Added: code/stage/trunk/libstage/texture_manager.cc =================================================================== --- code/stage/trunk/libstage/texture_manager.cc (rev 0) +++ code/stage/trunk/libstage/texture_manager.cc 2008-06-04 18:34:30 UTC (rev 6468) @@ -0,0 +1,65 @@ +/* + * texture_manager.cc + * Stage + * + * Created by Alex Couture-Beil on 03/06/08. + * Copyright 2008 __MyCompanyName__. All rights reserved. + * + */ + +#include "texture_manager.hh" + +GLuint TextureManager::loadTexture( const char *filename ) +{ + GLuint texName; + Fl_Shared_Image *img = Fl_Shared_Image::get( filename ); + if( img == NULL ) { + printf( "unable to open image: %s\n", filename ); + return 0; + } + + std::cout << "loading image 1 " << std::endl; + + //TODO display an error for incorrect depths + if( img->d() != 3 && img->d() != 4 ) { + printf( "unable to open image: %s - incorrect depth - should be 3 or 4\n", filename ); + return 0; + } + + std::cout << "loading image 2 " << std::endl; + + //TODO check for correct width/height - or convert it. + + guchar* pixels = (guchar*)(img->data()[0]); + + //vertically flip the image + int img_size = img->w() * img->h() * img->d(); + guchar* img_flip = new guchar[ img_size ]; + + const int row_width = img->w() * img->d(); + for( int i = 0; i < img->h(); i++ ) + memcpy( img_flip + ( i * row_width ), pixels + ( ( img->h() - i - 1) * row_width ), row_width ); + + + //create room for texture + glGenTextures(1, &texName); + std::cout << "loading image to: " << texName << std::endl; + + glBindTexture(GL_TEXTURE_2D, texName); + + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + + glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + + glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT); + glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); + + //glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); //or use GL_DECAL or GL_MODULATE instead of DECAL to mix colours and textures + + gluBuild2DMipmaps (GL_TEXTURE_2D, img->d(), img->w(), img->h(), ( img->d() == 3 ? GL_RGB : GL_RGBA ), + GL_UNSIGNED_BYTE, img_flip ); + + glBindTexture( GL_TEXTURE_2D, 0 ); + return texName; + } Added: code/stage/trunk/libstage/texture_manager.hh =================================================================== --- code/stage/trunk/libstage/texture_manager.hh (rev 0) +++ code/stage/trunk/libstage/texture_manager.hh 2008-06-04 18:34:30 UTC (rev 6468) @@ -0,0 +1,42 @@ +/* + * texture_manager.hh + * Stage + * + * Singleton class for loading textures + * + */ +#ifndef _TEXTURE_MANAGER_H_ +#define _TEXTURE_MANAGER_H_ + +#include "stage_internal.hh" + +#include <FL/Fl_Shared_Image.H> +#include <glib.h> +#include <iostream> + +///Singleton for loading textures (not threadsafe) +class TextureManager { +private: + TextureManager( void ) { } + +public: + + //TODO figure out where to store standard textures + GLuint _stall_texture_id; + + //TODO make this threadsafe + inline static TextureManager& getInstance( void ) { + static TextureManager* the_instance = NULL; + //TODO add a lock here + if( the_instance == NULL ) { + the_instance = new TextureManager; + } + return *the_instance; + } + + ///load a texture on the GPU, returned value is the texture ID, or 0 for failure + GLuint loadTexture( const char *filename ); + +}; + +#endif //_TEXTURE_MANAGER_H_ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-06-06 18:15:34
|
Revision: 6488 http://playerstage.svn.sourceforge.net/playerstage/?rev=6488&view=rev Author: alexcb Date: 2008-06-06 18:15:39 -0700 (Fri, 06 Jun 2008) Log Message: ----------- created camera class, and changed zoom and rotating behaviour to be local to the center of the screen (NOT world) Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/texture_manager.cc code/stage/trunk/libstage/worldgui.cc Added Paths: ----------- code/stage/trunk/libstage/camera.cc Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2008-06-07 00:44:46 UTC (rev 6487) +++ code/stage/trunk/libstage/CMakeLists.txt 2008-06-07 01:15:39 UTC (rev 6488) @@ -3,6 +3,7 @@ ancestor.cc block.cc canvas.cc + camera.cc gl.cc glcolorstack.cc model.cc Added: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc (rev 0) +++ code/stage/trunk/libstage/camera.cc 2008-06-07 01:15:39 UTC (rev 6488) @@ -0,0 +1,39 @@ +/* + * camera.cc + * Stage + * + * Created by Alex Couture-Beil on 06/06/08. + * Copyright 2008 __MyCompanyName__. All rights reserved. + * + */ + + +#include "stage_internal.hh" + +#include <iostream> + +void StgCamera::Draw( void ) +{ + static float i = 0; + i += 1; +// glRotatef( rtod(-_stheta), fabs(cos(_sphi)), 0, 0 ); +// glRotatef( rtod(_sphi), 0,0,1 ); // rotate about z - yaw +// +// glTranslatef( -_panx, -_pany, 0 ); +// glScalef( _scale, _scale, _scale ); + + + glRotatef( _pitch, 1.0, 0.0, 0.0 ); + glRotatef( _yaw, 0.0, 0.0, 1.0 ); + + //both of these are handled in the glOrtho call +// glScalef( _scale, _scale, _scale ); + glTranslatef( - _x * _scale, - _y * _scale, 0.0 ); + glScalef( _scale, _scale, _scale ); + +// glRotatef( 60.0, 1.0, 0.0, 0.0 ); + + //glRotatef( 60, 0.0, 1.0, 0.0 ); +// glRotatef( i, 0.0, 0.0, 1.0 ); + +} Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-07 00:44:46 UTC (rev 6487) +++ code/stage/trunk/libstage/canvas.cc 2008-06-07 01:15:39 UTC (rev 6488) @@ -32,8 +32,8 @@ last_selection = NULL; startx = starty = 0; - panx = pany = stheta = sphi = 0.0; - scale = 15.0; + //panx = pany = stheta = sphi = 0.0; + //scale = 15.0; interval = 50; //msec between redraws graphics = true; @@ -169,11 +169,7 @@ } else { - //scale -= 0.5 * (double)Fl::event_dy(); - scale -= 0.5 * (double)Fl::event_dy(); - if( scale < 1 ) - scale = 1; - + camera.scale( Fl::event_dy() ); invalidate(); } return 1; @@ -184,18 +180,17 @@ int dx = Fl::event_x() - startx; int dy = Fl::event_y() - starty; - stheta -= 0.01 * (double)dy; - sphi += 0.01 * (double)dx; - stheta = constrain( stheta, 0, M_PI/2.0 ); + camera.pitch( 0.5 * static_cast<double>( dy ) ); + camera.yaw( 0.5 * static_cast<double>( dx ) ); + invalidate(); } else if( Fl::event_state( FL_ALT ) ) - { + { int dx = Fl::event_x() - startx; int dy = Fl::event_y() - starty; - panx -= dx; - pany += dy; + camera.move( -dx, dy ); invalidate(); } @@ -256,8 +251,7 @@ } else { - panx -= dx; - pany += dy; + camera.move( -dx, dy ); invalidate(); // so the projection gets updated } break; @@ -304,13 +298,14 @@ world->TogglePause(); break; case ' ': // space bar - sphi = stheta = 0.0; + + camera.resetAngle(); invalidate(); break; - case FL_Left: panx += 10; break; - case FL_Right: panx -= 10; break; - case FL_Down: pany += 10; break; - case FL_Up: pany -= 10; break; + case FL_Left: camera.move( -10, 0 ); break; + case FL_Right: camera.move( 10, 0 );; break; + case FL_Down: camera.move( 0, -10 );; break; + case FL_Up: camera.move( 0, 10 );; break; default: return 0; // keypress unhandled } @@ -390,22 +385,29 @@ stg_bounds3d_t extent = world->GetExtent(); - glOrtho( -pixels_width/2.0, pixels_width/2.0, - -pixels_height/2.0, pixels_height/2.0, - extent.y.min*scale, extent.y.max*scale ); +// glFrustum( -pixels_width/2.0, pixels_width/2.0, +// -pixels_height/2.0, pixels_height/2.0, +// extent.y.min, extent.y.max ); +// glOrtho( -pixels_width/2.0, pixels_width/2.0, +// -pixels_height/2.0, pixels_height/2.0, +// extent.y.min*scale, extent.y.max*scale ); // set the modelview matrix glMatrixMode (GL_MODELVIEW); glLoadIdentity (); - // move the next two lines... - glTranslatef( -panx, -pany, 0 ); - glScalef( scale, scale, scale ); - // .. from here + //TODO insert a camera class here + + camera.Draw(); + +// // move the next two lines... +// glTranslatef( -panx, -pany, 0 ); +// glScalef( scale, scale, scale ); +// // .. from here +// +// glRotatef( rtod(-stheta), fabs(cos(sphi)), 0, 0 ); +// glRotatef( rtod(sphi), 0,0,1 ); // rotate about z - yaw - glRotatef( rtod(-stheta), fabs(cos(sphi)), 0, 0 ); - glRotatef( rtod(sphi), 0,0,1 ); // rotate about z - yaw - // ... to here to get rotation about the center of the window (but broken panning) // enable vertex arrays @@ -415,6 +417,27 @@ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); } + glMatrixMode (GL_PROJECTION); + glLoadIdentity (); + + + stg_bounds3d_t extent = world->GetExtent(); + double pixels_width = w(); + double pixels_height = h(); + +// gluPerspective( 60, pixels_width / pixels_height, 0.01, 1000 ); + + + + glOrtho( -pixels_width/2.0 / camera.getScale(), pixels_width/2.0 / camera.getScale(), + -pixels_height/2.0 / camera.getScale(), pixels_height/2.0 / camera.getScale(), + extent.y.min * camera.getScale() * 2, extent.y.max * camera.getScale() * 2 ); + + //draw camera + glMatrixMode (GL_MODELVIEW); + glLoadIdentity (); + camera.Draw(); + if( ! (showflags & STG_SHOW_TRAILS) ) glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); @@ -422,12 +445,12 @@ if( (showflags & STG_SHOW_FOLLOW) && last_selection ) { glLoadIdentity (); - double zclip = 20 * scale; //hypot(world->Width(), world->Height()) * scale; + double zclip = 20 * camera.getScale(); //hypot(world->Width(), world->Height()) * camera.getScale(); glTranslatef( 0,0, -zclip / 2.0 ); // meter scale - glScalef ( scale, scale, scale ); // zoom + glScalef ( camera.getScale(), camera.getScale(), camera.getScale() ); // zoom stg_pose_t gpose = last_selection->GetGlobalPose(); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-06-07 00:44:46 UTC (rev 6487) +++ code/stage/trunk/libstage/model.cc 2008-06-07 01:15:39 UTC (rev 6488) @@ -910,8 +910,8 @@ void StgModel::DrawImage( uint32_t texture_id, Stg::StgCanvas* canvas, float alpha ) { - double sphi = canvas->sphi; - double stheta = canvas->stheta; + float stheta = -dtor( canvas->camera.getPitch() ); + float sphi = dtor( canvas->camera.getYaw() ); glBindTexture( GL_TEXTURE_2D, 1 ); @@ -969,19 +969,23 @@ // draw speech bubble if( this->say_string ) { + float stheta = -dtor( canvas->camera.getPitch() ); + float sphi = dtor( canvas->camera.getYaw() ); + float scale = canvas->camera.getScale(); //TODO FIX THIS glPushMatrix(); // move above the robot glTranslatef( 0, 0, 0.5 ); - + // rotate to face screen - glRotatef( -rtod(global_pose.a + canvas->sphi), 0,0,1 ); - glRotatef( rtod(canvas->stheta), 1,0,0 ); + glRotatef( -rtod(global_pose.a + sphi), 0,0,1 ); + glRotatef( rtod(stheta), 1,0,0 ); - const float m = 4 / canvas->scale; // margin - float w = gl_width( this->say_string ) / canvas->scale; // scaled text width - float h = gl_height() / canvas->scale; // scaled text height + const float m = 4 / scale; // margin + float w = gl_width( this->say_string ) / scale; // scaled text width + float h = gl_height() / scale; // scaled text height + // draw inside of bubble PushColor( STG_BUBBLE_FILL ); glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-07 00:44:46 UTC (rev 6487) +++ code/stage/trunk/libstage/stage.hh 2008-06-07 01:15:39 UTC (rev 6488) @@ -53,6 +53,7 @@ #include <string.h> #include <sys/types.h> #include <sys/time.h> +#include <iostream> // we use GLib's data structures extensively. Perhaps we'll move to // C++ STL types to lose this dependency one day. @@ -1696,6 +1697,62 @@ #include <FL/gl.h> // FLTK takes care of platform-specific GL stuff #include <FL/glut.H> +class StgCamera +{ +private: + float _x, _y, _z; + float _pitch; //left-right (about y) + float _yaw; //up-down (about x) + float _scale; + +public: + StgCamera( void ) : _x( 0 ), _y( 0 ), _z( 0 ), _pitch( 0 ), _yaw( 0 ), _scale( 15 ) { } + void Draw(); + + void move( float x, float y ) { + x = x / ( _scale * _scale ); + y = y / ( _scale * _scale ); + + std::cout << _scale << std::endl; + + _x += cos( dtor( _yaw ) ) * x; + _y += -sin( dtor( _yaw ) ) * x; + + _x += sin( dtor( _yaw ) ) * y; + _y += cos( dtor( _yaw ) ) * y; + } + void yaw( float yaw ) { + _yaw += yaw; + } + void pitch( float pitch ) { + _pitch += pitch; + if( _pitch < -90 ) + _pitch = -90; + else if( _pitch > 0 ) + _pitch = 0; + } + inline float getYaw( void ) { return _yaw; } + inline float getPitch( void ) { return _pitch; } + + inline void setYaw( float yaw ) { _yaw = yaw; } + inline void setPitch( float pitch ) { _pitch = pitch; } + inline void setScale( float scale ) { _scale = scale; } + inline void setPose( float x, float y) { _x = x; _y = y; } + + + + void scale( float scale ) { _scale -= 0.5 * scale; + if( _scale < 1 ) _scale = 1; + + } + + inline void resetAngle( void ) { _pitch = _yaw = 0; } + + inline float getScale() { return _scale; } + inline float getX() { return _x; } + inline float getY() { return _y; } +}; + class StgCanvas : public Fl_Gl_Window { friend class StgWorldGui; // allow access to private members @@ -1703,8 +1760,9 @@ private: GlColorStack colorstack; - double scale; - double panx, pany, stheta, sphi; + + StgCamera camera; + int startx, starty; bool dragging; bool rotating; Modified: code/stage/trunk/libstage/texture_manager.cc =================================================================== --- code/stage/trunk/libstage/texture_manager.cc 2008-06-07 00:44:46 UTC (rev 6487) +++ code/stage/trunk/libstage/texture_manager.cc 2008-06-07 01:15:39 UTC (rev 6488) @@ -16,6 +16,7 @@ if( filename[ 0 ] == '/' || filename[ 0 ] == '~' ) return Fl_Shared_Image::get( filename ); + //TODO move this somewhere else, and include STAGE_PATH, and path relative to user supplied world file const char* prefixes[] = { ".", INSTALL_PREFIX "/share/stage", @@ -28,7 +29,6 @@ std::ostringstream oss; oss << prefixes[ i ] << "/" << filename; img = Fl_Shared_Image::get( oss.str().c_str() ); - std::cout << "loading from: " << oss.str() << std::endl; i++; } return img; @@ -39,13 +39,13 @@ GLuint texName; Fl_Shared_Image *img = loadImage( filename ); if( img == NULL ) { - printf( "unable to open image: %s\n", filename ); + fprintf( stderr, "unable to open image: %s\n", filename ); return 0; } //TODO display an error for incorrect depths if( img->d() != 3 && img->d() != 4 ) { - printf( "unable to open image: %s - incorrect depth - should be 3 or 4\n", filename ); + fprintf( stderr, "unable to open image: %s - incorrect depth - should be 3 or 4\n", filename ); return 0; } @@ -64,7 +64,6 @@ //create room for texture glGenTextures(1, &texName); - std::cout << "loading image to: " << texName << std::endl; glBindTexture(GL_TEXTURE_2D, texName); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-07 00:44:46 UTC (rev 6487) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-07 01:15:39 UTC (rev 6488) @@ -243,12 +243,14 @@ //larger than this size. size( width,height ); - canvas->panx = wf->ReadTupleFloat(wf_section, "center", 0, canvas->panx ); - canvas->pany = wf->ReadTupleFloat(wf_section, "center", 1, canvas->pany ); - canvas->stheta = wf->ReadTupleFloat( wf_section, "rotate", 0, canvas->stheta ); - canvas->sphi = wf->ReadTupleFloat( wf_section, "rotate", 1, canvas->sphi ); - canvas->scale = wf->ReadFloat(wf_section, "scale", canvas->scale ); - canvas->interval = wf->ReadInt(wf_section, "interval", canvas->interval ); + float x = wf->ReadTupleFloat(wf_section, "center", 0, 0 ); + float y = wf->ReadTupleFloat(wf_section, "center", 1, 0 ); + canvas->camera.setPose( x, y ); + + canvas->camera.setPitch( wf->ReadTupleFloat( wf_section, "rotate", 0, 0 ) ); + canvas->camera.setYaw( wf->ReadTupleFloat( wf_section, "rotate", 1, 0 ) ); + canvas->camera.setScale( wf->ReadFloat(wf_section, "scale", canvas->camera.getScale() ) ); + canvas->interval = wf->ReadInt(wf_section, "interval", canvas->interval ); // set the canvas visibilty flags uint32_t flags = canvas->GetShowFlags(); @@ -305,14 +307,14 @@ wf->WriteTupleFloat( wf_section, "size", 0, w() ); wf->WriteTupleFloat( wf_section, "size", 1, h() ); - wf->WriteFloat( wf_section, "scale", canvas->scale ); +// wf->WriteFloat( wf_section, "scale", canvas->scale ); +// +// wf->WriteTupleFloat( wf_section, "center", 0, canvas->panx ); +// wf->WriteTupleFloat( wf_section, "center", 1, canvas->pany ); +// +// wf->WriteTupleFloat( wf_section, "rotate", 0, canvas->stheta ); +// wf->WriteTupleFloat( wf_section, "rotate", 1, canvas->sphi ); - wf->WriteTupleFloat( wf_section, "center", 0, canvas->panx ); - wf->WriteTupleFloat( wf_section, "center", 1, canvas->pany ); - - wf->WriteTupleFloat( wf_section, "rotate", 0, canvas->stheta ); - wf->WriteTupleFloat( wf_section, "rotate", 1, canvas->sphi ); - uint32_t flags = canvas->GetShowFlags(); wf->WriteInt( wf_section, "show_blocks", flags & STG_SHOW_BLOCKS ); wf->WriteInt( wf_section, "show_grid", flags & STG_SHOW_GRID ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-06-09 11:34:39
|
Revision: 6491 http://playerstage.svn.sourceforge.net/playerstage/?rev=6491&view=rev Author: alexcb Date: 2008-06-09 11:34:44 -0700 (Mon, 09 Jun 2008) Log Message: ----------- fixed scale issue with recent camera changes Modified Paths: -------------- code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc 2008-06-09 15:24:07 UTC (rev 6490) +++ code/stage/trunk/libstage/camera.cc 2008-06-09 18:34:44 UTC (rev 6491) @@ -12,28 +12,27 @@ #include <iostream> -void StgCamera::Draw( void ) +void StgCamera::Draw( void ) const { - static float i = 0; - i += 1; -// glRotatef( rtod(-_stheta), fabs(cos(_sphi)), 0, 0 ); -// glRotatef( rtod(_sphi), 0,0,1 ); // rotate about z - yaw -// -// glTranslatef( -_panx, -_pany, 0 ); -// glScalef( _scale, _scale, _scale ); - + glMatrixMode (GL_MODELVIEW); + glLoadIdentity (); glRotatef( _pitch, 1.0, 0.0, 0.0 ); glRotatef( _yaw, 0.0, 0.0, 1.0 ); - //both of these are handled in the glOrtho call -// glScalef( _scale, _scale, _scale ); - glTranslatef( - _x * _scale, - _y * _scale, 0.0 ); - glScalef( _scale, _scale, _scale ); - -// glRotatef( 60.0, 1.0, 0.0, 0.0 ); + glTranslatef( - _x, - _y, 0.0 ); + //zooming needs to happen in the Projection code (don't use glScale for zoom) + +} - //glRotatef( 60, 0.0, 1.0, 0.0 ); -// glRotatef( i, 0.0, 0.0, 1.0 ); +void StgCamera::SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const +{ + glMatrixMode (GL_PROJECTION); + glLoadIdentity (); + glOrtho( -pixels_width/2.0 / _scale, pixels_width/2.0 / _scale, + -pixels_height/2.0 / _scale, pixels_height/2.0 / _scale, + y_min * _scale * 2, y_max * _scale * 2 ); + + glMatrixMode (GL_MODELVIEW); } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-09 15:24:07 UTC (rev 6490) +++ code/stage/trunk/libstage/canvas.cc 2008-06-09 18:34:44 UTC (rev 6491) @@ -169,7 +169,7 @@ } else { - camera.scale( Fl::event_dy() ); + camera.scale( Fl::event_dy(), Fl::event_x() - w() / 2, - ( Fl::event_y() - h() / 2 ) ); invalidate(); } return 1; @@ -374,70 +374,23 @@ // install a font gl_font( FL_HELVETICA, 12 ); - //double zclip = 20 * scale; //hypot(world->Width(), world->Height()) * scale; - double pixels_width = w(); - double pixels_height = h(); + stg_bounds3d_t extent = world->GetExtent(); + camera.SetProjection( w(), h(), extent.y.min, extent.y.max ); + camera.Draw(); - // map the viewport to pixel units by scaling it the same as the window - glMatrixMode (GL_PROJECTION); - glLoadIdentity (); - - stg_bounds3d_t extent = world->GetExtent(); -// glFrustum( -pixels_width/2.0, pixels_width/2.0, -// -pixels_height/2.0, pixels_height/2.0, -// extent.y.min, extent.y.max ); -// glOrtho( -pixels_width/2.0, pixels_width/2.0, -// -pixels_height/2.0, pixels_height/2.0, -// extent.y.min*scale, extent.y.max*scale ); - - // set the modelview matrix - glMatrixMode (GL_MODELVIEW); - glLoadIdentity (); - - //TODO insert a camera class here - camera.Draw(); - -// // move the next two lines... -// glTranslatef( -panx, -pany, 0 ); -// glScalef( scale, scale, scale ); -// // .. from here -// -// glRotatef( rtod(-stheta), fabs(cos(sphi)), 0, 0 ); -// glRotatef( rtod(sphi), 0,0,1 ); // rotate about z - yaw - - // ... to here to get rotation about the center of the window (but broken panning) - - // enable vertex arrays + // enable vertex arrays glEnableClientState( GL_VERTEX_ARRAY ); //glEnableClientState( GL_COLOR_ARRAY ); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - } - - glMatrixMode (GL_PROJECTION); - glLoadIdentity (); + } - stg_bounds3d_t extent = world->GetExtent(); - double pixels_width = w(); - double pixels_height = h(); -// gluPerspective( 60, pixels_width / pixels_height, 0.01, 1000 ); - - - glOrtho( -pixels_width/2.0 / camera.getScale(), pixels_width/2.0 / camera.getScale(), - -pixels_height/2.0 / camera.getScale(), pixels_height/2.0 / camera.getScale(), - extent.y.min * camera.getScale() * 2, extent.y.max * camera.getScale() * 2 ); - - //draw camera - glMatrixMode (GL_MODELVIEW); - glLoadIdentity (); - camera.Draw(); - if( ! (showflags & STG_SHOW_TRAILS) ) glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-06-09 15:24:07 UTC (rev 6490) +++ code/stage/trunk/libstage/model.cc 2008-06-09 18:34:44 UTC (rev 6491) @@ -971,7 +971,8 @@ { float stheta = -dtor( canvas->camera.getPitch() ); float sphi = dtor( canvas->camera.getYaw() ); - float scale = canvas->camera.getScale(); //TODO FIX THIS + float scale = canvas->camera.getScale(); + glPushMatrix(); // move above the robot Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-09 15:24:07 UTC (rev 6490) +++ code/stage/trunk/libstage/stage.hh 2008-06-09 18:34:44 UTC (rev 6491) @@ -1707,32 +1707,32 @@ public: StgCamera( void ) : _x( 0 ), _y( 0 ), _z( 0 ), _pitch( 0 ), _yaw( 0 ), _scale( 15 ) { } - void Draw(); + void Draw() const; + void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const; + - void move( float x, float y ) { - x = x / ( _scale * _scale ); - y = y / ( _scale * _scale ); + inline void move( float x, float y ) { + x = x / ( _scale ); + y = y / ( _scale ); - std::cout << _scale << std::endl; - _x += cos( dtor( _yaw ) ) * x; _y += -sin( dtor( _yaw ) ) * x; _x += sin( dtor( _yaw ) ) * y; _y += cos( dtor( _yaw ) ) * y; } - void yaw( float yaw ) { + inline void yaw( float yaw ) { _yaw += yaw; } - void pitch( float pitch ) { + inline void pitch( float pitch ) { _pitch += pitch; if( _pitch < -90 ) _pitch = -90; else if( _pitch > 0 ) _pitch = 0; } - inline float getYaw( void ) { return _yaw; } - inline float getPitch( void ) { return _pitch; } + inline float getYaw( void ) const { return _yaw; } + inline float getPitch( void ) const { return _pitch; } inline void setYaw( float yaw ) { _yaw = yaw; } inline void setPitch( float pitch ) { _pitch = pitch; } @@ -1741,16 +1741,17 @@ - void scale( float scale ) { _scale -= 0.5 * scale; + inline void scale( float scale, float center_x = 0, float center_y = 0 ) { + //TODO figure out how to zoom in centered on the given points (rather than just center of screen) + _scale -= 0.25 * scale * sqrt( _scale ); if( _scale < 1 ) _scale = 1; - } inline void resetAngle( void ) { _pitch = _yaw = 0; } - inline float getScale() { return _scale; } - inline float getX() { return _x; } - inline float getY() { return _y; } + inline float getScale() const { return _scale; } + inline float getX() const { return _x; } + inline float getY() const { return _y; } }; class StgCanvas : public Fl_Gl_Window This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-06-09 11:36:49
|
Revision: 6492 http://playerstage.svn.sourceforge.net/playerstage/?rev=6492&view=rev Author: jeremy_asher Date: 2008-06-09 11:36:48 -0700 (Mon, 09 Jun 2008) Log Message: ----------- Added Save As menu item Modified Paths: -------------- code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldfile.cc code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-09 18:34:44 UTC (rev 6491) +++ code/stage/trunk/libstage/stage.hh 2008-06-09 18:36:48 UTC (rev 6492) @@ -1059,7 +1059,7 @@ virtual void Load( const char* worldfile_path ); virtual void Reload(); - virtual void Save(); + virtual bool Save( const char* filename ); virtual bool Update(void); virtual void AddModel( StgModel* mod ); virtual void RemoveModel( StgModel* mod ); @@ -1839,10 +1839,11 @@ void Cycle(); virtual void Load( const char* filename ); - virtual void Save(); + virtual bool Save( const char* filename ); // static callback functions static void SaveCallback( Fl_Widget* wid, StgWorldGui* world ); + static void SaveAsCallback( Fl_Widget* wid, StgWorldGui* world ); virtual void PushColor( stg_color_t col ) { canvas->PushColor( col ); } Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-06-09 18:34:44 UTC (rev 6491) +++ code/stage/trunk/libstage/world.cc 2008-06-09 18:36:48 UTC (rev 6492) @@ -654,14 +654,14 @@ ((StgModel*)data)->Save(); } -void StgWorld::Save( void ) +bool StgWorld::Save( const char *filename ) { // ask every model to save itself //g_hash_table_foreach( this->models_by_id, stg_model_save_cb, NULL ); ForEachModel( _save_cb, NULL ); - this->wf->Save(NULL); + return this->wf->Save( filename ); } static void _reload_cb( gpointer key, gpointer data, gpointer user ) Modified: code/stage/trunk/libstage/worldfile.cc =================================================================== --- code/stage/trunk/libstage/worldfile.cc 2008-06-09 18:34:44 UTC (rev 6491) +++ code/stage/trunk/libstage/worldfile.cc 2008-06-09 18:36:48 UTC (rev 6492) @@ -232,15 +232,15 @@ filename = this->filename; // Open file - //FILE *file = fopen(filename, "w+"); - FILE *file = FileOpen(filename, "w+"); + FILE *file = fopen(filename, "w+"); + //FILE *file = FileOpen(filename, "w+"); if (!file) { PRINT_ERR2("unable to open world file %s : %s", filename, strerror(errno)); return false; } - + // TODO - make a backup before saving the file // Write the current set of tokens to the file Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-09 18:34:44 UTC (rev 6491) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-09 18:36:48 UTC (rev 6492) @@ -103,6 +103,7 @@ #include <FL/Fl_PNG_Image.H> #include <FL/Fl_Output.H> #include <FL/Fl_Multiline_Output.H> +#include <FL/Fl_File_Chooser.H> static const char* MITEM_VIEW_DATA = "View/Data"; static const char* MITEM_VIEW_BLOCKS = "View/Blocks"; @@ -188,7 +189,7 @@ mbar->add( "File", 0, 0, 0, FL_SUBMENU ); mbar->add( "File/Save File", FL_CTRL + 's', (Fl_Callback *)SaveCallback, this ); - //mbar->add( "File/Save File &As...", FL_CTRL + FL_SHIFT + 's', (Fl_Callback *)dummy_cb, 0, FL_MENU_DIVIDER ); + mbar->add( "File/Save File &As...", FL_CTRL + FL_SHIFT + 's', (Fl_Callback *)SaveAsCallback, this, FL_MENU_DIVIDER ); mbar->add( "File/Exit", FL_CTRL+'q', (Fl_Callback *)dummy_cb, 0 ); mbar->add( "View", 0, 0, 0, FL_SUBMENU ); @@ -297,11 +298,42 @@ void StgWorldGui::SaveCallback( Fl_Widget* wid, StgWorldGui* world ) { - world->Save(); + // save to current file + bool success = world->Save( NULL ); + if ( !success ) { + fl_alert( "Error saving world file." ); + } } -void StgWorldGui::Save( void ) +void StgWorldGui::SaveAsCallback( Fl_Widget* wid, StgWorldGui* world ) { + const char* curFilename = world->wf->filename; + const char* newFilename; + bool success; + const char* pattern = "World Files (*.world)"; + + Fl_File_Chooser fc( curFilename, pattern, Fl_File_Chooser::CREATE, "Save File As..." ); + fc.ok_label( "Save" ); + + fc.show(); + while (fc.shown()) + Fl::wait(); + + newFilename = fc.value(); + + if (newFilename != NULL) { + // todo: make sure file ends in .world + success = world->Save( newFilename ); + if ( !success ) { + fl_alert( "Error saving world file." ); + } + } + + +} + +bool StgWorldGui::Save( const char* filename ) +{ PRINT_DEBUG1( "%s.Save()", token ); wf->WriteTupleFloat( wf_section, "size", 0, w() ); @@ -326,7 +358,7 @@ // TODO - per model visualizations save - StgWorld::Save(); + return StgWorld::Save( filename ); } void StgWorld::UpdateCb( StgWorld* world ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-06-09 14:49:24
|
Revision: 6493 http://playerstage.svn.sourceforge.net/playerstage/?rev=6493&view=rev Author: alexcb Date: 2008-06-09 14:49:25 -0700 (Mon, 09 Jun 2008) Log Message: ----------- more camera improvements for zooming and moving when pitch is set Modified Paths: -------------- code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc 2008-06-09 18:36:48 UTC (rev 6492) +++ code/stage/trunk/libstage/camera.cc 2008-06-09 21:49:25 UTC (rev 6493) @@ -36,3 +36,43 @@ glMatrixMode (GL_MODELVIEW); } + +//TODO re-evaluate the way the camera is shifted when the mouse zooms - it might be possible to simplify +void StgCamera::scale( float scale, float shift_x, float w, float shift_y, float h ) +{ + float to_scale = -scale; + const float old_scale = _scale; + + //TODO setting up the factor can use some work + float factor = 1.0 + fabs( to_scale ) / 25; + if( factor < 1.1 ) + factor = 1.1; //this must be greater than 1. + else if( factor > 2.5 ) + factor = 2.5; + + //convert the shift distance to the range [-0.5, 0.5] + shift_x = shift_x / w - 0.5; + shift_y = shift_y / h - 0.5; + + //adjust the shift values based on the factor (this represents how much the positions grows/shrinks) + shift_x *= factor - 1.0; + shift_y *= factor - 1.0; + + if( to_scale > 0 ) { + //zoom in + _scale *= factor; + move( shift_x * w / _scale * _scale, + - shift_y * h / _scale * _scale ); + } + else { + //zoom out + _scale /= factor; + if( _scale < 1 ) { + _scale = 1; + } else { + //shift camera to follow where mouse zoomed out + move( - shift_x * w / old_scale * _scale, + shift_y * h / old_scale * _scale ); + } + } +} \ No newline at end of file Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-09 18:36:48 UTC (rev 6492) +++ code/stage/trunk/libstage/canvas.cc 2008-06-09 21:49:25 UTC (rev 6493) @@ -169,7 +169,7 @@ } else { - camera.scale( Fl::event_dy(), Fl::event_x() - w() / 2, - ( Fl::event_y() - h() / 2 ) ); + camera.scale( Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h() ); invalidate(); } return 1; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-09 18:36:48 UTC (rev 6492) +++ code/stage/trunk/libstage/stage.hh 2008-06-09 21:49:25 UTC (rev 6493) @@ -1711,19 +1711,26 @@ void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const; - inline void move( float x, float y ) { + inline void move( float x, float y ) { + //convert screen points into world points x = x / ( _scale ); y = y / ( _scale ); + //adjust for pitch angle + y = y / cos( dtor( _pitch ) ); + + //adjust for yaw andle _x += cos( dtor( _yaw ) ) * x; _y += -sin( dtor( _yaw ) ) * x; _x += sin( dtor( _yaw ) ) * y; _y += cos( dtor( _yaw ) ) * y; } + inline void yaw( float yaw ) { _yaw += yaw; } + inline void pitch( float pitch ) { _pitch += pitch; if( _pitch < -90 ) @@ -1731,6 +1738,7 @@ else if( _pitch > 0 ) _pitch = 0; } + inline float getYaw( void ) const { return _yaw; } inline float getPitch( void ) const { return _pitch; } @@ -1741,12 +1749,7 @@ - inline void scale( float scale, float center_x = 0, float center_y = 0 ) { - //TODO figure out how to zoom in centered on the given points (rather than just center of screen) - _scale -= 0.25 * scale * sqrt( _scale ); - if( _scale < 1 ) _scale = 1; - } - + void scale( float scale, float shift_x = 0, float h = 0, float shift_y = 0, float w = 0 ); inline void resetAngle( void ) { _pitch = _yaw = 0; } inline float getScale() const { return _scale; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-06-09 15:29:28
|
Revision: 6495 http://playerstage.svn.sourceforge.net/playerstage/?rev=6495&view=rev Author: jeremy_asher Date: 2008-06-09 15:29:28 -0700 (Mon, 09 Jun 2008) Log Message: ----------- Implemented save on quit, blocked escape closing window Modified Paths: -------------- code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-09 22:21:47 UTC (rev 6494) +++ code/stage/trunk/libstage/stage.hh 2008-06-09 22:29:28 UTC (rev 6495) @@ -1847,7 +1847,12 @@ // static callback functions static void SaveCallback( Fl_Widget* wid, StgWorldGui* world ); static void SaveAsCallback( Fl_Widget* wid, StgWorldGui* world ); + static void QuitCallback( Fl_Widget* wid, StgWorldGui* world ); + static void WindowCallback( Fl_Widget* wid, StgWorldGui* world ); + bool SaveAsDialog(); + bool CloseWindowQuery(); + virtual void PushColor( stg_color_t col ) { canvas->PushColor( col ); } Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-09 22:21:47 UTC (rev 6494) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-09 22:29:28 UTC (rev 6495) @@ -190,7 +190,7 @@ mbar->add( "File", 0, 0, 0, FL_SUBMENU ); mbar->add( "File/Save File", FL_CTRL + 's', (Fl_Callback *)SaveCallback, this ); mbar->add( "File/Save File &As...", FL_CTRL + FL_SHIFT + 's', (Fl_Callback *)SaveAsCallback, this, FL_MENU_DIVIDER ); - mbar->add( "File/Exit", FL_CTRL+'q', (Fl_Callback *)dummy_cb, 0 ); + mbar->add( "File/Exit", FL_CTRL+'q', (Fl_Callback *)QuitCallback, this ); mbar->add( "View", 0, 0, 0, FL_SUBMENU ); mbar->add( MITEM_VIEW_DATA, 'd', (Fl_Callback*)view_toggle_cb, (void*)canvas, @@ -221,6 +221,8 @@ mbar->add( "Help", 0, 0, 0, FL_SUBMENU ); mbar->add( "Help/About Stage...", NULL, (Fl_Callback *)About_cb ); //mbar->add( "Help/HTML Documentation", FL_CTRL + 'g', (Fl_Callback *)dummy_cb ); + + callback( (Fl_Callback*)WindowCallback, this ); show(); } @@ -307,12 +309,16 @@ void StgWorldGui::SaveAsCallback( Fl_Widget* wid, StgWorldGui* world ) { - const char* curFilename = world->wf->filename; + world->SaveAsDialog(); +} + +bool StgWorldGui::SaveAsDialog() +{ const char* newFilename; - bool success; + bool success = false; const char* pattern = "World Files (*.world)"; - Fl_File_Chooser fc( curFilename, pattern, Fl_File_Chooser::CREATE, "Save File As..." ); + Fl_File_Chooser fc( wf->filename, pattern, Fl_File_Chooser::CREATE, "Save File As..." ); fc.ok_label( "Save" ); fc.show(); @@ -323,15 +329,63 @@ if (newFilename != NULL) { // todo: make sure file ends in .world - success = world->Save( newFilename ); + success = Save( newFilename ); if ( !success ) { fl_alert( "Error saving world file." ); } } + + return success; +} +void StgWorldGui::QuitCallback( Fl_Widget* wid, StgWorldGui* world ) +{ + bool done = world->CloseWindowQuery(); + if (done) { + exit(0); + } +} + +bool StgWorldGui::CloseWindowQuery() +{ + int choice; + choice = fl_choice("Do you want to save?", + "Cancel", // ->0: defaults to ESC + "Yes", // ->1 + "No" // ->2 + ); + + switch (choice) { + case 1: // Yes + bool saved = SaveAsDialog(); + if ( saved ) { + return 1; + } + else { + return 0; + } + case 2: // No + return 1; + } + return 0; } +void StgWorldGui::WindowCallback( Fl_Widget* wid, StgWorldGui* world ) +{ + switch ( Fl::event() ) { + case FL_SHORTCUT: + if ( Fl::event_key() == FL_Escape ) + return; + case FL_CLOSE: // clicked close button + bool done = world->CloseWindowQuery(); + if ( !done ) + return; + } + + exit(0); +} + bool StgWorldGui::Save( const char* filename ) { PRINT_DEBUG1( "%s.Save()", token ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-06-09 17:09:15
|
Revision: 6497 http://playerstage.svn.sourceforge.net/playerstage/?rev=6497&view=rev Author: alexcb Date: 2008-06-09 17:09:22 -0700 (Mon, 09 Jun 2008) Log Message: ----------- created camera model class Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/typetable.cc Added Paths: ----------- code/stage/trunk/libstage/model_camera.cc Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2008-06-09 22:32:57 UTC (rev 6496) +++ code/stage/trunk/libstage/CMakeLists.txt 2008-06-10 00:09:22 UTC (rev 6497) @@ -10,6 +10,7 @@ model_blinkenlight.cc model_blobfinder.cc model_callbacks.cc + model_camera.cc model_fiducial.cc model_laser.cc model_load.cc Modified: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc 2008-06-09 22:32:57 UTC (rev 6496) +++ code/stage/trunk/libstage/camera.cc 2008-06-10 00:09:22 UTC (rev 6497) @@ -12,11 +12,56 @@ #include <iostream> -void StgCamera::Draw( void ) const +//perspective camera +//perspective camera +void StgPerspectiveCamera::Draw( void ) const { glMatrixMode (GL_MODELVIEW); glLoadIdentity (); + glRotatef( - _pitch, 1.0, 0.0, 0.0 ); + glRotatef( - _yaw, 0.0, 0.0, 1.0 ); + + std::cout << "y: " << _z << std::endl; + glTranslatef( - _x, - _y, - _z ); + //zooming needs to happen in the Projection code (don't use glScale for zoom) + +} + +void StgPerspectiveCamera::SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const +{ + glMatrixMode (GL_PROJECTION); + glLoadIdentity (); + + gluPerspective( 60.0, pixels_width/pixels_height, 0.01, 100 ); + + glMatrixMode (GL_MODELVIEW); +} + +void StgPerspectiveCamera::update( void ) +{ +// _x = 0; +// _y = 0; +// _z = i; + _pitch = 90.0; +// _yaw = 90.0; +} + + + + + + + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//Ortho camera +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void StgOrthoCamera::Draw( void ) const +{ + glMatrixMode (GL_MODELVIEW); + glLoadIdentity (); + glRotatef( _pitch, 1.0, 0.0, 0.0 ); glRotatef( _yaw, 0.0, 0.0, 1.0 ); @@ -25,7 +70,7 @@ } -void StgCamera::SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const +void StgOrthoCamera::SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const { glMatrixMode (GL_PROJECTION); glLoadIdentity (); @@ -38,7 +83,7 @@ } //TODO re-evaluate the way the camera is shifted when the mouse zooms - it might be possible to simplify -void StgCamera::scale( float scale, float shift_x, float w, float shift_y, float h ) +void StgOrthoCamera::scale( float scale, float shift_x, float w, float shift_y, float h ) { float to_scale = -scale; const float old_scale = _scale; Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-09 22:32:57 UTC (rev 6496) +++ code/stage/trunk/libstage/canvas.cc 2008-06-10 00:09:22 UTC (rev 6497) @@ -335,7 +335,180 @@ PopColor(); } -void StgCanvas::draw() +void StgCanvas::renderFrame() +{ + + if( ! (showflags & STG_SHOW_TRAILS) ) + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + + // if following selected, shift the view to above the selected robot + if( (showflags & STG_SHOW_FOLLOW) && last_selection ) + { + glLoadIdentity (); + double zclip = 20 * camera.getScale(); //hypot(world->Width(), world->Height()) * camera.getScale(); + glTranslatef( 0,0, + -zclip / 2.0 ); + + // meter scale + glScalef ( camera.getScale(), camera.getScale(), camera.getScale() ); // zoom + + stg_pose_t gpose = last_selection->GetGlobalPose(); + + // and put it in the center of the window + //glRotatef( -rtod(gpose.a), 0,0,1 ); + glTranslatef( -gpose.x, -gpose.y, 0 ); + } + + glPushMatrix(); + + // draw the world size rectangle in white, using the polygon offset + // so it doesn't z-fight with the models + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + glEnable(GL_POLYGON_OFFSET_FILL); + glPolygonOffset(2.0, 2.0); + + glScalef( 1.0/world->Resolution(), 1.0/world->Resolution(), 0 ); + ((StgWorldGui*)world)->DrawFloor(); + + glDisable(GL_POLYGON_OFFSET_FILL); + + if( (showflags & STG_SHOW_QUADTREE) || (showflags & STG_SHOW_OCCUPANCY) ) + { + glDisable( GL_LINE_SMOOTH ); + glLineWidth( 1 ); + glPolygonMode( GL_FRONT, GL_LINE ); + colorstack.Push(1,0,0); + + if( showflags & STG_SHOW_OCCUPANCY ) + ((StgWorldGui*)world)->DrawTree( false ); + + if( showflags & STG_SHOW_QUADTREE ) + ((StgWorldGui*)world)->DrawTree( true ); + + colorstack.Pop(); + + glEnable( GL_LINE_SMOOTH ); + } + + glPopMatrix(); + + if( showflags & STG_SHOW_GRID ) + DrawGlobalGrid(); + + for( GList* it=selected_models; it; it=it->next ) + ((StgModel*)it->data)->DrawSelected(); + + // draw the models + if( showflags ) // if any bits are set there's something to draw + { + if( showflags & STG_SHOW_FOOTPRINT ) + { + glDisable( GL_DEPTH_TEST ); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); + + for( GList* it=world->children; it; it=it->next ) + { + ((StgModel*)it->data)->DrawTrailFootprint(); + } + glEnable( GL_DEPTH_TEST ); + } + + if( showflags & STG_SHOW_TRAILRISE ) + { + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); + + for( GList* it=world->children; it; it=it->next ) + { + ((StgModel*)it->data)->DrawTrailBlocks(); + } + } + + if( showflags & STG_SHOW_ARROWS ) + { + glEnable( GL_DEPTH_TEST ); + for( GList* it=world->children; it; it=it->next ) + { + ((StgModel*)it->data)->DrawTrailArrows(); + } + } + + if( showflags & STG_SHOW_BLOCKS ) + { + for( GList* it=world->children; it; it=it->next ) + { + StgModel* mod = ((StgModel*)it->data); + + if( mod->displaylist == 0 ) + { + mod->displaylist = glGenLists(1); + mod->BuildDisplayList( showflags ); // ready to be rendered + } + + // move into this model's local coordinate frame + glPushMatrix(); + gl_pose_shift( &mod->pose ); + gl_pose_shift( &mod->geom.pose ); + + // render the pre-recorded graphics for this model and + // its children + glCallList( mod->displaylist ); + + glPopMatrix(); + } + } + + //mod->Draw( showflags ); // draw the stuff that changes every update + // draw everything else + for( GList* it=world->children; it; it=it->next ) + ((StgModel*)it->data)->Draw( showflags, this ); + } + + if( world->GetRayList() ) + { + glDisable( GL_DEPTH_TEST ); + PushColor( 0,0,0,0.5 ); + for( GList* it = world->GetRayList(); it; it=it->next ) + { + float* pts = (float*)it->data; + glBegin( GL_LINES ); + glVertex2f( pts[0], pts[1] ); + glVertex2f( pts[2], pts[3] ); + glEnd(); + } + PopColor(); + glEnable( GL_DEPTH_TEST ); + + world->ClearRays(); + } + + if( showflags & STG_SHOW_CLOCK ) + { + glPushMatrix(); + glLoadIdentity(); + glDisable( GL_DEPTH_TEST ); + + // if trails are on, we need to clear the clock background + + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + colorstack.Push( 0.8,0.8,1.0 ); // pale blue + glRectf( -w()/2, -h()/2, -w()/2 +120, -h()/2+20 ); + colorstack.Pop(); + + char clockstr[50]; + world->ClockString( clockstr, 50 ); + + colorstack.Push( 0,0,0 ); // black + gl_draw_string( -w()/2+4, -h()/2+4, 5, clockstr ); + colorstack.Pop(); + + glEnable( GL_DEPTH_TEST ); + glPopMatrix(); + } + +} + +void StgCanvas::draw() { static bool loaded_texture = false; // static int centerx = 0, centery = 0; @@ -388,176 +561,7 @@ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); } - - - - if( ! (showflags & STG_SHOW_TRAILS) ) - glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - - // if following selected, shift the view to above the selected robot - if( (showflags & STG_SHOW_FOLLOW) && last_selection ) - { - glLoadIdentity (); - double zclip = 20 * camera.getScale(); //hypot(world->Width(), world->Height()) * camera.getScale(); - glTranslatef( 0,0, - -zclip / 2.0 ); - - // meter scale - glScalef ( camera.getScale(), camera.getScale(), camera.getScale() ); // zoom - - stg_pose_t gpose = last_selection->GetGlobalPose(); - - // and put it in the center of the window - //glRotatef( -rtod(gpose.a), 0,0,1 ); - glTranslatef( -gpose.x, -gpose.y, 0 ); - } - - glPushMatrix(); - - // draw the world size rectangle in white, using the polygon offset - // so it doesn't z-fight with the models - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); - glEnable(GL_POLYGON_OFFSET_FILL); - glPolygonOffset(2.0, 2.0); - - glScalef( 1.0/world->Resolution(), 1.0/world->Resolution(), 0 ); - ((StgWorldGui*)world)->DrawFloor(); - - glDisable(GL_POLYGON_OFFSET_FILL); - - if( (showflags & STG_SHOW_QUADTREE) || (showflags & STG_SHOW_OCCUPANCY) ) - { - glDisable( GL_LINE_SMOOTH ); - glLineWidth( 1 ); - glPolygonMode( GL_FRONT, GL_LINE ); - colorstack.Push(1,0,0); - - if( showflags & STG_SHOW_OCCUPANCY ) - ((StgWorldGui*)world)->DrawTree( false ); - - if( showflags & STG_SHOW_QUADTREE ) - ((StgWorldGui*)world)->DrawTree( true ); - - colorstack.Pop(); - - glEnable( GL_LINE_SMOOTH ); - } - - glPopMatrix(); - - if( showflags & STG_SHOW_GRID ) - DrawGlobalGrid(); - - for( GList* it=selected_models; it; it=it->next ) - ((StgModel*)it->data)->DrawSelected(); - - // draw the models - if( showflags ) // if any bits are set there's something to draw - { - if( showflags & STG_SHOW_FOOTPRINT ) - { - glDisable( GL_DEPTH_TEST ); - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); - - for( GList* it=world->children; it; it=it->next ) - { - ((StgModel*)it->data)->DrawTrailFootprint(); - } - glEnable( GL_DEPTH_TEST ); - } - - if( showflags & STG_SHOW_TRAILRISE ) - { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); - - for( GList* it=world->children; it; it=it->next ) - { - ((StgModel*)it->data)->DrawTrailBlocks(); - } - } - - if( showflags & STG_SHOW_ARROWS ) - { - glEnable( GL_DEPTH_TEST ); - for( GList* it=world->children; it; it=it->next ) - { - ((StgModel*)it->data)->DrawTrailArrows(); - } - } - - if( showflags & STG_SHOW_BLOCKS ) - { - for( GList* it=world->children; it; it=it->next ) - { - StgModel* mod = ((StgModel*)it->data); - - if( mod->displaylist == 0 ) - { - mod->displaylist = glGenLists(1); - mod->BuildDisplayList( showflags ); // ready to be rendered - } - - // move into this model's local coordinate frame - glPushMatrix(); - gl_pose_shift( &mod->pose ); - gl_pose_shift( &mod->geom.pose ); - - // render the pre-recorded graphics for this model and - // its children - glCallList( mod->displaylist ); - - glPopMatrix(); - } - } - - //mod->Draw( showflags ); // draw the stuff that changes every update - // draw everything else - for( GList* it=world->children; it; it=it->next ) - ((StgModel*)it->data)->Draw( showflags, this ); - } - - if( world->GetRayList() ) - { - glDisable( GL_DEPTH_TEST ); - PushColor( 0,0,0,0.5 ); - for( GList* it = world->GetRayList(); it; it=it->next ) - { - float* pts = (float*)it->data; - glBegin( GL_LINES ); - glVertex2f( pts[0], pts[1] ); - glVertex2f( pts[2], pts[3] ); - glEnd(); - } - PopColor(); - glEnable( GL_DEPTH_TEST ); - - world->ClearRays(); - } - - if( showflags & STG_SHOW_CLOCK ) - { - glPushMatrix(); - glLoadIdentity(); - glDisable( GL_DEPTH_TEST ); - - // if trails are on, we need to clear the clock background - - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - - colorstack.Push( 0.8,0.8,1.0 ); // pale blue - glRectf( -w()/2, -h()/2, -w()/2 +120, -h()/2+20 ); - colorstack.Pop(); - - char clockstr[50]; - world->ClockString( clockstr, 50 ); - - colorstack.Push( 0,0,0 ); // black - gl_draw_string( -w()/2+4, -h()/2+4, 5, clockstr ); - colorstack.Pop(); - - glEnable( GL_DEPTH_TEST ); - glPopMatrix(); - } + renderFrame(); } void StgCanvas::resize(int X,int Y,int W,int H) Added: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc (rev 0) +++ code/stage/trunk/libstage/model_camera.cc 2008-06-10 00:09:22 UTC (rev 6497) @@ -0,0 +1,70 @@ +/////////////////////////////////////////////////////////////////////////// +// +// File: model_camera.cc +// Author: Alex Couture-Beil +// Date: 09 June 2008 +// +// CVS info: +// +/////////////////////////////////////////////////////////////////////////// + + +//#define DEBUG 1 +#include "stage_internal.hh" + +StgModelCamera::StgModelCamera( StgWorld* world, + StgModel* parent, + stg_id_t id, + char* typestr ) +: StgModel( world, parent, id, typestr ) +{ + PRINT_DEBUG2( "Constructing StgModelCamera %d (%s)\n", + id, typestr ); + + // Set up sensible defaults + + this->SetColor( stg_lookup_color( "green" ) ); + + + stg_geom_t geom; + memset( &geom, 0, sizeof(geom)); // no size + geom.size.x = 0.02; + geom.size.y = 0.02; + geom.size.z = 0.02; + this->SetGeom( geom ); + + this->Startup(); +} + +StgModelCamera::~StgModelCamera() +{ +} + +void StgModelCamera::Load( void ) +{ + StgModel::Load(); + Worldfile* wf = world->GetWorldFile(); + +} + + +void StgModelCamera::Update( void ) +{ + StgModel::Update(); + static StgPerspectiveCamera camera; + camera.update(); + camera.SetProjection( 400, 300, 0.01, 200.0 ); + camera.setPose( parent->GetGlobalPose().x, parent->GetGlobalPose().y, 0.1 ); + camera.setYaw( rtod( parent->GetGlobalPose().a ) - 90.0 ); + //camera.setPose( 0.1, 0.1, 0.1 ); + camera.Draw(); + std::cout << "updated" << std::endl; + std::cout << parent->GetGlobalPose().x << std::endl; +} + + +void StgModelCamera::Draw( uint32_t flags, StgCanvas* canvas ) +{ + StgModel::Draw( flags, canvas ); +} + Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-09 22:32:57 UTC (rev 6496) +++ code/stage/trunk/libstage/stage.hh 2008-06-10 00:09:22 UTC (rev 6497) @@ -1699,16 +1699,44 @@ class StgCamera { +public: + StgCamera() { } + virtual ~StgCamera() { } + + virtual void Draw() const = 0; + virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0; +}; + +class StgPerspectiveCamera : public StgCamera +{ private: float _x, _y, _z; float _pitch; //left-right (about y) float _yaw; //up-down (about x) + +public: + StgPerspectiveCamera( void ) : _x( 0 ), _y( 0 ), _z( 0 ), _pitch( 0 ), _yaw( 0 ) { } + + virtual void Draw() const; + virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const; + void update( void ); + + inline void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; } + inline void setYaw( float yaw ) { _yaw = yaw; } +}; + +class StgOrthoCamera : public StgCamera +{ +private: + float _x, _y, _z; + float _pitch; //left-right (about y) + float _yaw; //up-down (about x) float _scale; public: - StgCamera( void ) : _x( 0 ), _y( 0 ), _z( 0 ), _pitch( 0 ), _yaw( 0 ), _scale( 15 ) { } - void Draw() const; - void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const; + StgOrthoCamera( void ) : _x( 0 ), _y( 0 ), _z( 0 ), _pitch( 0 ), _yaw( 0 ), _scale( 15 ) { } + virtual void Draw() const; + virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const; inline void move( float x, float y ) { @@ -1765,7 +1793,7 @@ private: GlColorStack colorstack; - StgCamera camera; + StgOrthoCamera camera; int startx, starty; bool dragging; @@ -1791,6 +1819,7 @@ StgWorld* world; void FixViewport(int W,int H); + virtual void renderFrame(); virtual void draw(); virtual int handle( int event ); void resize(int X,int Y,int W,int H); @@ -2307,6 +2336,33 @@ } }; + // CAMERA MODEL ---------------------------------------------------- + class StgModelCamera : public StgModel + { + public: + + StgModelCamera( StgWorld* world, + StgModel* parent, + stg_id_t id, + char* typestr ); + + ~StgModelCamera(); + + virtual void Load(); + virtual void Update(); + virtual void Draw( uint32_t flags, StgCanvas* canvas ); + + // static wrapper for the constructor - all models must implement + // this method and add an entry in typetable.cc + static StgModel* Create( StgWorld* world, + StgModel* parent, + stg_id_t id, + char* typestr ) + { + return (StgModel*)new StgModelCamera( world, parent, id, typestr ); + } + }; + // POSITION MODEL -------------------------------------------------------- /** Define a position control method */ Modified: code/stage/trunk/libstage/typetable.cc =================================================================== --- code/stage/trunk/libstage/typetable.cc 2008-06-09 22:32:57 UTC (rev 6496) +++ code/stage/trunk/libstage/typetable.cc 2008-06-10 00:09:22 UTC (rev 6497) @@ -12,6 +12,7 @@ { "fiducial", StgModelFiducial::Create }, { "blobfinder", StgModelBlobfinder::Create }, { "blinkenlight", StgModelBlinkenlight::Create }, + { "camera", StgModelCamera::Create }, { NULL, NULL } // this must be the last entry }; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-06-10 10:43:37
|
Revision: 6521 http://playerstage.svn.sourceforge.net/playerstage/?rev=6521&view=rev Author: alexcb Date: 2008-06-10 10:43:46 -0700 (Tue, 10 Jun 2008) Log Message: ----------- removed trailing whitespace on end of lines Modified Paths: -------------- code/stage/trunk/libstage/ancestor.cc code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgrid.cc code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/ctrl.cc code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/glcolorstack.cc code/stage/trunk/libstage/main.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_props.cc code/stage/trunk/libstage/model_ranger.cc code/stage/trunk/libstage/model_wifi.cc code/stage/trunk/libstage/resource.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stest.cc code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldfile.cc code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/ancestor.cc =================================================================== --- code/stage/trunk/libstage/ancestor.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/ancestor.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -51,12 +51,12 @@ IncrementNumChildrenOfType( mod->typestr ); // store as a child - children = g_list_append( children, mod ); + children = g_list_append( children, mod ); } void StgAncestor::RemoveChild( StgModel* mod ) { - puts( "TODO: StgWorld::RemoveChild()" ); + puts( "TODO: StgWorld::RemoveChild()" ); } stg_pose_t StgAncestor::GetGlobalPose() Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/block.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -63,7 +63,7 @@ glPushMatrix(); glTranslatef( 0,0,zmax); glVertexPointer( 2, GL_DOUBLE, 0, pts ); - glDrawArrays( GL_POLYGON, 0, pt_count ); + glDrawArrays( GL_POLYGON, 0, pt_count ); glPopMatrix(); } @@ -108,14 +108,14 @@ // // draw the block outline in a darker version of the same color double r,g,b,a; - stg_color_unpack( color, &r, &g, &b, &a ); + stg_color_unpack( color, &r, &g, &b, &a ); PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a )); glPolygonMode(GL_FRONT_AND_BACK, GL_LINE ); - glDepthMask(GL_FALSE); + glDepthMask(GL_FALSE); DrawTop(); - DrawSides(); - glDepthMask(GL_TRUE); + DrawSides(); + glDepthMask(GL_TRUE); PopColor(); PopColor(); @@ -184,7 +184,7 @@ stg_list_entry_t* pt = &g_array_index( rendered_points, stg_list_entry_t, p); - *pt->head = g_slist_delete_link( *pt->head, pt->link ); + *pt->head = g_slist_delete_link( *pt->head, pt->link ); // decrement the region and superregion render counts (*pt->counter1)--; @@ -206,7 +206,7 @@ el.link = link; el.counter1 = c1; el.counter2 = c2; - g_array_append_val( rendered_points, el ); + g_array_append_val( rendered_points, el ); } @@ -217,7 +217,7 @@ return; // assuming the blocks currently fit in a square +/- one billion units - double minx, miny, maxx, maxy; + double minx, miny, maxx, maxy; minx = miny = billion; maxx = maxy = -billion; Modified: code/stage/trunk/libstage/blockgrid.cc =================================================================== --- code/stage/trunk/libstage/blockgrid.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/blockgrid.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -35,7 +35,7 @@ if( x < width && y < height ) { - cells[ x+y*width ] = g_slist_prepend( cells[ x+y*width ], block ); + cells[ x+y*width ] = g_slist_prepend( cells[ x+y*width ], block ); block->RecordRenderPoint( x, y ); } } @@ -46,7 +46,7 @@ if( x < width && y < height ) { - cells[ x+y*width ] = g_slist_remove( cells[ x+y*width ], block ); + cells[ x+y*width ] = g_slist_remove( cells[ x+y*width ], block ); } } @@ -62,7 +62,7 @@ { for( uint32_t x=0; x<width; x++ ) for( uint32_t y=0; y<height; y++ ) - RemoveBlock(x,y,block ); + RemoveBlock(x,y,block ); } void StgBlockGrid::Draw( bool drawall ) Modified: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/camera.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -120,4 +120,4 @@ shift_y * h / old_scale * _scale ); } } -} \ No newline at end of file +} Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/canvas.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -79,12 +79,12 @@ // id + a 100% alpha value for( GList* it=world->children; it; it=it->next ) { - StgModel* mod = (StgModel*)it->data; + StgModel* mod = (StgModel*)it->data; if( mod->GuiMask() & (STG_MOVE_TRANS | STG_MOVE_ROT )) { uint32_t col = (mod->Id() | 0xFF000000); - glColor4ubv( (GLubyte*)&col ); + glColor4ubv( (GLubyte*)&col ); mod->DrawPicker(); } } @@ -141,7 +141,7 @@ glGetIntegerv(GL_VIEWPORT, viewport); GLdouble modelview[16]; - glGetDoublev(GL_MODELVIEW_MATRIX, modelview); + glGetDoublev(GL_MODELVIEW_MATRIX, modelview); GLdouble projection[16]; glGetDoublev(GL_PROJECTION_MATRIX, projection); @@ -177,7 +177,7 @@ case FL_MOVE: // moused moved while no button was pressed if( Fl::event_state( FL_CTRL ) ) { - int dx = Fl::event_x() - startx; + int dx = Fl::event_x() - startx; int dy = Fl::event_y() - starty; camera.pitch( 0.5 * static_cast<double>( dy ) ); @@ -187,7 +187,7 @@ } else if( Fl::event_state( FL_ALT ) ) { - int dx = Fl::event_x() - startx; + int dx = Fl::event_x() - startx; int dy = Fl::event_y() - starty; camera.move( -dx, dy ); @@ -195,7 +195,7 @@ } startx = Fl::event_x(); - starty = Fl::event_y(); + starty = Fl::event_y(); return 1; case FL_PUSH: // button pressed @@ -225,7 +225,7 @@ case FL_DRAG: // mouse moved while button was pressed { - int dx = Fl::event_x() - startx; + int dx = Fl::event_x() - startx; int dy = Fl::event_y() - starty; switch( Fl::event_button() ) @@ -356,7 +356,7 @@ // and put it in the center of the window //glRotatef( -rtod(gpose.a), 0,0,1 ); - glTranslatef( -gpose.x, -gpose.y, 0 ); + glTranslatef( -gpose.x, -gpose.y, 0 ); } glPushMatrix(); @@ -393,7 +393,7 @@ glPopMatrix(); if( showflags & STG_SHOW_GRID ) - DrawGlobalGrid(); + DrawGlobalGrid(); for( GList* it=selected_models; it; it=it->next ) ((StgModel*)it->data)->DrawSelected(); @@ -492,14 +492,14 @@ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); colorstack.Push( 0.8,0.8,1.0 ); // pale blue - glRectf( -w()/2, -h()/2, -w()/2 +120, -h()/2+20 ); + glRectf( -w()/2, -h()/2, -w()/2 +120, -h()/2+20 ); colorstack.Pop(); char clockstr[50]; - world->ClockString( clockstr, 50 ); + world->ClockString( clockstr, 50 ); colorstack.Push( 0,0,0 ); // black - gl_draw_string( -w()/2+4, -h()/2+4, 5, clockstr ); + gl_draw_string( -w()/2+4, -h()/2+4, 5, clockstr ); colorstack.Pop(); glEnable( GL_DEPTH_TEST ); @@ -516,8 +516,8 @@ if (!valid()) { - valid(1); - FixViewport(w(), h()); + valid(1); + FixViewport(w(), h()); // set gl state that won't change every redraw glClearColor ( 0.7, 0.7, 0.8, 1.0); @@ -526,7 +526,7 @@ glEnable (GL_DEPTH_TEST); glDepthFunc (GL_LESS); glCullFace( GL_BACK ); - glEnable (GL_CULL_FACE); + glEnable (GL_CULL_FACE); glEnable( GL_BLEND ); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA ); glEnable( GL_LINE_SMOOTH ); Modified: code/stage/trunk/libstage/ctrl.cc =================================================================== --- code/stage/trunk/libstage/ctrl.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/ctrl.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -1,10 +1,10 @@ #include "stage.hh" using namespace Stg; -const double cruisespeed = 0.4; -const double avoidspeed = 0.05; +const double cruisespeed = 0.4; +const double avoidspeed = 0.05; const double avoidturn = 1.0; -const double minfrontdistance = 0.8; +const double minfrontdistance = 0.8; const bool verbose = false; typedef struct @@ -43,7 +43,7 @@ stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count ); assert(scan); - double newturnrate=0.0, newspeed=0.0; + double newturnrate=0.0, newspeed=0.0; bool obstruction = false; // find the closest distance to the left and right and check if Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/gl.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -33,7 +33,7 @@ void Stg::gl_draw_octagon( float w, float h, float m ) { glBegin(GL_POLYGON); - glVertex2f( m+w, 0 ); + glVertex2f( m+w, 0 ); glVertex2f( w+2*m, m ); glVertex2f( w+2*m, h+m ); glVertex2f( m+w, h+2*m ); Modified: code/stage/trunk/libstage/glcolorstack.cc =================================================================== --- code/stage/trunk/libstage/glcolorstack.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/glcolorstack.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -12,7 +12,7 @@ void GlColorStack::Push( GLdouble col[4] ) { - size_t sz = 4 * sizeof(col[0]); + size_t sz = 4 * sizeof(col[0]); GLdouble *keep = (GLdouble*)malloc( sz ); memcpy( keep, col, sz ); @@ -68,7 +68,7 @@ else { GLdouble *col = (GLdouble*)g_queue_pop_head( colorstack ); - glColor4dv( col ); + glColor4dv( col ); free( col ); } } Modified: code/stage/trunk/libstage/main.cc =================================================================== --- code/stage/trunk/libstage/main.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/main.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -30,8 +30,8 @@ printf( "option %s given", longopts[optindex].name ); break; case 'g': - usegui = false; - printf( "[GUI disabled]" ); + usegui = false; + printf( "[GUI disabled]" ); break; case '?': break; @@ -53,7 +53,7 @@ { if( optindex > 0 ) { - const char* worldfilename = argv[optindex]; + const char* worldfilename = argv[optindex]; StgWorldGui* world = new StgWorldGui( 400, 300, worldfilename ); world->Load( worldfilename ); world->Start(); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/model.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -117,7 +117,7 @@ const stg_watts_t STG_DEFAULT_MOD_ENERGY_GIVERATE = 0.0; const stg_meters_t STG_DEFAULT_MOD_ENERGY_PROBERANGE = 0.0; const stg_watts_t STG_DEFAULT_MOD_ENERGY_TRICKLERATE = 0.1; -const stg_meters_t STG_DEFAULT_MOD_GEOM_SIZEX = 0.10; +const stg_meters_t STG_DEFAULT_MOD_GEOM_SIZEX = 0.10; const stg_meters_t STG_DEFAULT_MOD_GEOM_SIZEY = 0.10; const stg_meters_t STG_DEFAULT_MOD_GEOM_SIZEZ = 0.10; const bool STG_DEFAULT_MOD_GRID = false; @@ -125,7 +125,7 @@ const stg_laser_return_t STG_DEFAULT_MOD_LASERRETURN = LaserVisible; const stg_meters_t STG_DEFAULT_MOD_MAP_RESOLUTION = 0.1; const stg_movemask_t STG_DEFAULT_MOD_MASK = (STG_MOVE_TRANS | STG_MOVE_ROT); -const stg_kg_t STG_DEFAULT_MOD_MASS = 10.0; +const stg_kg_t STG_DEFAULT_MOD_MASS = 10.0; const bool STG_DEFAULT_MOD_NOSE = false; const bool STG_DEFAULT_MOD_OBSTACLERETURN = true; const bool STG_DEFAULT_MOD_OUTLINE = true; @@ -152,7 +152,7 @@ this->id = id; this->typestr = typestr; - this->parent = parent; + this->parent = parent; this->world = world; this->debug = false; @@ -166,13 +166,13 @@ char* buf = new char[TOKEN_MAX]; snprintf( buf, TOKEN_MAX, "%s.%s:%d", - anc->Token(), typestr, cnt ); + anc->Token(), typestr, cnt ); this->token = strdup( buf ); delete buf; PRINT_DEBUG2( "model has token \"%s\" and typestr \"%s\"", - this->token, this->typestr ); + this->token, this->typestr ); anc->AddChild( this ); world->AddModel( this ); @@ -250,7 +250,7 @@ if( callbacks ) g_hash_table_destroy( callbacks ); - world->RemoveModel( this ); + world->RemoveModel( this ); } // this should be called after all models have loaded from the @@ -325,7 +325,7 @@ blocks = g_list_prepend( blocks, new StgBlock( this, pts, pt_count, zmin, zmax, - col, inherit_color )); + col, inherit_color )); // force recreation of display lists before drawing body_dirty = true; @@ -533,8 +533,8 @@ double sina = sin( gpose.a ); stg_velocity_t lv; - lv.x = gv.x * cosa + gv.y * sina; - lv.y = -gv.x * sina + gv.y * cosa; + lv.x = gv.x * cosa + gv.y * sina; + lv.y = -gv.x * sina + gv.y * cosa; lv.a = gv.a; this->SetVelocity( lv ); @@ -547,7 +547,7 @@ if( this->gpose_dirty ) { - stg_pose_t parent_pose; + stg_pose_t parent_pose; // find my parent's pose if( this->parent ) @@ -609,7 +609,7 @@ // recursive call for all the model's children for( GList* it=children; it; it=it->next ) - ((StgModel*)it->data)->MapWithChildren(); + ((StgModel*)it->data)->MapWithChildren(); } @@ -619,7 +619,7 @@ // recursive call for all the model's children for( GList* it=children; it; it=it->next ) - ((StgModel*)it->data)->UnMapWithChildren(); + ((StgModel*)it->data)->UnMapWithChildren(); } void StgModel::Map() @@ -699,7 +699,7 @@ { stg_pose_t gpose = GetGlobalPose(); - static char txt[256]; + static char txt[256]; snprintf(txt, sizeof(txt), "%s @ [%.2f,%.2f,%.2f,%.2f]", token, gpose.x, gpose.y, gpose.z, gpose.a ); @@ -793,7 +793,7 @@ gl_pose_shift( &checkpoint->pose ); gl_pose_shift( &geom.pose ); - stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); + stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); PushColor( r, g, b, 0.1 ); glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); @@ -824,7 +824,7 @@ pose.z = (world->sim_time - checkpoint->time) * timescale; glPushMatrix(); - gl_pose_shift( &pose ); + gl_pose_shift( &pose ); gl_pose_shift( &geom.pose ); glCallList( displaylist); glPopMatrix(); @@ -870,16 +870,16 @@ glPolygonMode(GL_FRONT_AND_BACK, GL_LINE ); - stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); + stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); PushColor( r/2, g/2, b/2, 1 ); // darker color - glDepthMask(GL_FALSE); + glDepthMask(GL_FALSE); glBegin( GL_TRIANGLES ); glVertex3f( 0, -dy, 0); glVertex3f( dx, 0, 0 ); glVertex3f( 0, +dy, 0 ); glEnd(); - glDepthMask(GL_TRUE); + glDepthMask(GL_TRUE); PopColor(); PopColor(); @@ -990,18 +990,18 @@ // draw inside of bubble PushColor( STG_BUBBLE_FILL ); glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); - glPolygonMode( GL_FRONT, GL_FILL ); + glPolygonMode( GL_FRONT, GL_FILL ); glEnable( GL_POLYGON_OFFSET_FILL ); glPolygonOffset( 1.0, 1.0 ); gl_draw_octagon( w, h, m ); glDisable( GL_POLYGON_OFFSET_FILL ); PopColor(); // draw outline of bubble - PushColor( STG_BUBBLE_BORDER ); + PushColor( STG_BUBBLE_BORDER ); glLineWidth( 1 ); glEnable( GL_LINE_SMOOTH ); glPolygonMode( GL_FRONT, GL_LINE ); - gl_draw_octagon( w, h, m ); + gl_draw_octagon( w, h, m ); glPopAttrib(); PopColor(); @@ -1059,14 +1059,14 @@ // draw the edges darker version of the same color double r,g,b,a; - stg_color_unpack( flag->color, &r, &g, &b, &a ); + stg_color_unpack( flag->color, &r, &g, &b, &a ); PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a )); gluQuadricDrawStyle( quadric, GLU_LINE ); gluSphere( quadric, flag->size/2.0, 4,2 ); - PopColor(); PopColor(); + PopColor(); glTranslatef( 0, 0, flag->size/2.0 ); } @@ -1199,13 +1199,13 @@ this->gpose_dirty = true; // our global pose may have changed for( GList* it = this->children; it; it=it->next ) - ((StgModel*)it->data)->GPoseDirtyTree(); + ((StgModel*)it->data)->GPoseDirtyTree(); } void StgModel::SetPose( stg_pose_t pose ) { //PRINT_DEBUG5( "%s.SetPose(%.2f %.2f %.2f %.2f)", - // this->token, pose->x, pose->y, pose->z, pose->a ); + // this->token, pose->x, pose->y, pose->z, pose->a ); // if the pose has changed, we need to do some work if( memcmp( &this->pose, &pose, sizeof(stg_pose_t) ) != 0 ) @@ -1261,14 +1261,14 @@ body_dirty = true; - Map(); + Map(); CallCallbacks( &this->geom ); } void StgModel::SetColor( stg_color_t col ) { - this->color = col; + this->color = col; body_dirty = true; CallCallbacks( &this->color ); } @@ -1460,7 +1460,7 @@ { // find the local poses of the ends of this block edge stg_point_t* pt1 = &pts[p]; - stg_point_t* pt2 = &pts[(p+1) % pt_count]; + stg_point_t* pt2 = &pts[(p+1) % pt_count]; double dx = pt2->x - pt1->x; double dy = pt2->y - pt1->y; @@ -1509,7 +1509,7 @@ { stg_trail_item_t checkpoint; memcpy( &checkpoint.pose, &pose, sizeof(pose)); - checkpoint.color = color; + checkpoint.color = color; checkpoint.time = world->sim_time; if( trail->len > 100 ) Modified: code/stage/trunk/libstage/model_blinkenlight.cc =================================================================== --- code/stage/trunk/libstage/model_blinkenlight.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/model_blinkenlight.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -120,7 +120,7 @@ // move into this model's local coordinate frame gl_pose_shift( &this->pose ); gl_pose_shift( &this->geom.pose ); - glLineWidth( 3 ); + glLineWidth( 3 ); LISTMETHOD( this->blocks, StgBlock*, Draw ); glLineWidth( 1 ); glPopMatrix(); // drop out of local coords Modified: code/stage/trunk/libstage/model_blobfinder.cc =================================================================== --- code/stage/trunk/libstage/model_blobfinder.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/model_blobfinder.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -98,7 +98,7 @@ blobs = g_array_new( false, true, sizeof(stg_blobfinder_blob_t)); // leave the color filter array empty initally - this tracks all colors - colors = g_array_new( false, true, sizeof(stg_color_t) ); + colors = g_array_new( false, true, sizeof(stg_color_t) ); } @@ -146,7 +146,7 @@ void StgModelBlobfinder::Load( void ) { - StgModel::Load(); + StgModel::Load(); Worldfile* wf = world->GetWorldFile(); @@ -187,7 +187,7 @@ Raytrace( pan, range, fov, blob_match, NULL, samples, scan_width, false ); // now the colors and ranges are filled in - time to do blob detection - double yRadsPerPixel = fov / scan_height; + double yRadsPerPixel = fov / scan_height; g_array_set_size( blobs, 0 ); @@ -252,7 +252,7 @@ // fill in an array entry for this blob stg_blobfinder_blob_t blob; - memset( &blob, 0, sizeof(blob) ); + memset( &blob, 0, sizeof(blob) ); blob.color = blobcol; blob.left = blobleft; blob.top = blobtop; Modified: code/stage/trunk/libstage/model_callbacks.cc =================================================================== --- code/stage/trunk/libstage/model_callbacks.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/model_callbacks.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -86,7 +86,7 @@ //printf( "Model %s has %d callbacks. Checking key %d\n", // mod->token, g_hash_table_size( mod->callbacks ), key ); - GList* cbs = (GList*)g_hash_table_lookup( callbacks, &key ); + GList* cbs = (GList*)g_hash_table_lookup( callbacks, &key ); //printf( "key %d has %d callbacks registered\n", // key, g_list_length( cbs ) ); @@ -121,7 +121,7 @@ for( ; doomed ; doomed = doomed->next ) this->RemoveCallback( address, (stg_model_callback_t)doomed->data ); - g_list_free( doomed ); + g_list_free( doomed ); } } Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/model_fiducial.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -75,7 +75,7 @@ this->ClearBlocks(); - stg_geom_t geom; + stg_geom_t geom; memset( &geom, 0, sizeof(geom)); SetGeom( geom ); @@ -83,7 +83,7 @@ min_range = DEFAULT_FIDUCIAL_RANGEMIN; max_range_anon = DEFAULT_FIDUCIAL_RANGEMAXANON; max_range_id = DEFAULT_FIDUCIAL_RANGEMAXID; - fov = DEFAULT_FIDUCIAL_FOV; + fov = DEFAULT_FIDUCIAL_FOV; key = 0; data = g_array_new( false, true, sizeof(stg_fiducial_t) ); @@ -122,9 +122,9 @@ stg_pose_t mypose = this->GetGlobalPose(); // are we within range? - stg_pose_t hispose = him->GetGlobalPose(); + stg_pose_t hispose = him->GetGlobalPose(); double dx = hispose.x - mypose.x; - double dy = hispose.y - mypose.y; + double dy = hispose.y - mypose.y; double range = hypot( dy, dx ); // printf( "range to target %.2f m ( @@ -178,7 +178,7 @@ stg_geom_t hisgeom = him->GetGeom(); // record where we saw him and what he looked like - stg_fiducial_t fid; + stg_fiducial_t fid; fid.range = range; fid.bearing = dtheta; fid.geom.x = hisgeom.size.x; @@ -296,7 +296,7 @@ snprintf(idstr, 31, "%d", fid.id ); PushColor( 0,0,0,1 ); // black - gl_draw_string( 0,0,0, idstr ); + gl_draw_string( 0,0,0, idstr ); PopColor(); glPopMatrix(); Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/model_laser.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -85,10 +85,10 @@ id, typestr ); // sensible laser defaults - interval = 1e3 * DEFAULT_LASER_INTERVAL_MS; + interval = 1e3 * DEFAULT_LASER_INTERVAL_MS; laser_return = LaserVisible; - stg_geom_t geom; + stg_geom_t geom; memset( &geom, 0, sizeof(geom)); geom.size.x = DEFAULT_LASER_SIZEX; geom.size.y = DEFAULT_LASER_SIZEY; @@ -101,7 +101,7 @@ range_min = DEFAULT_LASER_MINRANGE; range_max = DEFAULT_LASER_MAXRANGE; fov = DEFAULT_LASER_FOV; - sample_count = DEFAULT_LASER_SAMPLES; + sample_count = DEFAULT_LASER_SAMPLES; resolution = DEFAULT_LASER_RESOLUTION; // don't allocate sample buffer memory until Update() is called @@ -120,14 +120,14 @@ void StgModelLaser::Load( void ) { - StgModel::Load(); + StgModel::Load(); Worldfile* wf = world->GetWorldFile(); - sample_count = wf->ReadInt( id, "samples", sample_count ); + sample_count = wf->ReadInt( id, "samples", sample_count ); range_min = wf->ReadLength( id, "range_min", range_min); range_max = wf->ReadLength( id, "range_max", range_max ); - fov = wf->ReadAngle( id, "fov", fov ); + fov = wf->ReadAngle( id, "fov", fov ); resolution = wf->ReadInt( id, "laser_sample_skip", resolution ); if( resolution < 1 ) @@ -153,7 +153,7 @@ range_min = cfg.range_bounds.min; range_max = cfg.range_bounds.max; fov = cfg.fov; - resolution = cfg.resolution; + resolution = cfg.resolution; } static bool laser_raytrace_match( StgBlock* testblock, @@ -173,7 +173,7 @@ void StgModelLaser::Update( void ) { double bearing = -fov/2.0; - double sample_incr = fov / (double)(sample_count-1); + double sample_incr = fov / (double)(sample_count-1); samples = g_renew( stg_laser_sample_t, samples, sample_count ); @@ -204,7 +204,7 @@ samples[t].reflectance = 0; // todo - lower bound on range - bearing += sample_incr; + bearing += sample_incr; } // we may need to interpolate the samples we skipped @@ -231,7 +231,7 @@ data_dirty = true; - StgModel::Update(); + StgModel::Update(); } @@ -289,13 +289,13 @@ stg_laser_sample_t* StgModelLaser::GetSamples( uint32_t* count ) { - if( count ) *count = sample_count; - return samples; + if( count ) *count = sample_count; + return samples; } void StgModelLaser::SetSamples( stg_laser_sample_t* samples, uint32_t count) { - this->samples = g_renew( stg_laser_sample_t, this->samples, sample_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; @@ -322,11 +322,11 @@ glPointSize( 4.0 ); - glVertexPointer( 2, GL_FLOAT, 0, pts ); + glVertexPointer( 2, GL_FLOAT, 0, pts ); for( unsigned int s=0; s<sample_count; s++ ) { - double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; + double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; pts[2*s+2] = (float)(samples[s].range * cos(ray_angle) ); pts[2*s+3] = (float)(samples[s].range * sin(ray_angle) ); @@ -359,9 +359,9 @@ // reset PopColor(); - PopColor(); + PopColor(); glDepthMask( GL_TRUE ); - glPopMatrix(); + glPopMatrix(); } Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/model_load.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -75,7 +75,7 @@ stg_geom_t geom = GetGeom(); geom.size.x = wf->ReadTupleLength(this->id, "size3", 0, geom.size.x ); geom.size.y = wf->ReadTupleLength(this->id, "size3", 1, geom.size.y ); - geom.size.z = wf->ReadTupleLength(this->id, "size3", 2, geom.size.z ); + geom.size.z = wf->ReadTupleLength(this->id, "size3", 2, geom.size.z ); this->SetGeom( geom ); } @@ -83,7 +83,7 @@ { stg_pose_t pose = GetPose(); pose.x = wf->ReadTupleLength(this->id, "pose", 0, pose.x ); - pose.y = wf->ReadTupleLength(this->id, "pose", 1, pose.y ); + pose.y = wf->ReadTupleLength(this->id, "pose", 1, pose.y ); pose.a = wf->ReadTupleAngle(this->id, "pose", 2, pose.a ); this->SetPose( pose ); } @@ -92,8 +92,8 @@ { stg_pose_t pose = GetPose(); pose.x = wf->ReadTupleLength(this->id, "pose4", 0, pose.x ); - pose.y = wf->ReadTupleLength(this->id, "pose4", 1, pose.y ); - pose.z = wf->ReadTupleLength(this->id, "pose4", 2, pose.z ); + pose.y = wf->ReadTupleLength(this->id, "pose4", 1, pose.y ); + pose.z = wf->ReadTupleLength(this->id, "pose4", 2, pose.z ); pose.a = wf->ReadTupleAngle( this->id, "pose4", 3, pose.a ); this->SetPose( pose ); @@ -104,7 +104,7 @@ stg_velocity_t vel = GetVelocity(); vel.x = wf->ReadTupleLength(this->id, "velocity", 0, vel.x ); vel.y = wf->ReadTupleLength(this->id, "velocity", 1, vel.y ); - vel.a = wf->ReadTupleAngle(this->id, "velocity", 3, vel.a ); + vel.a = wf->ReadTupleAngle(this->id, "velocity", 3, vel.a ); this->SetVelocity( vel ); if( vel.x || vel.y || vel.z || vel.a ) @@ -117,18 +117,18 @@ vel.x = wf->ReadTupleLength(this->id, "velocity4", 0, vel.x ); vel.y = wf->ReadTupleLength(this->id, "velocity4", 1, vel.y ); vel.z = wf->ReadTupleLength(this->id, "velocity4", 2, vel.z ); - vel.a = wf->ReadTupleAngle(this->id, "velocity4", 3, vel.a ); + vel.a = wf->ReadTupleAngle(this->id, "velocity4", 3, vel.a ); this->SetVelocity( vel ); } if( wf->PropertyExists( this->id, "boundary" )) { - this->SetBoundary( wf->ReadInt(this->id, "boundary", this->boundary )); + this->SetBoundary( wf->ReadInt(this->id, "boundary", this->boundary )); } if( wf->PropertyExists( this->id, "color" )) { - stg_color_t col = 0xFFFF0000; // red; + stg_color_t col = 0xFFFF0000; // red; const char* colorstr = wf->ReadString( this->id, "color", NULL ); if( colorstr ) { @@ -138,7 +138,7 @@ col |= 0xFF000000; // set the alpha channel to max } else - col = stg_lookup_color( colorstr ); + col = stg_lookup_color( colorstr ); this->SetColor( col ); } @@ -175,7 +175,7 @@ PRINT_DEBUG1( "attempting to load image %s", full ); stg_rotrect_t* rects = NULL; - unsigned int rect_count = 0; + unsigned int rect_count = 0; unsigned int width, height; if( stg_rotrects_from_image_file( full, &rects, @@ -205,8 +205,8 @@ double epsilon = 0.01; this->AddBlockRect(0,0, epsilon, height ); this->AddBlockRect(0,0, width, epsilon ); - this->AddBlockRect(0, height-epsilon, width, epsilon ); - this->AddBlockRect(width-epsilon,0, epsilon, height ); + this->AddBlockRect(0, height-epsilon, width, epsilon ); + this->AddBlockRect(width-epsilon,0, epsilon, height ); } StgBlock::ScaleList( this->blocks, &this->geom.size ); @@ -229,7 +229,7 @@ //printf( "expecting %d blocks\n", blockcount ); - char key[256]; + char key[256]; for( int l=0; l<blockcount; l++ ) { snprintf(key, sizeof(key), "block[%d].points", l); @@ -269,7 +269,7 @@ const char* colorstr = wf->ReadString( this->id, key, NULL ); if( colorstr ) { - blockcol = stg_lookup_color( colorstr ); + blockcol = stg_lookup_color( colorstr ); inherit_color = false; } @@ -288,8 +288,8 @@ double height = geom.size.y; this->AddBlockRect(-width/2.0, -height/2.0, epsilon, height ); this->AddBlockRect(-width/2.0, -height/2.0, width, epsilon ); - this->AddBlockRect(-width/2.0, height/2.0-epsilon, width, epsilon ); - this->AddBlockRect(width/2.0-epsilon, -height/2.0, epsilon, height ); + this->AddBlockRect(-width/2.0, height/2.0-epsilon, width, epsilon ); + this->AddBlockRect(width/2.0-epsilon, -height/2.0, epsilon, height ); } this->Map(); @@ -320,19 +320,19 @@ this->SetGripperReturn( wf->ReadInt( this->id, "gripper_return", this->gripper_return )); if( wf->PropertyExists( this->id, "gui_nose" )) - this->SetGuiNose( wf->ReadInt(this->id, "gui_nose", this->gui_nose )); + this->SetGuiNose( wf->ReadInt(this->id, "gui_nose", this->gui_nose )); if( wf->PropertyExists( this->id, "gui_grid" )) - this->SetGuiGrid( wf->ReadInt(this->id, "gui_grid", this->gui_grid )); + this->SetGuiGrid( wf->ReadInt(this->id, "gui_grid", this->gui_grid )); if( wf->PropertyExists( this->id, "gui_outline" )) - this->SetGuiOutline( wf->ReadInt(this->id, "gui_outline", this->gui_outline )); + this->SetGuiOutline( wf->ReadInt(this->id, "gui_outline", this->gui_outline )); if( wf->PropertyExists( this->id, "gui_movemask" )) this->SetGuiMask( wf->ReadInt(this->id, "gui_movemask", this->gui_mask )); if( wf->PropertyExists( this->id, "map_resolution" )) - this->SetMapResolution( wf->ReadFloat(this->id, "map_resolution", this->map_resolution )); + this->SetMapResolution( wf->ReadFloat(this->id, "map_resolution", this->map_resolution )); if( wf->PropertyExists( this->id, "ctrl" )) { @@ -345,7 +345,7 @@ } if( wf->PropertyExists( this->id, "say" )) - this->Say( wf->ReadString(this->id, "say", NULL )); + this->Say( wf->ReadString(this->id, "say", NULL )); // call any type-specific load callbacks this->CallCallbacks( &this->load ); @@ -358,7 +358,7 @@ } if( this->debug ) - printf( "Model \"%s\" is in debug mode\n", token ); + printf( "Model \"%s\" is in debug mode\n", token ); PRINT_DEBUG1( "Model \"%s\" loading complete", token ); } @@ -404,7 +404,7 @@ char* stagepath = getenv("STAGEPATH"); if( stagepath == NULL ) - stagepath = "."; + stagepath = "."; lt_dlsetsearchpath( stagepath ); @@ -427,7 +427,7 @@ printf( "Libtool error: %s. Can't open your plugin controller. Quitting\n", lt_dlerror() ); // report the error from libtool - PRINT_ERR1( "Failed to open \"%s\". Check that it can be found by searching the directories in your STAGEPATH environment variable, or the current directory if STAGEPATH is not set.]\n", lib ); + PRINT_ERR1( "Failed to open \"%s\". Check that it can be found by searching the directories in your STAGEPATH environment variable, or the current directory if STAGEPATH is not set.]\n", lib ); exit(-1); } Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/model_position.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -92,7 +92,7 @@ id, typestr ); // no power consumed until we're subscribed - this->SetWatts( 0 ); + this->SetWatts( 0 ); // sensible position defaults stg_velocity_t vel; @@ -104,12 +104,12 @@ // configure the position-specific stuff // control - memset( &goal, 0, sizeof(goal)); + memset( &goal, 0, sizeof(goal)); drive_mode = POSITION_DRIVE_DEFAULT; control_mode = POSITION_CONTROL_DEFAULT; // localization - localization_mode = POSITION_LOCALIZATION_DEFAULT; + localization_mode = POSITION_LOCALIZATION_DEFAULT; integration_error.x = drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_X - @@ -175,7 +175,7 @@ // specified est_origin = this->GetGlobalPose(); - keyword = "localization_origin"; + keyword = "localization_origin"; if( wf->PropertyExists( this->id, keyword ) ) { est_origin.x = wf->ReadTupleLength( id, keyword, 0, est_origin.x ); @@ -191,12 +191,12 @@ double cosa = cos(est_origin.a); double sina = sin(est_origin.a); double dx = gpose.x - est_origin.x; - double dy = gpose.y - est_origin.y; - est_pose.x = dx * cosa + dy * sina; + double dy = gpose.y - est_origin.y; + est_pose.x = dx * cosa + dy * sina; est_pose.y = dy * cosa - dx * sina; // zero position error: assume we know exactly where we are on startup - memset( &est_pose_error, 0, sizeof(est_pose_error)); + memset( &est_pose_error, 0, sizeof(est_pose_error)); } // odometry model parameters @@ -384,7 +384,7 @@ // this->watts = STG_POSITION_WATTS + //fabs(vel->x) * STG_POSITION_WATTS_KGMS * this->mass + //fabs(vel->y) * STG_POSITION_WATTS_KGMS * this->mass + - //fabs(vel->a) * STG_POSITION_WATTS_KGMS * this->mass; + //fabs(vel->a) * STG_POSITION_WATTS_KGMS * this->mass; //PRINT_DEBUG4( "model %s velocity (%.2f %.2f %.2f)", // this->token, @@ -407,8 +407,8 @@ double cosa = cos(est_origin.a); double sina = sin(est_origin.a); double dx = gpose.x - est_origin.x; - double dy = gpose.y - est_origin.y; - est_pose.x = dx * cosa + dy * sina; + double dy = gpose.y - est_origin.y; + est_pose.x = dx * cosa + dy * sina; est_pose.y = dy * cosa - dx * sina; } @@ -426,7 +426,7 @@ double dx = (vel.x * dt) * (1.0 + integration_error.x ); double dy = (vel.y * dt) * (1.0 + integration_error.y ); - est_pose.x += dx * cosa + dy * sina; + est_pose.x += dx * cosa + dy * sina; est_pose.y -= dy * cosa - dx * sina; } @@ -449,7 +449,7 @@ this->SetWatts( STG_POSITION_WATTS ); - //stg_model_position_odom_reset( mod ); + //stg_model_position_odom_reset( mod ); } void StgModelPosition::Shutdown( void ) @@ -457,8 +457,8 @@ PRINT_DEBUG( "position shutdown" ); // safety features! - bzero( &goal, sizeof(goal) ); - bzero( &velocity, sizeof(velocity) ); + bzero( &goal, sizeof(goal) ); + bzero( &velocity, sizeof(velocity) ); this->SetWatts( 0 ); @@ -468,35 +468,35 @@ void StgModelPosition::SetSpeed( double x, double y, double a ) { control_mode = STG_POSITION_CONTROL_VELOCITY; - goal.x = x; - goal.y = y; - goal.z = 0; - goal.a = a; + goal.x = x; + goal.y = y; + goal.z = 0; + goal.a = a; } void StgModelPosition::SetXSpeed( double x ) { control_mode = STG_POSITION_CONTROL_VELOCITY; - goal.x = x; + goal.x = x; } void StgModelPosition::SetYSpeed( double y ) { control_mode = STG_POSITION_CONTROL_VELOCITY; - goal.y = y; + goal.y = y; } void StgModelPosition::SetZSpeed( double z ) { control_mode = STG_POSITION_CONTROL_VELOCITY; - goal.z = z; + goal.z = z; } void StgModelPosition::SetTurnSpeed( double a ) { control_mode = STG_POSITION_CONTROL_VELOCITY; - goal.a = a; + goal.a = a; } @@ -509,10 +509,10 @@ void StgModelPosition::GoTo( double x, double y, double a ) { control_mode = STG_POSITION_CONTROL_POSITION; - goal.x = x; - goal.y = y; - goal.z = 0; - goal.a = a; + goal.x = x; + goal.y = y; + goal.z = 0; + goal.a = a; } void StgModelPosition::GoTo( stg_pose_t pose ) Modified: code/stage/trunk/libstage/model_props.cc =================================================================== --- code/stage/trunk/libstage/model_props.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/model_props.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -79,7 +79,7 @@ } // otherwise it's an arbitary property and we store the pointer - g_datalist_set_data( &this->props, key, data ); + g_datalist_set_data( &this->props, key, data ); return 0; // ok } Modified: code/stage/trunk/libstage/model_ranger.cc =================================================================== --- code/stage/trunk/libstage/model_ranger.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/model_ranger.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -264,7 +264,7 @@ return; if( samples == NULL ) - samples = new stg_meters_t[sensor_count]; + samples = new stg_meters_t[sensor_count]; assert( samples ); //PRINT_DEBUG2( "[%d] updating ranger %s", (int)world->sim_time_ms, token ); @@ -283,8 +283,8 @@ NULL, &ray ); - samples[t] = MAX( ray.range, sensors[t].bounds_range.min ); - //sensors[t].error = TODO; + samples[t] = MAX( ray.range, sensors[t].bounds_range.min ); + //sensors[t].error = TODO; } } @@ -339,7 +339,7 @@ { if( samples[s] > 0.0 ) { - stg_ranger_sensor_t* rngr = &sensors[s]; + stg_ranger_sensor_t* rngr = &sensors[s]; //double dx = rngr->size.x/2.0; //double dy = rngr->size.y/2.0; @@ -352,20 +352,20 @@ glEnd(); // sensor FOV - double sidelen = samples[s]; - double da = rngr->fov/2.0; + double sidelen = samples[s]; + double da = rngr->fov/2.0; unsigned int index = s*9; pts[index+0] = rngr->pose.x; pts[index+1] = rngr->pose.y; pts[index+2] = rngr->pose.z; - pts[index+3] = rngr->pose.x + sidelen*cos(rngr->pose.a - da ); - pts[index+4] = rngr->pose.y + sidelen*sin(rngr->pose.a - da ); + pts[index+3] = rngr->pose.x + sidelen*cos(rngr->pose.a - da ); + pts[index+4] = rngr->pose.y + sidelen*sin(rngr->pose.a - da ); pts[index+5] = rngr->pose.z; - pts[index+6] = rngr->pose.x + sidelen*cos(rngr->pose.a + da ); - pts[index+7] = rngr->pose.y + sidelen*sin(rngr->pose.a + da ); + pts[index+6] = rngr->pose.x + sidelen*cos(rngr->pose.a + da ); + pts[index+7] = rngr->pose.y + sidelen*sin(rngr->pose.a + da ); pts[index+8] = rngr->pose.z; } } @@ -373,15 +373,15 @@ //PopColor(); // draw the filled triangles in transparent blue - glDepthMask( GL_FALSE ); + glDepthMask( GL_FALSE ); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); PushColor( 0, 1, 0, 0.1 ); // transparent pale green glEnableClientState( GL_VERTEX_ARRAY ); - glVertexPointer( 3, GL_FLOAT, 0, pts ); + glVertexPointer( 3, GL_FLOAT, 0, pts ); glDrawArrays( GL_TRIANGLES, 0, 3 * sensor_count ); // restore state - glDepthMask( GL_TRUE ); + glDepthMask( GL_TRUE ); PopColor(); glPopMatrix(); Modified: code/stage/trunk/libstage/model_wifi.cc =================================================================== --- code/stage/trunk/libstage/model_wifi.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/model_wifi.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -55,7 +55,7 @@ wifi_init( stg_model_t* mod ) { // we don't consume any power until subscribed - mod->watts = 0.0; + mod->watts = 0.0; // override the default methods; these will be called by the simualtion // engine @@ -102,7 +102,7 @@ // get the sensor's pose in global coords stg_pose_t pz; - memcpy( &pz, &geom.pose, sizeof(pz) ); + memcpy( &pz, &geom.pose, sizeof(pz) ); stg_model_local_to_global( mod, &pz ); Modified: code/stage/trunk/libstage/resource.cc =================================================================== --- code/stage/trunk/libstage/resource.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/resource.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -2,7 +2,7 @@ StgFlag::StgFlag( stg_color_t color, double size ) { - this->color = color; + this->color = color; this->size = size; } Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/stage.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -33,8 +33,8 @@ if(!setlocale(LC_ALL,"POSIX")) PRINT_WARN("Failed to setlocale(); config file may not be parse correctly\n" ); - typetable = stg_create_typetable(); - init_called = true; + typetable = stg_create_typetable(); + init_called = true; // ask FLTK to load support for various image formats fl_register_images(); @@ -113,8 +113,8 @@ for( unsigned int p=0; p<pt_count; p++ ) { - int32_t x = pts[p].x; - int32_t y = pts[p].y; + int32_t x = pts[p].x; + int32_t y = pts[p].y; int32_t dx = pts[(p+1)%pt_count].x - x; int32_t dy = pts[(p+1)%pt_count].y - y; @@ -153,14 +153,14 @@ stg_msec_t Stg::stg_realtime( void ) { struct timeval tv; - gettimeofday( &tv, NULL ); + gettimeofday( &tv, NULL ); stg_msec_t timenow = (stg_msec_t)( tv.tv_sec*1e3 + tv.tv_usec/1e3 ); return timenow; } stg_msec_t Stg::stg_realtime_since_start( void ) { - static stg_msec_t starttime = 0; + static stg_msec_t starttime = 0; stg_msec_t timenow = stg_realtime(); if( starttime == 0 ) @@ -273,8 +273,8 @@ { // test the origin of the rect if( rects[r].pose.x < minx ) minx = rects[r].pose.x; - if( rects[r].pose.y < miny ) miny = rects[r].pose.y; - if( rects[r].pose.x > maxx ) maxx = rects[r].pose.x; + if( rects[r].pose.y < miny ) miny = rects[r].pose.y; + if( rects[r].pose.x > maxx ) maxx = rects[r].pose.x; if( rects[r].pose.y > maxy ) maxy = rects[r].pose.y; // test the extremes of the rect @@ -325,7 +325,7 @@ double cosa = cos(p1.a); double sina = sin(p1.a); - stg_pose_t result; + stg_pose_t result; result.x = p1.x + p2.x * cosa - p2.y * sina; result.y = p1.y + p2.x * sina + p2.y * cosa; result.z = p1.z + p2.z; @@ -442,7 +442,7 @@ for( ; x < img_width && ! pb_pixel_is_set(img,x,y,threshold); x++ ) { // handle horizontal cropping - //double ppx = x * sx; + //double ppx = x * sx; //if (ppx < this->crop_ax || ppx > this->crop_bx) //continue; @@ -455,7 +455,7 @@ //if (ppy < this->crop_ay || ppy > this->crop_by) //continue; - yy++; + yy++; } // now yy is the depth of a line of non-zero pixels @@ -490,7 +490,7 @@ //printf( "rect %d (%.2f %.2f %.2f %.2f %.2f\n", // *rect_count, - // latest->x, latest->y, latest->a, latest->w, latest->h ); + // latest->x, latest->y, latest->a, latest->w, latest->h ); } } @@ -531,7 +531,7 @@ pts[2].x = 1; pts[2].y = 1; pts[3].x = 0; - pts[3].y = 1; + pts[3].y = 1; return pts; } Modified: code/stage/trunk/libstage/stest.cc =================================================================== --- code/stage/trunk/libstage/stest.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/stest.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -53,7 +53,7 @@ world.Load( argv[1] ); - char namebuf[256]; + char namebuf[256]; robot_t* robots = new robot_t[POPSIZE]; for( int i=0; i<POPSIZE; i++ ) @@ -107,7 +107,7 @@ for( unsigned int s=0; s< rgr->sensor_count; s++ ) { - double srange = rgr->samples[s]; + double srange = rgr->samples[s]; dx += srange * cos( rgr->sensors[s].pose.a ); dy += srange * sin( rgr->sensors[s].pose.a ); @@ -148,8 +148,8 @@ //printf( "robot %s [%.2f %.2f %.2f %.2f]\n", //robots[i].position->Token(), vel.x, vel.y, vel.z, vel.a ); - // uint32_t bcount=0; - //stg_blobfinder_blob_t* blobs = robots[i].blobfinder->GetBlobs( &bcount ); + // uint32_t bcount=0; + //stg_blobfinder_blob_t* blobs = robots[i].blobfinder->GetBlobs( &bcount ); //printf( "robot %s sees %u blobs\n", robots[i].blobfinder->Token(), bcount ); @@ -157,6 +157,6 @@ } - delete[] robots; + delete[] robots; exit( 0 ); } Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/world.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -75,7 +75,7 @@ SuperRegion* StgWorld::CreateSuperRegion( int32_t x, int32_t y ) { - SuperRegion* sr = new SuperRegion( x, y ); + SuperRegion* sr = new SuperRegion( x, y ); g_hash_table_insert( superregions, &sr->origin, sr ); return sr; } @@ -83,7 +83,7 @@ void StgWorld::DestroySuperRegion( SuperRegion* sr ) { g_hash_table_remove( superregions, &sr->origin ); - delete sr; + delete sr; } StgWorld::StgWorld( void ) @@ -91,7 +91,7 @@ Initialize( "MyWorld", STG_DEFAULT_WORLD_INTERVAL_SIM, STG_DEFAULT_WORLD_INTERVAL_REAL, - STG_DEFAULT_WORLD_PPM ); + STG_DEFAULT_WORLD_PPM ); } StgWorld::StgWorld( const char* token, @@ -115,7 +115,7 @@ this->id = StgWorld::next_id++; this->ray_list = NULL; - this->quit_time = 0; + this->quit_time = 0; assert(token); this->token = (char*)g_malloc(Stg::TOKEN_MAX); @@ -144,8 +144,8 @@ (GEqualFunc)PointIntEqual ); this->total_subs = 0; - this->paused = false; - this->destroy = false; + this->paused = false; + this->destroy = false; // store a global table of all blocks, so they can be rendered all // at once. @@ -174,8 +174,8 @@ void StgWorld::RemoveModel( StgModel* mod ) { - g_hash_table_remove( models_by_id, mod ); - g_hash_table_remove( models_by_name, mod ); + g_hash_table_remove( models_by_id, mod ); + g_hash_table_remove( models_by_name, mod ); } void StgWorld::ClockString( char* str, size_t maxlen ) @@ -185,10 +185,10 @@ const uint32_t usec_per_second = 1000000; const uint32_t usec_per_msec = 1000; - uint32_t hours = sim_time / usec_per_hour; - uint32_t minutes = (sim_time % usec_per_hour) / usec_per_minute; - uint32_t seconds = (sim_time % usec_per_minute) / usec_per_second; - uint32_t msec = (sim_time % usec_per_second) / usec_per_msec; + uint32_t hours = sim_time / usec_per_hour; + uint32_t minutes = (sim_time % usec_per_hour) / usec_per_minute; + uint32_t seconds = (sim_time % usec_per_minute) / usec_per_second; + uint32_t msec = (sim_time % usec_per_second) / usec_per_msec; // find the average length of the last few realtime intervals; stg_usec_t average_real_interval = 0; @@ -233,7 +233,7 @@ void StgWorld::Load( const char* worldfile_path ) { - printf( " [Loading %s]", worldfile_path ); + printf( " [Loading %s]", worldfile_path ); fflush(stdout); stg_usec_t load_start_time = RealTimeNow(); @@ -262,7 +262,7 @@ if( wf->PropertyExists( entity, "resolution" ) ) this->ppm = - 1.0 / wf->ReadFloat( entity, "resolution", STG_DEFAULT_WORLD_PPM ); + 1.0 / wf->ReadFloat( entity, "resolution", STG_DEFAULT_WORLD_PPM ); this->paused = wf->ReadInt( entity, "paused", this->paused ); @@ -300,7 +300,7 @@ else { PRINT_ERR1( "Unknown model type %s in world file.", - typestr ); + typestr ); exit( 1 ); } @@ -425,7 +425,7 @@ void StgWorld::AddModel( StgModel* mod ) { //PRINT_DEBUG3( "World %s adding model %d %s to hash tables ", - // token, mod->id, mod->Token() ); + // token, mod->id, mod->Token() ); g_hash_table_insert( this->models_by_id, (gpointer)mod->Id(), mod ); AddModelName( mod ); @@ -434,7 +434,7 @@ void StgWorld::AddModelName( StgModel* mod ) { - g_hash_table_insert( this->models_by_name, (gpointer)mod->Token(), mod ); + g_hash_table_insert( this->models_by_name, (gpointer)mod->Token(), mod ); } StgModel* StgWorld::GetModel( const char* name ) @@ -460,7 +460,7 @@ float* drawpts = new float[4]; drawpts[0] = x1; drawpts[1] = y1; - drawpts[2] = x2; + drawpts[2] = x2; drawpts[3] = y2; ray_list = g_list_append( ray_list, drawpts ); } @@ -489,12 +489,12 @@ bool ztest ) // number of samples { pose.a -= fov/2.0; // direction of first ray - stg_radians_t angle_incr = fov/(double)sample_count; + stg_radians_t angle_incr = fov/(double)sample_count; for( uint32_t s=0; s < sample_count; s++ ) { Raytrace( pose, range, func, model, arg, &samples[s], ztest ); - pose.a += angle_incr; + pose.a += angle_incr; } } @@ -534,7 +534,7 @@ // fast integer line 3d algorithm adapted from Cohen's code from // Graphics Gems IV - int n, sx, sy, sz, exy, exz, ezy, ax, ay, az, bx, by, bz; + int n, sx, sy, sz, exy, exz, ezy, ax, ay, az, bx, by, bz; sx = sgn(dx); sy = sgn(dy); sz = sgn(dz); ax = abs(dx); ay = abs(dy); az = abs(dz); bx = 2*ax; by = 2*ay; bz = 2*az; @@ -566,7 +566,7 @@ sup.y = y >> SRBITS; // printf( "pixel [%d %d]\tS[ %d %d ]\t", - // x, y, sup.x, sup.y ); + // x, y, sup.x, sup.y ); if( ! (sup.x == lastsup.x && sup.y == lastsup.y )) { @@ -581,11 +581,11 @@ reg.x = (x - ( sup.x << SRBITS)) >> RBITS; reg.y = (y - ( sup.y << SRBITS)) >> RBITS; - // printf( "R[ %d %d ]\t", reg.x, reg.y ); + // printf( "R[ %d %d ]\t", reg.x, reg.y ); if( ! (reg.x == lastreg.x && reg.y == lastreg.y )) { - r = sr->GetRegion( reg.x, reg.y ); + r = sr->GetRegion( reg.x, reg.y ); lastreg = reg; } @@ -596,13 +596,13 @@ cell.x = x - ((sup.x << SRBITS) + (reg.x << RBITS)); cell.y = y - ((sup.y << SRBITS) + (reg.y << RBITS)); - // printf( "C[ %d %d ]\t", cell.x, cell.y ); + // printf( "C[ %d %d ]\t", cell.x, cell.y ); for( GSList* list = r->GetCell( cell.x, cell.y )->list; list; list = list->next ) { - StgBlock* block = (StgBlock*)list->data; + StgBlock* block = (StgBlock*)list->data; assert( block ); // if this block does not belong to the searching model and it @@ -678,12 +678,12 @@ void StgWorld::StartUpdatingModel( StgModel* mod ) { if( g_list_find( this->update_list, mod ) == NULL ) - this->update_list = g_list_append( this->update_list, mod ); + this->update_list = g_list_append( this->update_list, mod ); } void StgWorld::StopUpdatingModel( StgModel* mod ) { - this->update_list = g_list_remove( this->update_list, mod ); + this->update_list = g_list_remove( this->update_list, mod ); } // int32_t StgWorld::MetersToPixels( stg_meters_t m ) @@ -700,7 +700,7 @@ sup.y = y >> SRBITS; //printf( "ADDBLOCKPIXEL pixel [%d %d] S[ %d %d ]\t", - // x, y, sup.x, sup.y ); + // x, y, sup.x, sup.y ); SuperRegion* sr = (SuperRegion*) g_hash_table_lookup( rinfo->world->superregions, (void*)&sup ); @@ -727,7 +727,7 @@ // find the pixel coords inside this superregion stg_point_int_t cell; - cell.x = x - ( sup.x << SRBITS); + cell.x = x - ( sup.x << SRBITS); cell.y = y - ( sup.y << SRBITS); //printf( "C[ %d %d]", cell.x, cell.y ); @@ -742,7 +742,7 @@ extent.x.min = MIN( extent.x.min, pt.x); extent.x.max = MAX( extent.x.max, pt.x ); extent.y.min = MIN( extent.y.min, pt.y ); - extent.y.max = MAX( extent.y.max, pt.y ); + extent.y.max = MAX( extent.y.max, pt.y ); extent.z.min = MIN( extent.z.min, pt.z ); - extent.z.max = MAX( extent.z.max, pt.z ); + extent.z.max = MAX( extent.z.max, pt.z ); } Modified: code/stage/trunk/libstage/worldfile.cc =================================================================== --- code/stage/trunk/libstage/worldfile.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/worldfile.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -511,7 +511,7 @@ fullpath = (char*) malloc(PATH_MAX); memset(fullpath, 0, PATH_MAX); strcat( fullpath, dirname(tmp)); - strcat( fullpath, "/" ); + strcat( fullpath, "/" ); strcat( fullpath, filename ); assert(strlen(fullpath) + 1 < PATH_MAX); free(tmp); @@ -524,9 +524,9 @@ char *tmp = strdup(this->filename); fullpath = (char*) malloc(PATH_MAX); getcwd(fullpath, PATH_MAX); - strcat( fullpath, "/" ); + strcat( fullpath, "/" ); strcat( fullpath, dirname(tmp)); - strcat( fullpath, "/" ); + strcat( fullpath, "/" ); strcat( fullpath, filename ); assert(strlen(fullpath) + 1 < PATH_MAX); free(tmp); @@ -691,7 +691,7 @@ if (token->include > 0) continue; if (token->type == TokenString) - fprintf(file, "\"%s\"", token->value); + fprintf(file, "\"%s\"", token->value); else fprintf(file, "%s", token->value); } @@ -728,7 +728,7 @@ this->tokens = (CToken*) realloc(this->tokens, this->token_size * sizeof(this->tokens[0])); } - this->tokens[this->token_count].include = include; + this->tokens[this->token_count].include = include; this->tokens[this->token_count].type = type; this->tokens[this->token_count].value = strdup(value); this->token_count++; @@ -1398,7 +1398,7 @@ // else // printf( "key %s not found\n", key ); - return prop; + return prop; } @@ -1472,7 +1472,7 @@ CProperty* property = GetProperty(entity, name); if( property == NULL ) return; - SetPropertyValue(property, 0, value); + SetPropertyValue(property, 0, value); } @@ -1612,7 +1612,7 @@ char *fullpath = (char*) malloc(PATH_MAX); memset(fullpath, 0, PATH_MAX); strcat( fullpath, dirname(tmp)); - strcat( fullpath, "/" ); + strcat( fullpath, "/" ); strcat( fullpath, filename ); assert(strlen(fullpath) + 1 < PATH_MAX); free(tmp); @@ -1627,9 +1627,9 @@ char *tmp = strdup(this->filename); char *fullpath = (char*) malloc(PATH_MAX); getcwd(fullpath, PATH_MAX); - strcat( fullpath, "/" ); + strcat( fullpath, "/" ); strcat( fullpath, dirname(tmp)); - strcat( fullpath, "/" ); + strcat( fullpath, "/" ); strcat( fullpath, filename ); assert(strlen(fullpath) + 1 < PATH_MAX); free(tmp); @@ -1662,11 +1662,11 @@ { /* TODO property = InsertProperty(entity, name); - SetPropertyValue(property, index, value); + SetPropertyValue(property, index, value); */ } else - SetPropertyValue(property, index, value); + SetPropertyValue(property, index, value); } Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-10 11:32:54 UTC (rev 6520) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-10 17:43:46 UTC (rev 6521) @@ -194,29 +194,29 @@ mbar->add( "View", 0, 0, 0, FL_SUBMENU ); mbar->add( MITEM_VIEW_DATA, 'd', (Fl_Callback*)view_toggle_cb, (void*)canvas, - FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_DATA ? FL_MENU_VALUE : 0 )); + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_DATA ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_BLOCKS, 'b', (Fl_Callback*)view_toggle_cb, (void*)canvas, - FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_BLOCKS ? FL_MENU_VALUE : 0 )); + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_BLOCKS ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_GRID, 'g', (Fl_Callback*)view_toggle_cb, (void*)canvas, - FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_GRID ? FL_MENU_VALUE : 0 )); + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_GRID ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_OCCUPANCY, FL_ALT+'o', (Fl_Callback*)view_toggle_cb, (void*)canvas, - FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_OCCUPANCY ? FL_MENU_VALUE : 0 )); + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_OCCUPANCY ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_QUADTREE, FL_ALT+'t', (Fl_Callback*)view_toggle_cb, (void*)canvas, - FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_QUADTREE ? FL_MENU_VALUE : 0 )); + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_QUADTREE ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_FOLLOW, 'f', (Fl_Callback*)view_toggle_cb, (void*)canvas, - FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_FOLLOW ? FL_MENU_VALUE : 0 )); + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_FOLLOW ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_CLOCK, 'c', (Fl_Callback*)view_toggle_cb, (void*)canvas, - FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_CLOCK ? FL_MENU_VALUE : 0 )); + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_CLOCK ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_TRAILS, 't', (Fl_Callback*)view_toggle_cb, (void*)canvas, - FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_TRAILS ? FL_MENU_VALUE : 0 )); + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_TRAILS ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_FOOTPRINTS, FL_CTRL+'f', (Fl_Callback*)view_toggle_cb, (void*)canvas, - FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_FOOTPRINT ? FL_MENU_VALUE : 0 )); + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_FOOTPRINT ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_ARROWS, FL_CTRL+'a', (Fl_Callback*)view_toggle_cb, (void*)canvas, - FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_ARROWS ? FL_MENU_VALUE : 0 )); + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_ARROWS ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_BLOCKSRISING, FL_CTRL+'t', (Fl_Callb... [truncated message content] |
From: <al...@us...> - 2008-06-10 11:23:38
|
Revision: 6524 http://playerstage.svn.sourceforge.net/playerstage/?rev=6524&view=rev Author: alexcb Date: 2008-06-10 11:23:46 -0700 (Tue, 10 Jun 2008) Log Message: ----------- fixed indentation issues (converted to tabs) Modified Paths: -------------- code/stage/trunk/libstage/ancestor.cc code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgrid.cc code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/ctrl.cc code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/glcolorstack.cc code/stage/trunk/libstage/glutgraphics.cc code/stage/trunk/libstage/main.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_props.cc code/stage/trunk/libstage/model_ranger.cc code/stage/trunk/libstage/model_wifi.cc code/stage/trunk/libstage/resource.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/stagecpp.cc code/stage/trunk/libstage/stest.cc code/stage/trunk/libstage/typetable.cc code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldfile.cc code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/ancestor.cc =================================================================== --- code/stage/trunk/libstage/ancestor.cc 2008-06-10 18:11:59 UTC (rev 6523) +++ code/stage/trunk/libstage/ancestor.cc 2008-06-10 18:23:46 UTC (rev 6524) @@ -2,67 +2,67 @@ StgAncestor::StgAncestor() { - token = NULL; - children = NULL; - child_types = g_hash_table_new( g_str_hash, g_str_equal ); - debug = false; + token = NULL; + children = NULL; + child_types = g_hash_table_new( g_str_hash, g_str_equal ); + debug = false; } StgAncestor::~StgAncestor() { - if( children ) - { - for( GList* it = children; it; it=it->next ) - delete (StgModel*)it->data; - - g_list_free( children ); - } + if( children ) + { + for( GList* it = children; it; it=it->next ) + delete (StgModel*)it->data; - g_hash_table_destroy( child_types ); + g_list_free( children ); + } + + g_hash_table_destroy( child_types ); } unsigned int StgAncestor::GetNumChildrenOfType( const char* typestr ) { - unsigned int *c = (unsigned int*)g_hash_table_lookup( child_types, typestr); + unsigned int *c = (unsigned int*)g_hash_table_lookup( child_types, typestr); - if( c ) - return *c; - else - return 0; + if( c ) + return *c; + else + return 0; } void StgAncestor::IncrementNumChildrenOfType( const char* typestr ) { - unsigned int* c = (unsigned int*)g_hash_table_lookup( child_types, typestr); - - if( c == NULL ) - { - c = new unsigned int; - g_hash_table_insert( child_types, (gpointer)typestr, (gpointer)c); - *c = 1; - } - else - (*c)++; + unsigned int* c = (unsigned int*)g_hash_table_lookup( child_types, typestr); + + if( c == NULL ) + { + c = new unsigned int; + g_hash_table_insert( child_types, (gpointer)typestr, (gpointer)c); + *c = 1; + } + else + (*c)++; } void StgAncestor::AddChild( StgModel* mod ) { - // increment the count of models of this type - IncrementNumChildrenOfType( mod->typestr ); - - // store as a child - children = g_list_append( children, mod ); + // increment the count of models of this type + IncrementNumChildrenOfType( mod->typestr ); + + // store as a child + children = g_list_append( children, mod ); } void StgAncestor::RemoveChild( StgModel* mod ) { - puts( "TODO: StgWorld::RemoveChild()" ); + puts( "TODO: StgWorld::RemoveChild()" ); } stg_pose_t StgAncestor::GetGlobalPose() { - stg_pose_t pose; - bzero( &pose, sizeof(pose)); - return pose; + stg_pose_t pose; + bzero( &pose, sizeof(pose)); + return pose; } Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2008-06-10 18:11:59 UTC (rev 6523) +++ code/stage/trunk/libstage/block.cc 2008-06-10 18:23:46 UTC (rev 6524) @@ -3,278 +3,278 @@ typedef struct { - GSList** head; - GSList* link; - unsigned int* counter1; - unsigned int* counter2; + GSList** head; + GSList* link; + unsigned int* counter1; + unsigned int* counter2; } stg_list_entry_t; /** Create a new block. A model's body is a list of these - blocks. The point data is copied, so pts can safely be freed - after calling this.*/ + blocks. The point data is copied, so pts can safely be freed + after calling this.*/ StgBlock::StgBlock( StgModel* mod, - stg_point_t* pts, - size_t pt_count, - stg_meters_t zmin, - stg_meters_t zmax, - stg_color_t color, - bool inherit_color ) + stg_point_t* pts, + size_t pt_count, + stg_meters_t zmin, + stg_meters_t zmax, + stg_color_t color, + bool inherit_color ) { - this->mod = mod; - this->pt_count = pt_count; - this->pts = (stg_point_t*)g_memdup( pts, pt_count * sizeof(stg_point_t)); - // allocate space for the integer version of the block vertices - this->pts_global_pixels = new stg_point_int_t[pt_count]; + this->mod = mod; + this->pt_count = pt_count; + this->pts = (stg_point_t*)g_memdup( pts, pt_count * sizeof(stg_point_t)); + // allocate space for the integer version of the block vertices + this->pts_global_pixels = new stg_point_int_t[pt_count]; - this->zmin = zmin; - this->zmax = zmax; - this->color = color; - this->inherit_color = inherit_color; - this->rendered_points = - g_array_new( FALSE, FALSE, sizeof(stg_list_entry_t)); - - // flag these as unset until StgBlock::Map() is called. - this->global_zmin = -1; - this->global_zmax = -1; + this->zmin = zmin; + this->zmax = zmax; + this->color = color; + this->inherit_color = inherit_color; + this->rendered_points = + g_array_new( FALSE, FALSE, sizeof(stg_list_entry_t)); + + // flag these as unset until StgBlock::Map() is called. + this->global_zmin = -1; + this->global_zmax = -1; } StgBlock::~StgBlock() { - this->UnMap(); - g_free( pts ); - g_array_free( rendered_points, TRUE ); + this->UnMap(); + g_free( pts ); + g_array_free( rendered_points, TRUE ); - //delete[] edge_indices; + //delete[] edge_indices; } void Stg::stg_block_list_destroy( GList* list ) { - GList* it; - for( it=list; it; it=it->next ) - delete (StgBlock*)it->data; - g_list_free( list ); + GList* it; + for( it=list; it; it=it->next ) + delete (StgBlock*)it->data; + g_list_free( list ); } void StgBlock::DrawTop() { - // draw a top that fits over the side strip - glPushMatrix(); - glTranslatef( 0,0,zmax); - glVertexPointer( 2, GL_DOUBLE, 0, pts ); - glDrawArrays( GL_POLYGON, 0, pt_count ); - glPopMatrix(); + // draw a top that fits over the side strip + glPushMatrix(); + glTranslatef( 0,0,zmax); + glVertexPointer( 2, GL_DOUBLE, 0, pts ); + glDrawArrays( GL_POLYGON, 0, pt_count ); + glPopMatrix(); } void StgBlock::DrawSides() { - // construct a strip that wraps around the polygon - glBegin(GL_QUAD_STRIP); - for( unsigned int p=0; p<pt_count; p++) - { - glVertex3f( pts[p].x, pts[p].y, zmax ); - glVertex3f( pts[p].x, pts[p].y, zmin ); - } - // close the strip - glVertex3f( pts[0].x, pts[0].y, zmax ); - glVertex3f( pts[0].x, pts[0].y, zmin ); - glEnd(); + // construct a strip that wraps around the polygon + glBegin(GL_QUAD_STRIP); + for( unsigned int p=0; p<pt_count; p++) + { + glVertex3f( pts[p].x, pts[p].y, zmax ); + glVertex3f( pts[p].x, pts[p].y, zmin ); + } + // close the strip + glVertex3f( pts[0].x, pts[0].y, zmax ); + glVertex3f( pts[0].x, pts[0].y, zmin ); + glEnd(); } void StgBlock::DrawFootPrint() { - glBegin(GL_POLYGON); + glBegin(GL_POLYGON); - for( unsigned int p=0; p<pt_count; p++ ) - glVertex2f( pts[p].x, pts[p].y ); - - glEnd(); + for( unsigned int p=0; p<pt_count; p++ ) + glVertex2f( pts[p].x, pts[p].y ); + + glEnd(); } void StgBlock::Draw() { - // draw filled color polygons - stg_color_t color = Color(); + // draw filled color polygons + stg_color_t color = Color(); - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); - PushColor( color ); - glEnable(GL_POLYGON_OFFSET_FILL); - glPolygonOffset(1.0, 1.0); - DrawSides(); - DrawTop(); - glDisable(GL_POLYGON_OFFSET_FILL); - -// // draw the block outline in a darker version of the same color - double r,g,b,a; - stg_color_unpack( color, &r, &g, &b, &a ); - PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a )); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); + PushColor( color ); + glEnable(GL_POLYGON_OFFSET_FILL); + glPolygonOffset(1.0, 1.0); + DrawSides(); + DrawTop(); + glDisable(GL_POLYGON_OFFSET_FILL); - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE ); - glDepthMask(GL_FALSE); - DrawTop(); - DrawSides(); - glDepthMask(GL_TRUE); - - PopColor(); - PopColor(); + // // draw the block outline in a darker version of the same color + double r,g,b,a; + stg_color_unpack( color, &r, &g, &b, &a ); + PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a )); + + glPolygonMode(GL_FRONT_AND_BACK, GL_LINE ); + glDepthMask(GL_FALSE); + DrawTop(); + DrawSides(); + glDepthMask(GL_TRUE); + + PopColor(); + PopColor(); } void StgBlock::DrawSolid( void ) { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); - DrawSides(); - DrawTop(); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); + DrawSides(); + DrawTop(); } void StgBlock::Map() { - PRINT_DEBUG2( "%s mapping block with %d points", - mod->Token(), - (int)pt_count ); + PRINT_DEBUG2( "%s mapping block with %d points", + mod->Token(), + (int)pt_count ); - // update the global coordinate list - stg_point3_t global; - stg_point3_t local; - - for( unsigned int p=0; p<pt_count; p++ ) - { - local.x = pts[p].x; - local.y = pts[p].y; - local.z = zmin; - - global = mod->LocalToGlobal( local ); - - pts_global_pixels[p].x = mod->GetWorld()->MetersToPixels( global.x ); - pts_global_pixels[p].y = mod->GetWorld()->MetersToPixels( global.y ); - - PRINT_DEBUG2("loc [%.2f %.2f]", - pts[p].x, - pts[p].y ); - - PRINT_DEBUG2("glb [%d %d]", - pts_global_pixels[p].x, - pts_global_pixels[p].y ); - } - - // store the block's global vertical bounds for inspection by the - // raytracer - global_zmin = global.z; - global_zmax = global.z + (zmax-zmin); - - stg_render_info_t render_info; - render_info.world = mod->GetWorld(); - render_info.block = this; - - stg_polygon_3d( pts_global_pixels, pt_count, - (stg_line3d_func_t)StgWorld::AddBlockPixel, - (void*)&render_info ); - + // update the global coordinate list + stg_point3_t global; + stg_point3_t local; + + for( unsigned int p=0; p<pt_count; p++ ) + { + local.x = pts[p].x; + local.y = pts[p].y; + local.z = zmin; + + global = mod->LocalToGlobal( local ); + + pts_global_pixels[p].x = mod->GetWorld()->MetersToPixels( global.x ); + pts_global_pixels[p].y = mod->GetWorld()->MetersToPixels( global.y ); + + PRINT_DEBUG2("loc [%.2f %.2f]", + pts[p].x, + pts[p].y ); + + PRINT_DEBUG2("glb [%d %d]", + pts_global_pixels[p].x, + pts_global_pixels[p].y ); + } + + // store the block's global vertical bounds for inspection by the + // raytracer + global_zmin = global.z; + global_zmax = global.z + (zmax-zmin); + + stg_render_info_t render_info; + render_info.world = mod->GetWorld(); + render_info.block = this; + + stg_polygon_3d( pts_global_pixels, pt_count, + (stg_line3d_func_t)StgWorld::AddBlockPixel, + (void*)&render_info ); + } void StgBlock::UnMap() { - PRINT_DEBUG2( "UnMapping block of model %s with %d points", - mod->Token(), - (int)pt_count); - - for( unsigned int p=0; p<rendered_points->len; p++ ) - { - stg_list_entry_t* pt = - &g_array_index( rendered_points, stg_list_entry_t, p); - - *pt->head = g_slist_delete_link( *pt->head, pt->link ); - - // decrement the region and superregion render counts - (*pt->counter1)--; - (*pt->counter2)--; - } + PRINT_DEBUG2( "UnMapping block of model %s with %d points", + mod->Token(), + (int)pt_count); - // forget the points we have unrendered (but keep their storage) - g_array_set_size( rendered_points,0 ); + for( unsigned int p=0; p<rendered_points->len; p++ ) + { + stg_list_entry_t* pt = + &g_array_index( rendered_points, stg_list_entry_t, p); + + *pt->head = g_slist_delete_link( *pt->head, pt->link ); + + // decrement the region and superregion render counts + (*pt->counter1)--; + (*pt->counter2)--; + } + + // forget the points we have unrendered (but keep their storage) + g_array_set_size( rendered_points,0 ); } void StgBlock::RecordRenderPoint( GSList** head, - GSList* link, - unsigned int* c1, - unsigned int* c2 ) + GSList* link, + unsigned int* c1, + unsigned int* c2 ) { - // store this index in the block for later fast deletion - stg_list_entry_t el; - el.head = head; - el.link = link; - el.counter1 = c1; - el.counter2 = c2; - g_array_append_val( rendered_points, el ); + // store this index in the block for later fast deletion + stg_list_entry_t el; + el.head = head; + el.link = link; + el.counter1 = c1; + el.counter2 = c2; + g_array_append_val( rendered_points, el ); } void StgBlock::ScaleList( GList* blocks, - stg_size_t* size ) + stg_size_t* size ) { - if( g_list_length( blocks ) < 1 ) - return; + if( g_list_length( blocks ) < 1 ) + return; - // assuming the blocks currently fit in a square +/- one billion units - double minx, miny, maxx, maxy; - minx = miny = billion; - maxx = maxy = -billion; + // assuming the blocks currently fit in a square +/- one billion units + double minx, miny, maxx, maxy; + minx = miny = billion; + maxx = maxy = -billion; - double maxz = 0; - - GList* it; - for( it=blocks; it; it=it->next ) // examine all the blocks - { - // examine all the points in the polygon - StgBlock* block = (StgBlock*)it->data; - - block->UnMap(); // just in case - - for( unsigned int p=0; p < block->pt_count; p++ ) + double maxz = 0; + + GList* it; + for( it=blocks; it; it=it->next ) // examine all the blocks { - stg_point_t* pt = &block->pts[p]; - if( pt->x < minx ) minx = pt->x; - if( pt->y < miny ) miny = pt->y; - if( pt->x > maxx ) maxx = pt->x; - if( pt->y > maxy ) maxy = pt->y; + // examine all the points in the polygon + StgBlock* block = (StgBlock*)it->data; - assert( ! isnan( pt->x ) ); - assert( ! isnan( pt->y ) ); - } + block->UnMap(); // just in case - - if( block->zmax > maxz ) - maxz = block->zmax; - } + for( unsigned int p=0; p < block->pt_count; p++ ) + { + stg_point_t* pt = &block->pts[p]; + if( pt->x < minx ) minx = pt->x; + if( pt->y < miny ) miny = pt->y; + if( pt->x > maxx ) maxx = pt->x; + if( pt->y > maxy ) maxy = pt->y; - // now normalize all lengths so that the lines all fit inside - // the specified rectangle - double scalex = (maxx - minx); - double scaley = (maxy - miny); + assert( ! isnan( pt->x ) ); + assert( ! isnan( pt->y ) ); + } - double scalez = size->z / maxz; - for( it=blocks; it; it=it->next ) // examine all the blocks again - { - StgBlock* block = (StgBlock*)it->data; - - // scale all the points in the block - // TODO - scaling data in the block instead? - for( unsigned int p=0; p < block->pt_count; p++ ) - { - stg_point_t* pt = &block->pts[p]; - - pt->x = ((pt->x - minx) / scalex * size->x) - size->x/2.0; - pt->y = ((pt->y - miny) / scaley * size->y) - size->y/2.0; + if( block->zmax > maxz ) + maxz = block->zmax; + } - assert( ! isnan( pt->x ) ); - assert( ! isnan( pt->y ) ); + // now normalize all lengths so that the lines all fit inside + // the specified rectangle + double scalex = (maxx - minx); + double scaley = (maxy - miny); + + double scalez = size->z / maxz; + + for( it=blocks; it; it=it->next ) // examine all the blocks again + { + StgBlock* block = (StgBlock*)it->data; + + // scale all the points in the block + // TODO - scaling data in the block instead? + for( unsigned int p=0; p < block->pt_count; p++ ) + { + stg_point_t* pt = &block->pts[p]; + + pt->x = ((pt->x - minx) / scalex * size->x) - size->x/2.0; + pt->y = ((pt->y - miny) / scaley * size->y) - size->y/2.0; + + assert( ! isnan( pt->x ) ); + assert( ! isnan( pt->y ) ); + } + + // todo - scale min properly + block->zmax *= scalez; + block->zmin *= scalez; } - - // todo - scale min properly - block->zmax *= scalez; - block->zmin *= scalez; - } } Modified: code/stage/trunk/libstage/blockgrid.cc =================================================================== --- code/stage/trunk/libstage/blockgrid.cc 2008-06-10 18:11:59 UTC (rev 6523) +++ code/stage/trunk/libstage/blockgrid.cc 2008-06-10 18:23:46 UTC (rev 6524) @@ -9,88 +9,88 @@ StgBlockGrid::StgBlockGrid( uint32_t width, uint32_t height ) { - this->width = width; - this->height = height; - this->cells = (GSList**)calloc( sizeof(GList*), width * height ); + this->width = width; + this->height = height; + this->cells = (GSList**)calloc( sizeof(GList*), width * height ); - assert(this->cells); + assert(this->cells); } StgBlockGrid::~StgBlockGrid() { - for( uint32_t x=0; x<width; x++ ) - for( uint32_t y=0; y<height; y++ ) - { - if( cells[x + y*width] ) - g_slist_free( cells[x+y*width] ); - } - - delete[] cells; - //delete[] map; + for( uint32_t x=0; x<width; x++ ) + for( uint32_t y=0; y<height; y++ ) + { + if( cells[x + y*width] ) + g_slist_free( cells[x+y*width] ); + } + + delete[] cells; + //delete[] map; } void StgBlockGrid::AddBlock( uint32_t x, uint32_t y, StgBlock* block ) { - //printf( "add block %u %u\n", x, y ); + //printf( "add block %u %u\n", x, y ); - if( x < width && y < height ) - { - cells[ x+y*width ] = g_slist_prepend( cells[ x+y*width ], block ); - block->RecordRenderPoint( x, y ); - } + if( x < width && y < height ) + { + cells[ x+y*width ] = g_slist_prepend( cells[ x+y*width ], block ); + block->RecordRenderPoint( x, y ); + } } void StgBlockGrid::RemoveBlock( uint32_t x, uint32_t y, StgBlock* block ) { - //printf( "remove block %u %u\n", x, y ); - - if( x < width && y < height ) - { - cells[ x+y*width ] = g_slist_remove( cells[ x+y*width ], block ); - } + //printf( "remove block %u %u\n", x, y ); + + if( x < width && y < height ) + { + cells[ x+y*width ] = g_slist_remove( cells[ x+y*width ], block ); + } } GSList* StgBlockGrid::GetList( uint32_t x, uint32_t y ) { - if( x < width && y < height ) - return cells[ x+y*width ]; - else - return NULL; + if( x < width && y < height ) + return cells[ x+y*width ]; + else + return NULL; } void StgBlockGrid::GlobalRemoveBlock( StgBlock* block ) { - for( uint32_t x=0; x<width; x++ ) - for( uint32_t y=0; y<height; y++ ) - RemoveBlock(x,y,block ); + for( uint32_t x=0; x<width; x++ ) + for( uint32_t y=0; y<height; y++ ) + RemoveBlock(x,y,block ); } void StgBlockGrid::Draw( bool drawall ) { - for( uint32_t x=0; x<width; x++ ) - for( uint32_t y=0; y<height; y++ ) - { - //stg_bigblock_t* bb = &map[ x + y*bwidth ]; - - //if( drawall || bb->lists ) -// { -// glRecti( x<<numbits,y<<numbits,(x+1)<<numbits,(y+1)<<numbits ); - -// if( bb->lists ) -// for( uint32_t a=0; a<NSQR; a++ ) -// { -// if( drawall || bb->lists[ a ] ) -// glRecti( (x<<numbits)+a%NSIZE, -// (y<<numbits)+a/NSIZE, -// (x<<numbits)+a%NSIZE+1, -// (y<<numbits)+a/NSIZE+1 ); - -// } -// } + for( uint32_t x=0; x<width; x++ ) + for( uint32_t y=0; y<height; y++ ) + { + //stg_bigblock_t* bb = &map[ x + y*bwidth ]; - if( cells[x+y*width] ) - glRecti( x,y, x+1, y+ 1 ); - - } + //if( drawall || bb->lists ) + // { + // glRecti( x<<numbits,y<<numbits,(x+1)<<numbits,(y+1)<<numbits ); + + // if( bb->lists ) + // for( uint32_t a=0; a<NSQR; a++ ) + // { + // if( drawall || bb->lists[ a ] ) + // glRecti( (x<<numbits)+a%NSIZE, + // (y<<numbits)+a/NSIZE, + // (x<<numbits)+a%NSIZE+1, + // (y<<numbits)+a/NSIZE+1 ); + + // } + // } + + if( cells[x+y*width] ) + glRecti( x,y, x+1, y+ 1 ); + + } } Modified: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc 2008-06-10 18:11:59 UTC (rev 6523) +++ code/stage/trunk/libstage/camera.cc 2008-06-10 18:23:46 UTC (rev 6524) @@ -18,33 +18,33 @@ { glMatrixMode (GL_MODELVIEW); glLoadIdentity (); - + glRotatef( - _pitch, 1.0, 0.0, 0.0 ); glRotatef( - _yaw, 0.0, 0.0, 1.0 ); - + std::cout << "y: " << _z << std::endl; glTranslatef( - _x, - _y, - _z ); //zooming needs to happen in the Projection code (don't use glScale for zoom) - + } void StgPerspectiveCamera::SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const { glMatrixMode (GL_PROJECTION); glLoadIdentity (); - + gluPerspective( 60.0, pixels_width/pixels_height, 0.01, 100 ); - + glMatrixMode (GL_MODELVIEW); } void StgPerspectiveCamera::update( void ) { -// _x = 0; -// _y = 0; -// _z = i; + // _x = 0; + // _y = 0; + // _z = i; _pitch = 90.0; -// _yaw = 90.0; + // _yaw = 90.0; } @@ -61,24 +61,24 @@ { glMatrixMode (GL_MODELVIEW); glLoadIdentity (); - + glRotatef( _pitch, 1.0, 0.0, 0.0 ); glRotatef( _yaw, 0.0, 0.0, 1.0 ); - + glTranslatef( - _x, - _y, 0.0 ); //zooming needs to happen in the Projection code (don't use glScale for zoom) - + } void StgOrthoCamera::SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const { glMatrixMode (GL_PROJECTION); glLoadIdentity (); - + glOrtho( -pixels_width/2.0 / _scale, pixels_width/2.0 / _scale, -pixels_height/2.0 / _scale, pixels_height/2.0 / _scale, y_min * _scale * 2, y_max * _scale * 2 ); - + glMatrixMode (GL_MODELVIEW); } @@ -87,27 +87,27 @@ { float to_scale = -scale; const float old_scale = _scale; - + //TODO setting up the factor can use some work float factor = 1.0 + fabs( to_scale ) / 25; if( factor < 1.1 ) factor = 1.1; //this must be greater than 1. else if( factor > 2.5 ) factor = 2.5; - + //convert the shift distance to the range [-0.5, 0.5] shift_x = shift_x / w - 0.5; shift_y = shift_y / h - 0.5; - + //adjust the shift values based on the factor (this represents how much the positions grows/shrinks) shift_x *= factor - 1.0; shift_y *= factor - 1.0; - + if( to_scale > 0 ) { //zoom in _scale *= factor; move( shift_x * w / _scale * _scale, - - shift_y * h / _scale * _scale ); + - shift_y * h / _scale * _scale ); } else { //zoom out @@ -117,7 +117,7 @@ } else { //shift camera to follow where mouse zoomed out move( - shift_x * w / old_scale * _scale, - shift_y * h / old_scale * _scale ); + shift_y * h / old_scale * _scale ); } } } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-10 18:11:59 UTC (rev 6523) +++ code/stage/trunk/libstage/canvas.cc 2008-06-10 18:23:46 UTC (rev 6524) @@ -1,8 +1,8 @@ /** canvas.cc - Implement the main world viewing area in FLTK and OpenGL. - Author: Richard Vaughan (va...@sf...) - $Id: canvas.cc,v 1.12 2008-03-03 07:01:12 rtv Exp $ -*/ + Implement the main world viewing area in FLTK and OpenGL. +Author: Richard Vaughan (va...@sf...) +$Id: canvas.cc,v 1.12 2008-03-03 07:01:12 rtv Exp $ + */ #include "stage_internal.hh" #include "texture_manager.hh" @@ -12,41 +12,41 @@ void StgCanvas::TimerCallback( StgCanvas* c ) { - c->redraw(); - - Fl::repeat_timeout(((double)c->interval/1000), - (Fl_Timeout_Handler)StgCanvas::TimerCallback, - c); + c->redraw(); + + Fl::repeat_timeout(((double)c->interval/1000), + (Fl_Timeout_Handler)StgCanvas::TimerCallback, + c); } -StgCanvas::StgCanvas( StgWorld* world, int x, int y, int w, int h) - : Fl_Gl_Window(x,y,w,h) + StgCanvas::StgCanvas( StgWorld* world, int x, int y, int w, int h) +: Fl_Gl_Window(x,y,w,h) { - end(); - //show(); // must do this so that the GL context is created before configuring GL - // but that line causes a segfault in Linux/X11! TODO: test in OS X + end(); + //show(); // must do this so that the GL context is created before configuring GL + // but that line causes a segfault in Linux/X11! TODO: test in OS X - this->world = world; - selected_models = NULL; - last_selection = NULL; + this->world = world; + selected_models = NULL; + last_selection = NULL; - startx = starty = 0; - //panx = pany = stheta = sphi = 0.0; - //scale = 15.0; - interval = 50; //msec between redraws + startx = starty = 0; + //panx = pany = stheta = sphi = 0.0; + //scale = 15.0; + interval = 50; //msec between redraws - graphics = true; - dragging = false; - rotating = false; + graphics = true; + dragging = false; + rotating = false; - showflags = STG_SHOW_CLOCK | STG_SHOW_BLOCKS | STG_SHOW_GRID | STG_SHOW_DATA; + showflags = STG_SHOW_CLOCK | STG_SHOW_BLOCKS | STG_SHOW_GRID | STG_SHOW_DATA; - // start the timer that causes regular redraws - Fl::add_timeout( ((double)interval/1000), - (Fl_Timeout_Handler)StgCanvas::TimerCallback, - this); - } + // start the timer that causes regular redraws + Fl::add_timeout( ((double)interval/1000), + (Fl_Timeout_Handler)StgCanvas::TimerCallback, + this); +} StgCanvas::~StgCanvas() { @@ -54,350 +54,350 @@ void StgCanvas::InvertView( uint32_t invertflags ) { - showflags = (showflags ^ invertflags); + showflags = (showflags ^ invertflags); -// printf( "flags %u data %d grid %d blocks %d follow %d clock %d tree %d occ %d\n", -// showflags, -// showflags & STG_SHOW_DATA, -// showflags & STG_SHOW_GRID, -// showflags & STG_SHOW_BLOCKS, -// showflags & STG_SHOW_FOLLOW, -// showflags & STG_SHOW_CLOCK, -// showflags & STG_SHOW_QUADTREE, -// showflags & STG_SHOW_OCCUPANCY ); + // printf( "flags %u data %d grid %d blocks %d follow %d clock %d tree %d occ %d\n", + // showflags, + // showflags & STG_SHOW_DATA, + // showflags & STG_SHOW_GRID, + // showflags & STG_SHOW_BLOCKS, + // showflags & STG_SHOW_FOLLOW, + // showflags & STG_SHOW_CLOCK, + // showflags & STG_SHOW_QUADTREE, + // showflags & STG_SHOW_OCCUPANCY ); } StgModel* StgCanvas::Select( int x, int y ) { - // render all models in a unique color - make_current(); // make sure the GL context is current - glClearColor ( 0,0,0,1 ); - glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - glDisable(GL_DITHER); - - // render all top-level, draggable models in a color that is their - // id + a 100% alpha value - for( GList* it=world->children; it; it=it->next ) - { - StgModel* mod = (StgModel*)it->data; + // render all models in a unique color + make_current(); // make sure the GL context is current + glClearColor ( 0,0,0,1 ); + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + glDisable(GL_DITHER); - if( mod->GuiMask() & (STG_MOVE_TRANS | STG_MOVE_ROT )) + // render all top-level, draggable models in a color that is their + // id + a 100% alpha value + for( GList* it=world->children; it; it=it->next ) { - uint32_t col = (mod->Id() | 0xFF000000); - glColor4ubv( (GLubyte*)&col ); - mod->DrawPicker(); + StgModel* mod = (StgModel*)it->data; + + if( mod->GuiMask() & (STG_MOVE_TRANS | STG_MOVE_ROT )) + { + uint32_t col = (mod->Id() | 0xFF000000); + glColor4ubv( (GLubyte*)&col ); + mod->DrawPicker(); + } } - } - - // read the color of the pixel in the back buffer under the mouse - // pointer - GLint viewport[4]; - glGetIntegerv(GL_VIEWPORT,viewport); - - uint32_t id; - glReadPixels( x,viewport[3]-y,1,1, - GL_RGBA,GL_UNSIGNED_BYTE,(void*)&id ); - - // strip off the alpha channel byte to retrieve the model id - id &= 0x00FFFFFF; - - StgModel* mod = world->GetModel( id ); - - //printf("%p %s %d\n", mod, mod ? mod->Token() : "", id ); - - glEnable(GL_DITHER); - glClearColor ( 0.7, 0.7, 0.8, 1.0); - //glClearColor ( 1,1,1,1 ); - - if( mod ) // we clicked on a root model - { - // if it's already selected - if( GList* link = g_list_find( selected_models, mod ) ) + + // read the color of the pixel in the back buffer under the mouse + // pointer + GLint viewport[4]; + glGetIntegerv(GL_VIEWPORT,viewport); + + uint32_t id; + glReadPixels( x,viewport[3]-y,1,1, + GL_RGBA,GL_UNSIGNED_BYTE,(void*)&id ); + + // strip off the alpha channel byte to retrieve the model id + id &= 0x00FFFFFF; + + StgModel* mod = world->GetModel( id ); + + //printf("%p %s %d\n", mod, mod ? mod->Token() : "", id ); + + glEnable(GL_DITHER); + glClearColor ( 0.7, 0.7, 0.8, 1.0); + //glClearColor ( 1,1,1,1 ); + + if( mod ) // we clicked on a root model { - // remove it from the selected list - selected_models = - g_list_remove_link( selected_models, link ); + // if it's already selected + if( GList* link = g_list_find( selected_models, mod ) ) + { + // remove it from the selected list + selected_models = + g_list_remove_link( selected_models, link ); - mod->Disable(); - } - else - { - last_selection = mod; - selected_models = g_list_prepend( selected_models, mod ); - mod->Enable(); + mod->Disable(); + } + else + { + last_selection = mod; + selected_models = g_list_prepend( selected_models, mod ); + mod->Enable(); + } + + invalidate(); } - - invalidate(); - } - return mod; + return mod; } // convert from 2d window pixel to 3d world coordinates void StgCanvas::CanvasToWorld( int px, int py, - double *wx, double *wy, double* wz ) + double *wx, double *wy, double* wz ) { - int viewport[4]; - glGetIntegerv(GL_VIEWPORT, viewport); - - GLdouble modelview[16]; - glGetDoublev(GL_MODELVIEW_MATRIX, modelview); - - GLdouble projection[16]; - glGetDoublev(GL_PROJECTION_MATRIX, projection); - - GLfloat pz; - glReadPixels( px, h()-py, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &pz ); - gluUnProject( px, w()-py, pz, modelview, projection, viewport, wx,wy,wz ); + int viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + + GLdouble modelview[16]; + glGetDoublev(GL_MODELVIEW_MATRIX, modelview); + + GLdouble projection[16]; + glGetDoublev(GL_PROJECTION_MATRIX, projection); + + GLfloat pz; + glReadPixels( px, h()-py, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &pz ); + gluUnProject( px, w()-py, pz, modelview, projection, viewport, wx,wy,wz ); } int StgCanvas::handle(int event) { - - switch(event) - { - case FL_MOUSEWHEEL: - if( selected_models ) + + switch(event) { - // rotate all selected models - for( GList* it = selected_models; it; it=it->next ) - { - StgModel* mod = (StgModel*)it->data; - mod->AddToPose( 0,0,0, 0.1*(double)Fl::event_dy() ); - } - redraw(); - } - else - { - camera.scale( Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h() ); - invalidate(); - } - return 1; + case FL_MOUSEWHEEL: + if( selected_models ) + { + // rotate all selected models + for( GList* it = selected_models; it; it=it->next ) + { + StgModel* mod = (StgModel*)it->data; + mod->AddToPose( 0,0,0, 0.1*(double)Fl::event_dy() ); + } + redraw(); + } + else + { + camera.scale( Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h() ); + invalidate(); + } + return 1; - case FL_MOVE: // moused moved while no button was pressed - if( Fl::event_state( FL_CTRL ) ) - { - int dx = Fl::event_x() - startx; - int dy = Fl::event_y() - starty; - - camera.pitch( 0.5 * static_cast<double>( dy ) ); - camera.yaw( 0.5 * static_cast<double>( dx ) ); - - invalidate(); - } - else if( Fl::event_state( FL_ALT ) ) - { - int dx = Fl::event_x() - startx; - int dy = Fl::event_y() - starty; - - camera.move( -dx, dy ); - invalidate(); - } - - startx = Fl::event_x(); - starty = Fl::event_y(); - return 1; + case FL_MOVE: // moused moved while no button was pressed + if( Fl::event_state( FL_CTRL ) ) + { + int dx = Fl::event_x() - startx; + int dy = Fl::event_y() - starty; - case FL_PUSH: // button pressed - switch( Fl::event_button() ) - { - case 1: - startx = Fl::event_x(); - starty = Fl::event_y(); - if( Select( startx, starty ) ) - dragging = true; - return 1; - case 3: - { - puts( "button 3" ); - startx = Fl::event_x(); - starty = Fl::event_y(); - if( Select( startx, starty ) ) - { - printf( "rotating" ); - rotating = true; - } - return 1; - } - default: - return 0; - } - - case FL_DRAG: // mouse moved while button was pressed - { - int dx = Fl::event_x() - startx; - int dy = Fl::event_y() - starty; - - switch( Fl::event_button() ) - { - case 1: - if( dragging ) - { - assert(selected_models); - - double sx,sy,sz; - CanvasToWorld( startx, starty, - &sx, &sy, &sz ); - double x,y,z; - CanvasToWorld( Fl::event_x(), Fl::event_y(), - &x, &y, &z ); - - // move all selected models to the mouse pointer - for( GList* it = selected_models; it; it=it->next ) - { - StgModel* mod = (StgModel*)it->data; - mod->AddToPose( x-sx, y-sy, 0, 0 ); - } - } - else - { - camera.move( -dx, dy ); - invalidate(); // so the projection gets updated - } - break; - case 3: // right button - if( rotating ) - { - // move all selected models to the mouse pointer - for( GList* it = selected_models; it; it=it->next ) - { - StgModel* mod = (StgModel*)it->data; - mod->AddToPose( 0,0,0, 0.05*dx ); - } - } - break; - } - } - startx = Fl::event_x(); - starty = Fl::event_y(); - return 1; // end case FL_DRAG - - case FL_RELEASE: // mouse button released - // unselect everyone unless shift is pressed - if( ! Fl::event_state( FL_SHIFT ) ) - { - for( GList* it=selected_models; it; it=it->next ) - ((StgModel*)it->data)->Enable(); - - g_list_free( selected_models ); - selected_models = NULL; - dragging = false; - rotating = false; - redraw(); - } - return 1; + camera.pitch( 0.5 * static_cast<double>( dy ) ); + camera.yaw( 0.5 * static_cast<double>( dx ) ); - case FL_FOCUS : - case FL_UNFOCUS : - //.... Return 1 if you want keyboard events, 0 otherwise - return 1; - case FL_KEYBOARD: - switch( Fl::event_key() ) - { - case 'p': // pause - world->TogglePause(); - break; - case ' ': // space bar + invalidate(); + } + else if( Fl::event_state( FL_ALT ) ) + { + int dx = Fl::event_x() - startx; + int dy = Fl::event_y() - starty; - camera.resetAngle(); - invalidate(); - break; - case FL_Left: camera.move( -10, 0 ); break; - case FL_Right: camera.move( 10, 0 );; break; - case FL_Down: camera.move( 0, -10 );; break; - case FL_Up: camera.move( 0, 10 );; break; - default: - return 0; // keypress unhandled + camera.move( -dx, dy ); + invalidate(); + } + + startx = Fl::event_x(); + starty = Fl::event_y(); + return 1; + + case FL_PUSH: // button pressed + switch( Fl::event_button() ) + { + case 1: + startx = Fl::event_x(); + starty = Fl::event_y(); + if( Select( startx, starty ) ) + dragging = true; + return 1; + case 3: + { + puts( "button 3" ); + startx = Fl::event_x(); + starty = Fl::event_y(); + if( Select( startx, starty ) ) + { + printf( "rotating" ); + rotating = true; + } + return 1; + } + default: + return 0; + } + + case FL_DRAG: // mouse moved while button was pressed + { + int dx = Fl::event_x() - startx; + int dy = Fl::event_y() - starty; + + switch( Fl::event_button() ) + { + case 1: + if( dragging ) + { + assert(selected_models); + + double sx,sy,sz; + CanvasToWorld( startx, starty, + &sx, &sy, &sz ); + double x,y,z; + CanvasToWorld( Fl::event_x(), Fl::event_y(), + &x, &y, &z ); + + // move all selected models to the mouse pointer + for( GList* it = selected_models; it; it=it->next ) + { + StgModel* mod = (StgModel*)it->data; + mod->AddToPose( x-sx, y-sy, 0, 0 ); + } + } + else + { + camera.move( -dx, dy ); + invalidate(); // so the projection gets updated + } + break; + case 3: // right button + if( rotating ) + { + // move all selected models to the mouse pointer + for( GList* it = selected_models; it; it=it->next ) + { + StgModel* mod = (StgModel*)it->data; + mod->AddToPose( 0,0,0, 0.05*dx ); + } + } + break; + } + } + startx = Fl::event_x(); + starty = Fl::event_y(); + return 1; // end case FL_DRAG + + case FL_RELEASE: // mouse button released + // unselect everyone unless shift is pressed + if( ! Fl::event_state( FL_SHIFT ) ) + { + for( GList* it=selected_models; it; it=it->next ) + ((StgModel*)it->data)->Enable(); + + g_list_free( selected_models ); + selected_models = NULL; + dragging = false; + rotating = false; + redraw(); + } + return 1; + + case FL_FOCUS : + case FL_UNFOCUS : + //.... Return 1 if you want keyboard events, 0 otherwise + return 1; + case FL_KEYBOARD: + switch( Fl::event_key() ) + { + case 'p': // pause + world->TogglePause(); + break; + case ' ': // space bar + + camera.resetAngle(); + invalidate(); + break; + case FL_Left: camera.move( -10, 0 ); break; + case FL_Right: camera.move( 10, 0 );; break; + case FL_Down: camera.move( 0, -10 );; break; + case FL_Up: camera.move( 0, 10 );; break; + default: + return 0; // keypress unhandled + } + invalidate(); // update projection + return 1; + //case FL_SHORTCUT: + ///... shortcut, key is in Fl::event_key(), ascii in Fl::event_text() + // ... Return 1 if you understand/use the shortcut event, 0 otherwise... + //return 1; + default: + // pass other events to the base class... + //printf( "EVENT %d\n", event ); + return Fl_Gl_Window::handle(event); } - invalidate(); // update projection - return 1; - //case FL_SHORTCUT: - ///... shortcut, key is in Fl::event_key(), ascii in Fl::event_text() - // ... Return 1 if you understand/use the shortcut event, 0 otherwise... - //return 1; - default: - // pass other events to the base class... - //printf( "EVENT %d\n", event ); - return Fl_Gl_Window::handle(event); - } } void StgCanvas::FixViewport(int W,int H) { - glLoadIdentity(); - glViewport(0,0,W,H); + glLoadIdentity(); + glViewport(0,0,W,H); } void StgCanvas::DrawGlobalGrid() { - PushColor( 0,0,0,0.2 ); - gl_draw_grid( world->GetExtent() ); - PopColor(); + PushColor( 0,0,0,0.2 ); + gl_draw_grid( world->GetExtent() ); + PopColor(); } void StgCanvas::renderFrame() { - + if( ! (showflags & STG_SHOW_TRAILS) ) glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - + // if following selected, shift the view to above the selected robot if( (showflags & STG_SHOW_FOLLOW) && last_selection ) - { + { glLoadIdentity (); double zclip = 20 * camera.getScale(); //hypot(world->Width(), world->Height()) * camera.getScale(); glTranslatef( 0,0, - -zclip / 2.0 ); - + -zclip / 2.0 ); + // meter scale glScalef ( camera.getScale(), camera.getScale(), camera.getScale() ); // zoom - + stg_pose_t gpose = last_selection->GetGlobalPose(); - + // and put it in the center of the window //glRotatef( -rtod(gpose.a), 0,0,1 ); glTranslatef( -gpose.x, -gpose.y, 0 ); } - + glPushMatrix(); - + // draw the world size rectangle in white, using the polygon offset // so it doesn't z-fight with the models glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(2.0, 2.0); - + glScalef( 1.0/world->Resolution(), 1.0/world->Resolution(), 0 ); ((StgWorldGui*)world)->DrawFloor(); - + glDisable(GL_POLYGON_OFFSET_FILL); - + if( (showflags & STG_SHOW_QUADTREE) || (showflags & STG_SHOW_OCCUPANCY) ) { glDisable( GL_LINE_SMOOTH ); glLineWidth( 1 ); glPolygonMode( GL_FRONT, GL_LINE ); colorstack.Push(1,0,0); - + if( showflags & STG_SHOW_OCCUPANCY ) ((StgWorldGui*)world)->DrawTree( false ); - + if( showflags & STG_SHOW_QUADTREE ) ((StgWorldGui*)world)->DrawTree( true ); - + colorstack.Pop(); - + glEnable( GL_LINE_SMOOTH ); } - + glPopMatrix(); - + if( showflags & STG_SHOW_GRID ) DrawGlobalGrid(); - + for( GList* it=selected_models; it; it=it->next ) ((StgModel*)it->data)->DrawSelected(); - + // draw the models if( showflags ) // if any bits are set there's something to draw { @@ -405,24 +405,24 @@ { glDisable( GL_DEPTH_TEST ); glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); - + for( GList* it=world->children; it; it=it->next ) { ((StgModel*)it->data)->DrawTrailFootprint(); } glEnable( GL_DEPTH_TEST ); } - + if( showflags & STG_SHOW_TRAILRISE ) { glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); - + for( GList* it=world->children; it; it=it->next ) { ((StgModel*)it->data)->DrawTrailBlocks(); } } - + if( showflags & STG_SHOW_ARROWS ) { glEnable( GL_DEPTH_TEST ); @@ -431,38 +431,38 @@ ((StgModel*)it->data)->DrawTrailArrows(); } } - + if( showflags & STG_SHOW_BLOCKS ) { for( GList* it=world->children; it; it=it->next ) { StgModel* mod = ((StgModel*)it->data); - + if( mod->displaylist == 0 ) { mod->displaylist = glGenLists(1); mod->BuildDisplayList( showflags ); // ready to be rendered } - + // move into this model's local coordinate frame glPushMatrix(); gl_pose_shift( &mod->pose ); gl_pose_shift( &mod->geom.pose ); - + // render the pre-recorded graphics for this model and // its children glCallList( mod->displaylist ); - + glPopMatrix(); } } - + //mod->Draw( showflags ); // draw the stuff that changes every update // draw everything else for( GList* it=world->children; it; it=it->next ) ((StgModel*)it->data)->Draw( showflags, this ); } - + if( world->GetRayList() ) { glDisable( GL_DEPTH_TEST ); @@ -477,38 +477,38 @@ } PopColor(); glEnable( GL_DEPTH_TEST ); - + world->ClearRays(); } - + if( showflags & STG_SHOW_CLOCK ) - { + { //use orthogonal projeciton without any zoom glMatrixMode (GL_PROJECTION); glPushMatrix(); //save old projection glLoadIdentity (); glOrtho( -w()/2.0, w()/2.0, -h()/2.0, h()/2.0, -100, 100 ); glMatrixMode (GL_MODELVIEW); - + glPushMatrix(); glLoadIdentity(); glDisable( GL_DEPTH_TEST ); - + // if trails are on, we need to clear the clock background - + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - + colorstack.Push( 0.8,0.8,1.0 ); // pale blue glRectf( -w()/2, -h()/2, -w()/2 +120, -h()/2+20 ); colorstack.Pop(); - + char clockstr[50]; world->ClockString( clockstr, 50 ); - + colorstack.Push( 0,0,0 ); // black gl_draw_string( -w()/2+4, -h()/2+4, 5, clockstr ); colorstack.Pop(); - + glEnable( GL_DEPTH_TEST ); glPopMatrix(); @@ -516,70 +516,70 @@ glMatrixMode (GL_PROJECTION); glPopMatrix(); glMatrixMode (GL_MODELVIEW); - } - + } + } void StgCanvas::draw() { static bool loaded_texture = false; - // static int centerx = 0, centery = 0; - //puts( "CANVAS" ); - - if (!valid()) - { - valid(1); - FixViewport(w(), h()); + // static int centerx = 0, centery = 0; + //puts( "CANVAS" ); - // set gl state that won't change every redraw - glClearColor ( 0.7, 0.7, 0.8, 1.0); - //glClearColor ( 1,1,1,1 ); - glDisable(GL_LIGHTING); - glEnable (GL_DEPTH_TEST); - glDepthFunc (GL_LESS); - glCullFace( GL_BACK ); - glEnable (GL_CULL_FACE); - glEnable( GL_BLEND ); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA ); - glEnable( GL_LINE_SMOOTH ); - glHint (GL_LINE_SMOOTH_HINT, GL_FASTEST); - glDepthMask(GL_TRUE); + if (!valid()) + { + valid(1); + FixViewport(w(), h()); + + // set gl state that won't change every redraw + glClearColor ( 0.7, 0.7, 0.8, 1.0); + //glClearColor ( 1,1,1,1 ); + glDisable(GL_LIGHTING); + glEnable (GL_DEPTH_TEST); + glDepthFunc (GL_LESS); + glCullFace( GL_BACK ); + glEnable (GL_CULL_FACE); + glEnable( GL_BLEND ); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA ); + glEnable( GL_LINE_SMOOTH ); + glHint (GL_LINE_SMOOTH_HINT, GL_FASTEST); + glDepthMask(GL_TRUE); glEnable( GL_TEXTURE_2D ); - - + + //TODO find a better home for loading textures if( loaded_texture == false ) { GLuint stall_id = TextureManager::getInstance().loadTexture( "assets/stall.png" ); TextureManager::getInstance()._stall_texture_id = stall_id; - + loaded_texture = true; } - - // install a font - gl_font( FL_HELVETICA, 12 ); - + + // install a font + gl_font( FL_HELVETICA, 12 ); + stg_bounds3d_t extent = world->GetExtent(); camera.SetProjection( w(), h(), extent.y.min, extent.y.max ); camera.Draw(); - - + + // enable vertex arrays - glEnableClientState( GL_VERTEX_ARRAY ); - //glEnableClientState( GL_COLOR_ARRAY ); + glEnableClientState( GL_VERTEX_ARRAY ); + //glEnableClientState( GL_COLOR_ARRAY ); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - } - + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + } + renderFrame(); } void StgCanvas::resize(int X,int Y,int W,int H) { - Fl_Gl_Window::resize(X,Y,W,H); - FixViewport(W,H); - invalidate(); + Fl_Gl_Window::resize(X,Y,W,H); + FixViewport(W,H); + invalidate(); } Modified: code/stage/trunk/libstage/ctrl.cc =================================================================== --- code/stage/trunk/libstage/ctrl.cc 2008-06-10 18:11:59 UTC (rev 6523) +++ code/stage/trunk/libstage/ctrl.cc 2008-06-10 18:23:46 UTC (rev 6524) @@ -9,9 +9,9 @@ typedef struct { - StgModelPosition* pos; - StgModelLaser* laser; - int avoidcount, randcount; + StgModelPosition* pos; + StgModelLaser* laser; + int avoidcount, randcount; } robot_t; int LaserUpdate( StgModel* mod, robot_t* robot ); @@ -20,102 +20,102 @@ // Stage calls this when the model starts up extern "C" int Init( StgModel* mod ) { - robot_t* robot = new robot_t; - - robot->pos = (StgModelPosition*)mod; - robot->laser = (StgModelLaser*)mod->GetModel( "laser:0" ); - robot->avoidcount = 0; - robot->randcount = 0; + robot_t* robot = new robot_t; - assert( robot->laser ); - robot->laser->Subscribe(); - - robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot ); - //robot->pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, robot ); - return 0; //ok + robot->pos = (StgModelPosition*)mod; + robot->laser = (StgModelLaser*)mod->GetModel( "laser:0" ); + robot->avoidcount = 0; + robot->randcount = 0; + + assert( robot->laser ); + robot->laser->Subscribe(); + + robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot ); + //robot->pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, robot ); + return 0; //ok } // inspect the laser data and decide what to do int LaserUpdate( StgModel* mod, robot_t* robot ) { - // get the data - uint32_t sample_count=0; - stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count ); - assert(scan); - - double newturnrate=0.0, newspeed=0.0; - bool obstruction = false; - - // find the closest distance to the left and right and check if - // there's anything in front - double minleft = 1e6; - double minright = 1e6; - - for (uint32_t i = 0; i < sample_count; i++) - { - if( scan[i].range < minfrontdistance) - obstruction = true; - - if( i > sample_count/2 ) - minleft = MIN( minleft, scan[i].range ); - else - minright = MIN( minright, scan[i].range ); - } - - if( obstruction || robot->avoidcount ) - { - if( verbose ) puts( "Avoid" ); + // get the data + uint32_t sample_count=0; + stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count ); + assert(scan); - robot->pos->SetXSpeed( avoidspeed ); - - /* once we start avoiding, select a turn direction and stick - with it for a few iterations */ - if( robot->avoidcount == 0 ) - { - if( verbose ) puts( "Avoid START" ); - robot->avoidcount = 5; - - if( minleft < minright ) - robot->pos->SetTurnSpeed( -avoidturn ); - else - robot->pos->SetTurnSpeed( +avoidturn ); - } + double newturnrate=0.0, newspeed=0.0; + bool obstruction = false; - robot->avoidcount--; - } - else - { - if( verbose ) puts( "Cruise" ); + // find the closest distance to the left and right and check if + // there's anything in front + double minleft = 1e6; + double minright = 1e6; - robot->avoidcount = 0; - robot->pos->SetXSpeed( cruisespeed ); - - /* update turnrate every few updates */ - if( robot->randcount == 0 ) + for (uint32_t i = 0; i < sample_count; i++) { - if( verbose )puts( "Random turn" ); - - /* make random int tween -30 and 30 */ - //newturnrate = dtor( rand() % 61 - 30 ); - - robot->randcount = 20; - - robot->pos->SetTurnSpeed( dtor( rand() % 11 - 5 ) ); + if( scan[i].range < minfrontdistance) + obstruction = true; + + if( i > sample_count/2 ) + minleft = MIN( minleft, scan[i].range ); + else + minright = MIN( minright, scan[i].range ); } - - robot->randcount--; - } - - return 0; + + if( obstruction || robot->avoidcount ) + { + if( verbose ) puts( "Avoid" ); + + robot->pos->SetXSpeed( avoidspeed ); + + /* once we start avoiding, select a turn direction and stick + with it for a few iterations */ + if( robot->avoidcount == 0 ) + { + if( verbose ) puts( "Avoid START" ); + robot->avoidcount = 5; + + if( minleft < minright ) + robot->pos->SetTurnSpeed( -avoidturn ); + else + robot->pos->SetTurnSpeed( +avoidturn ); + } + + robot->avoidcount--; + } + else + { + if( verbose ) puts( "Cruise" ); + + robot->avoidcount = 0; + robot->pos->SetXSpeed( cruisespeed ); + + /* update turnrate every few updates */ + if( robot->randcount == 0 ) + { + if( verbose )puts( "Random turn" ); + + /* make random int tween -30 and 30 */ + //newturnrate = dtor( rand() % 61 - 30 ); + + robot->randcount = 20; + + robot->pos->SetTurnSpeed( dtor( rand() % 11 - 5 ) ); + } + + robot->randcount--; + } + + return 0; } int PositionUpdate( StgModel* mod, robot_t* robot ) { - stg_pose_t pose = robot->pos->GetPose(); + stg_pose_t pose = robot->pos->GetPose(); - printf( "Pose: [%.2f %.2f %.2f %.2f]\n", - pose.x, pose.y, pose.z, pose.a ); + printf( "Pose: [%.2f %.2f %.2f %.2f]\n", + pose.x, pose.y, pose.z, pose.a ); - return 0; // run again + return 0; // run again } Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2008-06-10 18:11:59 UTC (rev 6523) +++ code/stage/trunk/libstage/gl.cc 2008-06-10 18:23:46 UTC (rev 6524) @@ -4,78 +4,78 @@ // transform the current coordinate frame by the given pose void Stg::gl_coord_shift( double x, double y, double z, double a ) { - glTranslatef( x,y,z ); - glRotatef( rtod(a), 0,0,1 ); + glTranslatef( x,y,z ); + glRotatef( rtod(a), 0,0,1 ); } // transform the current coordinate frame by the given pose void Stg::gl_pose_shift( stg_pose_t* pose ) { - gl_coord_shift( pose->x, pose->y, pose->z, pose->a ); + gl_coord_shift( pose->x, pose->y, pose->z, pose->a ); } // TODO - this could be faster, but we don't draw a lot of text void Stg::gl_draw_string( float x, float y, float z, const char *str ) { - const char *c; - glRasterPos3f(x, y,z); - for (c=str; *c != '\0'; c++) - glutBitmapCharacter( GLUT_BITMAP_HELVETICA_12, *c); + const char *c; + glRasterPos3f(x, y,z); + for (c=str; *c != '\0'; c++) + glutBitmapCharacter( GLUT_BITMAP_HELVETICA_12, *c); } void Stg::gl_speech_bubble( float x, float y, float z, const char* str ) { - gl_draw_string( x, y, z, str ); + gl_draw_string( x, y, z, str ); } // draw an octagon with center rectangle dimensions w/h // and outside margin m void Stg::gl_draw_octagon( float w, float h, float m ) { - glBegin(GL_POLYGON); - glVertex2f( m+w, 0 ); - glVertex2f( w+2*m, m ); - glVertex2f( w+2*m, h+m ); - glVertex2f( m+w, h+2*m ); - glVertex2f( m, h+2*m ); - glVertex2f( 0, h+m ); - glVertex2f( 0, m ); - glVertex2f( m, 0 ); - glEnd(); + glBegin(GL_POLYGON); + glVertex2f( m+w, 0 ); + glVertex2f( w+2*m, m ); + glVertex2f( w+2*m, h+m ); + glVertex2f( m+w, h+2*m ); + glVertex2f( m, h+2*m ); + glVertex2f( 0, h+m ); + glVertex2f( 0, m ); + glVertex2f( m, 0 ); + glEnd(); } void Stg::gl_draw_grid( stg_bounds3d_t vol ) { - glBegin(GL_LINES); - - for( double i = floor(vol.x.min); i < vol.x.max; i++) - { - glVertex2f( i, vol.y.min ); - glVertex2f( i, vol.y.max ); - } - - for( double i = floor(vol.y.min); i < vol.y.max; i++) - { - glVertex2f( vol.x.min, i ); - glVertex2f( vol.x.max, i ); - } - - glEnd(); - - char str[16]; + glBegin(GL_LINES); - for( double i = floor(vol.x.min); i < vol.x.max; i++) - { - snprintf( str, 16, "%d", (int)i ); - gl_draw_string( i, 0, 0.00, str ); - } + for( double i = floor(vol.x.min); i < vol.x.max; i++) + { + glVertex2f( i, vol.y.min ); + glVertex2f( i, vol.y.max ); + } - for( double i = floor(vol.y.min); i < vol.y.max; i++) - { - snprintf( str, 16, "%d", (int)i ); - gl_draw_string( 0, i, 0.00, str ); - } + for( double i = floor(vol.y.min); i < vol.y.max; i++) + { + glVertex2f( vol.x.min, i ); + glVertex2f( vol.x.max, i ); + } + + glEnd(); + + char str[16]; + + for( double i = floor(vol.x.min); i < vol.x.max; i++) + { + snprintf( str, 16, "%d", (int)i ); + gl_draw_string( i, 0, 0.00, str ); + } + + for( double i = floor(vol.y.min); i < vol.y.max; i++) + { + snprintf( str, 16, "%d", (int)i ); + gl_draw_string( 0, i, 0.00, str ); + } } Modified: code/stage/trunk/libstage/glcolorstack.cc =================================================================== --- code/stage/trunk/libstage/glcolorstack.cc 2008-06-10 18:11:59 UTC (rev 6523) +++ code/stage/trunk/libstage/glcolorstack.cc 2008-06-10 18:23:46 UTC (rev 6524) @@ -2,73 +2,73 @@ GlColorStack::GlColorStack() { - colorstack = g_queue_new(); + colorstack = g_queue_new(); } GlColorStack::~GlColorStack() { - + } void GlColorStack::Push( GLdouble col[4] ) { - size_t sz = 4 * sizeof(col[0]); - GLdouble *keep = (GLdouble*)malloc( sz ); - memcpy( keep, col, sz ); - - g_queue_push_head( colorstack, keep ); - - // and select this color in GL - glColor4dv( col ); + size_t sz = 4 * sizeof(col[0]); + GLdouble *keep = (GLdouble*)malloc( sz ); + memcpy( keep, col, sz ); + + g_queue_push_head( colorstack, keep ); + + // and select this color in GL + glColor4dv( col ); } void GlColorStack::Push( double r, double g, double b ) { - GLdouble col[4]; - col[0] = r; - col[1] = g; - col[2] = b; - col[3] = 1.0; - - Push( col ); + GLdouble col[4]; + col[0] = r; + col[1] = g; + col[2] = b; + col[3] = 1.0; + + Push( col ); } // a convenience wrapper around push_color() void GlColorStack::Push( double r, double g, double b, double a ) { - GLdouble col[4]; - col[0] = r; - col[1] = g; - col[2] = b; - col[3] = a; - - Push( col ); + GLdouble col[4]; + col[0] = r; + col[1] = g; + col[2] = b; + col[3] = a; + + Push( col ); } void GlColorStack::Push( stg_color_t col ) { - GLdouble d[4]; + GLdouble d[4]; - d[0] = ((col & 0x00FF0000) >> 16) / 256.0; - d[1] = ((col & 0x0000FF00) >> 8) / 256.0; - d[2] = ((col & 0x000000FF) >> 0) / 256.0; - d[3] = (((col & 0xFF000000) >> 24) / 256.0); + d[0] = ((col & 0x00FF0000) >> 16) / 256.0; + d[1] = ((col & 0x0000FF00) >> 8) / 256.0; + d[2] = ((col & 0x000000FF) >> 0) / 256.0; + d[3] = (((col & 0xFF000000) >> 24) / 256.0); - Push( d ); + Push( d ); } // reset GL to the color we stored using store_color() void GlColorStack::Pop( void ) { - if( g_queue_is_empty( colorstack ) ) - PRINT_WARN1( "Attempted to ColorStack.Pop() but ColorStack %p is empty", - this ); - else - { - GLdouble *col = (GLdouble*)g_queue_pop_head( colorstack ); - glColor4dv( col ); - free( col ); - } + if( g_queue_is_empty( colorstack ) ) + PRINT_WARN1( "Attempted to ColorStack.Pop() but ColorStack %p is empty", + this ); + else + { + GLdouble *col = (GLdouble*)g_queue_pop_head( colorstack ); + glColor4dv( col ); + free( col ); + } } Modified: code/stage/trunk/libstage/glutgraphics.cc =================================================================== --- code/stage/trunk/libstage/glutgraphics.cc 2008-06-10 18:11:59 UTC (rev 6523) +++ code/stage/trunk/libstage/glutgraphics.cc 2008-06-10 18:23:46 UTC (rev 6524) @@ -29,9 +29,9 @@ // (e.g., HAVE_CFMAKERAW), then #include <config.h>, like so: /* #if HAVE_CONFIG_H - #include <config.h> +#include <config.h> #endif -*/ + */ #include <unistd.h> #include <string.h> @@ -58,117 +58,117 @@ float s = 0.0; GLfloat angle1 = 0.0, angle2 = 0.0; -void + void output(GLfloat x, GLfloat y, char *text) { - char *p; + char *p; - glPushMatrix(); - glTranslatef(x, y, 0); - for (p = text; *p; p++) - glutStrokeCharacter(GLUT_STROKE_ROMAN, *p); - glPopMatrix(); + glPushMatrix(); + glTranslatef(x, y, 0); + for (p = text; *p; p++) + glutStrokeCharacter(GLUT_STROKE_ROMAN, *p); + glPopMatrix(); } -void + void display(void) { - static GLfloat amb[] = - {0.4, 0.4, 0.4, 0.0}; - static GLfloat dif[] = - {1.0, 1.0, 1.0, 0.0}; + static GLfloat amb[] = + {0.4, 0.4, 0.4, 0.0}; + static GLfloat dif[] = + {1.0, 1.0, 1.0, 0.0}; - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glEnable(GL_LIGHT1); - glDisable(GL_LIGHT2); - amb[3] = dif[3] = cos(s) / 2.0 + 0.5; - glMaterialfv(GL_FRONT, GL_AMBIENT, amb); - glMaterialfv(GL_FRONT, GL_DIFFUSE, dif); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_LIGHT1); + glDisable(GL_LIGHT2); + amb[3] = dif[3] = cos(s) / 2.0 + 0.5; + glMaterialfv(GL_FRONT, GL_AMBIENT, amb); + glMaterialfv(GL_FRONT, GL_DIFFUSE, dif); - glPushMatrix(); - glTranslatef(-0.3, -0.3, 0.0); - glRotatef(angle1, 1.0, 5.0, 0.0); - glCallList(1); /* render ico display list */ - glPopMatrix(); + glPushMatrix(); + glTranslatef(-0.3, -0.3, 0.0); + glRotatef(angle1, 1.0, 5.0, 0.0); + glCallList(1); /* render ico display list */ + glPopMatrix(); - glClear(GL_DEPTH_BUFFER_BIT); - glEnable(GL_LIGHT2); - glDisable(GL_LIGHT1); - amb[3] = dif[3] = 0.5 - cos(s * .95) / 2.0; - glMaterialfv(GL_FRONT, GL_AMBIENT, amb); - glMaterialfv(GL_FRONT, GL_DIFFUSE, dif); + glClear(GL_DEPTH_BUFFER_BIT); + glEnable(GL_LIGHT2); + glDisable(GL_LIGHT1); + amb[3] = dif[3] = 0.5 - cos(s * .95) / 2.0; + glMaterialfv(GL_FRONT, GL_AMBIENT, amb); + glMaterialfv(GL_FRONT, GL_DIFFUSE, dif); - glPushMatrix(); - glTranslatef(0.3, 0.3, 0.0); - glRotatef(angle2, 1.0, 0.0, 5.0); - glCallList(1); /* render ico display list */ - glPopMatrix(); + glPushMatrix(); + glTranslatef(0.3, 0.3, 0.0); + glR... [truncated message content] |
From: <jer...@us...> - 2008-06-10 14:43:58
|
Revision: 6528 http://playerstage.svn.sourceforge.net/playerstage/?rev=6528&view=rev Author: jeremy_asher Date: 2008-06-10 14:44:07 -0700 (Tue, 10 Jun 2008) Log Message: ----------- Re-implemented help/about Modified Paths: -------------- code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-10 20:51:30 UTC (rev 6527) +++ code/stage/trunk/libstage/stage.hh 2008-06-10 21:44:07 UTC (rev 6528) @@ -1879,6 +1879,7 @@ static void SaveAsCallback( Fl_Widget* wid, StgWorldGui* world ); static void QuitCallback( Fl_Widget* wid, StgWorldGui* world ); static void About_cb( Fl_Widget* wid ); + static void HelpAboutCallback( Fl_Widget* wid ); static void view_toggle_cb(Fl_Menu_Bar* menubar, StgCanvas* canvas ); static void WindowCallback( Fl_Widget* wid, StgWorldGui* world ); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-10 20:51:30 UTC (rev 6527) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-10 21:44:07 UTC (rev 6528) @@ -102,20 +102,20 @@ #include <FL/Fl_Shared_Image.H> #include <FL/Fl_PNG_Image.H> #include <FL/Fl_Output.H> -#include <FL/Fl_Multiline_Output.H> +#include <FL/Fl_Text_Display.H> #include <FL/Fl_File_Chooser.H> - static const char* MITEM_VIEW_DATA = "View/Data"; - static const char* MITEM_VIEW_BLOCKS = "View/Blocks"; - static const char* MITEM_VIEW_GRID = "View/Grid"; - static const char* MITEM_VIEW_OCCUPANCY = "View/Occupancy"; - static const char* MITEM_VIEW_QUADTREE = "View/Tree"; - static const char* MITEM_VIEW_FOLLOW = "View/Follow"; - static const char* MITEM_VIEW_CLOCK = "View/Clock"; - static const char* MITEM_VIEW_FOOTPRINTS = "View/Trails/Footprints"; - static const char* MITEM_VIEW_BLOCKSRISING = "View/Trails/Blocks rising"; - static const char* MITEM_VIEW_ARROWS = "View/Trails/Arrows rising"; - static const char* MITEM_VIEW_TRAILS = "View/Trail"; +static const char* MITEM_VIEW_DATA = "&View/&Data"; +static const char* MITEM_VIEW_BLOCKS = "&View/&Blocks"; +static const char* MITEM_VIEW_GRID = "&View/&Grid"; +static const char* MITEM_VIEW_OCCUPANCY = "&View/&Occupancy"; +static const char* MITEM_VIEW_QUADTREE = "&View/&Tree"; +static const char* MITEM_VIEW_FOLLOW = "&View/&Follow"; +static const char* MITEM_VIEW_CLOCK = "&View/&Clock"; +static const char* MITEM_VIEW_FOOTPRINTS = "&View/T&rails/&Footprints"; +static const char* MITEM_VIEW_BLOCKSRISING = "&View/T&rails/&Blocks rising"; +static const char* MITEM_VIEW_ARROWS = "&View/T&rails/&Arrows rising"; +static const char* MITEM_VIEW_TRAILS = "&View/&Trail"; // hack - get this from somewhere sensible, like CMake's config file const char* PACKAGE_STRING = "Stage-3.dev"; @@ -138,13 +138,13 @@ resizable(canvas); end(); - mbar->add( "File", 0, 0, 0, FL_SUBMENU ); + mbar->add( "&File", 0, 0, 0, FL_SUBMENU ); mbar->add( "File/&Load World...", FL_CTRL + 'l', (Fl_Callback *)LoadCallback, this, FL_MENU_DIVIDER ); mbar->add( "File/Save World", FL_CTRL + 's', (Fl_Callback *)SaveCallback, this ); mbar->add( "File/Save World &As...", FL_CTRL + FL_SHIFT + 's', (Fl_Callback *)SaveAsCallback, this, FL_MENU_DIVIDER ); mbar->add( "File/Exit", FL_CTRL+'q', (Fl_Callback *)QuitCallback, this ); - mbar->add( "View", 0, 0, 0, FL_SUBMENU ); + mbar->add( "&View", 0, 0, 0, FL_SUBMENU ); mbar->add( MITEM_VIEW_DATA, 'd', (Fl_Callback*)view_toggle_cb, (void*)canvas, FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_DATA ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_BLOCKS, 'b', (Fl_Callback*)view_toggle_cb, (void*)canvas, @@ -170,8 +170,8 @@ mbar->add( MITEM_VIEW_BLOCKSRISING, FL_CTRL+'t', (Fl_Callback*)view_toggle_cb, (void*)canvas, FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_TRAILRISE ? FL_MENU_VALUE : 0 )); - mbar->add( "Help", 0, 0, 0, FL_SUBMENU ); - mbar->add( "Help/About Stage...", NULL, (Fl_Callback *)About_cb ); + mbar->add( "&Help", 0, 0, 0, FL_SUBMENU ); + mbar->add( "Help/&About Stage...", NULL, (Fl_Callback *)About_cb ); //mbar->add( "Help/HTML Documentation", FL_CTRL + 'g', (Fl_Callback *)dummy_cb ); callback( (Fl_Callback*)WindowCallback, this ); @@ -370,35 +370,64 @@ void StgWorldGui::About_cb( Fl_Widget* ) { fl_register_images(); - Fl_Window win(400,200); // make a window - Fl_Box box(10,20,400-20,80 ); // widget that will contain image + + const int Width = 400; + const int Height = 220; + const int Spc = 10; + const int ButtonH = 25; + const int ButtonW = 60; + + Fl_Window win( Width, Height ); // make a window - // temporary hack + // <temporary hack> const char* stagepath = getenv("STAGEPATH"); const char* logopath = "../../../assets/logo.png"; char* fullpath = (char*)malloc( strlen(stagepath) + strlen(logopath) + 2 ); strcpy( fullpath, stagepath ); strcat( fullpath, "/" ); strcat( fullpath, logopath ); - printf("fullpath: %s\n", fullpath); + printf("fullpath: %s\n", fullpath); Fl_PNG_Image png(fullpath); // load image into ram free( fullpath ); + + Fl_Box box( Spc, Spc, + Width-2*Spc, png.h() ); // widget that will contain image + + + // </temporary hack> + box.image(png); // attach image to box - Fl_Multiline_Output text( 20,120, 400-20, 100 ); + Fl_Text_Display text( Spc, png.h()+2*Spc, + Width-2*Spc, Height-png.h()-ButtonH-4*Spc ); text.box( FL_NO_BOX ); - char buf[256]; - snprintf( buf, 255, - "%s\n" - "Part of the Player Project\n" - "http://playerstage.sourceforge.net\n" - "Copyright 2000-2008 Richard Vaughan and contributors", - PACKAGE_STRING ); - text.value( buf ); + text.color(win.color()); + + const char* AboutText = + "\n" + "Part of the Player Project\n" + "http://playerstage.sourceforge.net\n" + "Copyright 2000-2008 Richard Vaughan and contributors"; + + Fl_Text_Buffer tBuffer; + tBuffer.append( PACKAGE_STRING ); + tBuffer.append( AboutText ); + text.buffer( tBuffer ); + + Fl_Return_Button button( (Width - ButtonW)/2, Height-Spc-ButtonH, + ButtonW, ButtonH, + "&OK" ); + button.callback( (Fl_Callback*)HelpAboutCallback ); + win.show(); - Fl::run(); + while (win.shown()) + Fl::wait(); } +void StgWorldGui::HelpAboutCallback( Fl_Widget* wid ) { + wid->window()->hide(); +} + bool StgWorldGui::Save( const char* filename ) { PRINT_DEBUG1( "%s.Save()", token ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-06-10 15:22:22
|
Revision: 6529 http://playerstage.svn.sourceforge.net/playerstage/?rev=6529&view=rev Author: alexcb Date: 2008-06-10 15:22:24 -0700 (Tue, 10 Jun 2008) Log Message: ----------- added getFrame method to take a snapshot from the robot's camera Modified Paths: -------------- code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/texture_manager.cc Modified: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc 2008-06-10 21:44:07 UTC (rev 6528) +++ code/stage/trunk/libstage/camera.cc 2008-06-10 22:22:24 UTC (rev 6529) @@ -33,7 +33,7 @@ glMatrixMode (GL_PROJECTION); glLoadIdentity (); - gluPerspective( 60.0, pixels_width/pixels_height, 0.01, 100 ); + gluPerspective( 120.0, pixels_width/pixels_height, 0.01, 100 ); glMatrixMode (GL_MODELVIEW); } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-10 21:44:07 UTC (rev 6528) +++ code/stage/trunk/libstage/canvas.cc 2008-06-10 22:22:24 UTC (rev 6529) @@ -335,14 +335,18 @@ PopColor(); } -void StgCanvas::renderFrame() +void StgCanvas::renderFrame( bool robot_camera ) { - if( ! (showflags & STG_SHOW_TRAILS) ) + uint32_t showflags = this->showflags; + if( robot_camera == true ) + showflags = STG_SHOW_BLOCKS; + + if( ! (showflags & STG_SHOW_TRAILS) || robot_camera ) glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); // if following selected, shift the view to above the selected robot - if( (showflags & STG_SHOW_FOLLOW) && last_selection ) + if( (showflags & STG_SHOW_FOLLOW) && last_selection && robot_camera == false ) { glLoadIdentity (); double zclip = 20 * camera.getScale(); //hypot(world->Width(), world->Height()) * camera.getScale(); @@ -372,7 +376,7 @@ glDisable(GL_POLYGON_OFFSET_FILL); - if( (showflags & STG_SHOW_QUADTREE) || (showflags & STG_SHOW_OCCUPANCY) ) + if( (showflags & STG_SHOW_QUADTREE) || (showflags & STG_SHOW_OCCUPANCY) && robot_camera == false ) { glDisable( GL_LINE_SMOOTH ); glLineWidth( 1 ); @@ -392,7 +396,7 @@ glPopMatrix(); - if( showflags & STG_SHOW_GRID ) + if( showflags & STG_SHOW_GRID && robot_camera == false ) DrawGlobalGrid(); for( GList* it=selected_models; it; it=it->next ) Modified: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc 2008-06-10 21:44:07 UTC (rev 6528) +++ code/stage/trunk/libstage/model_camera.cc 2008-06-10 22:22:24 UTC (rev 6529) @@ -11,38 +11,49 @@ //#define DEBUG 1 #include "stage_internal.hh" +#include <sstream> + StgModelCamera::StgModelCamera( StgWorld* world, StgModel* parent, stg_id_t id, char* typestr ) -: StgModel( world, parent, id, typestr ) +: StgModel( world, parent, id, typestr ), +_frame_data( NULL ), _frame_data_width( 0 ), _frame_data_height( 0 ) { PRINT_DEBUG2( "Constructing StgModelCamera %d (%s)\n", id, typestr ); + StgWorldGui* world_gui = dynamic_cast< StgWorldGui* >( world ); + + if( world_gui == NULL ) { + printf( "Unable to use Camera Model - it must be run with a GUI world\n" ); + assert( 0 ); + } + + _canvas = world_gui->GetCanvas(); + // Set up sensible defaults - this->SetColor( stg_lookup_color( "green" ) ); + SetColor( stg_lookup_color( "green" ) ); - stg_geom_t geom; memset( &geom, 0, sizeof(geom)); // no size - geom.size.x = 0.02; - geom.size.y = 0.02; - geom.size.z = 0.02; - this->SetGeom( geom ); + //TODO can't draw this as it blocks the laser + SetGeom( geom ); - this->Startup(); + Startup(); } StgModelCamera::~StgModelCamera() { + if( _frame_data != NULL ) + delete _frame_data; } void StgModelCamera::Load( void ) { - StgModel::Load(); + StgModel::Load(); Worldfile* wf = world->GetWorldFile(); } @@ -51,15 +62,39 @@ void StgModelCamera::Update( void ) { StgModel::Update(); - static StgPerspectiveCamera camera; +} + + +const char* StgModelCamera::GetFrame( int width, int height ) +{ + static StgPerspectiveCamera camera; //shared between _ALL_ instances + + if( width == 0 || height == 0 ) + return NULL; + + if( width != _frame_data_width || height != _frame_data_height ) { + if( _frame_data != NULL ) + delete _frame_data; + _frame_data = new char[ 3 * width * height ]; + } + + glViewport( 0, 0, width, height ); + camera.update(); - camera.SetProjection( 400, 300, 0.01, 200.0 ); + camera.SetProjection( width, height, 0.01, 200.0 ); camera.setPose( parent->GetGlobalPose().x, parent->GetGlobalPose().y, 0.1 ); camera.setYaw( rtod( parent->GetGlobalPose().a ) - 90.0 ); - //camera.setPose( 0.1, 0.1, 0.1 ); camera.Draw(); - std::cout << "updated" << std::endl; - std::cout << parent->GetGlobalPose().x << std::endl; + + _canvas->renderFrame( true ); + + glReadPixels(0, 0, width, height, + GL_RGB, + GL_UNSIGNED_BYTE, + _frame_data ); + + _canvas->invalidate(); + return _frame_data; } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-10 21:44:07 UTC (rev 6528) +++ code/stage/trunk/libstage/stage.hh 2008-06-10 22:22:24 UTC (rev 6529) @@ -1819,7 +1819,8 @@ StgWorld* world; void FixViewport(int W,int H); - virtual void renderFrame(); + //robot_camera = true + virtual void renderFrame( bool robot_camera = false ); virtual void draw(); virtual int handle( int event ); void resize(int X,int Y,int W,int H); @@ -1897,6 +1898,8 @@ void DrawTree( bool leaves ); void DrawFloor(); + + StgCanvas* GetCanvas( void ) { return canvas; } }; @@ -2343,6 +2346,13 @@ // CAMERA MODEL ---------------------------------------------------- class StgModelCamera : public StgModel { + private: + StgCanvas* _canvas; + + char* _frame_data; + int _frame_data_width; + int _frame_data_height; + public: StgModelCamera( StgWorld* world, @@ -2355,6 +2365,9 @@ virtual void Load(); virtual void Update(); virtual void Draw( uint32_t flags, StgCanvas* canvas ); + + ///Take a screenshot from the camera's perspective + const char* GetFrame( int width, int height ); // static wrapper for the constructor - all models must implement // this method and add an entry in typetable.cc Modified: code/stage/trunk/libstage/texture_manager.cc =================================================================== --- code/stage/trunk/libstage/texture_manager.cc 2008-06-10 21:44:07 UTC (rev 6528) +++ code/stage/trunk/libstage/texture_manager.cc 2008-06-10 22:22:24 UTC (rev 6529) @@ -16,7 +16,7 @@ if( filename[ 0 ] == '/' || filename[ 0 ] == '~' ) return Fl_Shared_Image::get( filename ); - //TODO move this somewhere else, and include STAGE_PATH, and path relative to user supplied world file + //TODO move this somewhere else, and include STAGEPATH, and path relative to user supplied world file const char* prefixes[] = { ".", INSTALL_PREFIX "/share/stage", This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-06-11 11:18:41
|
Revision: 6537 http://playerstage.svn.sourceforge.net/playerstage/?rev=6537&view=rev Author: jeremy_asher Date: 2008-06-11 11:18:50 -0700 (Wed, 11 Jun 2008) Log Message: ----------- Implemented basic File/Load capability Modified Paths: -------------- code/stage/trunk/libstage/main.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/main.cc =================================================================== --- code/stage/trunk/libstage/main.cc 2008-06-11 03:38:09 UTC (rev 6536) +++ code/stage/trunk/libstage/main.cc 2008-06-11 18:18:50 UTC (rev 6537) @@ -63,7 +63,8 @@ } if( loaded_world_file == false ) { - printf( "No world file specified.\n" ); + // replace this with a loading dialog/window + StgWorldGui* world = new StgWorldGui( 400, 300 ); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-11 03:38:09 UTC (rev 6536) +++ code/stage/trunk/libstage/stage.hh 2008-06-11 18:18:50 UTC (rev 6537) @@ -1012,7 +1012,7 @@ void RemoveBlock( int x, int y, StgBlock* block ); //{ }//bgrid->RemoveBlock( x, y, block ); }; - protected: +protected: stg_usec_t interval_real; ///< real-time interval between updates - set this to zero for 'as fast as possible GHashTable* superregions; @@ -1032,8 +1032,7 @@ //GHashTable* blocks; GArray lines; - public: - +public: StgWorld(); StgWorld( const char* token, @@ -1041,56 +1040,57 @@ stg_msec_t interval_real, double ppm ); -virtual ~StgWorld(); + virtual ~StgWorld(); -stg_usec_t SimTimeNow(void){ return sim_time;} ; -stg_usec_t RealTimeNow(void); -stg_usec_t RealTimeSinceStart(void); -void PauseUntilNextUpdateTime(void); -void IdleUntilNextUpdateTime( int (*idler)(void) ); + stg_usec_t SimTimeNow(void){ return sim_time;} ; + stg_usec_t RealTimeNow(void); + stg_usec_t RealTimeSinceStart(void); + void PauseUntilNextUpdateTime(void); + void IdleUntilNextUpdateTime( int (*idler)(void) ); -void AddBlock( StgBlock* block ); -void RemoveBlock( StgBlock* block ); + void AddBlock( StgBlock* block ); + void RemoveBlock( StgBlock* block ); -stg_usec_t GetSimInterval(){ return interval_sim; }; + stg_usec_t GetSimInterval(){ return interval_sim; }; -Worldfile* GetWorldFile(){ return wf; }; + Worldfile* GetWorldFile(){ return wf; }; -virtual void Load( const char* worldfile_path ); -virtual void Reload(); -virtual bool Save( const char* filename ); -virtual bool Update(void); -virtual void AddModel( StgModel* mod ); -virtual void RemoveModel( StgModel* mod ); + virtual void Load( const char* worldfile_path ); + virtual void UnLoad(); + virtual void Reload(); + virtual bool Save( const char* filename ); + virtual bool Update(void); + virtual void AddModel( StgModel* mod ); + virtual void RemoveModel( StgModel* mod ); -void Start(){ paused = false; }; -void Stop(){ paused = true; }; -void TogglePause(){ paused = !paused; }; -bool TestQuit(){ return( quit || quit_all ); } -void Quit(){ quit = true; } -void QuitAll(){ quit_all = true; } -void CancelQuit(){ quit = false; } -void CancelQuitAll(){ quit_all = false; } + void Start(){ paused = false; }; + void Stop(){ paused = true; }; + void TogglePause(){ paused = !paused; }; + bool TestQuit(){ return( quit || quit_all ); } + void Quit(){ quit = true; } + void QuitAll(){ quit_all = true; } + void CancelQuit(){ quit = false; } + void CancelQuitAll(){ quit_all = false; } -double Resolution(){ return ppm; }; + double Resolution(){ return ppm; }; -StgModel* GetModel( const stg_id_t id ); -StgModel* GetModel( const char* name ); + StgModel* GetModel( const stg_id_t id ); + StgModel* GetModel( const char* name ); -GList* GetRayList(){ return ray_list; }; -void ClearRays(); + GList* GetRayList(){ return ray_list; }; + void ClearRays(); -void ClockString( char* str, size_t maxlen ); + void ClockString( char* str, size_t maxlen ); -stg_bounds3d_t GetExtent(){ return extent; }; + stg_bounds3d_t GetExtent(){ return extent; }; -void ForEachModel( GHFunc func, void* arg ) -{ g_hash_table_foreach( models_by_id, func, arg ); }; + void ForEachModel( GHFunc func, void* arg ) + { g_hash_table_foreach( models_by_id, func, arg ); }; -long unsigned int GetUpdateCount() -{ return updates; } + long unsigned int GetUpdateCount() + { return updates; } }; @@ -1869,9 +1869,11 @@ /** Start the simulation and GUI. Does not return */ static void Run(); void Start(); + void Stop(); void Cycle(); virtual void Load( const char* filename ); + virtual void UnLoad(); virtual bool Save( const char* filename ); // static callback functions Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-06-11 03:38:09 UTC (rev 6536) +++ code/stage/trunk/libstage/world.cc 2008-06-11 18:18:50 UTC (rev 6537) @@ -231,6 +231,19 @@ mod->Init(); } +// delete a model from the hash table +void destroy_model( gpointer dummy1, StgModel* mod, gpointer dummy2 ) +{ + free(mod); +} + +// delete a model from the hash table +void destroy_sregion( gpointer dummy1, SuperRegion* sr, gpointer dummy2 ) +{ + free(sr); +} + + void StgWorld::Load( const char* worldfile_path ) { printf( " [Loading %s]", worldfile_path ); @@ -320,6 +333,38 @@ } +void StgWorld::UnLoad() +{ + if( wf ) delete wf; + //if( bgrid ) delete bgrid; + + g_list_foreach( children, (GFunc)destroy_model, NULL ); + g_list_free( children ); + children = NULL; + + g_hash_table_remove_all( child_types ); // small memory leak + + g_hash_table_remove_all( models_by_id ); + + g_hash_table_remove_all( models_by_name ); + + g_list_free( velocity_list ); + velocity_list = NULL; + + g_list_free( update_list ); + update_list = NULL; + + g_list_free( ray_list ); + ray_list = NULL; + + g_hash_table_foreach( superregions, (GHFunc)destroy_sregion, NULL ); + g_hash_table_remove_all( superregions ); + + g_free( token ); + this->token = (char*)g_malloc(Stg::TOKEN_MAX); // necessary? + +} + stg_usec_t StgWorld::RealTimeNow() { struct timeval tv; Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-11 03:38:09 UTC (rev 6536) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-11 18:18:50 UTC (rev 6537) @@ -180,13 +180,15 @@ StgWorldGui::~StgWorldGui() { + delete mbar; + delete canvas; } void StgWorldGui::Load( const char* filename ) { PRINT_DEBUG1( "%s.Load()", token ); - StgWorld::Load( filename); + StgWorld::Load( filename ); wf_section = wf->LookupEntity( "window" ); if( wf_section < 1) // no section defined @@ -250,9 +252,43 @@ // TODO - per model visualizations load } +void StgWorldGui::UnLoad() +{ + //canvas->UnLoad(); +// delete canvas; +// canvas = new StgCanvas( this,0,30,640,480 ); + StgWorld::UnLoad(); +// canvas->camera.setPose( 0, 0 ); +} + void StgWorldGui::LoadCallback( Fl_Widget* wid, StgWorldGui* world ) { - // implement me + const char* filename; + const char* worldsPath; + //bool success; + const char* pattern = "World Files (*.world)"; + + // todo: replace this with real path + worldsPath = "/Users/jeremya/stage_trunk/worlds"; + Fl_File_Chooser fc( worldsPath, pattern, Fl_File_Chooser::CREATE, "Load World File..." ); + fc.ok_label( "Load" ); + + fc.show(); + while (fc.shown()) + Fl::wait(); + + filename = fc.value(); + + if (filename != NULL) { + // if (initialized) { + world->Stop(); + world->UnLoad(); + // } + + // todo: make sure loading is successful + world->Load( filename ); + world->Start(); + } } void StgWorldGui::SaveCallback( Fl_Widget* wid, StgWorldGui* world ) @@ -482,6 +518,13 @@ Fl::add_idle( (Fl_Timeout_Handler)idle_callback, this ); } +void StgWorldGui::Stop() +{ + Fl::remove_timeout( (Fl_Timeout_Handler)UpdateCb, this ); + Fl::remove_idle( (Fl_Timeout_Handler)idle_callback, this ); +} + + void StgWorldGui::Run() { Fl::run(); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-06-11 17:17:50
|
Revision: 6545 http://playerstage.svn.sourceforge.net/playerstage/?rev=6545&view=rev Author: alexcb Date: 2008-06-11 17:17:52 -0700 (Wed, 11 Jun 2008) Log Message: ----------- added perspective camera view option, and simulated laser model with opengl depth buffer Modified Paths: -------------- code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/texture_manager.cc code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc 2008-06-11 20:54:18 UTC (rev 6544) +++ code/stage/trunk/libstage/camera.cc 2008-06-12 00:17:52 UTC (rev 6545) @@ -3,7 +3,6 @@ * Stage * * Created by Alex Couture-Beil on 06/06/08. - * Copyright 2008 __MyCompanyName__. All rights reserved. * */ @@ -14,15 +13,19 @@ //perspective camera //perspective camera +StgPerspectiveCamera::StgPerspectiveCamera( void ) : + _x( 0 ), _y( 0 ), _z( 0 ), _pitch( 90 ), _yaw( 0 ), _z_near( 0.1 ), _z_far( 20.0 ) +{ +} + void StgPerspectiveCamera::Draw( void ) const { glMatrixMode (GL_MODELVIEW); glLoadIdentity (); - + glRotatef( - _pitch, 1.0, 0.0, 0.0 ); glRotatef( - _yaw, 0.0, 0.0, 1.0 ); - std::cout << "y: " << _z << std::endl; glTranslatef( - _x, - _y, - _z ); //zooming needs to happen in the Projection code (don't use glScale for zoom) @@ -32,19 +35,25 @@ { glMatrixMode (GL_PROJECTION); glLoadIdentity (); + + gluPerspective( 60.0, pixels_width/pixels_height, _z_near, _z_far ); + + glMatrixMode (GL_MODELVIEW); +} - gluPerspective( 120.0, pixels_width/pixels_height, 0.01, 100 ); - +void StgPerspectiveCamera::SetProjection( float aspect ) const +{ + glMatrixMode (GL_PROJECTION); + glLoadIdentity (); + + gluPerspective( 60.0, aspect, _z_near, _z_far ); + glMatrixMode (GL_MODELVIEW); } void StgPerspectiveCamera::update( void ) { - // _x = 0; - // _y = 0; - // _z = i; - _pitch = 90.0; - // _yaw = 90.0; + } @@ -53,7 +62,6 @@ - ////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Ortho camera ////////////////////////////////////////////////////////////////////////////////////////////////////////////// Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-11 20:54:18 UTC (rev 6544) +++ code/stage/trunk/libstage/canvas.cc 2008-06-12 00:17:52 UTC (rev 6545) @@ -31,6 +31,10 @@ selected_models = NULL; last_selection = NULL; + use_perspective_camera = false; + perspective_camera.setPose( -3.0, 0.0, 1.0 ); + perspective_camera._pitch = 70.0; //look down + startx = starty = 0; //panx = pany = stheta = sphi = 0.0; //scale = 15.0; @@ -169,7 +173,11 @@ } else { - camera.scale( Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h() ); + if( use_perspective_camera == true ) { + perspective_camera._z += Fl::event_dy() / 10.0; + } else { + camera.scale( Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h() ); + } invalidate(); } return 1; @@ -180,22 +188,35 @@ int dx = Fl::event_x() - startx; int dy = Fl::event_y() - starty; - camera.pitch( 0.5 * static_cast<double>( dy ) ); - camera.yaw( 0.5 * static_cast<double>( dx ) ); + if( use_perspective_camera == true ) { + perspective_camera._yaw += -dx; + perspective_camera._pitch += -dy; + } else { + camera.pitch( 0.5 * static_cast<double>( dy ) ); + camera.yaw( 0.5 * static_cast<double>( dx ) ); + } invalidate(); } else if( Fl::event_state( FL_ALT ) ) - { + { int dx = Fl::event_x() - startx; int dy = Fl::event_y() - starty; - camera.move( -dx, dy ); + if( use_perspective_camera == true ) { + perspective_camera._x += -dx / 100.0 * perspective_camera._z; + perspective_camera._y += dy / 100.0 * perspective_camera._z; + } else { + camera.move( -dx, dy ); + + } invalidate(); + } startx = Fl::event_x(); starty = Fl::event_y(); + return 1; case FL_PUSH: // button pressed @@ -545,9 +566,13 @@ // install a font gl_font( FL_HELVETICA, 12 ); - stg_bounds3d_t extent = world->GetExtent(); - camera.SetProjection( w(), h(), extent.y.min, extent.y.max ); - camera.Draw(); + if( use_perspective_camera == true ) { + perspective_camera.SetProjection( w(), h(), 0.01, 0.5 ); + } else { + stg_bounds3d_t extent = world->GetExtent(); + camera.SetProjection( w(), h(), extent.y.min, extent.y.max ); + camera.Draw(); + } // enable vertex arrays glEnableClientState( GL_VERTEX_ARRAY ); @@ -566,6 +591,10 @@ camera.Draw(); } + if( use_perspective_camera == true ) { + perspective_camera.Draw(); + } + renderFrame(); } Modified: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc 2008-06-11 20:54:18 UTC (rev 6544) +++ code/stage/trunk/libstage/model_camera.cc 2008-06-12 00:17:52 UTC (rev 6545) @@ -64,8 +64,41 @@ StgModel::Update(); } +float* StgModelCamera::laser() +{ + int h = 32; + int w = 32; + + static StgPerspectiveCamera camera; -const char* StgModelCamera::GetFrame( int width, int height ) + static GLfloat* data_gl = NULL; + static GLfloat* data = NULL; + if( data == NULL ) { + data = new GLfloat[ h * w ]; + data_gl = new GLfloat[ h * w ]; + + } + + glViewport( 0, 0, w, h ); + camera.update(); + camera.SetProjection( 1.0 ); + + camera.setPose( parent->GetGlobalPose().x, parent->GetGlobalPose().y, 0.1 ); + camera.setYaw( rtod( parent->GetGlobalPose().a ) - 90.0 ); + camera.Draw(); + + _canvas->renderFrame( true ); + + glReadPixels(0, h / 2, w, 1, GL_DEPTH_COMPONENT, GL_FLOAT, data_gl ); + + for( int i = 0; i < w; i++ ) { + data[ w-1-i ] = camera.realDistance( data_gl[ i ] ); + } + _canvas->invalidate(); + return data; +} + +const char* StgModelCamera::GetFrame( int width, int height, bool depth_buffer ) { static StgPerspectiveCamera camera; //shared between _ALL_ instances @@ -75,24 +108,32 @@ if( width != _frame_data_width || height != _frame_data_height ) { if( _frame_data != NULL ) delete _frame_data; - _frame_data = new char[ 3 * width * height ]; + _frame_data = new char[ 4 * width * height ]; //assumes a max of depth 4 } + glViewport( 0, 0, width, height ); - camera.update(); - camera.SetProjection( width, height, 0.01, 200.0 ); + camera.SetProjection( width, height, 0.0, 0.0 ); camera.setPose( parent->GetGlobalPose().x, parent->GetGlobalPose().y, 0.1 ); camera.setYaw( rtod( parent->GetGlobalPose().a ) - 90.0 ); camera.Draw(); _canvas->renderFrame( true ); - glReadPixels(0, 0, width, height, - GL_RGB, - GL_UNSIGNED_BYTE, - _frame_data ); - + if( depth_buffer == true ) { + glReadPixels(0, 0, width, height, + GL_DEPTH_COMPONENT, //GL_RGB, + GL_FLOAT, //GL_UNSIGNED_BYTE, + _frame_data ); + } else { + glReadPixels(0, 0, width, height, + GL_RGB, + GL_UNSIGNED_BYTE, + _frame_data ); + } + + _canvas->invalidate(); return _frame_data; } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-11 20:54:18 UTC (rev 6544) +++ code/stage/trunk/libstage/stage.hh 2008-06-12 00:17:52 UTC (rev 6545) @@ -1703,26 +1703,36 @@ StgCamera() { } virtual ~StgCamera() { } - virtual void Draw() const = 0; + virtual void Draw( void ) const = 0; virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0; }; class StgPerspectiveCamera : public StgCamera { - private: + public: //TODO make this private float _x, _y, _z; float _pitch; //left-right (about y) float _yaw; //up-down (about x) + private: + float _z_near; + float _z_far; public: - StgPerspectiveCamera( void ) : _x( 0 ), _y( 0 ), _z( 0 ), _pitch( 0 ), _yaw( 0 ) { } + StgPerspectiveCamera( void ); - virtual void Draw() const; - virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const; + virtual void Draw( void ) const; + virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const; + void SetProjection( float aspect ) const; void update( void ); inline void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; } inline void setYaw( float yaw ) { _yaw = yaw; } + + inline float realDistance( float z_buf_val ) const { + //formulat found at http://www.cs.unc.edu/~hoff/techrep/openglz.html + //Z = Zn*Zf / (Zf - z*(Zf-Zn)) + return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) ); + } }; class StgOrthoCamera : public StgCamera @@ -1738,7 +1748,6 @@ virtual void Draw() const; virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const; - inline void move( float x, float y ) { //convert screen points into world points x = x / ( _scale ); @@ -1794,6 +1803,8 @@ GlColorStack colorstack; StgOrthoCamera camera; + StgPerspectiveCamera perspective_camera; + bool use_perspective_camera; int startx, starty; bool dragging; @@ -2369,7 +2380,10 @@ virtual void Draw( uint32_t flags, StgCanvas* canvas ); ///Take a screenshot from the camera's perspective - const char* GetFrame( int width, int height ); + const char* GetFrame( int width, int height, bool depth_buffer ); + + ///Imiate laser scan + float* laser(); // static wrapper for the constructor - all models must implement // this method and add an entry in typetable.cc @@ -2379,7 +2393,7 @@ char* typestr ) { return (StgModel*)new StgModelCamera( world, parent, id, typestr ); - } + } }; // POSITION MODEL -------------------------------------------------------- Modified: code/stage/trunk/libstage/texture_manager.cc =================================================================== --- code/stage/trunk/libstage/texture_manager.cc 2008-06-11 20:54:18 UTC (rev 6544) +++ code/stage/trunk/libstage/texture_manager.cc 2008-06-12 00:17:52 UTC (rev 6545) @@ -3,7 +3,6 @@ * Stage * * Created by Alex Couture-Beil on 03/06/08. - * Copyright 2008 __MyCompanyName__. All rights reserved. * */ Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-11 20:54:18 UTC (rev 6544) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-12 00:17:52 UTC (rev 6545) @@ -116,6 +116,7 @@ static const char* MITEM_VIEW_BLOCKSRISING = "&View/T&rails/&Blocks rising"; static const char* MITEM_VIEW_ARROWS = "&View/T&rails/&Arrows rising"; static const char* MITEM_VIEW_TRAILS = "&View/&Trail"; +static const char* MITEM_VIEW_PERSPECTIVE = "&View/Perspective camera"; // hack - get this from somewhere sensible, like CMake's config file const char* PACKAGE_STRING = "Stage-3.dev"; @@ -158,7 +159,9 @@ mbar->add( MITEM_VIEW_FOLLOW, 'f', (Fl_Callback*)view_toggle_cb, (void*)canvas, FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_FOLLOW ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_CLOCK, 'c', (Fl_Callback*)view_toggle_cb, (void*)canvas, - FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_CLOCK ? FL_MENU_VALUE : 0 )); + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_CLOCK ? FL_MENU_VALUE : 0 )); + mbar->add( MITEM_VIEW_PERSPECTIVE, 'r', (Fl_Callback*)view_toggle_cb, (void*)canvas, + FL_MENU_TOGGLE| (canvas->use_perspective_camera )); mbar->add( MITEM_VIEW_TRAILS, 't', (Fl_Callback*)view_toggle_cb, (void*)canvas, FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_TRAILS ? FL_MENU_VALUE : 0 )); @@ -398,6 +401,7 @@ else if( strcmp(picked, MITEM_VIEW_ARROWS ) == 0 ) canvas->InvertView( STG_SHOW_ARROWS ); else if( strcmp(picked, MITEM_VIEW_TRAILS ) == 0 ) canvas->InvertView( STG_SHOW_TRAILS ); else if( strcmp(picked, MITEM_VIEW_BLOCKSRISING ) == 0 ) canvas->InvertView( STG_SHOW_TRAILRISE ); + else if( strcmp(picked, MITEM_VIEW_PERSPECTIVE ) == 0 ) { canvas->use_perspective_camera = ! canvas->use_perspective_camera; canvas->invalidate(); } else PRINT_ERR1( "Unrecognized menu item \"%s\" not handled", picked ); //printf( "value: %d\n", item->value() ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-06-12 15:27:26
|
Revision: 6548 http://playerstage.svn.sourceforge.net/playerstage/?rev=6548&view=rev Author: jeremy_asher Date: 2008-06-12 15:27:04 -0700 (Thu, 12 Jun 2008) Log Message: ----------- Implemented FileManager class to help with resource locating Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/texture_manager.cc code/stage/trunk/libstage/worldgui.cc Added Paths: ----------- code/stage/trunk/libstage/file_manager.cc code/stage/trunk/libstage/file_manager.hh Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2008-06-12 16:55:06 UTC (rev 6547) +++ code/stage/trunk/libstage/CMakeLists.txt 2008-06-12 22:27:04 UTC (rev 6548) @@ -1,6 +1,8 @@ add_library( stage SHARED stage.hh # get headers into IDE projects + file_manager.hh + file_manager.cc ancestor.cc block.cc canvas.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-12 16:55:06 UTC (rev 6547) +++ code/stage/trunk/libstage/canvas.cc 2008-06-12 22:27:04 UTC (rev 6548) @@ -7,6 +7,7 @@ #include "stage_internal.hh" #include "texture_manager.hh" #include "replace.h" +#include <string> using namespace Stg; @@ -557,7 +558,14 @@ //TODO find a better home for loading textures if( loaded_texture == false ) { - GLuint stall_id = TextureManager::getInstance().loadTexture( "assets/stall.png" ); + std::string fullpath; + fullpath = world->fileMan.fullPath( "stall.png" ); + if ( fullpath == "" ) { + PRINT_DEBUG( "Unable to load texture.\n" ); + } + + GLuint stall_id = TextureManager::getInstance().loadTexture( fullpath.c_str() ); + TextureManager::getInstance()._stall_texture_id = stall_id; loaded_texture = true; Added: code/stage/trunk/libstage/file_manager.cc =================================================================== --- code/stage/trunk/libstage/file_manager.cc (rev 0) +++ code/stage/trunk/libstage/file_manager.cc 2008-06-12 22:27:04 UTC (rev 6548) @@ -0,0 +1,65 @@ +#include "file_manager.hh" +#include "stage.hh" // to get PRINT_DEBUG + +namespace Stg +{ + FileManager::FileManager() { + SharePath = INSTALL_PREFIX "/share/stage"; + AssetPath = SharePath + '/' + "assets"; + CtrlPath = getenv("STAGEPATH"); + WorldsRoot = "."; + + paths.push_back( "." ); + paths.push_back( SharePath ); + paths.push_back( AssetPath ); + paths.push_back( CtrlPath ); + } + + std::string FileManager::fullPath( std::string filename ) { + PRINT_DEBUG1("FileManager: trying %s\n", filename.c_str()); + if ( readable( filename ) ) + return filename; + + for ( unsigned int i=0; i<paths.size(); i++ ) { + std::string path = paths[i] + '/' + filename; + PRINT_DEBUG1("FileManager: trying %s\n", path.c_str()); + if ( readable( path ) ) { + return path; + } + } + + PRINT_DEBUG("FileManager: Not found.\n"); + return ""; + } + + /*std::string FileManager::fullPathImage( std::string filename ) { + std::string path = ImgPath + '/' + filename; + if ( readable ( path ) ) + return path; + else + return ""; + }*/ + + bool FileManager::readable( std::string path ) { + std::ifstream iFile; + iFile.open( path.c_str() ); + if ( iFile.is_open() ) { + iFile.close(); + return true; + } + else { + return false; + } + } + + std::string FileManager::stripFilename( std::string path ) { + std::string pathChars( "\\/" ); + size_t loc = path.find_last_of( pathChars ); + if ( loc < 0 ) + return path; + else + return path.substr( 0, loc ); + } + +}; // namespace Stg + Added: code/stage/trunk/libstage/file_manager.hh =================================================================== --- code/stage/trunk/libstage/file_manager.hh (rev 0) +++ code/stage/trunk/libstage/file_manager.hh 2008-06-12 22:27:04 UTC (rev 6548) @@ -0,0 +1,38 @@ +#ifndef _FILE_MANAGER_HH_ +#define _FILE_MANAGER_HH_ + +// Normally passed by build scripts +#ifndef INSTALL_PREFIX +#define INSTALL_PREFIX "." +#endif + +#include <fstream> +#include <string> +#include <vector> + +namespace Stg { + + class FileManager { + private: + std::vector<std::string> paths; + std::string SharePath; + std::string AssetPath; + std::string CtrlPath; + std::string WorldsRoot; + + std::string stripFilename( std::string path ); + public: + FileManager(); + + std::string fullPath( std::string filename ); + //std::string fullPathImage( std::string filename ); + + inline const std::string worldsRoot() const { return WorldsRoot; } + inline void newWorld( std::string worldfile ) { + WorldsRoot = stripFilename( worldfile ); } + + bool readable( std::string path ); + }; + +}; +#endif Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-12 16:55:06 UTC (rev 6547) +++ code/stage/trunk/libstage/stage.hh 2008-06-12 22:27:04 UTC (rev 6548) @@ -79,6 +79,7 @@ #include <GL/glu.h> #endif +#include "file_manager.hh" /** The Stage library uses its own namespace */ namespace Stg @@ -928,8 +929,8 @@ friend class StgBlock; friend class StgTime; - private: - +private: + static bool quit_all; // quit all worlds ASAP static unsigned int next_id; //< initialized to zero, used tob //allocate unique sequential world ids @@ -1041,6 +1042,8 @@ double ppm ); virtual ~StgWorld(); + + FileManager fileMan; stg_usec_t SimTimeNow(void){ return sim_time;} ; stg_usec_t RealTimeNow(void); @@ -1892,9 +1895,9 @@ static void SaveCallback( Fl_Widget* wid, StgWorldGui* world ); static void SaveAsCallback( Fl_Widget* wid, StgWorldGui* world ); static void QuitCallback( Fl_Widget* wid, StgWorldGui* world ); - static void About_cb( Fl_Widget* wid ); + static void About_cb( Fl_Widget* wid, StgWorldGui* world ); static void HelpAboutCallback( Fl_Widget* wid ); - static void view_toggle_cb(Fl_Menu_Bar* menubar, StgCanvas* canvas ); + static void view_toggle_cb( Fl_Menu_Bar* menubar, StgCanvas* canvas ); static void WindowCallback( Fl_Widget* wid, StgWorldGui* world ); bool SaveAsDialog(); Modified: code/stage/trunk/libstage/texture_manager.cc =================================================================== --- code/stage/trunk/libstage/texture_manager.cc 2008-06-12 16:55:06 UTC (rev 6547) +++ code/stage/trunk/libstage/texture_manager.cc 2008-06-12 22:27:04 UTC (rev 6548) @@ -7,29 +7,15 @@ */ #include "texture_manager.hh" +#include "file_manager.hh" #include <sstream> -//TODO Windows Port Fl_Shared_Image* TextureManager::loadImage( const char* filename ) -{ - if( filename[ 0 ] == '/' || filename[ 0 ] == '~' ) - return Fl_Shared_Image::get( filename ); - - //TODO move this somewhere else, and include STAGEPATH, and path relative to user supplied world file - const char* prefixes[] = { - ".", - INSTALL_PREFIX "/share/stage", - NULL - }; - +{ Fl_Shared_Image *img = NULL; - int i = 0; - while( img == NULL && prefixes[ i ] != NULL ) { - std::ostringstream oss; - oss << prefixes[ i ] << "/" << filename; - img = Fl_Shared_Image::get( oss.str().c_str() ); - i++; - } + + img = Fl_Shared_Image::get( filename ); + return img; } Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-12 16:55:06 UTC (rev 6547) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-12 22:27:04 UTC (rev 6548) @@ -105,6 +105,8 @@ #include <FL/Fl_Text_Display.H> #include <FL/Fl_File_Chooser.H> +#include "file_manager.hh" + static const char* MITEM_VIEW_DATA = "&View/&Data"; static const char* MITEM_VIEW_BLOCKS = "&View/&Blocks"; static const char* MITEM_VIEW_GRID = "&View/&Grid"; @@ -174,7 +176,7 @@ FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_TRAILRISE ? FL_MENU_VALUE : 0 )); mbar->add( "&Help", 0, 0, 0, FL_SUBMENU ); - mbar->add( "Help/&About Stage...", NULL, (Fl_Callback *)About_cb ); + mbar->add( "Help/&About Stage...", NULL, (Fl_Callback *)About_cb, this ); //mbar->add( "Help/HTML Documentation", FL_CTRL + 'g', (Fl_Callback *)dummy_cb ); callback( (Fl_Callback*)WindowCallback, this ); @@ -271,8 +273,7 @@ //bool success; const char* pattern = "World Files (*.world)"; - // todo: replace this with real path - worldsPath = "/Users/jeremya/stage_trunk/worlds"; + worldsPath = world->fileMan.worldsRoot().c_str(); Fl_File_Chooser fc( worldsPath, pattern, Fl_File_Chooser::CREATE, "Load World File..." ); fc.ok_label( "Load" ); @@ -282,15 +283,25 @@ filename = fc.value(); - if (filename != NULL) { - // if (initialized) { - world->Stop(); - world->UnLoad(); - // } + if (filename != NULL) { // chose something + if ( world->fileMan.readable( filename ) ) { + // file is readable, clear and load + + // if (initialized) { + world->Stop(); + world->UnLoad(); + // } + + // todo: make sure loading is successful + world->fileMan.newWorld( filename ); + world->Load( filename ); + world->Start(); // if (stopped) + } + else { + fl_alert( "Unable to read selected world file." ); + } - // todo: make sure loading is successful - world->Load( filename ); - world->Start(); + } } @@ -407,7 +418,7 @@ //printf( "value: %d\n", item->value() ); } -void StgWorldGui::About_cb( Fl_Widget* ) +void StgWorldGui::About_cb( Fl_Widget*, StgWorldGui* world ) { fl_register_images(); @@ -416,30 +427,22 @@ const int Spc = 10; const int ButtonH = 25; const int ButtonW = 60; + const int pngH = 82; + //const int pngW = 264; Fl_Window win( Width, Height ); // make a window - // <temporary hack> - const char* stagepath = getenv("STAGEPATH"); - const char* logopath = "../../../assets/logo.png"; - char* fullpath = (char*)malloc( strlen(stagepath) + strlen(logopath) + 2 ); - strcpy( fullpath, stagepath ); - strcat( fullpath, "/" ); - strcat( fullpath, logopath ); - printf("fullpath: %s\n", fullpath); - Fl_PNG_Image png(fullpath); // load image into ram - free( fullpath ); - Fl_Box box( Spc, Spc, - Width-2*Spc, png.h() ); // widget that will contain image - + Width-2*Spc, pngH ); // widget that will contain image - // </temporary hack> + std::string fullpath; + fullpath = world->fileMan.fullPath( "stagelogo.png" ); + Fl_PNG_Image png( fullpath.c_str() ); // load image into ram box.image(png); // attach image to box - - Fl_Text_Display text( Spc, png.h()+2*Spc, - Width-2*Spc, Height-png.h()-ButtonH-4*Spc ); + + Fl_Text_Display text( Spc, pngH+2*Spc, + Width-2*Spc, Height-pngH-ButtonH-4*Spc ); text.box( FL_NO_BOX ); text.color(win.color()); @@ -462,6 +465,8 @@ win.show(); while (win.shown()) Fl::wait(); + + } void StgWorldGui::HelpAboutCallback( Fl_Widget* wid ) { This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-06-12 16:35:21
|
Revision: 6552 http://playerstage.svn.sourceforge.net/playerstage/?rev=6552&view=rev Author: alexcb Date: 2008-06-12 16:35:26 -0700 (Thu, 12 Jun 2008) Log Message: ----------- camera model refactoring Modified Paths: -------------- code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc 2008-06-12 23:35:17 UTC (rev 6551) +++ code/stage/trunk/libstage/camera.cc 2008-06-12 23:35:26 UTC (rev 6552) @@ -14,7 +14,7 @@ //perspective camera //perspective camera StgPerspectiveCamera::StgPerspectiveCamera( void ) : - _x( 0 ), _y( 0 ), _z( 0 ), _pitch( 90 ), _yaw( 0 ), _z_near( 0.1 ), _z_far( 20.0 ) + _x( 0 ), _y( 0 ), _z( 0 ), _pitch( 90 ), _yaw( 0 ), _z_near( 0.2 ), _z_far( 40.0 ), _fov( 60 ) { } @@ -33,12 +33,7 @@ void StgPerspectiveCamera::SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const { - glMatrixMode (GL_PROJECTION); - glLoadIdentity (); - - gluPerspective( 60.0, pixels_width/pixels_height, _z_near, _z_far ); - - glMatrixMode (GL_MODELVIEW); + SetProjection( pixels_width/pixels_height ); } void StgPerspectiveCamera::SetProjection( float aspect ) const @@ -46,7 +41,7 @@ glMatrixMode (GL_PROJECTION); glLoadIdentity (); - gluPerspective( 60.0, aspect, _z_near, _z_far ); + gluPerspective( _fov, aspect, _z_near, _z_far ); glMatrixMode (GL_MODELVIEW); } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-12 23:35:17 UTC (rev 6551) +++ code/stage/trunk/libstage/canvas.cc 2008-06-12 23:35:26 UTC (rev 6552) @@ -34,7 +34,7 @@ use_perspective_camera = false; perspective_camera.setPose( -3.0, 0.0, 1.0 ); - perspective_camera._pitch = 70.0; //look down + perspective_camera.setPitch( 70.0 ); //look down startx = starty = 0; //panx = pany = stheta = sphi = 0.0; @@ -175,7 +175,7 @@ else { if( use_perspective_camera == true ) { - perspective_camera._z += Fl::event_dy() / 10.0; + perspective_camera.scroll( Fl::event_dy() / 10.0 ); } else { camera.scale( Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h() ); } @@ -190,8 +190,8 @@ int dy = Fl::event_y() - starty; if( use_perspective_camera == true ) { - perspective_camera._yaw += -dx; - perspective_camera._pitch += -dy; + perspective_camera.addYaw( -dx ); + perspective_camera.addPitch( -dy ); } else { camera.pitch( 0.5 * static_cast<double>( dy ) ); camera.yaw( 0.5 * static_cast<double>( dx ) ); @@ -205,8 +205,7 @@ int dy = Fl::event_y() - starty; if( use_perspective_camera == true ) { - perspective_camera._x += -dx / 100.0 * perspective_camera._z; - perspective_camera._y += dy / 100.0 * perspective_camera._z; + perspective_camera.move( -dx, dy, 0.0 ); } else { camera.move( -dx, dy ); @@ -359,15 +358,13 @@ void StgCanvas::renderFrame( bool robot_camera ) { - uint32_t showflags = this->showflags; if( robot_camera == true ) showflags = STG_SHOW_BLOCKS; - if( ! (showflags & STG_SHOW_TRAILS) || robot_camera ) + if( ! (showflags & STG_SHOW_TRAILS) || robot_camera == true ) glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - glPushMatrix(); // draw the world size rectangle in white, using the polygon offset @@ -532,9 +529,11 @@ void StgCanvas::draw() { static bool loaded_texture = false; - // static int centerx = 0, centery = 0; - //puts( "CANVAS" ); + //Enable the following to debug camera model +// if( loaded_texture == true && use_perspective_camera == true ) +// return; + if (!valid()) { valid(1); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-06-12 23:35:17 UTC (rev 6551) +++ code/stage/trunk/libstage/model.cc 2008-06-12 23:35:26 UTC (rev 6552) @@ -967,7 +967,7 @@ DrawBlinkenlights(); // draw speech bubble - if( this->say_string ) + if( say_string && flags & STG_SHOW_STATUS ) { float stheta = -dtor( canvas->camera.getPitch() ); float sphi = dtor( canvas->camera.getYaw() ); Modified: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc 2008-06-12 23:35:17 UTC (rev 6551) +++ code/stage/trunk/libstage/model_camera.cc 2008-06-12 23:35:26 UTC (rev 6552) @@ -55,7 +55,11 @@ { StgModel::Load(); Worldfile* wf = world->GetWorldFile(); - + int fov = wf->ReadLength( id, "fov", -1 ); + if( fov > 0 ) { + _camera.setFov( fov ); + } + } @@ -66,11 +70,10 @@ float* StgModelCamera::laser() { - int h = 32; - int w = 32; + //TODO allow the h and w to be passed by user + int h = 320; + int w = 320; - static StgPerspectiveCamera camera; - static GLfloat* data_gl = NULL; static GLfloat* data = NULL; if( data == NULL ) { @@ -80,20 +83,25 @@ } glViewport( 0, 0, w, h ); - camera.update(); - camera.SetProjection( 1.0 ); + _camera.update(); + _camera.SetProjection( 1.0 ); - camera.setPose( parent->GetGlobalPose().x, parent->GetGlobalPose().y, 0.1 ); - camera.setYaw( rtod( parent->GetGlobalPose().a ) - 90.0 ); - camera.Draw(); + _camera.setPose( parent->GetGlobalPose().x, parent->GetGlobalPose().y, 0.3 ); + _camera.setYaw( rtod( parent->GetGlobalPose().a ) - 90.0 ); + _camera.Draw(); _canvas->renderFrame( true ); glReadPixels(0, h / 2, w, 1, GL_DEPTH_COMPONENT, GL_FLOAT, data_gl ); for( int i = 0; i < w; i++ ) { - data[ w-1-i ] = camera.realDistance( data_gl[ i ] ); + data[ w-1-i ] = _camera.realDistance( data_gl[ i ] ); } + for( int i = 0; i < 32; i++ ) { + for( int j = 1; j < 10; j++ ) + data[ i ] += data[ i * 10 + j ]; + data[ i ] /= 10.0; + } _canvas->invalidate(); return data; } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-12 23:35:17 UTC (rev 6551) +++ code/stage/trunk/libstage/stage.hh 2008-06-12 23:35:26 UTC (rev 6552) @@ -521,6 +521,7 @@ const uint32_t STG_SHOW_FOOTPRINT = (1<<10); const uint32_t STG_SHOW_BLOCKS_2D = (1<<10); const uint32_t STG_SHOW_TRAILRISE = (1<<11); + const uint32_t STG_SHOW_STATUS = (1<<12); // forward declare class StgWorld; @@ -1712,13 +1713,14 @@ class StgPerspectiveCamera : public StgCamera { - public: //TODO make this private + private: float _x, _y, _z; float _pitch; //left-right (about y) float _yaw; //up-down (about x) - private: + float _z_near; float _z_far; + float _fov; public: StgPerspectiveCamera( void ); @@ -1729,13 +1731,33 @@ void update( void ); inline void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; } + inline void addPose( float x, float y, float z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; } + inline void move( float x, float y, float z ) + { + //scale relative to zoom level + _x *= _z; + _y *= _z; + + //adjust for yaw angle + std::cout << "yaw:" << _yaw << std::endl; + _x += cos( dtor( _yaw ) ) * x; + _y += -sin( dtor( _yaw ) ) * x; + + _x += sin( dtor( _yaw ) ) * y; + _y += cos( dtor( _yaw ) ) * y; + } + inline void setFov( float fov ) { _fov = fov; } inline void setYaw( float yaw ) { _yaw = yaw; } + inline void addYaw( float yaw ) { _yaw += yaw; } + inline void setPitch( float pitch ) { _pitch = pitch; } + inline void addPitch( float pitch ) { _pitch += pitch; } inline float realDistance( float z_buf_val ) const { //formulat found at http://www.cs.unc.edu/~hoff/techrep/openglz.html //Z = Zn*Zf / (Zf - z*(Zf-Zn)) return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) ); } + inline void scroll( float dy ) { _z += dy; } }; class StgOrthoCamera : public StgCamera @@ -1759,7 +1781,7 @@ //adjust for pitch angle y = y / cos( dtor( _pitch ) ); - //adjust for yaw andle + //adjust for yaw angle _x += cos( dtor( _yaw ) ) * x; _y += -sin( dtor( _yaw ) ) * x; @@ -2369,6 +2391,8 @@ int _frame_data_width; int _frame_data_height; + StgPerspectiveCamera _camera; + public: StgModelCamera( StgWorld* world, This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-06-13 22:37:00
|
Revision: 6577 http://playerstage.svn.sourceforge.net/playerstage/?rev=6577&view=rev Author: rtv Date: 2008-06-13 22:37:08 -0700 (Fri, 13 Jun 2008) Log Message: ----------- cleaning and expanding API, adding tests Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/test.cc code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-14 03:06:19 UTC (rev 6576) +++ code/stage/trunk/libstage/canvas.cc 2008-06-14 05:37:08 UTC (rev 6577) @@ -445,11 +445,11 @@ StgModel* mod = ((StgModel*)it->data); if( mod->displaylist == 0 ) - { - mod->displaylist = glGenLists(1); - mod->BuildDisplayList( showflags ); // ready to be rendered - } + mod->displaylist = glGenLists(1); + if( mod->body_dirty ) + mod->BuildDisplayList( showflags ); // ready to be rendered + // move into this model's local coordinate frame glPushMatrix(); gl_pose_shift( &mod->pose ); @@ -534,7 +534,7 @@ // if( loaded_texture == true && use_perspective_camera == true ) // return; - if (!valid()) + if (!valid() || world->dirty ) { valid(1); FixViewport(w(), h()); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-06-14 03:06:19 UTC (rev 6576) +++ code/stage/trunk/libstage/model.cc 2008-06-14 05:37:08 UTC (rev 6577) @@ -328,7 +328,7 @@ col, inherit_color )); // force recreation of display lists before drawing - body_dirty = true; + NeedRedraw(); } @@ -336,7 +336,7 @@ { stg_block_list_destroy( blocks ); blocks = NULL; - body_dirty = true; + NeedRedraw(); } void StgModel::AddBlockRect( double x, double y, @@ -1191,7 +1191,8 @@ void StgModel::NeedRedraw( void ) { - this->world->dirty = true; + this->body_dirty = true; + //this->world->dirty = true; } void StgModel::GPoseDirtyTree( void ) @@ -1259,7 +1260,7 @@ StgBlock::ScaleList( blocks, &geom.size ); - body_dirty = true; + NeedRedraw(); Map(); @@ -1269,7 +1270,7 @@ void StgModel::SetColor( stg_color_t col ) { this->color = col; - body_dirty = true; + NeedRedraw(); CallCallbacks( &this->color ); } @@ -1413,13 +1414,22 @@ } -// Check to see if the given change in pose will yield a collision -// with obstacles. Returns a pointer to the first entity we are in -// collision with, and stores the location of the hit in hitx,hity (if -// non-null) Returns NULL if no collision. +void StgModel::PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, + stg_meters_t ymin, stg_meters_t ymax ) +{ + while( TestCollision( NULL, NULL ) ) + SetPose( random_pose( xmin,xmax, ymin, ymax )); +} -StgModel* StgModel::TestCollision( stg_pose_t* posedelta, - double* hitx, double* hity ) + +StgModel* StgModel::TestCollision( stg_meters_t* hitx, stg_meters_t* hity ) +{ + return TestCollision( new_pose(0,0,0,0), hitx, hity ); +} + +StgModel* StgModel::TestCollision( stg_pose_t posedelta, + stg_meters_t* hitx, + stg_meters_t* hity ) { /* stg_model_t* child_hit = NULL; */ @@ -1480,7 +1490,7 @@ // raytrace in local coordinates stg_raytrace_sample_t sample; - Raytrace( pose_sum( *posedelta, edgepose), + Raytrace( pose_sum( posedelta, edgepose), range, (stg_block_match_func_t)collision_match, NULL, @@ -1529,7 +1539,7 @@ p.a += velocity.a * interval; // test to see if this pose change would cause us to crash - StgModel* hitthing = this->TestCollision( &p, NULL, NULL ); + StgModel* hitthing = this->TestCollision( p, NULL, NULL ); //double hitx=0, hity=0; //StgModel* hitthing = this->TestCollision( &p, &hitx, &hity ); Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2008-06-14 03:06:19 UTC (rev 6576) +++ code/stage/trunk/libstage/stage.cc 2008-06-14 05:37:08 UTC (rev 6577) @@ -316,6 +316,16 @@ }; +stg_pose_t Stg::random_pose( stg_meters_t xmin, stg_meters_t xmax, + stg_meters_t ymin, stg_meters_t ymax ) +{ + return new_pose( xmin + drand48() * (xmax-xmin), + ymin + drand48() * (ymax-ymin), + 0, + normalize( drand48() * (2.0 * M_PI) )); +} + + // sets [result] to the pose of [p2] in [p1]'s coordinate system void Stg::stg_pose_sum( stg_pose_t* result, stg_pose_t* p1, stg_pose_t* p2 ) { Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-14 03:06:19 UTC (rev 6576) +++ code/stage/trunk/libstage/stage.hh 2008-06-14 05:37:08 UTC (rev 6577) @@ -376,6 +376,11 @@ parameters */ stg_pose_t new_pose( stg_meters_t x, stg_meters_t y, stg_meters_t z, stg_radians_t a ); + /** return a random pose within the bounding rectangle, with z=0 and + angle random */ + stg_pose_t random_pose( stg_meters_t xmin, stg_meters_t xmax, + stg_meters_t ymin, stg_meters_t ymax ); + const uint32_t STG_MOVE_TRANS = (1 << 0); const uint32_t STG_MOVE_ROT = (1 << 1); const uint32_t STG_MOVE_SCALE = (1 << 2); @@ -1205,11 +1210,26 @@ // worldfile section identifier StgWorld* world; // pointer to the world in which this model exists - - StgModel* TestCollision( stg_pose_t* pose, - double* hitx, double* hity ); - - + + /** Check to see if the given change in pose will yield a collision + with obstacles. Returns a pointer to the first entity we are in + collision with, and stores the location of the hit in hitx,hity (if + non-null) Returns NULL if no collision. */ + StgModel* TestCollision( stg_pose_t pose, + stg_meters_t* hitx, + stg_meters_t* hity ); + + /** Check to see if the current pose is in a collision + with obstacles. Returns a pointer to the first entity we are in + collision with, and stores the location of the hit in hitx,hity (if + non-null) Returns NULL if no collision. */ + StgModel* TestCollision( stg_meters_t* hitx, + stg_meters_t* hity ); + +public: void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, + stg_meters_t ymin, stg_meters_t ymax ); + +protected: void Map(); void UnMap(); Modified: code/stage/trunk/libstage/test.cc =================================================================== --- code/stage/trunk/libstage/test.cc 2008-06-14 03:06:19 UTC (rev 6576) +++ code/stage/trunk/libstage/test.cc 2008-06-14 05:37:08 UTC (rev 6577) @@ -44,7 +44,7 @@ world.Start(); - for( stg_meters_t x=0; x<5; x+=0.01 ) + for( stg_meters_t x=0; x<5; x+=0.1 ) { pose = new_pose( x, 0, 0, 0 ); mod.SetPose( pose ); @@ -52,7 +52,7 @@ interact( &world ); } - for( stg_meters_t y=0; y<5; y+=0.01 ) + for( stg_meters_t y=0; y<5; y+=0.1 ) { pose = new_pose( 0, y, 0, 0 ); mod.SetPose( pose ); @@ -60,7 +60,7 @@ interact( &world ); } - for( stg_meters_t z=0; z<5; z+=0.01 ) + for( stg_meters_t z=0; z<5; z+=0.1 ) { pose = new_pose( 0, 0, z, 0 ); mod.SetPose( pose ); @@ -68,7 +68,7 @@ interact( &world ); } - for( stg_radians_t a=0; a<dtor(360); a+=dtor(1) ) + for( stg_radians_t a=0; a<dtor(360); a+=dtor(2) ) { pose = new_pose( 0, 0, 0, a ); mod.SetPose( pose ); @@ -77,7 +77,7 @@ interact( &world ); } - for( stg_radians_t a=0; a<dtor(360); a+=dtor(1) ) + for( stg_radians_t a=0; a<dtor(360); a+=dtor(2) ) { pose = new_pose( cos(a), sin(a), 0, 0 ); mod.SetPose( pose ); @@ -85,20 +85,69 @@ interact( &world ); } + mod.SetPose( new_pose( 0,0,0,0 )); + stg_geom_t geom; bzero( &geom, sizeof(geom) ); for( stg_meters_t x=0.01; x<5; x+=0.1 ) - for( stg_meters_t y=0.01; y<5; y+=0.1 ) - //for( stg_meters_t z=0.01; z<5; z+=0.01 ) - { - geom.size.x = x; - geom.size.y = y; - geom.size.z = 1.0; - - mod.SetGeom( geom ); - interact( &world ); - } + { + geom.size.x = x; + geom.size.y = 1.0; + geom.size.z = 1.0; + + mod.SetGeom( geom ); + interact( &world ); + } + for( stg_meters_t y=0.01; y<5; y+=0.1 ) + { + geom.size.x = 5; + geom.size.y = y; + geom.size.z = 1.0; + + mod.SetGeom( geom ); + interact( &world ); + } + + for( stg_meters_t z=0.01; z<5; z+=0.1 ) + { + geom.size.x = 1; + geom.size.y = 1; + geom.size.z = z; + + mod.SetGeom( geom ); + interact( &world ); + } + + geom.size.x = 0.5; + geom.size.y = 0.5; + geom.size.z = 0.5; + + mod.SetGeom( geom ); + + +#define POP 100 + + StgModel* m[POP]; + for( int i=0; i<POP; i++ ) + { + m[i] = new StgModel( &world, NULL, 0, "model" ); + m[i]->SetGeom( geom ); + //m[i]->SetPose( random_pose( -10,10, -10,10 ) ); + m[i]->PlaceInFreeSpace( -10, 10, -10, 10 ); + m[i]->SetColor( lrand48() | 0xFF000000 ); + interact( &world ); + } + + +// for( int i=0; i<POP; i++ ) +// { +// m[i]->PlaceInFreeSpace( -10, 10, -10, 10 ); +// m[i]->SetColor( 0xFF00FF00 ); +// interact( &world ); +// } + + StgWorldGui::Run(); } Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-06-14 03:06:19 UTC (rev 6576) +++ code/stage/trunk/libstage/world.cc 2008-06-14 05:37:08 UTC (rev 6577) @@ -77,6 +77,8 @@ { SuperRegion* sr = new SuperRegion( x, y ); g_hash_table_insert( superregions, &sr->origin, sr ); + + dirty = true; // force redraw return sr; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-06-16 10:48:08
|
Revision: 6585 http://playerstage.svn.sourceforge.net/playerstage/?rev=6585&view=rev Author: alexcb Date: 2008-06-16 10:48:06 -0700 (Mon, 16 Jun 2008) Log Message: ----------- camera model data visualization Modified Paths: -------------- code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc 2008-06-16 16:15:18 UTC (rev 6584) +++ code/stage/trunk/libstage/camera.cc 2008-06-16 17:48:06 UTC (rev 6585) @@ -18,6 +18,20 @@ { } +void StgPerspectiveCamera::move( float x, float y, float z ) +{ + //scale relative to zoom level + x *= _z / 100.0; + y *= _z / 100.0; + + //adjust for yaw angle + _x += cos( dtor( _yaw ) ) * x; + _x += -sin( dtor( _yaw ) ) * y; + + _y += sin( dtor( _yaw ) ) * x; + _y += cos( dtor( _yaw ) ) * y; + } + void StgPerspectiveCamera::Draw( void ) const { glMatrixMode (GL_MODELVIEW); @@ -33,7 +47,26 @@ void StgPerspectiveCamera::SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const { - SetProjection( pixels_width/pixels_height ); +// SetProjection( pixels_width/pixels_height ); + + glMatrixMode (GL_PROJECTION); + glLoadIdentity (); + + float near_clip = _z_near; + float far_clip = _z_far; + float aspect = 1.0; + float top = tan( dtor( _fov ) /2.0 ) * near_clip; + float bottom = -top; + float left = - aspect * top; + float right = -left; + + top = 0.001; + bottom = -top; + + glFrustum( left, right, bottom, top, near_clip, far_clip ); + + glMatrixMode (GL_MODELVIEW); + } void StgPerspectiveCamera::SetProjection( float aspect ) const @@ -42,6 +75,7 @@ glLoadIdentity (); gluPerspective( _fov, aspect, _z_near, _z_far ); +// glFrustum( -10, 10, -10, 10, _z_near, _z_far ); glMatrixMode (GL_MODELVIEW); } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-16 16:15:18 UTC (rev 6584) +++ code/stage/trunk/libstage/canvas.cc 2008-06-16 17:48:06 UTC (rev 6585) @@ -574,7 +574,7 @@ gl_font( FL_HELVETICA, 12 ); if( use_perspective_camera == true ) { - perspective_camera.SetProjection( w(), h(), 0.01, 0.5 ); + perspective_camera.SetProjection( w() / h() ); } else { stg_bounds3d_t extent = world->GetExtent(); camera.SetProjection( w(), h(), extent.y.min, extent.y.max ); Modified: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc 2008-06-16 16:15:18 UTC (rev 6584) +++ code/stage/trunk/libstage/model_camera.cc 2008-06-16 17:48:06 UTC (rev 6585) @@ -19,7 +19,7 @@ stg_id_t id, char* typestr ) : StgModel( world, parent, id, typestr ), -_frame_data( NULL ), _frame_data_width( 0 ), _frame_data_height( 0 ) +_frame_data( NULL ), _frame_data_width( 0 ), _frame_data_height( 0 ), _width( 0 ) { PRINT_DEBUG2( "Constructing StgModelCamera %d (%s)\n", id, typestr ); @@ -55,55 +55,60 @@ { StgModel::Load(); Worldfile* wf = world->GetWorldFile(); - int fov = wf->ReadLength( id, "fov", -1 ); - if( fov > 0 ) { - _camera.setFov( fov ); - } + + _camera.setFov( wf->ReadLength( id, "fov", _camera.fov() ) ); + _camera.setYaw( wf->ReadLength( id, "yaw", _camera.yaw() ) ); + _camera.setPitch( wf->ReadLength( id, "pitch", _camera.pitch() ) ); + + _width = wf->ReadLength( id, "width", _width ); + + //TODO move to constructor + _frame_data_width = _width; + _frame_data_height = 100; } void StgModelCamera::Update( void ) { + GetFrame( _frame_data_width, _frame_data_height, true ); + StgModel::Update(); } float* StgModelCamera::laser() { - //TODO allow the h and w to be passed by user - int h = 320; - int w = 320; - - static GLfloat* data_gl = NULL; - static GLfloat* data = NULL; - if( data == NULL ) { - data = new GLfloat[ h * w ]; - data_gl = new GLfloat[ h * w ]; - - } - - glViewport( 0, 0, w, h ); - _camera.update(); - _camera.SetProjection( 1.0 ); - - _camera.setPose( parent->GetGlobalPose().x, parent->GetGlobalPose().y, 0.3 ); - _camera.setYaw( rtod( parent->GetGlobalPose().a ) - 90.0 ); - _camera.Draw(); - - _canvas->renderFrame( true ); - - glReadPixels(0, h / 2, w, 1, GL_DEPTH_COMPONENT, GL_FLOAT, data_gl ); - - for( int i = 0; i < w; i++ ) { - data[ w-1-i ] = _camera.realDistance( data_gl[ i ] ); - } - for( int i = 0; i < 32; i++ ) { - for( int j = 1; j < 10; j++ ) - data[ i ] += data[ i * 10 + j ]; - data[ i ] /= 10.0; - } - _canvas->invalidate(); - return data; + return NULL; +// //TODO allow the h and w to be passed by user +// int w = _width; +// int h = 1; +// +// static GLfloat* data_gl = NULL; +// static GLfloat* data = NULL; +// if( data == NULL ) { +// data = new GLfloat[ h * w ]; +// data_gl = new GLfloat[ h * w ]; +// +// } +// +// glViewport( 0, 0, w, h ); +// _camera.update(); +// _camera.SetProjection( w, h, 0.1, 40.0 ); +// +// _camera.setPose( parent->GetGlobalPose().x, parent->GetGlobalPose().y, 0.3 ); +// _camera.setYaw( rtod( parent->GetGlobalPose().a ) - 90.0 ); +// _camera.Draw(); +// +// _canvas->renderFrame( true ); +// +// glReadPixels(0, 0, w, h, GL_DEPTH_COMPONENT, GL_FLOAT, data_gl ); +// +// for( int i = 0; i < w; i++ ) { +// data[ w-1-i ] = _camera.realDistance( data_gl[ i ]); +// } +// +// _canvas->invalidate(); +// return data; } const char* StgModelCamera::GetFrame( int width, int height, bool depth_buffer ) @@ -113,7 +118,7 @@ if( width == 0 || height == 0 ) return NULL; - if( width != _frame_data_width || height != _frame_data_height ) { + if( width != _frame_data_width || height != _frame_data_height || _frame_data == NULL ) { if( _frame_data != NULL ) delete _frame_data; _frame_data = new char[ 4 * width * height ]; //assumes a max of depth 4 @@ -134,6 +139,11 @@ GL_DEPTH_COMPONENT, //GL_RGB, GL_FLOAT, //GL_UNSIGNED_BYTE, _frame_data ); + //transform length into linear length + float* data = ( float* )( _frame_data ); //TODO use static_cast here + int buf_size = width * height; + for( int i = 0; i < buf_size; i++ ) + data[ i ] = _camera.realDistance( data[ i ] ) + 0.1; } else { glReadPixels(0, 0, width, height, GL_RGB, @@ -146,7 +156,50 @@ return _frame_data; } +//TODO create lines outlineing camera frustrum, then iterate over each depth measurement and create a square +void StgModelCamera::DataVisualize( void ) +{ + float w_fov = _camera.fov(); + float h_fov = _camera.fov(); + float length = 8.0; + + float start_fov = 90 + w_fov / 2.0; + float end_fov = 90 - w_fov / 2.0; + + float x, y; + + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); //TODO this doesn't seem to work. + float z = cos( dtor( h_fov / 2.0 ) ) * length; + + glColor4f(1.0, 0.0, 0.0, 1.0 ); + + const float* data = ( float* )( _frame_data ); //TODO use static_cast here + int w = _frame_data_width, h = _frame_data_height; + for( int i = 0; i < w; i++ ) { + float z_a = h_fov / _width * static_cast< float >( i ); + for( int j = 0; j < h; j++ ) { + float length = data[ i + j * w ]; + float a; + float z = cos( z_a ) * length; + //TODO rename j to not conflict with loop + int j = i + 1; + if( j < _width/2 ) + a = 90 - w_fov / _width * static_cast< float >( _width/2 - 1 - j ); + else + a = 90 + w_fov / _width * static_cast< float >( j - _width/2 ); + x = sin( dtor( a ) ) * length; + y = cos( dtor( a ) ) * length; + glBegin( GL_LINE_LOOP ); + glVertex3f( 0.0, 0.0, 0.0 ); + glVertex3f( x, y, 0.0 ); + glEnd(); + + } + } + +} + void StgModelCamera::Draw( uint32_t flags, StgCanvas* canvas ) { StgModel::Draw( flags, canvas ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-16 16:15:18 UTC (rev 6584) +++ code/stage/trunk/libstage/stage.hh 2008-06-16 17:48:06 UTC (rev 6585) @@ -1769,22 +1769,12 @@ inline void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; } inline void addPose( float x, float y, float z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; } - inline void move( float x, float y, float z ) - { - //scale relative to zoom level - _x *= _z; - _y *= _z; - - //adjust for yaw angle - std::cout << "yaw:" << _yaw << std::endl; - _x += cos( dtor( _yaw ) ) * x; - _y += -sin( dtor( _yaw ) ) * x; - - _x += sin( dtor( _yaw ) ) * y; - _y += cos( dtor( _yaw ) ) * y; - } + void move( float x, float y, float z ); inline void setFov( float fov ) { _fov = fov; } inline void setYaw( float yaw ) { _yaw = yaw; } + inline float yaw( void ) const { return _yaw; } + inline float pitch( void ) const { return _pitch; } + inline float fov( void ) const { return _fov; } inline void addYaw( float yaw ) { _yaw += yaw; } inline void setPitch( float pitch ) { _pitch = pitch; } inline void addPitch( float pitch ) { _pitch += pitch; } @@ -2428,6 +2418,8 @@ int _frame_data_width; int _frame_data_height; + int _width; //TODO merge into frame_data_width + StgPerspectiveCamera _camera; public: @@ -2442,7 +2434,10 @@ virtual void Load(); virtual void Update(); virtual void Draw( uint32_t flags, StgCanvas* canvas ); + virtual void DataVisualize(); + inline int getWidth( void ) const { return _width; } + ///Take a screenshot from the camera's perspective const char* GetFrame( int width, int height, bool depth_buffer ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-06-16 16:09:08
|
Revision: 6595 http://playerstage.svn.sourceforge.net/playerstage/?rev=6595&view=rev Author: rtv Date: 2008-06-16 16:09:03 -0700 (Mon, 16 Jun 2008) Log Message: ----------- fixed picker Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-16 22:59:12 UTC (rev 6594) +++ code/stage/trunk/libstage/canvas.cc 2008-06-16 23:09:03 UTC (rev 6595) @@ -75,26 +75,31 @@ StgModel* StgCanvas::Select( int x, int y ) { // TODO XX - return NULL; + //return NULL; // render all models in a unique color make_current(); // make sure the GL context is current - glClearColor ( 0,0,0,1 ); + glClearColor ( 1,1,1,1 ); // white glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + glDisable(GL_DITHER); + glDisable(GL_BLEND); // turns off alpha blending, so we read back + // exactly what we write to a pixel // render all top-level, draggable models in a color that is their - // id + a 100% alpha value + // id for( GList* it=world->children; it; it=it->next ) { StgModel* mod = (StgModel*)it->data; + + if( mod->GuiMask() & (STG_MOVE_TRANS | STG_MOVE_ROT )) + { + glColor4ubv( (GLubyte*)&mod->id ); - if( mod->GuiMask() & (STG_MOVE_TRANS | STG_MOVE_ROT )) - { - // TODO XX - uint32_t col = (uint32_t)mod; //(mod->Id() | 0xFF000000); - glColor4ubv( (GLubyte*)&col ); - mod->DrawPicker(); + // printf( "model %d color %d %x\n", + // mod->id, mod->id, mod->id ); + + mod->DrawPicker(); } } @@ -106,17 +111,15 @@ uint32_t id; glReadPixels( x,viewport[3]-y,1,1, GL_RGBA,GL_UNSIGNED_BYTE,(void*)&id ); + + StgModel* mod = (StgModel*)g_hash_table_lookup( StgModel::modelsbyid, (void*)id ); - // strip off the alpha channel byte to retrieve the model id - //id &= 0x00FFFFFF; + //printf("%p %s %d %x\n", mod, mod ? mod->Token() : "(none)", id, id ); - StgModel* mod = (StgModel*)id;//world->GetModel( id ); - - //printf("%p %s %d\n", mod, mod ? mod->Token() : "", id ); - + // put things back the way we found them glEnable(GL_DITHER); + glEnable(GL_BLEND); glClearColor ( 0.7, 0.7, 0.8, 1.0); - //glClearColor ( 1,1,1,1 ); if( mod ) // we clicked on a root model { Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-06-16 22:59:12 UTC (rev 6594) +++ code/stage/trunk/libstage/model.cc 2008-06-16 23:09:03 UTC (rev 6595) @@ -108,7 +108,8 @@ #include "texture_manager.hh" #include <limits.h> - // basic model + +//static const members const bool StgModel::DEFAULT_BLOBRETURN = true; const bool StgModel::DEFAULT_BOUNDARY = false; const stg_color_t StgModel::DEFAULT_COLOR = (0xFFFF0000); // solid red @@ -131,18 +132,23 @@ const bool StgModel::DEFAULT_OUTLINE = true; const bool StgModel::DEFAULT_RANGERRETURN = true; - // speech bubble colors const stg_color_t StgModel::BUBBLE_FILL = 0xFFC8C8FF; // light blue/grey const stg_color_t StgModel::BUBBLE_BORDER = 0xFF000000; // black const stg_color_t StgModel::BUBBLE_TEXT = 0xFF000000; // black +// static members +uint32_t StgModel::count = 0; + +GHashTable* StgModel::modelsbyid = g_hash_table_new( NULL, NULL ); + // constructor StgModel::StgModel( StgWorld* world, StgModel* parent, const stg_model_type_t type ) : StgAncestor() -{ +{ + assert( modelsbyid ); assert( world ); PRINT_DEBUG3( "Constructing model world: %s parent: %s type: %d ", @@ -154,8 +160,12 @@ this->world = world; this->debug = false; this->type = type; + this->id = StgModel::count++; // assign a unique ID and increment + // the global model counter - // Adding this model to its ancestor also gives this model a + g_hash_table_insert( modelsbyid, (void*)this->id, this ); + + // Adding this model to its ancestor also gives this model a // sensible default name StgAncestor* anc = parent ? (StgAncestor*)parent : (StgAncestor*)world; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-16 22:59:12 UTC (rev 6594) +++ code/stage/trunk/libstage/stage.hh 2008-06-16 23:09:03 UTC (rev 6595) @@ -1170,7 +1170,16 @@ friend class StgWorld; friend class StgWorldGui; friend class StgCanvas; + +private: + /** the number of models instatiated - used to assign unique IDs */ + static uint32_t count; + static GHashTable* modelsbyid; +public: + /** unique process-wide identifier for this model */ + uint32_t id; + protected: // basic model This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-06-16 20:43:49
|
Revision: 6616 http://playerstage.svn.sourceforge.net/playerstage/?rev=6616&view=rev Author: rtv Date: 2008-06-16 20:43:58 -0700 (Mon, 16 Jun 2008) Log Message: ----------- repaired test program Modified Paths: -------------- code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/test.cc code/stage/trunk/libstageplugin/p_driver.cc Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-17 03:35:12 UTC (rev 6615) +++ code/stage/trunk/libstage/stage.hh 2008-06-17 03:43:58 UTC (rev 6616) @@ -1103,9 +1103,9 @@ double ppm = DEFAULT_PPM ); virtual ~StgWorld(); - - FileManager fileMan; + FileManager fileMan; + stg_usec_t SimTimeNow(void){ return sim_time;} ; stg_usec_t RealTimeNow(void); stg_usec_t RealTimeSinceStart(void); @@ -2030,6 +2030,12 @@ virtual void Load( const char* filename ); virtual void UnLoad(); virtual bool Save( const char* filename ); + + /** Set the minimum real time interval between world updates, in + microeconds. */ + void SetRealTimeInterval( stg_usec_t usec ) + { interval_real = usec; } + // static callback functions static void LoadCallback( Fl_Widget* wid, StgWorldGui* world ); Modified: code/stage/trunk/libstage/test.cc =================================================================== --- code/stage/trunk/libstage/test.cc 2008-06-17 03:35:12 UTC (rev 6615) +++ code/stage/trunk/libstage/test.cc 2008-06-17 03:43:58 UTC (rev 6616) @@ -37,8 +37,12 @@ Init( &argc, &argv); StgWorldGui world( 400,400, "Test" ); + + world.SetRealTimeInterval( 20000 ); + world.Start(); + stg_geom_t geom; bzero( &geom, sizeof(geom) ); @@ -159,7 +163,7 @@ //m[i]->PlaceInFreeSpace( -10, 10, -10, 10 ); top->SetColor( lrand48() | 0xFF000000 ); - interact( &world ); + //interact( &world ); } for( int i=0; i<POP; i++ ) @@ -172,7 +176,7 @@ m[i]->SetVelocity( v ); - interact( &world ); + //interact( &world ); } // for( int i=0; i<POP; i++ ) Modified: code/stage/trunk/libstageplugin/p_driver.cc =================================================================== --- code/stage/trunk/libstageplugin/p_driver.cc 2008-06-17 03:35:12 UTC (rev 6615) +++ code/stage/trunk/libstageplugin/p_driver.cc 2008-06-17 03:43:58 UTC (rev 6616) @@ -564,30 +564,28 @@ { Interface* interface = (Interface*)g_ptr_array_index( this->devices, i ); assert( interface ); - + switch( interface->addr.interf ) { case PLAYER_SIMULATION_CODE: - //if( stg_world_update( this->world, FALSE ) ) - world->Cycle(); - //player_quit = TRUE; // set Player's global quit flag - break; - + world->Update(); + break; + default: - { - // Has enough time elapsed since the last time we published on this - // interface? This really needs some thought, as each model/interface - // should have a configurable publishing rate. For now, I'm using the - // world's update rate (which appears to be stored as msec). - BPG - double currtime; - GlobalTime->GetTimeDouble(&currtime); - if((currtime - interface->last_publish_time) >= - (interface->publish_interval_msec / 1e3)) - { - interface->Publish(); - interface->last_publish_time = currtime; - } - } + { + // Has enough time elapsed since the last time we published on this + // interface? This really needs some thought, as each model/interface + // should have a configurable publishing rate. For now, I'm using the + // world's update rate (which appears to be stored as msec). - BPG + double currtime; + GlobalTime->GetTimeDouble(&currtime); + if((currtime - interface->last_publish_time) >= + (interface->publish_interval_msec / 1e3)) + { + interface->Publish(); + interface->last_publish_time = currtime; + } + } } } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-06-17 11:19:47
|
Revision: 6619 http://playerstage.svn.sourceforge.net/playerstage/?rev=6619&view=rev Author: jeremy_asher Date: 2008-06-17 11:19:50 -0700 (Tue, 17 Jun 2008) Log Message: ----------- fixed some warnings Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-06-17 17:45:11 UTC (rev 6618) +++ code/stage/trunk/libstage/model.cc 2008-06-17 18:19:50 UTC (rev 6619) @@ -225,15 +225,10 @@ this->on_velocity_list = false; this->last_update = 0; - this->interval = 1e4; // 10msec + this->interval = (stg_usec_t)1e4; // 10msec this->initfunc = NULL; - this->startup_hook = NULL; - this->shutdown_hook = NULL; - this->load_hook = NULL; - this->save_hook = NULL; - this->wf = NULL; this->wf_entity = 0; @@ -722,18 +717,18 @@ void StgModel::UpdateIfDue( void ) { - if( world->sim_time >= - (last_update + interval) ) - this->Update(); + if( world->sim_time >= + (last_update + interval) ) + this->Update(); } void StgModel::Update( void ) { - //printf( "[%lu] %s update (%d subs)\n", - // this->world->sim_time_ms, this->token, this->subs ); - - CallCallbacks( &update_hook ); - last_update = world->sim_time; + //printf( "[%llu] %s update (%d subs)\n", + // this->world->sim_time, this->token, this->subs ); + + CallCallbacks( &update_hook ); + last_update = world->sim_time; } void StgModel::DrawSelected() Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-17 17:45:11 UTC (rev 6618) +++ code/stage/trunk/libstage/stage.hh 2008-06-17 18:19:50 UTC (rev 6619) @@ -88,6 +88,7 @@ class StgCanvas; class Worldfile; class StgWorld; + class StgWorldGui; class StgModel; /** Initialize the Stage library */ Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-06-17 17:45:11 UTC (rev 6618) +++ code/stage/trunk/libstage/world.cc 2008-06-17 18:19:50 UTC (rev 6619) @@ -215,7 +215,8 @@ wf->ReadString( entity, "name", token ); this->interval_sim = (stg_usec_t)thousand * - wf->ReadInt( entity, "interval_sim", this->interval_sim/thousand ); + wf->ReadInt( entity, "interval_sim", + (int)(this->interval_sim/thousand) ); if( wf->PropertyExists( entity, "quit_time" ) ) this->quit_time = (stg_usec_t)million * Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-17 17:45:11 UTC (rev 6618) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-17 18:19:50 UTC (rev 6619) @@ -264,7 +264,7 @@ wf->ReadInt( wf_section, "paused", this->paused ); this->interval_real = (stg_usec_t)thousand * - wf->ReadInt( wf_section, "interval_real", this->interval_real/thousand ); + wf->ReadInt( wf_section, "interval_real", (int)(this->interval_real/thousand) ); int width = (int)wf->ReadTupleFloat(wf_section, "size", 0, w() ); int height = (int)wf->ReadTupleFloat(wf_section, "size", 1, h() ); @@ -585,6 +585,7 @@ bool val = paused ? true : StgWorld::Update(); + stg_usec_t interval; stg_usec_t timenow; @@ -598,19 +599,16 @@ double sleeptime = (double)interval_real - (double)interval; if( sleeptime > 0 ) - usleep( MIN(sleeptime,100000) ); // check the GUI at 10Hz min + usleep( (stg_usec_t)MIN(sleeptime,100000) ); // check the GUI at 10Hz min } while( interval < interval_real ); real_time_of_last_update = timenow; - //stg_usec_t timenow = RealTimeSinceStart(); interval_log[updates%INTERVAL_LOG_LEN] = interval_real;//timenow - real_time_now; - // real_time_now = timenow; - return val; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-06-17 14:29:04
|
Revision: 6630 http://playerstage.svn.sourceforge.net/playerstage/?rev=6630&view=rev Author: jeremy_asher Date: 2008-06-17 14:29:13 -0700 (Tue, 17 Jun 2008) Log Message: ----------- Added view menu toggle item and hotkey for status flag (currently stall icon and speech bubbles), enabled by default Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-06-17 20:54:06 UTC (rev 6629) +++ code/stage/trunk/libstage/canvas.cc 2008-06-17 21:29:13 UTC (rev 6630) @@ -45,7 +45,7 @@ dragging = false; rotating = false; - showflags = STG_SHOW_CLOCK | STG_SHOW_BLOCKS | STG_SHOW_GRID | STG_SHOW_DATA; + showflags = STG_SHOW_CLOCK | STG_SHOW_BLOCKS | STG_SHOW_GRID | STG_SHOW_DATA | STG_SHOW_STATUS; // // start the timer that causes regular redraws Fl::add_timeout( ((double)interval/1000), @@ -260,14 +260,10 @@ return 1; case 3: { - puts( "button 3" ); startx = Fl::event_x(); starty = Fl::event_y(); if( Select( startx, starty ) ) - { - printf( "rotating" ); rotating = true; - } return 1; } default: Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-06-17 20:54:06 UTC (rev 6629) +++ code/stage/trunk/libstage/model.cc 2008-06-17 21:29:13 UTC (rev 6630) @@ -946,58 +946,61 @@ if( blinkenlights ) DrawBlinkenlights(); - // draw speech bubble - if( say_string && flags & STG_SHOW_STATUS ) - { - float stheta = -dtor( canvas->camera.getPitch() ); - float sphi = dtor( canvas->camera.getYaw() ); - float scale = canvas->camera.getScale(); + + if ( flags & STG_SHOW_STATUS ) { + // draw speech bubble + if( say_string ) + { + float stheta = -dtor( canvas->camera.getPitch() ); + float sphi = dtor( canvas->camera.getYaw() ); + float scale = canvas->camera.getScale(); - glPushMatrix(); + glPushMatrix(); - // move above the robot - glTranslatef( 0, 0, 0.5 ); + // move above the robot + glTranslatef( 0, 0, 0.5 ); - // rotate to face screen - glRotatef( -rtod(global_pose.a + sphi), 0,0,1 ); - glRotatef( rtod(stheta), 1,0,0 ); + // rotate to face screen + glRotatef( -rtod(global_pose.a + sphi), 0,0,1 ); + glRotatef( rtod(stheta), 1,0,0 ); - const float m = 4 / scale; // margin - float w = gl_width( this->say_string ) / scale; // scaled text width - float h = gl_height() / scale; // scaled text height + const float m = 4 / scale; // margin + float w = gl_width( this->say_string ) / scale; // scaled text width + float h = gl_height() / scale; // scaled text height - // draw inside of bubble - PushColor( BUBBLE_FILL ); - glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); - glPolygonMode( GL_FRONT, GL_FILL ); - glEnable( GL_POLYGON_OFFSET_FILL ); - glPolygonOffset( 1.0, 1.0 ); - gl_draw_octagon( w, h, m ); - glDisable( GL_POLYGON_OFFSET_FILL ); - PopColor(); - // draw outline of bubble - PushColor( BUBBLE_BORDER ); - glLineWidth( 1 ); - glEnable( GL_LINE_SMOOTH ); - glPolygonMode( GL_FRONT, GL_LINE ); - gl_draw_octagon( w, h, m ); - glPopAttrib(); - PopColor(); + // draw inside of bubble + PushColor( BUBBLE_FILL ); + glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); + glPolygonMode( GL_FRONT, GL_FILL ); + glEnable( GL_POLYGON_OFFSET_FILL ); + glPolygonOffset( 1.0, 1.0 ); + gl_draw_octagon( w, h, m ); + glDisable( GL_POLYGON_OFFSET_FILL ); + PopColor(); + // draw outline of bubble + PushColor( BUBBLE_BORDER ); + glLineWidth( 1 ); + glEnable( GL_LINE_SMOOTH ); + glPolygonMode( GL_FRONT, GL_LINE ); + gl_draw_octagon( w, h, m ); + glPopAttrib(); + PopColor(); - // draw text - PushColor( BUBBLE_TEXT ); - glTranslatef( 0, 0, 0.1 ); // draw text forwards of bubble - gl_draw_string( m, m, 0.0, this->say_string ); - PopColor(); + // draw text + PushColor( BUBBLE_TEXT ); + glTranslatef( 0, 0, 0.1 ); // draw text forwards of bubble + gl_draw_string( m, m, 0.0, this->say_string ); + PopColor(); - glPopMatrix(); - } + glPopMatrix(); + } - if( stall ) - { - DrawImage( TextureManager::getInstance()._stall_texture_id, canvas, 0.85 ); + if( stall ) + { + DrawImage( TextureManager::getInstance()._stall_texture_id, canvas, 0.85 ); + } } // shift up the CS to the top of this model @@ -1115,7 +1118,7 @@ glPopMatrix(); // drop out of local coords } -void StgModel::BuildDisplayList( int flags ) +void StgModel::BuildDisplayList( uint32_t flags ) { glNewList( displaylist, GL_COMPILE ); DrawBlocks(); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-17 20:54:06 UTC (rev 6629) +++ code/stage/trunk/libstage/stage.hh 2008-06-17 21:29:13 UTC (rev 6630) @@ -1398,7 +1398,7 @@ int displaylist; /** Compile the display list for this model */ - void BuildDisplayList( int flags ); + void BuildDisplayList( uint32_t flags ); stg_model_type_t type; Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-17 20:54:06 UTC (rev 6629) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-17 21:29:13 UTC (rev 6630) @@ -118,6 +118,7 @@ static const char* MITEM_VIEW_BLOCKSRISING = "&View/T&rails/&Blocks rising"; static const char* MITEM_VIEW_ARROWS = "&View/T&rails/&Arrows rising"; static const char* MITEM_VIEW_TRAILS = "&View/&Trail"; +static const char* MITEM_VIEW_STATUS = "&View/&Status"; static const char* MITEM_VIEW_PERSPECTIVE = "&View/Perspective camera"; // this should be set by CMake @@ -175,6 +176,9 @@ mbar->add( MITEM_VIEW_TRAILS, 't', (Fl_Callback*)view_toggle_cb, (void*)canvas, FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_TRAILS ? FL_MENU_VALUE : 0 )); + + mbar->add( MITEM_VIEW_STATUS, 's', (Fl_Callback*)view_toggle_cb, (void*)canvas, + FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_STATUS ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_FOOTPRINTS, FL_CTRL+'f', (Fl_Callback*)view_toggle_cb, (void*)canvas, FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_FOOTPRINT ? FL_MENU_VALUE : 0 )); @@ -293,9 +297,10 @@ uint32_t trailsrising = wf->ReadInt(wf_section, "show_trails_rising", flags & STG_SHOW_TRAILRISE ) ? STG_SHOW_TRAILRISE : 0; uint32_t arrows = wf->ReadInt(wf_section, "show_arrows", flags & STG_SHOW_ARROWS ) ? STG_SHOW_ARROWS : 0; uint32_t footprints = wf->ReadInt(wf_section, "show_footprints", flags & STG_SHOW_FOOTPRINT ) ? STG_SHOW_FOOTPRINT : 0; + uint32_t status = wf->ReadInt(wf_section, "show_status", flags & STG_SHOW_STATUS ) ? STG_SHOW_STATUS : 0; canvas->SetShowFlags( grid | data | follow | blocks | quadtree | clock - | trails | arrows | footprints | trailsrising ); + | trails | arrows | footprints | trailsrising | status ); canvas->invalidate(); // we probably changed something // fix the GUI menu checkboxes to match @@ -321,6 +326,10 @@ item = (Fl_Menu_Item*)mbar->find_item( MITEM_VIEW_QUADTREE ); (flags & STG_SHOW_QUADTREE) ? item->check() : item->clear(); + item = (Fl_Menu_Item*)mbar->find_item( MITEM_VIEW_STATUS ); + (flags & STG_SHOW_STATUS) ? item->check() : item->clear(); + + // TODO - per model visualizations load } @@ -483,6 +492,7 @@ else if( strcmp(picked, MITEM_VIEW_ARROWS ) == 0 ) canvas->InvertView( STG_SHOW_ARROWS ); else if( strcmp(picked, MITEM_VIEW_TRAILS ) == 0 ) canvas->InvertView( STG_SHOW_TRAILS ); else if( strcmp(picked, MITEM_VIEW_BLOCKSRISING ) == 0 ) canvas->InvertView( STG_SHOW_TRAILRISE ); + else if( strcmp(picked, MITEM_VIEW_STATUS ) == 0 ) canvas->InvertView( STG_SHOW_STATUS ); else if( strcmp(picked, MITEM_VIEW_PERSPECTIVE ) == 0 ) { canvas->use_perspective_camera = ! canvas->use_perspective_camera; canvas->invalidate(); } else PRINT_ERR1( "Unrecognized menu item \"%s\" not handled", picked ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-06-18 14:49:49
|
Revision: 6643 http://playerstage.svn.sourceforge.net/playerstage/?rev=6643&view=rev Author: jeremy_asher Date: 2008-06-18 14:49:58 -0700 (Wed, 18 Jun 2008) Log Message: ----------- Fixed saving bug Modified Paths: -------------- code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-18 21:16:44 UTC (rev 6642) +++ code/stage/trunk/libstage/stage.hh 2008-06-18 21:49:58 UTC (rev 6643) @@ -1010,7 +1010,7 @@ //stg_id_t id; - GHashTable* models_by_id; ///< the models that make up the world, indexed by id + //GHashTable* models_by_id; ///< the models that make up the world, indexed by id GHashTable* models_by_name; ///< the models that make up the world, indexed by name GList* velocity_list; ///< a list of models that have non-zero velocity, for efficient updating @@ -1133,7 +1133,7 @@ /** call func( model, arg ) for each model in the world */ void ForEachModel( GHFunc func, void* arg ) - { g_hash_table_foreach( models_by_id, func, arg ); }; + { g_hash_table_foreach( models_by_name, func, arg ); }; /** Return the number of times the world has been updated. */ long unsigned int GetUpdateCount() Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-18 21:16:44 UTC (rev 6642) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-18 21:49:58 UTC (rev 6643) @@ -594,6 +594,9 @@ // TODO - per model visualizations save } + + // TODO - error checking + return true; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-06-18 15:55:24
|
Revision: 6645 http://playerstage.svn.sourceforge.net/playerstage/?rev=6645&view=rev Author: jeremy_asher Date: 2008-06-18 15:55:33 -0700 (Wed, 18 Jun 2008) Log Message: ----------- Added options dialog skeleton Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Added Paths: ----------- code/stage/trunk/libstage/options_dlg.cc code/stage/trunk/libstage/options_dlg.hh Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2008-06-18 21:56:47 UTC (rev 6644) +++ code/stage/trunk/libstage/CMakeLists.txt 2008-06-18 22:55:33 UTC (rev 6645) @@ -1,32 +1,34 @@ -add_library( stage SHARED - world.cc - worldgui.cc - worldfile.cc - stage.cc - typetable.cc - stage.hh # get headers into IDE projects - file_manager.hh - file_manager.cc - ancestor.cc - block.cc - canvas.cc - camera.cc - gl.cc - glcolorstack.cc - model.cc - model_blinkenlight.cc - model_blobfinder.cc - model_callbacks.cc - model_camera.cc - model_fiducial.cc - model_laser.cc - model_load.cc - model_position.cc - model_props.cc - model_ranger.cc - resource.cc - texture_manager.cc +add_library(stage SHARED + world.cc + worldgui.cc + worldfile.cc + stage.cc + typetable.cc + stage.hh + file_manager.hh + file_manager.cc + ancestor.cc + block.cc + canvas.cc + camera.cc + gl.cc + glcolorstack.cc + model.cc + model_blinkenlight.cc + model_blobfinder.cc + model_callbacks.cc + model_camera.cc + model_fiducial.cc + model_laser.cc + model_load.cc + model_position.cc + model_props.cc + model_ranger.cc + resource.cc + texture_manager.cc + options_dlg.cc + options_dlg.hh ) target_link_libraries( stage Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2008-06-18 21:56:47 UTC (rev 6644) +++ code/stage/trunk/libstage/gl.cc 2008-06-18 22:55:33 UTC (rev 6645) @@ -17,10 +17,11 @@ // TODO - this could be faster, but we don't draw a lot of text void Stg::gl_draw_string( float x, float y, float z, const char *str ) { - const char *c; - glRasterPos3f(x, y,z); - for (c=str; *c != '\0'; c++) - glutBitmapCharacter( GLUT_BITMAP_HELVETICA_12, *c); + //const char *c; + glRasterPos3f( x, y, z ); + gl_draw(str); +// for (c=str; *c != '\0'; c++) +// glutBitmapCharacter( GLUT_BITMAP_HELVETICA_12, *c); } void Stg::gl_speech_bubble( float x, float y, float z, const char* str ) Added: code/stage/trunk/libstage/options_dlg.cc =================================================================== --- code/stage/trunk/libstage/options_dlg.cc (rev 0) +++ code/stage/trunk/libstage/options_dlg.cc 2008-06-18 22:55:33 UTC (rev 6645) @@ -0,0 +1,12 @@ +#include "options_dlg.hh" +#include <FL/Fl.H> + +OptionsDlg::OptionsDlg( std::vector<Option> options ) : Fl_Window( 180, 250, "Options" ) { + end(); +} + +void OptionsDlg::display() { + Fl_Window::show(); + while ( shown() ) + Fl::wait(); +} Added: code/stage/trunk/libstage/options_dlg.hh =================================================================== --- code/stage/trunk/libstage/options_dlg.hh (rev 0) +++ code/stage/trunk/libstage/options_dlg.hh 2008-06-18 22:55:33 UTC (rev 6645) @@ -0,0 +1,29 @@ +#ifndef _OPTIONS_DLG_H_ +#define _OPTIONS_DLG_H_ + +#include <FL/Fl_Window.H> + +#include <string> +#include <vector> + + +class Option { +private: + std::string name; + bool value; +public: + Option( std::string n, bool v ) : name(n), value(v) { } + inline const bool get() const { return value; } + inline void set( bool val ) { value = val; } +}; + + +class OptionsDlg : private Fl_Window { +private: +public: + OptionsDlg( std::vector<Option> options ); + + void display(); +}; + +#endif \ No newline at end of file Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-18 21:56:47 UTC (rev 6644) +++ code/stage/trunk/libstage/stage.hh 2008-06-18 22:55:33 UTC (rev 6645) @@ -67,7 +67,7 @@ #include <FL/Fl_Window.H> #include <FL/fl_draw.H> #include <FL/gl.h> // FLTK takes care of platform-specific GL stuff -#include <FL/glut.H> +//#include <FL/glut.H> #ifdef __APPLE__ #include <OpenGL/glu.h> @@ -2037,6 +2037,7 @@ static void HelpAboutCallback( Fl_Widget* wid ); static void view_toggle_cb( Fl_Menu_Bar* menubar, StgCanvas* canvas ); static void WindowCallback( Fl_Widget* wid, StgWorldGui* world ); + static void optionsDlgCb( Fl_Widget*, StgCanvas* canvas ); bool SaveAsDialog(); bool CloseWindowQuery(); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-18 21:56:47 UTC (rev 6644) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-18 22:55:33 UTC (rev 6645) @@ -106,6 +106,7 @@ #include <FL/Fl_File_Chooser.H> #include "file_manager.hh" +#include "options_dlg.hh" static const char* MITEM_VIEW_DATA = "&View/&Data"; static const char* MITEM_VIEW_BLOCKS = "&View/&Blocks"; @@ -129,8 +130,7 @@ - StgWorldGui::StgWorldGui(int W,int H,const char* L) -: Fl_Window(0,0,W,H,L) +StgWorldGui::StgWorldGui(int W,int H,const char* L) : Fl_Window(0,0,W,H,L) { //size_range( 100,100 ); // set minimum window size @@ -186,6 +186,8 @@ FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_ARROWS ? FL_MENU_VALUE : 0 )); mbar->add( MITEM_VIEW_BLOCKSRISING, FL_CTRL+'t', (Fl_Callback*)view_toggle_cb, (void*)canvas, FL_MENU_TOGGLE| (canvas->showflags & STG_SHOW_TRAILRISE ? FL_MENU_VALUE : 0 )); + + mbar->add( "View/&Options", FL_CTRL + 'o', (Fl_Callback *)optionsDlgCb, canvas ); mbar->add( "&Help", 0, 0, 0, FL_SUBMENU ); mbar->add( "Help/&About Stage...", 0, (Fl_Callback *)About_cb, this ); @@ -504,6 +506,12 @@ //printf( "value: %d\n", item->value() ); } +void StgWorldGui::optionsDlgCb( Fl_Widget*, StgCanvas* canvas ) { + std::vector<Option> options; + OptionsDlg od( options ); + od.display(); +} + void StgWorldGui::About_cb( Fl_Widget*, StgWorldGui* world ) { fl_register_images(); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-06-19 14:49:05
|
Revision: 6647 http://playerstage.svn.sourceforge.net/playerstage/?rev=6647&view=rev Author: jeremy_asher Date: 2008-06-19 14:49:14 -0700 (Thu, 19 Jun 2008) Log Message: ----------- Updated Options framework Modified Paths: -------------- code/stage/trunk/libstage/options_dlg.cc code/stage/trunk/libstage/options_dlg.hh code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/options_dlg.cc =================================================================== --- code/stage/trunk/libstage/options_dlg.cc 2008-06-19 16:13:34 UTC (rev 6646) +++ code/stage/trunk/libstage/options_dlg.cc 2008-06-19 21:49:14 UTC (rev 6647) @@ -1,12 +1,56 @@ #include "options_dlg.hh" #include <FL/Fl.H> -OptionsDlg::OptionsDlg( std::vector<Option> options ) : Fl_Window( 180, 250, "Options" ) { - end(); + +OptionsDlg::OptionsDlg( const std::vector<Option>& opts, Fl_Callback* cb, int w, int h ) : +Fl_Window( w, h, "Options" ), +options( opts ), +changedCb( cb ) { + const int hm = w/6; + const int vm = 2; + const int btnH = 25; + + scroll = new Fl_Scroll( 0, 0, w, h-btnH-2*vm ); + scroll->type( Fl_Scroll::VERTICAL ); + + Fl_Check_Button* check; + for ( unsigned int i=0; i<options.size(); i++ ) { + check = new Fl_Check_Button( 0,30*i, w, 30, options[ i ].name().c_str() ); + if ( options[ i ].val() ) + check->set(); + check->callback( checkChanged, this ); + } + scroll->end(); + + button = new Fl_Button( hm, h-btnH-vm, w-2*hm, btnH, "Apply to all" ); + button->callback( applyAllPress, this ); + this->end(); } +OptionsDlg::~OptionsDlg() { + delete button; + delete scroll; // deletes members +} + void OptionsDlg::display() { Fl_Window::show(); while ( shown() ) Fl::wait(); } + +void OptionsDlg::checkChanged( Fl_Widget* w, void* p ) { + Fl_Check_Button* check = static_cast<Fl_Check_Button*>( w ); + OptionsDlg* oDlg = static_cast<OptionsDlg*>( p ); + + int item = oDlg->scroll->find( check ); + oDlg->options[ item ].set( check->value() ); + oDlg->changedItem = oDlg->options[ item ]; + oDlg->changedCb( oDlg, NULL ); +} + +void OptionsDlg::applyAllPress( Fl_Widget* w, void* p ) { + OptionsDlg* oDlg = static_cast<OptionsDlg*>( p ); + + oDlg->changedItem = Option( -1, "", false ); + oDlg->changedCb( oDlg, NULL ); +} Modified: code/stage/trunk/libstage/options_dlg.hh =================================================================== --- code/stage/trunk/libstage/options_dlg.hh 2008-06-19 16:13:34 UTC (rev 6646) +++ code/stage/trunk/libstage/options_dlg.hh 2008-06-19 21:49:14 UTC (rev 6647) @@ -2,6 +2,9 @@ #define _OPTIONS_DLG_H_ #include <FL/Fl_Window.H> +#include <FL/Fl_Scroll.H> +#include <FL/Fl_Check_Button.H> +#include <FL/Fl_Button.H> #include <string> #include <vector> @@ -9,21 +12,36 @@ class Option { private: - std::string name; + std::string optName; bool value; + int index; public: - Option( std::string n, bool v ) : name(n), value(v) { } - inline const bool get() const { return value; } - inline void set( bool val ) { value = val; } + Option() { } + Option( int i, std::string n, bool v ) : index( i ), optName( n ), value( v ) { } + Option( const Option& o ) : index( o.index ), optName( o.optName ), value( o.value ) { } + const std::string name() const { return optName; } + const int id() const { return index; } + const bool val() const { return value; } + void set( bool val ) { value = val; } }; -class OptionsDlg : private Fl_Window { +class OptionsDlg : protected Fl_Window { private: + std::vector<Option> options; + Fl_Scroll* scroll; + Fl_Button* button; + Fl_Callback* changedCb; + static void checkChanged( Fl_Widget* w, void* p ); + static void applyAllPress( Fl_Widget* w, void* p ); + Option changedItem; public: - OptionsDlg( std::vector<Option> options ); - - void display(); + OptionsDlg( const std::vector<Option>& opts, Fl_Callback* cb, int w, int h ); + //void setChangeCb( Fl_Callback* cb ) { changedCb=cb; } + virtual ~OptionsDlg(); + void display(); + + const Option changed() const { return changedItem; } }; -#endif \ No newline at end of file +#endif Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-06-19 16:13:34 UTC (rev 6646) +++ code/stage/trunk/libstage/stage.hh 2008-06-19 21:49:14 UTC (rev 6647) @@ -76,6 +76,7 @@ #endif #include "file_manager.hh" +#include "options_dlg.hh" /** The Stage library uses its own namespace */ namespace Stg @@ -2037,7 +2038,8 @@ static void HelpAboutCallback( Fl_Widget* wid ); static void view_toggle_cb( Fl_Menu_Bar* menubar, StgCanvas* canvas ); static void WindowCallback( Fl_Widget* wid, StgWorldGui* world ); - static void optionsDlgCb( Fl_Widget*, StgCanvas* canvas ); + static void optionsDlgCb( Fl_Widget* w, StgCanvas* canvas ); + static void optionChangeCb( Fl_Widget* w, void* p ); bool SaveAsDialog(); bool CloseWindowQuery(); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-06-19 16:13:34 UTC (rev 6646) +++ code/stage/trunk/libstage/worldgui.cc 2008-06-19 21:49:14 UTC (rev 6647) @@ -105,8 +105,6 @@ #include <FL/Fl_Text_Display.H> #include <FL/Fl_File_Chooser.H> -#include "file_manager.hh" -#include "options_dlg.hh" static const char* MITEM_VIEW_DATA = "&View/&Data"; static const char* MITEM_VIEW_BLOCKS = "&View/&Blocks"; @@ -506,12 +504,25 @@ //printf( "value: %d\n", item->value() ); } -void StgWorldGui::optionsDlgCb( Fl_Widget*, StgCanvas* canvas ) { +void StgWorldGui::optionsDlgCb( Fl_Widget* w, StgCanvas* canvas ) { std::vector<Option> options; - OptionsDlg od( options ); - od.display(); + for (int i=0; i<10; i++) { + Option o( i, "Option", i%2*true ); + options.push_back(o); + } + + + OptionsDlg oDlg( options, optionChangeCb, 180, 250 ); + oDlg.display(); } +void StgWorldGui::optionChangeCb( Fl_Widget* w, void* p ) { + OptionsDlg* oDlg = static_cast<OptionsDlg*>( w ); + Option o = oDlg->changed(); + printf( "\"%s\"[%d] changed to %d!\n", o.name().c_str(), o.id(), o.val() ); + // update flag(s) +} + void StgWorldGui::About_cb( Fl_Widget*, StgWorldGui* world ) { fl_register_images(); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |