Skip to content
Browse files

removing accidentally added files

  • Loading branch information...
1 parent d30c3c4 commit d13c672354ad9589d3bcf059cbf1cfa3b13ad7e6 @rtv committed
Showing with 0 additions and 2,145 deletions.
  1. +0 −1,358 libstage/model.cc~
  2. +0 −346 libstage/model_camera.cc~
  3. +0 −441 libstage/model_ranger.cc~
View
1,358 libstage/model.cc~
@@ -1,1358 +0,0 @@
-/**
- $Rev$
-**/
-
-/** @defgroup model
-
- The basic model simulates an object with basic properties; position,
- size, velocity, color, visibility to various sensors, etc. The basic
- model also has a body made up of a list of lines. Internally, the
- basic model is used as the base class for all other model types. You
- can use the basic model to simulate environmental objects.
-
- API: Stg::Model
-
- <h2>Worldfile properties</h2>
-
- @par Summary and default values
-
- @verbatim
- model
- (
- pose [ 0.0 0.0 0.0 0.0 ]
- size [ 0.1 0.1 0.1 ]
- origin [ 0.0 0.0 0.0 0.0 ]
- velocity [ 0.0 0.0 0.0 0.0 ]
-
- update_interval 100
-
- color "red"
- color_rgba [ 0.0 0.0 0.0 1.0 ]
- bitmap ""
- ctrl ""
-
- # determine how the model appears in various sensors
- fiducial_return 0
- fiducial_key 0
- obstacle_return 1
- ranger_return 1.0
- blob_return 1
- ranger_return 1.0
- gripper_return 0
-
- # GUI properties
- gui_nose 0
- gui_grid 0
- gui_outline 1
- gui_move 0 (1 if the model has no parents);
-
- boundary 0
- mass 10.0
- map_resolution 0.1
- say ""
- alwayson 0
-
- stack_children 1
- )
- @endverbatim
-
- @par Details
-
- - pose [ x:<float> y:<float> z:<float> heading:<float> ] \n
- specify the pose of the model in its parent's coordinate system
-
- - size [ x:<float> y:<float> z:<float> ]\n specify the size of the
- model in each dimension
-
- - origin [ x:<float> y:<float> z:<float> heading:<float> ]\n
- specify the position of the object's center, relative to its pose
-
- - velocity [ x:<float> y:<float> z:<float> heading:<float>
- omega:<float> ]\n Specify the initial velocity of the model. Note
- that if the model hits an obstacle, its velocity will be set to
- zero.
-
- - update_interval int (defaults to 100) The amount of simulated
- time in milliseconds between calls to Model::Update(). Controls
- the frequency with which this model's data is generated.
-
- - velocity_enable int (defaults to 0)\n Most models ignore their
- velocity state. This saves on processing time since most models
- never have their velocity set to anything but zero. Some
- subclasses (e.g. ModelPosition) change this default as they are
- expecting to move. Users can specify a non-zero value here to
- enable velocity control of this model. This achieves the same as
- calling Model::VelocityEnable()
-
- - color <string>\n specify the color of the object using a color
- name from the X11 database (rgb.txt)
-
- - bitmap filename:<string>\n Draw the model by interpreting the
- lines in a bitmap (bmp, jpeg, gif, png supported). The file is
- opened and parsed into a set of lines. The lines are scaled to
- fit inside the rectangle defined by the model's current size.
-
- - ctrl <string>\n Specify the controller module for the model, and
- its argument string. For example, the string "foo bar bash" will
- load libfoo.so, which will have its Init() function called with
- the entire string as an argument (including the library name). It
- is up to the controller to parse the string if it needs
- arguments."
-
- - fiducial_return fiducial_id:<int>\n if non-zero, this model is
- detected by fiducialfinder sensors. The value is used as the
- fiducial ID.
-
- - fiducial_key <int> models are only detected by fiducialfinders
- if the fiducial_key values of model and fiducialfinder match. This
- allows you to have several independent types of fiducial in the
- same environment, each type only showing up in fiducialfinders
- that are "tuned" for it.
-
- - obstacle_return <int>\n if 1, this model can collide with other
- models that have this property set
-
- - blob_return <int>\n if 1, this model can be detected in the
- blob_finder (depending on its color)
-
- - ranger_return <float>\n This model is detected with this
- reflectance value by ranger_model sensors. If negative, this model
- is invisible to ranger sensors, and does not block propogation of
- range-sensing rays. This models an idealized reflectance sensor,
- and replaces the normal/bright reflectance of deprecated laser
- model. Defaults to 1.0.
-
- - gripper_return <int>\n iff 1, this model can be gripped by a
- gripper and can be pushed around by collisions with anything that
- has a non-zero obstacle_return.
-
- - gui_nose <int>\n if 1, draw a nose on the model showing its
- heading (positive X axis)
-
- - gui_grid <int>\n if 1, draw a scaling grid over the model
-
- - gui_outline <int>\n if 1, draw a bounding box around the model,
- indicating its size
-
- - gui_move <int>\n if 1, the model can be moved by the mouse in
- the GUI window
-
- - stack_children <int>\n If non-zero (the default), the coordinate
- system of child models is offset in z so that its origin is on
- _top_ of this model, making it easy to stack models together. If
- zero, the child coordinate system is not offset in z, making it
- easy to define objects in a single local coordinate system.
-*/
-
-// todo
-// - friction <float>\n Determines the proportion of velocity lost
-// per second. For example, 0.1 would mean that the object would lose
-// 10% of its speed due to friction per second. A value of zero (the
-// default) means this model can not be pushed around (infinite
-// friction).
-
-#ifndef _GNU_SOURCE
-#define _GNU_SOURCE
-#endif
-
-#include <map>
-
-//#define DEBUG 0
-#include "stage.hh"
-#include "worldfile.hh"
-using namespace Stg;
-
-// static members
-uint32_t Model::count(0);
-uint32_t Model::trail_length(50);
-uint64_t Model::trail_interval(5);
-std::map<Stg::id_t,Model*> Model::modelsbyid;
-std::map<std::string, creator_t> Model::name_map;
-
-//static const members
-static const double DEFAULT_FRICTION = 0.0;
-
-void Bounds::Load( Worldfile* wf, const int section, const char* keyword )
-{
- if( CProperty* prop = wf->GetProperty( section, keyword ) )
- {
- if( prop->values.size() != 2 )
- {
- puts( "" ); // newline
- PRINT_ERR1( "Loading 1D bounds. Need a vector of length 2: found %d.\n",
- (int)prop->values.size() );
- exit(-1);
- }
-
- min = atof( wf->GetPropertyValue( prop, 0 )) * wf->unit_length;
- max = atof( wf->GetPropertyValue( prop, 1 )) * wf->unit_length;
- }
-}
-
-bool Color::Load( Worldfile* wf, const int section )
-{
- if( wf->PropertyExists( section, "color" ))
- {
- const std::string& colorstr = wf->ReadString( section, "color", "" );
- if( colorstr != "" )
- {
- if( colorstr == "random" )
- {
- r = drand48();
- g = drand48();
- b = drand48();
- a = 1.0;
- }
- else
- {
- Color c = Color( colorstr );
- r = c.r;
- g = c.g;
- b = c.b;
- a = c.a;
- }
- }
- return true;
- }
-
- if( wf->PropertyExists( section, "color_rgba" ))
- {
- if (wf->GetProperty(section,"color_rgba")->values.size() < 4)
- {
- PRINT_ERR1( "color_rgba requires 4 values, found %d\n",
- (int)wf->GetProperty(section,"color_rgba")->values.size() );
- exit(-1);
- }
- else
- {
- r = wf->ReadTupleFloat( section, "color_rgba", 0, r );
- g = wf->ReadTupleFloat( section, "color_rgba", 1, g );
- b = wf->ReadTupleFloat( section, "color_rgba", 2, b );
- a = wf->ReadTupleFloat( section, "color_rgba", 3, a );
- }
-
- return true;
- }
-
- return false;
-}
-
-void Size::Load( Worldfile* wf, const int section, const char* keyword )
-{
- if( CProperty* prop = wf->GetProperty( section, keyword ) )
- {
- if( prop->values.size() != 3 )
- {
- puts( "" ); // newline
- PRINT_ERR1( "Loading size. Need a vector of length 3: found %d.\n",
- (int)prop->values.size() );
- exit(-1);
- }
-
- x = atof( wf->GetPropertyValue( prop, 0 )) * wf->unit_length;
- y = atof( wf->GetPropertyValue( prop, 1 )) * wf->unit_length;
- z = atof( wf->GetPropertyValue( prop, 2 )) * wf->unit_length;
- }
-}
-
-void Size::Save( Worldfile* wf, int section, const char* keyword ) const
-{
- wf->WriteTupleLength( section, keyword, 0, x );
- wf->WriteTupleLength( section, keyword, 1, y );
- wf->WriteTupleLength( section, keyword, 2, z );
-}
-
-void Pose::Load( Worldfile* wf, const int section, const char* keyword )
-{
- CProperty* prop = wf->GetProperty( section, keyword );
-
- if( prop )
- {
- if( prop->values.size() != 4 )
- {
- puts( "" ); // newline
- PRINT_ERR1( "Loading pose. Need a vector of length 4: found %d.\n",
- (int)prop->values.size() );
- exit(-1);
- }
-
- x = atof( wf->GetPropertyValue( prop, 0 )) * wf->unit_length;
- y = atof( wf->GetPropertyValue( prop, 1 )) * wf->unit_length;
- z = atof( wf->GetPropertyValue( prop, 2 )) * wf->unit_length;
- a = atof( wf->GetPropertyValue( prop, 3 )) * wf->unit_angle;
- }
-}
-
-void Pose::Save( Worldfile* wf, const int section, const char* keyword )
-{
- wf->WriteTupleLength( section, keyword, 0, x );
- wf->WriteTupleLength( section, keyword, 1, y );
- wf->WriteTupleLength( section, keyword, 2, z );
- wf->WriteTupleAngle( section, keyword, 3, a );
-}
-
-Model::Visibility::Visibility() :
- blob_return( true ),
- fiducial_key( 0 ),
- fiducial_return( 0 ),
- gripper_return( false ),
- obstacle_return( true ),
- ranger_return( 1.0 )
-{ /* nothing to do */ }
-
-
-void Model::Visibility::Load( Worldfile* wf, int wf_entity )
-{
- blob_return = wf->ReadInt( wf_entity, "blob_return", blob_return);
- fiducial_key = wf->ReadInt( wf_entity, "fiducial_key", fiducial_key);
- fiducial_return = wf->ReadInt( wf_entity, "fiducial_return", fiducial_return);
- gripper_return = wf->ReadInt( wf_entity, "gripper_return", gripper_return);
- obstacle_return = wf->ReadInt( wf_entity, "obstacle_return", obstacle_return);
- ranger_return = wf->ReadFloat( wf_entity, "ranger_return", ranger_return);
-}
-
-void Model::Visibility::Save( Worldfile* wf, int wf_entity )
-{
- wf->WriteInt( wf_entity, "blob_return", blob_return);
- wf->WriteInt( wf_entity, "fiducial_key", fiducial_key);
- wf->WriteInt( wf_entity, "fiducial_return", fiducial_return);
- wf->WriteInt( wf_entity, "gripper_return", gripper_return);
- wf->WriteInt( wf_entity, "obstacle_return", obstacle_return);
- wf->WriteFloat( wf_entity, "ranger_return", ranger_return);
-}
-
-
-Model::GuiState::GuiState() :
- grid( false ),
- move( false ),
- nose( false ),
- outline( false )
-{ /* nothing to do */}
-
-void Model::GuiState::Load( Worldfile* wf, int wf_entity )
-{
- nose = wf->ReadInt( wf_entity, "gui_nose", nose);
- grid = wf->ReadInt( wf_entity, "gui_grid", grid);
- outline = wf->ReadInt( wf_entity, "gui_outline", outline);
- move = wf->ReadInt( wf_entity, "gui_move", move );
-}
-
-// constructor
-Model::Model( World* world,
- Model* parent,
- const std::string& type ) :
- Ancestor(),
- mapped(false),
- drawOptions(),
- alwayson(false),
- blockgroup(),
- blocks_dl(0),
- boundary(false),
- callbacks(__CB_TYPE_COUNT), // one slot in the vector for each type
- color( 1,0,0 ), // red
- data_fresh(false),
- disabled(false),
- cv_list(),
- flag_list(),
- friction(DEFAULT_FRICTION),
- geom(),
- has_default_block( true ),
- id( Model::count++ ),
- interval((usec_t)1e5), // 100msec
- interval_energy((usec_t)1e5), // 100msec
- interval_pose((usec_t)1e5), // 100msec
- last_update(0),
- log_state(false),
- map_resolution(0.1),
- mass(0),
- parent(parent),
- pose(),
- power_pack( NULL ),
- pps_charging(),
- rastervis(),
- rebuild_displaylist(true),
- say_string(),
- stack_children( true ),
- stall(false),
- subs(0),
- thread_safe( false ),
- trail(trail_length),
- trail_index(0),
- type(type),
- event_queue_num( 0 ),
- used(false),
- velocity(),
- velocity_enable( false ),
- watts(0.0),
- watts_give(0.0),
- watts_take(0.0),
- wf(NULL),
- wf_entity(0),
- world(world),
- world_gui( dynamic_cast<WorldGui*>( world ) )
-{
- assert( world );
-
- PRINT_DEBUG3( "Constructing model world: %s parent: %s type: %s \n",
- world->Token(),
- parent ? parent->Token() : "(null)",
- type.c_str() );
-
- modelsbyid[id] = this;
-
- // Adding this model to its ancestor also gives this model a
- // sensible default name
- if ( parent )
- parent->AddChild( this );
- else
- {
- world->AddChild( this );
- // top level models are draggable in the GUI by default
- gui.move = true;
- }
-
- world->AddModel( this );
-
- // now we can add the basic square shape
- AddBlockRect( -0.5, -0.5, 1.0, 1.0, 1.0 );
-
- AddVisualizer( &rastervis, false );
-
- PRINT_DEBUG2( "finished model %s @ %p", this->Token(), this );
-}
-
-Model::~Model( void )
-{
- // children are removed in ancestor class
-
- if( world ) // if I'm not a worldless dummy model
- {
- UnMap(0); // remove from the movable model array
- UnMap(1); // remove from the moveable model array
-
- // remove myself from my parent's child list, or the world's child
- // list if I have no parent
- EraseAll( this, parent ? parent->children : world->children );
-
- // erase from the static map of all models
- modelsbyid.erase(id);
-
- world->RemoveModel( this );
- }
-}
-
-
-void Model::InitControllers()
-{
- CallCallbacks( CB_INIT );
-}
-
-
-void Model::AddFlag( Flag* flag )
-{
- if( flag )
- {
- flag_list.push_back( flag );
- CallCallbacks( CB_FLAGINCR );
- }
-}
-
-void Model::RemoveFlag( Flag* flag )
-{
- if( flag )
- {
- EraseAll( flag, flag_list );
- CallCallbacks( CB_FLAGDECR );
- }
-}
-
-void Model::PushFlag( Flag* flag )
-{
- if( flag )
- {
- flag_list.push_front( flag);
- CallCallbacks( CB_FLAGINCR );
- }
-}
-
-Model::Flag* Model::PopFlag()
-{
- if( flag_list.size() == 0 )
- return NULL;
-
- Flag* flag = flag_list.front();
- flag_list.pop_front();
-
- CallCallbacks( CB_FLAGDECR );
-
- return flag;
-}
-
-void Model::ClearBlocks()
-{
- blockgroup.UnMap(0);
- blockgroup.UnMap(1);
- blockgroup.Clear();
- //no need to Map() - we have no blocks
- NeedRedraw();
-}
-
-void Model::LoadBlock( Worldfile* wf, int entity )
-{
- if( has_default_block )
- {
- blockgroup.Clear();
- has_default_block = false;
- }
-
- blockgroup.LoadBlock( this, wf, entity );
-}
-
-
-Block* Model::AddBlockRect( meters_t x,
- meters_t y,
- meters_t dx,
- meters_t dy,
- meters_t dz )
-{
- UnMap(0);
- UnMap(1);
-
- std::vector<point_t> pts(4);
- pts[0].x = x;
- pts[0].y = y;
- pts[1].x = x + dx;
- pts[1].y = y;
- pts[2].x = x + dx;
- pts[2].y = y + dy;
- pts[3].x = x;
- pts[3].y = y + dy;
-
- Block* newblock = new Block( this,
- pts,
- 0, dz,
- color,
- true,
- false );
-
- blockgroup.AppendBlock( newblock );
-
- Map(0);
- Map(1);
-
- return newblock;
-}
-
-
-RaytraceResult Model::Raytrace( const Pose &pose,
- const meters_t range,
- const ray_test_func_t func,
- const void* arg,
- const bool ztest )
-{
- return world->Raytrace( LocalToGlobal(pose),
- range,
- func,
- this,
- arg,
- ztest );
-}
-
-RaytraceResult Model::Raytrace( const radians_t bearing,
- const meters_t range,
- const ray_test_func_t func,
- const void* arg,
- const bool ztest )
-{
- return world->Raytrace( LocalToGlobal(Pose(0,0,0,bearing)),
- range,
- func,
- this,
- arg,
- ztest );
-}
-
-
-void Model::Raytrace( const radians_t bearing,
- const meters_t range,
- const radians_t fov,
- const ray_test_func_t func,
- const void* arg,
- RaytraceResult* samples,
- const uint32_t sample_count,
- const bool ztest )
-{
- world->Raytrace( LocalToGlobal(Pose( 0,0,0,bearing)),
- range,
- fov,
- func,
- this,
- arg,
- samples,
- sample_count,
- ztest );
-}
-
-// convert a global pose into the model's local coordinate system
-Pose Model::GlobalToLocal( const Pose& pose ) const
-{
- // get model's global pose
- const Pose org( GetGlobalPose() );
-
- // compute global pose in local coords
- const double sx = (pose.x - org.x) * cos(org.a) + (pose.y - org.y) * sin(org.a);
- const double sy = -(pose.x - org.x) * sin(org.a) + (pose.y - org.y) * cos(org.a);
- const double sz = pose.z - org.z;
- const double sa = pose.a - org.a;
-
- return Pose( sx, sy, sz, sa );
-}
-
-void Model::Say( const std::string& str )
-{
- say_string = str;
-}
-
-// returns true iff model [testmod] is an antecedent of this model
-bool Model::IsAntecedent( const Model* testmod ) const
-{
- if( parent == NULL )
- return false;
-
- if( parent == testmod )
- return true;
-
- return parent->IsAntecedent( testmod );
-}
-
-// returns true iff model [testmod] is a descendent of this model
-bool Model::IsDescendent( const Model* testmod ) const
-{
- if( this == testmod )
- return true;
-
- FOR_EACH( it, children )
- if( (*it)->IsDescendent( testmod ) )
- return true;
-
- // neither mod nor a child of this matches testmod
- return false;
-}
-
-bool Model::IsRelated( const Model* that ) const
-{
- // is it me?
- if( this == that )
- return true;
-
- // wind up to top-level object
- Model* candidate = (Model*)this;
- while( candidate->parent )
- candidate = candidate->parent;
-
- // and recurse down the tree
- return candidate->IsDescendent( that );
-}
-
-point_t Model::LocalToGlobal( const point_t& pt) const
-{
- const Pose gpose = LocalToGlobal( Pose( pt.x, pt.y, 0, 0 ) );
- return point_t( gpose.x, gpose.y );
-}
-
-
-void Model::LocalToPixels( const std::vector<point_t>& local,
- std::vector<point_int_t>& global) const
-{
- const Pose gpose = GetGlobalPose() + geom.pose;
-
- FOR_EACH( it, local )
- {
- Pose ptpose = gpose + Pose( it->x, it->y, 0, 0 );
- global.push_back( point_int_t( (int32_t)floor( ptpose.x * world->ppm) ,
- (int32_t)floor( ptpose.y * world->ppm) ));
- }
-}
-
-void Model::MapWithChildren( unsigned int layer )
-{
- UnMap(layer);
- Map(layer);
-
- // recursive call for all the model's children
- FOR_EACH( it, children )
- (*it)->MapWithChildren(layer);
-}
-
-void Model::MapFromRoot( unsigned int layer )
-{
- Root()->MapWithChildren(layer);
-}
-
-void Model::UnMapWithChildren(unsigned int layer)
-{
- UnMap(layer);
-
- // recursive call for all the model's children
- FOR_EACH( it, children )
- (*it)->UnMapWithChildren(layer);
-}
-
-void Model::UnMapFromRoot(unsigned int layer)
-{
- Root()->UnMapWithChildren(layer);
-}
-
-void Model::Subscribe( void )
-{
- subs++;
- world->total_subs++;
- world->dirty = true; // need redraw
-
- //printf( "subscribe to %s %d\n", Token(), subs );
-
- // if this is the first sub, call startup
- if( subs == 1 )
- Startup();
-}
-
-void Model::Unsubscribe( void )
-{
- subs--;
- world->total_subs--;
- world->dirty = true; // need redraw
-
- //printf( "unsubscribe from %s %d\n", Token(), subs );
-
- // if this is the last sub, call shutdown
- if( subs == 0 )
- this->Shutdown();
-}
-
-
-void pose_invert( Pose* pose )
-{
- pose->x = -pose->x;
- pose->y = -pose->y;
- pose->a = pose->a;
-}
-
-void Model::Print( char* prefix ) const
-{
- if( prefix )
- printf( "%s model ", prefix );
- else
- printf( "Model ");
-
- printf( "%s:%s\n",
- // id,
- world->Token(),
- token.c_str() );
-
- FOR_EACH( it, children )
- (*it)->Print( prefix );
-}
-
-const char* Model::PrintWithPose() const
-{
- const Pose gpose = GetGlobalPose();
-
- static char txt[256];
- snprintf(txt, sizeof(txt), "%s @ [%.2f,%.2f,%.2f,%.2f]",
- token.c_str(),
- gpose.x, gpose.y, gpose.z, gpose.a );
-
- return txt;
-}
-
-void Model::Startup( void )
-{
- //printf( "Startup model %s\n", this->token );
- //printf( "model %s using queue %d\n", token, event_queue_num );
-
- // if we're thread safe, we can use an event queue >0
- event_queue_num = world->GetEventQueue( this );
-
- // put my first update request in the world's queue
- if( thread_safe )
- world->Enqueue( event_queue_num, interval, this, UpdateWrapper, NULL );
- else
- world->Enqueue( 0, interval, this, UpdateWrapper, NULL );
-
- if( velocity_enable )
- world->active_velocity.insert(this);
-
- if( FindPowerPack() )
- world->active_energy.insert( this );
-
- CallCallbacks( CB_STARTUP );
-}
-
-void Model::Shutdown( void )
-{
- //printf( "Shutdown model %s\n", this->token );
- CallCallbacks( CB_SHUTDOWN );
-
- world->active_energy.erase( this );
- world->active_velocity.erase( this );
- velocity_enable = false;
-
- // allows data visualizations to be cleared.
- NeedRedraw();
-}
-
-
-void Model::Update( void )
-{
- //printf( "Q%d model %p %s update\n", event_queue_num, this, Token() );
-
- // CallCallbacks( CB_UPDATE );
-
- last_update = world->sim_time;
-
- if( subs > 0 ) // no subscriptions means we don't need to be updated
- world->Enqueue( event_queue_num, interval, this, UpdateWrapper, NULL );
-
- // if we updated the model then it needs to have its update
- // callback called in series back in the main thread. It's
- // not safe to run user callbacks in a worker thread, as
- // they may make OpenGL calls or unsafe Stage API calls,
- // etc. We queue up the callback into a queue specific to
-
- if( ! callbacks[Model::CB_UPDATE].empty() )
- world->pending_update_callbacks[event_queue_num].push(this);
-}
-
-void Model::CallUpdateCallbacks( void )
-{
- CallCallbacks( CB_UPDATE );
-}
-
-meters_t Model::ModelHeight() const
-{
- meters_t m_child = 0; //max size of any child
- FOR_EACH( it, children )
- m_child = std::max( m_child, (*it)->ModelHeight() );
-
- //height of model + max( child height )
- return geom.size.z + m_child;
-}
-
-void Model::AddToPose( double dx, double dy, double dz, double da )
-{
- Pose pose = this->GetPose();
- pose.x += dx;
- pose.y += dy;
- pose.z += dz;
- pose.a += da;
-
- this->SetPose( pose );
-}
-
-void Model::AddToPose( const Pose& pose )
-{
- this->AddToPose( pose.x, pose.y, pose.z, pose.a );
-}
-
-void Model::PlaceInFreeSpace( meters_t xmin, meters_t xmax,
- meters_t ymin, meters_t ymax )
-{
- while( TestCollision() )
- SetPose( Pose::Random( xmin,xmax, ymin, ymax ));
-}
-
-void Model::AppendTouchingModels( ModelPtrSet& touchers )
-{
- blockgroup.AppendTouchingModels( touchers );
-}
-
-
-Model* Model::TestCollision()
-{
- Model* hitmod = blockgroup.TestCollision();
-
- if( hitmod == NULL )
- FOR_EACH( it, children )
- {
- hitmod = (*it)->TestCollision();
- if( hitmod )
- break;
- }
-
- //printf( "mod %s test collision done.\n", token );
- return hitmod;
-}
-
-void Model::UpdateCharge()
-{
- PowerPack* mypp = FindPowerPack();
- assert( mypp );
-
- if( watts > 0 ) // dissipation rate
- {
- // consume energy stored in the power pack
- mypp->Dissipate( watts * (interval_energy * 1e-6), GetGlobalPose() );
- }
-
- if( watts_give > 0 ) // transmission to other powerpacks max rate
- {
- // detach charger from all the packs charged last time
- FOR_EACH( it, pps_charging )
- (*it)->ChargeStop();
- pps_charging.clear();
-
- // run through and update all appropriate touchers
- ModelPtrSet touchers;
- AppendTouchingModels( touchers );
-
- FOR_EACH( it, touchers )
- {
- Model* toucher = (*it);
- PowerPack* hispp =toucher->FindPowerPack();
-
- if( hispp && toucher->watts_take > 0.0)
- {
- //printf( " toucher %s can take up to %.2f wats\n",
- // toucher->Token(), toucher->watts_take );
-
- const watts_t rate = std::min( watts_give, toucher->watts_take );
- const joules_t amount = rate * interval_energy * 1e-6;
-
- //printf ( "moving %.2f joules from %s to %s\n",
- // amount, token, toucher->token );
-
- // set his charging flag
- hispp->ChargeStart();
-
- // move some joules from me to him
- mypp->TransferTo( hispp, amount );
-
- // remember who we are charging so we can detatch next time
- pps_charging.push_front( hispp );
- }
- }
- }
-}
-
-
-void Model::Move( void )
-{
- if( velocity.IsZero() )
- return;
-
- if( disabled )
- return;
-
- // convert usec to sec
- const double interval( (double)world->sim_interval / 1e6 );
-
- // find the change of pose due to our velocity vector
- const Pose p( velocity.x * interval,
- velocity.y * interval,
- velocity.z * interval,
- normalize( velocity.a * interval ));
-
- // the pose we're trying to achieve (unless something stops us)
- const Pose newpose( pose + p );
-
- // stash the original pose so we can put things back if we hit
- const Pose startpose( pose );
-
- pose = newpose; // do the move provisionally - we might undo it below
-
- //const unsigned int layer( world->updates%2 );
-
- const unsigned int layer( world->updates%2 );
-
- UnMapWithChildren( layer ); // remove from all blocks
- MapWithChildren( layer ); // render into new blocks
-
- if( TestCollision() ) // crunch!
- {
- // put things back the way they were
- // this is expensive, but it happens _very_ rarely for most people
- pose = startpose;
- UnMapWithChildren( layer );
- MapWithChildren( layer );
- SetStall(true);
- }
- else
- {
- world->dirty = true; // need redraw
- SetStall(false);
- }
-}
-
-
-void Model::UpdateTrail()
-{
- // get the current item and increment the counter
- TrailItem* item = &trail[trail_index++];
-
- // record the current info
- item->time = world->sim_time;
- item->pose = GetGlobalPose();
- item->color = color;
-
- // wrap around ring buffer
- trail_index %= trail_length;
-}
-
-Model* Model::GetUnsubscribedModelOfType( const std::string& type ) const
-{
- if( (this->type == type) && (this->subs == 0) )
- return const_cast<Model*> (this); // discard const
-
- // this model is no use. try children recursively
- FOR_EACH( it, children )
- {
- Model* found = (*it)->GetUnsubscribedModelOfType( type );
- if( found )
- return found;
- }
-
- // nothing matching below this model
- return NULL;
-}
-
-void Model::NeedRedraw( void )
-{
- this->rebuild_displaylist = true;
-
- if( parent )
- parent->NeedRedraw();
- else
- world->NeedRedraw();
-}
-
-void Model::Redraw( void )
-{
- world->Redraw();
-}
-
-Model* Model::GetUnusedModelOfType( const std::string& type )
-{
- //printf( "searching for type %d in model %s type %d\n", type, token, this->type );
-
- if( (this->type == type) && (!this->used ) )
- {
- this->used = true;
- return this;
- }
-
- // this model is no use. try children recursively
- FOR_EACH( it, children )
- {
- Model* found = (*it)->GetUnusedModelOfType( type );
- if( found )
- return found;
- }
-
- // nothing matching below this model
- if( ! parent ) PRINT_WARN1( "Request for unused model of type %s failed", type.c_str() );
- return NULL;
-}
-
-kg_t Model::GetTotalMass() const
-{
- kg_t sum = mass;
-
- FOR_EACH( it, children )
- sum += (*it)->GetTotalMass();
-
- return sum;
-}
-
-kg_t Model::GetMassOfChildren() const
-{
- return( GetTotalMass() - mass);
-}
-
-void Model::Map( unsigned int layer )
-{
- if( ! mapped )
- {
- // render all blocks in the group at my global pose and size
- blockgroup.Map( layer );
- mapped = true;
- }
-}
-
-void Model::UnMap( unsigned int layer )
-{
- if( mapped )
- {
- blockgroup.UnMap(layer);
- mapped = false;
- }
-}
-
-void Model::BecomeParentOf( Model* child )
-{
- if( child->parent )
- child->parent->RemoveChild( child );
- else
- world->RemoveChild( child );
-
- child->parent = this;
-
- this->AddChild( child );
-
- world->dirty = true;
-}
-
-PowerPack* Model::FindPowerPack() const
-{
- if( power_pack )
- return power_pack;
-
- if( parent )
- return parent->FindPowerPack();
-
- return NULL;
-}
-
-void Model::RegisterOption( Option* opt )
-{
- world->RegisterOption( opt );
-}
-
-
-void Model::Rasterize( uint8_t* data,
- unsigned int width,
- unsigned int height,
- meters_t cellwidth,
- meters_t cellheight )
-{
- rastervis.ClearPts();
- blockgroup.Rasterize( data, width, height, cellwidth, cellheight );
- rastervis.SetData( data, width, height, cellwidth, cellheight );
-}
-
-void Model::SetFriction( double friction )
-{
- this->friction = friction;
-}
-
-Model* Model::GetChild( const std::string& modelname ) const
-{
- // construct the full model name and look it up
-
- const std::string fullname = token + "." + modelname;
-
- Model* mod = world->GetModel( fullname );
-
- if( mod == NULL )
- PRINT_WARN1( "Model %s not found", fullname.c_str() );
-
- return mod;
-}
-
-
-//***************************************************************
-// Raster data visualizer
-//
-Model::RasterVis::RasterVis()
- : Visualizer( "Rasterization", "raster_vis" ),
- data(NULL),
- width(0),
- height(0),
- cellwidth(0),
- cellheight(0),
- pts()
-{
-
-}
-
-void Model::RasterVis::Visualize( Model* mod, Camera* cam )
-{
- (void)cam; // avoid warning about unused var
-
- if( data == NULL )
- return;
-
- // go into world coordinates
- glPushMatrix();
- mod->PushColor( 1,0,0,0.5 );
-
- Gl::pose_inverse_shift( mod->GetGlobalPose() );
-
- if( pts.size() > 0 )
- {
- glPushMatrix();
- //Size sz = mod->blockgroup.GetSize();
- //glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 );
- //glScalef( mod->geom.size.x / sz.x, mod->geom.size.y / sz.y, 1 );
-
- // now we're in world meters coordinates
- glPointSize( 4 );
- glBegin( GL_POINTS );
-
- FOR_EACH( it, pts )
- {
- point_t& pt = *it;
- glVertex2f( pt.x, pt.y );
-
- char buf[128];
- snprintf( buf, 127, "[%.2f x %.2f]", pt.x, pt.y );
- Gl::draw_string( pt.x, pt.y, 0, buf );
- }
- glEnd();
-
- mod->PopColor();
-
- glPopMatrix();
- }
-
- // go into bitmap pixel coords
- glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 );
- //glScalef( mod->geom.size.x / width, mod->geom.size.y / height, 1 );
-
- glScalef( cellwidth, cellheight, 1 );
-
- mod->PushColor( 0,0,0,0.5 );
- glPolygonMode( GL_FRONT, GL_FILL );
- for( unsigned int y=0; y<height; ++y )
- for( unsigned int x=0; x<width; ++x )
- {
- // printf( "[%u %u] ", x, y );
- if( data[ x + y*width ] )
- glRectf( x, y, x+1, y+1 );
- }
-
- glTranslatef( 0,0,0.01 );
-
- mod->PushColor( 0,0,0,1 );
- glPolygonMode( GL_FRONT, GL_LINE );
- for( unsigned int y=0; y<height; ++y )
- for( unsigned int x=0; x<width; ++x )
- {
- if( data[ x + y*width ] )
- glRectf( x, y, x+1, y+1 );
-
-// char buf[128];
-// snprintf( buf, 127, "[%u x %u]", x, y );
-// Gl::draw_string( x, y, 0, buf );
- }
-
-
- glPolygonMode( GL_FRONT, GL_FILL );
-
- mod->PopColor();
- mod->PopColor();
-
- mod->PushColor( 0,0,0,1 );
- char buf[128];
- snprintf( buf, 127, "[%u x %u]", width, height );
- glTranslatef( 0,0,0.01 );
- Gl::draw_string( 1, height-1, 0, buf );
-
- mod->PopColor();
-
- glPopMatrix();
-}
-
-void Model::RasterVis::SetData( uint8_t* data,
- const unsigned int width,
- const unsigned int height,
- const meters_t cellwidth,
- const meters_t cellheight )
-{
- // copy the raster for test visualization
- if( this->data )
- delete[] this->data;
- size_t len = sizeof(uint8_t) * width * height;
- //printf( "allocating %lu bytes\n", len );
- this->data = new uint8_t[len];
- memcpy( this->data, data, len );
- this->width = width;
- this->height = height;
- this->cellwidth = cellwidth;
- this->cellheight = cellheight;
-}
-
-
-void Model::RasterVis::AddPoint( const meters_t x, const meters_t y )
-{
- pts.push_back( point_t( x, y ) );
-}
-
-void Model::RasterVis::ClearPts()
-{
- pts.clear();
-}
-
-
-Model::Flag::Flag( const Color color, const double size )
- : color(color), size(size), displaylist(0)
-{
-}
-
-Model::Flag* Model::Flag::Nibble( double chunk )
-{
- Flag* piece = NULL;
-
- if( size > 0 )
- {
- chunk = std::min( chunk, this->size );
- piece = new Flag( this->color, chunk );
- this->size -= chunk;
- }
-
- return piece;
-}
-
-
-void Model::Flag::SetColor( const Color& c )
-{
- color = c;
-
- if( displaylist )
- {
- // force recreation of list
- glDeleteLists( displaylist, 1 );
- displaylist = 0;
- }
-}
-
-void Model::Flag::SetSize( double sz )
-{
- size = sz;
-
- if( displaylist )
- {
- // force recreation of list
- glDeleteLists( displaylist, 1 );
- displaylist = 0;
- }
-}
-
-
-void Model::Flag::Draw( GLUquadric* quadric )
-{
- if( displaylist == 0 )
- {
- displaylist = glGenLists(1);
- assert( displaylist > 0 );
-
- glNewList( displaylist, GL_COMPILE );
-
- glColor4f( color.r, color.g, color.b, color.a );
-
- glEnable(GL_POLYGON_OFFSET_FILL);
- glPolygonOffset(1.0, 1.0);
- gluQuadricDrawStyle( quadric, GLU_FILL );
- gluSphere( quadric, size/2.0, 4,2 );
- glDisable(GL_POLYGON_OFFSET_FILL);
-
- // draw the edges darker version of the same color
- glColor4f( color.r/2.0, color.g/2.0, color.b/2.0, color.a/2.0 );
-
- gluQuadricDrawStyle( quadric, GLU_LINE );
- gluSphere( quadric, size/2.0, 4,2 );
-
- glEndList();
- }
-
- glCallList( displaylist );
-}
View
346 libstage/model_camera.cc~
@@ -1,346 +0,0 @@
-///////////////////////////////////////////////////////////////////////////
-//
-// File: model_camera.cc
-// Author: Alex Couture-Beil
-// Date: 09 June 2008
-//
-// CVS info:
-//
-///////////////////////////////////////////////////////////////////////////
-
-#define CAMERA_HEIGHT 0.5
-#define CAMERA_NEAR_CLIP 0.2
-#define CAMERA_FAR_CLIP 8.0
-
-//#define DEBUG 1
-#include "canvas.hh"
-#include "worldfile.hh"
-
-using namespace Stg;
-
-#include <sstream>
-#include <iomanip>
-
-//TODO make instance attempt to register an option (as customvisualizations do)
-Option ModelCamera::showCameraData( "Show Camera Data", "show_camera", "", true, NULL );
-
-static const Size DEFAULT_SIZE( 0.1, 0.07, 0.05 );
-static const char DEFAULT_GEOM_COLOR[] = "black";
-static const float DEFAULT_HFOV = 70;
-static const float DEFAULT_VFOV = 40;
-
-/**
-@ingroup model
-@defgroup model_camera Camera model
-The camera model simulates a camera
-
-API: Stg::ModelCamera
-
-<h2>Worldfile properties</h2>
-
-@par Summary and default values
-
-@verbatim
-camera
-(
- # laser properties
- resolution [ 32 32 ]
- range [ 0.2 8.0 ]
- fov [ 70.0 40.0 ]
- pantilt [ 0.0 0.0 ]
-
- # model properties
- size [ 0.1 0.07 0.05 ]
- color "black"
- watts 100.0 # TODO find watts for sony pan-tilt camera
-)
-@endverbatim
-
-@par Details
-
-- resolution [ width:<int> height:<int> ]\n
- the resolution of the pixel samples
-- range [ min:<float> max:<float> ]\n
- the range reported by the camera, in meters. Objects closer than `min', or farther than `max' will not be displayed.
- The smaller the `min' number, the less persision in depth - don't set this value too close to 0.
-- fov [ horizontal: <float> vertical: <float> ]\n
- angle, in degrees, for the horizontal and vertical field of view.
-- pantilt [ pan:<float> tilt:<float> ]
- angle, in degrees, where the camera is looking. pan is the left-right positioning, and tilt is the up-down positioning.
-*/
-
-//caclulate the corss product, and store results in the first vertex
-void cross( float& x1, float& y1, float& z1, float x2, float y2, float z2 )
-{
- float x3, y3, z3;
-
- x3 = y1 * z2 - z1 * y2;
- y3 = z1 * x2 - x1 * z2;
- z3 = x1 * y2 - y1 * x2;
-
- x1 = x3;
- y1 = y3;
- z1 = z3;
-}
-
-
-ModelCamera::ModelCamera( World* world,
- Model* parent,
- const std::string& type ) :
- Model( world, parent, type ),
- _canvas( NULL ),
- _frame_data( NULL ),
- _frame_color_data( NULL ),
- _valid_vertexbuf_cache( false ),
- _vertexbuf_cache( NULL ),
- _width( 32 ),
- _height( 32 ),
- _camera_quads_size( 0 ),
- _camera_quads( NULL ),
- _camera_colors( NULL ),
- _camera(),
- _yaw_offset( 0.0 ),
- _pitch_offset( 0.0 )
-{
- PRINT_DEBUG2( "Constructing ModelCamera %d (%s)\n",
- id, typestr );
-
- WorldGui* world_gui = dynamic_cast< WorldGui* >( 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();
-
- _camera.setPitch( 90.0 );
-
- Geom geom;
- memset( &geom, 0, sizeof(geom));
- geom.size = DEFAULT_SIZE;
- SetGeom( geom );
-
- // set default color
- SetColor( Color( DEFAULT_GEOM_COLOR) );
-
- RegisterOption( &showCameraData );
-
- Startup();
-}
-
-ModelCamera::~ModelCamera()
-{
- if( _frame_data != NULL ) {
- //dont forget about GetFrame() //TODO merge these together
- delete[] _frame_data;
- delete[] _frame_color_data;
- delete[] _vertexbuf_cache;
- delete[] _camera_quads;
- delete[] _camera_colors;
- }
-}
-
-void ModelCamera::Load( void )
-{
- Model::Load();
-
- float horizFov = wf->ReadTupleFloat( wf_entity, "fov", 0, DEFAULT_HFOV );
- float vertFov = wf->ReadTupleFloat( wf_entity, "fov", 1, DEFAULT_VFOV );
- _camera.setFov( horizFov, vertFov );
-
- float range_min = wf->ReadTupleLength( wf_entity, "range", 0, CAMERA_NEAR_CLIP );
- float range_max = wf->ReadTupleLength( wf_entity, "range", 1, CAMERA_FAR_CLIP );
- _camera.setClip( range_min, range_max );
-
- _yaw_offset = wf->ReadTupleFloat( wf_entity, "pantilt", 0, _yaw_offset );
- _pitch_offset = wf->ReadTupleFloat( wf_entity, "pantilt", 1, _pitch_offset );
-
- _width = static_cast< int >( wf->ReadTupleFloat( wf_entity, "resolution", 0, _width ) );
- _height = static_cast< int >( wf->ReadTupleFloat( wf_entity, "resolution", 1, _height ) );
-
-}
-
-
-void ModelCamera::Update( void )
-{
- GetFrame();
- Model::Update();
-}
-
-bool ModelCamera::GetFrame( void )
-{
- if( _width == 0 || _height == 0 )
- return false;
-
- if( _frame_data == NULL ) {
- if( _frame_data != NULL ) {
- //don't forget about destructor
- delete[] _frame_data;
- delete[] _frame_color_data;
- delete[] _vertexbuf_cache;
- delete[] _camera_quads;
- delete[] _camera_colors;
- }
- _frame_data = new GLfloat[ _width * _height ]; //assumes a max of depth 4
- _frame_color_data = new GLubyte[ 4 * _width * _height ]; //for RGBA
-
- _vertexbuf_cache = new ColoredVertex[ _width * _height ]; //for unit vectors
-
- _camera_quads_size = _height * _width * 4 * 3; //one quad per pixel, 3 vertex per quad
- _camera_quads = new GLfloat[ _camera_quads_size ];
- _camera_colors = new GLubyte[ _camera_quads_size ];
- }
-
- //TODO overcome issue when glviewport is set LARGER than the window side
- //currently it just clips and draws outside areas black - resulting in bad glreadpixel data
- if( _width > _canvas->w() )
- _width = _canvas->w();
- if( _height > _canvas->h() )
- _height = _canvas->h();
-
- GLint viewport[4];
- glGetIntegerv(GL_VIEWPORT,viewport);
-
- glViewport( 0, 0, _width, _height );
- _camera.update();
- _camera.SetProjection();
- float height = GetGlobalPose().z;
- //TODO reposition the camera so it isn't inside the model ( or don't draw the parent when calling renderframe )
- _camera.setPose( parent->GetGlobalPose().x, parent->GetGlobalPose().y, height ); //TODO use something smarter than a #define - make it configurable
- _camera.setYaw( rtod( parent->GetGlobalPose().a ) - 90.0 - _yaw_offset ); //-90.0 points the camera infront of the robot instead of pointing right
- _camera.setPitch( 90.0 - _pitch_offset );
- _camera.Draw();
-
-
- glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
- _canvas->DrawFloor();
- _canvas->DrawBlocks();
-
- //read depth buffer
- glReadPixels(0, 0, _width, _height,
- 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 ] );
-
- //read color buffer
- glReadPixels(0, 0, _width, _height,
- GL_RGBA,
- GL_UNSIGNED_BYTE,
- _frame_color_data );
-
-
- glViewport( viewport[0], viewport[1], viewport[2], viewport[3] );
- _canvas->invalidate();
- _canvas->setDirtyBuffer();
- return true;
-}
-
-//TODO create lines outlining camera frustrum, then iterate over each depth measurement and create a square
-void ModelCamera::DataVisualize( Camera* cam )
-{
-
- if( _frame_data == NULL || !showCameraData )
- return;
-
- float w_fov = _camera.horizFov();
- float h_fov = _camera.vertFov();
-
- float start_fov = w_fov / 2.0 + 180.0; //start at right
- float start_vert_fov = h_fov / 2.0 + 90.0; //start at top
-
- int w = _width;
- int h = _height;
- float a_space = w_fov / w; //degrees between each sample
- float vert_a_space = h_fov / h; //degrees between each vertical sample
-
- //Create unit vectors representing a sphere - and cache it
- if( _valid_vertexbuf_cache == false ) {
- for( int j = 0; j < h; j++ ) {
- for( int i = 0; i < w; i++ ) {
-
- float a = start_fov - static_cast< float >( i ) * a_space;
- float vert_a = start_vert_fov - static_cast< float >( h - j - 1 ) * vert_a_space;
-
- int index = i + j * w;
- ColoredVertex* vertex = _vertexbuf_cache + index;
-
- //calculate and cache normal unit vectors of the sphere
- float x = -sin( dtor( vert_a ) ) * cos( dtor( a ) );
- float y = -sin( dtor( vert_a ) ) * sin( dtor( a ) );
- float z = -cos( dtor( vert_a ) );
-
- //rotate them about the pitch and yaw offsets - keeping distortion of the sphere at the extremes
- a = dtor( - _yaw_offset );
- vertex->x = x * cos( a ) - y * sin( a );
- vertex->y = x * sin( a ) + y * cos( a );
- vertex->z = z;
-
- x = vertex->x; y = vertex->y; z = vertex->z;
- a = dtor( _pitch_offset );
- vertex->x = z * sin( a ) + x * cos( a );
- vertex->y = y;
- vertex->z = z * cos( a ) - x * sin( a );
- }
- }
- _valid_vertexbuf_cache = true;
- }
-
- glDisable( GL_CULL_FACE );
- //glBegin( GL_QUADS );
-
-
- //Scale cached unit vectors with depth-buffer
- float* depth_data = ( float* )( _frame_data );
- for( int j = 0; j < h; j++ ) {
- for( int i = 0; i < w; i++ ) {
- int index = i + j * w;
- const ColoredVertex* unit_vertex = _vertexbuf_cache + index;
- const float length = depth_data[ index ];
-
- //scale unitvectors with depth-buffer
- float sx = unit_vertex->x * length;
- float sy = unit_vertex->y * length;
- float sz = unit_vertex->z * length;
-
- //create a quad based on the current camera pixel, and normal vector
- //the quad size is porpotial to the depth distance
- float x, y, z;
- x = 0; y = 0; z = length * M_PI * a_space / 360.0;
- cross( x, y, z, unit_vertex->x, unit_vertex->y, unit_vertex->z );
-
- z = length * M_PI * vert_a_space / 360.0;
-
- GLfloat* p = _camera_quads + index * 4 * 3;
- p[ 0 ] = sx - x; p[ 1 ] = sy - y; p[ 2 ] = sz - z;
- p[ 3 ] = sx - x; p[ 4 ] = sy - y; p[ 5 ] = sz + z;
- p[ 6 ] = sx + x; p[ 7 ] = sy + y; p[ 8 ] = sz + z;
- p[ 9 ] = sx + x; p[ 10 ] = sy + y; p[ 11 ] = sz - z;
-
- //copy color for each vertex
- //TODO using a color index would be smarter
- const GLubyte* color = _frame_color_data + index * 4;
- for( int i = 0; i < 4; i++ ) {
- GLubyte* cp = _camera_colors + index * 4 * 3 + i * 3;
- memcpy( cp, color, sizeof( GLubyte ) * 3 );
- }
- }
- }
-
-
- //glEnableClientState( GL_VERTEX_ARRAY );
- glEnableClientState( GL_COLOR_ARRAY );
-
- glVertexPointer( 3, GL_FLOAT, 0, _camera_quads );
- glColorPointer( 3, GL_UNSIGNED_BYTE, 0, _camera_colors );
- glDrawArrays( GL_QUADS, 0, w * h * 4 );
-
- glDisableClientState( GL_COLOR_ARRAY );
-// glDisableClientState( GL_VERTEX_ARRAY ); //TODO FIXME - something is using vertex arrays without properly enabling it
-
-}
-
View
441 libstage/model_ranger.cc~
@@ -1,441 +0,0 @@
-///////////////////////////////////////////////////////////////////////////
-//
-// File: model_ranger.cc
-// Author: Richard Vaughan
-// Date: 10 June 2004
-//
-// CVS info:
-// $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_ranger.cc,v $
-// $Author: rtv $
-// $Revision$
-//
-///////////////////////////////////////////////////////////////////////////
-
-/**
- @ingroup model
- @defgroup model_ranger Ranger model
- The ranger model simulates an array of sonar or infra-red (IR) range sensors.
-
- API: Stg::ModelRanger
-
- <h2>Worldfile properties</h2>
-
- @par Summary and default values
-
- @verbatim
- ranger
- (
- # ranger-specific properties
-
- sensor (
- pose [ x y z a ]
- size [ x y z ]
- fov a
- range [min max]
- )
-
- # generic model properties with non-default values
- watts 2.0
- color_rgba [ 0 1 0 0.15 ]
- )
-
- @endverbatim
-
- @par Notes
-
- The ranger model allows configuration of the pose, size and view parameters of each transducer seperately (using spose[index], ssize[index] and sview[index]). However, most users will set a common size and view (using ssize and sview), and just specify individual transducer poses.
-
- @par Details
-
- - pose[\<transducer index\>] [ x:<float> y:<float> theta:<float> ]\n
- pose of the transducer relative to its parent.
- - ssize [float float]
- - [x y]
- - size in meters. Has no effect on the data, but controls how the sensor looks in the Stage window.
- - size[\<transducer index\>] [float float]
- - per-transducer version of the ssize property. Overrides the common setting.
- - sview [float float float]
- - [range_min range_max fov]
- - minimum range and maximum range in meters, field of view angle in degrees. Currently fov has no effect on the sensor model, other than being shown in the confgiuration graphic for the ranger device.
- - sview[\<transducer index\>] [float float float]
- - per-transducer version of the sview property. Overrides the common setting.
-
-*/
-
-//#define DEBUG 1
-
-#include "stage.hh"
-#include "worldfile.hh"
-#include "option.hh"
-using namespace Stg;
-
-static const watts_t RANGER_WATTSPERSENSOR = 0.2;
-static const Size RANGER_SIZE( 0.15, 0.15, 0.2 ); // SICK LMS size
-
-//static const Color RANGER_COLOR( 0,0,1 );
-static const Color RANGER_CONFIG_COLOR( 0,0,0.5 );
-//static const Color RANGER_GEOM_COLOR( 1,0,1 );
-
-// static members
-Option ModelRanger::Vis::showTransducers( "Ranger transducers", "show_ranger_transducers", "", false, NULL );
-Option ModelRanger::Vis::showArea( "Ranger area", "show_ranger_ranges", "", true, NULL );
-Option ModelRanger::Vis::showStrikes( "Ranger strikes", "show_ranger_strikes", "", false, NULL );
-Option ModelRanger::Vis::showFov( "Ranger FOV", "show_ranger_fov", "", false, NULL );
-Option ModelRanger::Vis::showBeams( "Ranger beams", "show_ranger_beams", "", false, NULL );
-
-
-ModelRanger::ModelRanger( World* world,
- Model* parent,
- const std::string& type )
- : Model( world, parent, type ),
- vis( world )
-{
- PRINT_DEBUG2( "Constructing ModelRanger %d (%s)\n",
- id, type );
-
- // Set up sensible defaults
-
- // assert that Update() is reentrant for this derived model
- thread_safe = true;
-
- this->SetColor( RANGER_CONFIG_COLOR );
-
- // remove the polygon: ranger has no body
- this->ClearBlocks();
-
- this->SetGeom( Geom( Pose(), RANGER_SIZE ));
-
- AddVisualizer( &vis, true );
-}
-
-ModelRanger::~ModelRanger()
-{
-}
-
-void ModelRanger::Startup( void )
-{
- Model::Startup();
- this->SetWatts( RANGER_WATTSPERSENSOR * sensors.size() );
-}
-
-
-void ModelRanger::Shutdown( void )
-{
- PRINT_DEBUG( "ranger shutdown" );
-
- this->SetWatts( 0 );
-
- Model::Shutdown();
-}
-
-void ModelRanger::LoadSensor( Worldfile* wf, int entity )
-{
- //static int c=0;
- // printf( "ranger %s loading sensor %d\n", token.c_str(), c++ );
- Sensor s;
- s.Load( wf, entity );
- sensors.push_back(s);
-}
-
-
-void ModelRanger::Sensor::Load( Worldfile* wf, int entity )
-{
- //static int c=0;
- // printf( "ranger %s loading sensor %d\n", token.c_str(), c++ );
-
- pose.Load( wf, entity, "pose" );
- size.Load( wf, entity, "size" );
- range.Load( wf, entity, "range" );
- col.Load( wf, entity );
- fov = wf->ReadAngle( entity, "fov", fov );
- sample_count = wf->ReadInt( entity, "samples", sample_count );
- //ranges.resize(sample_count);
- //intensities.resize(sample_count);
-}
-
-void ModelRanger::Load( void )
-{
- Model::Load();
-}
-
-static bool ranger_match( Model* hit,
- Model* finder,
- const void* dummy )
-{
- (void)dummy; // avoid warning about unused var
-
- // Ignore the model that's looking and things that are invisible to
- // rangers
- return( (!hit->IsRelated( finder )) && (sgn(hit->vis.ranger_return) != -1 ) );
-}
-
-void ModelRanger::Update( void )
-{
- // raytrace new range data for all sensors
- FOR_EACH( it, sensors )
- it->Update( this );
-
- Model::Update();
-}
-
-void ModelRanger::Sensor::Update( ModelRanger* mod )
-{
- ranges.resize( sample_count );
- intensities.resize( sample_count );
-
- //printf( "update sensor, has ranges size %u\n", (unsigned int)ranges.size() );
-
- double bearing( sample_count > 1 ? -fov/2.0 : 0.0 );
- // make the first and last rays exactly at the extremes of the FOV
- double sample_incr( fov / std::max(sample_count-1, (unsigned int)1) );
-
- // find the global origin of our first emmitted ray
- Pose rayorg( pose );//mod->GetPose() );
- rayorg.a += bearing;
- rayorg.z += size.z/2.0;
- rayorg = mod->LocalToGlobal(rayorg);
-
- // set up a ray to trace
- Ray ray( mod, rayorg, range.max, ranger_match, NULL, true );
-
- World* world = mod->GetWorld();
-
- // trace the ray, incrementing its heading for each sample
- for( size_t t(0); t<sample_count; t++ )
- {
- const RaytraceResult& r ( world->Raytrace( ray ) );
- ranges[t] = r.range;
- intensities[t] = r.mod ? r.mod->vis.ranger_return : 0.0;
-
- // point the ray to the next angle
- ray.origin.a += sample_incr;
-
- //printf( "ranger %s sensor %p pose %s sample %d range %.2f ref %.2f\n",
- // mod->Token(),
- // this,
- // pose.String().c_str(),
- // t,
- // ranges[t].range,
- // ranges[t].reflectance );
- }
-}
-
-std::string ModelRanger::Sensor::String() const
-{
- char buf[256];
- snprintf( buf, 256, "[ samples %u, range [%.2f %.2f] ]",
- sample_count, range.min, range.max );
- return( std::string( buf ) );
-}
-
-void ModelRanger::Sensor::Visualize( ModelRanger::Vis* vis, ModelRanger* rgr ) const
-{
- size_t sample_count( this->sample_count );
-
- //glTranslatef( 0,0, ranger->GetGeom().size.z/2.0 ); // shoot the ranger beam out at the right height
-
- // pack the ranger hit points into a vertex array for fast rendering
- GLfloat pts[2*(sample_count+1)];
- glVertexPointer( 2, GL_FLOAT, 0, &pts[0] );
-
- pts[0] = 0.0;
- pts[1] = 0.0;
-
- glDepthMask( GL_FALSE );
- glPointSize( 2 );
-
- glPushMatrix();
-
- Gl::pose_shift( pose );
-
- if( vis->showTransducers )
- {
- // draw the sensor body as a rectangle
- rgr->PushColor( col );
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- glRectf( -size.x/2.0, -size.y/2.0, size.x/2.0, size.y/2.0 );
- rgr->PopColor();
- }
-
- Color c( col );
- c.a = 0.15; // transparent version of sensor color
- rgr->PushColor( c );
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
-
- if( ranges.size() ) // if we have some data
- {
- if( sample_count == 1 )
- {
- // only one sample, so we fake up some beam width for beauty
- const double sidelen = ranges[0];
- const double da = fov/2.0;
-
- sample_count = 3;
-
- pts[2] = sidelen*cos(-da );
- pts[3] = sidelen*sin(-da );
-
- pts[4] = sidelen*cos(+da );
- pts[5] = sidelen*sin(+da );
- }
- else
- {
- for( size_t s(0); s<sample_count; s++ )
- {
- double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0;
- pts[2*s+2] = (float)(ranges[s] * cos(ray_angle) );
- pts[2*s+3] = (float)(ranges[s] * sin(ray_angle) );
- }
- }
- }
-
- if( vis->showArea )
- {
- if( sample_count > 1 )
- // draw the filled polygon in transparent blue
- glDrawArrays( GL_POLYGON, 0, sample_count+1 );
- }
-
- glDepthMask( GL_TRUE );
-
- if( vis->showStrikes )
- {
- // TODO - paint the stike point in a color based on intensity
-// // if the sample is unusually bright, draw a little blob
-// if( intensities[s] > 0.0 )
-// {
-// // this happens rarely so we can do it in immediate mode
-// glBegin( GL_POINTS );
-// glVertex2f( pts[2*s+2], pts[2*s+3] );
-// glEnd();
-// }
-
- // draw the beam strike points
- c.a = 0.8;
- rgr->PushColor( c );
- glDrawArrays( GL_POINTS, 0, sample_count+1 );
- rgr->PopColor();
- }
-
- if( vis->showFov )
- {
- for( size_t s(0); s<sample_count; s++ )
- {
- double ray_angle((s * (fov / (sample_count-1))) - fov/2.0);
- pts[2*s+2] = (float)(range.max * cos(ray_angle) );
- pts[2*s+3] = (float)(range.max * sin(ray_angle) );
- }
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- c.a = 0.5;
- rgr->PushColor( c );
- glDrawArrays( GL_POLYGON, 0, sample_count+1 );
- rgr->PopColor();
- }
-
- if( vis->showBeams )
- {
- // darker version of the same color
- c.r /= 2.0;
- c.g /= 2.0;
- c.b /= 2.0;
- c.a = 1.0;
-
- rgr->PushColor( c );
- glBegin( GL_LINES );
-
- for( size_t s(0); s<sample_count; s++ )
- {
-
- glVertex2f( 0,0 );
- double ray_angle( sample_count == 1 ? 0 : (s * (fov / (sample_count-1))) - fov/2.0 );
- glVertex2f( ranges[s] * cos(ray_angle),
- ranges[s] * sin(ray_angle) );
-
- }
- glEnd();
- rgr->PopColor();
- }
-
- rgr->PopColor();
-
- glPopMatrix();
-}
-
-void ModelRanger::Print( char* prefix ) const
-{
- Model::Print( prefix );
-
- printf( "\tRanges " );
- for( size_t i(0); i<sensors.size(); i++ )
- {
- printf( "[ " );
- for( size_t j(0); j<sensors[i].ranges.size(); j++ )
- printf( "%.2f ", sensors[i].ranges[j] );
-
- printf( " ]" );
- }
-
- printf( "\n\tIntensities " );
- for( size_t i(0); i<sensors.size(); i++ )
- {
- printf( "[ " );
- for( size_t j(0); j<sensors[i].intensities.size(); j++ )
- printf( "%.2f ", sensors[i].intensities[j] );
-
- printf( " ]" );
- }
- puts("");
-}
-
-
-
-// VIS -------------------------------------------------------------------
-
-ModelRanger::Vis::Vis( World* world )
- : Visualizer( "Ranger", "ranger_vis" )
-{
- world->RegisterOption( &showArea );
- world->RegisterOption( &showStrikes );
- world->RegisterOption( &showFov );
- world->RegisterOption( &showBeams );
- world->RegisterOption( &showTransducers );
-}
-
-void ModelRanger::Vis::Visualize( Model* mod, Camera* cam )
-{
- (void)cam; // avoid warning about unused var
-
- ModelRanger* ranger( dynamic_cast<ModelRanger*>(mod) );
-
- const std::vector<Sensor>& sensors( ranger->GetSensors() );
-
- FOR_EACH( it, sensors )
- it->Visualize( this, ranger );
-
- const size_t sensor_count = sensors.size();
-
- if( showTransducers )
- {
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
- ranger->PushColor( 0,0,0,1 );
-
- for( size_t s(0); s<sensor_count; s++ )
- {
- const Sensor& rngr(sensors[s]);
-
- glPointSize( 4 );
- glBegin( GL_POINTS );
- glVertex3f( rngr.pose.x, rngr.pose.y, rngr.pose.z );
- glEnd();
-
- char buf[8];
- snprintf( buf, 8, "%d", (int)s );
- Gl::draw_string( rngr.pose.x, rngr.pose.y, rngr.pose.z, buf );
-
- }
- ranger->PopColor();
- }
-
-}
-

0 comments on commit d13c672

Please sign in to comment.
Something went wrong with that request. Please try again.