Revision: 7145
http://playerstage.svn.sourceforge.net/playerstage/?rev=7145&view=rev
Author: rtv
Date: 2008-11-15 02:46:11 +0000 (Sat, 15 Nov 2008)
Log Message:
-----------
major changes for 3.1
Modified Paths:
--------------
code/stage/trunk/libstage/stage.hh
Property Changed:
----------------
code/stage/trunk/libstage/stage.hh
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2008-11-15 02:38:24 UTC (rev 7144)
+++ code/stage/trunk/libstage/stage.hh 2008-11-15 02:46:11 UTC (rev 7145)
@@ -58,8 +58,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>
-
+ // except GLU for some reason
#ifdef __APPLE__
#include <OpenGL/glu.h>
#else
@@ -72,20 +71,20 @@
/** The Stage library uses its own namespace */
namespace Stg
{
- // foreward declare
- class StgCanvas;
- class Worldfile;
- class StgWorld;
- class StgWorldGui;
- class StgModel;
- class OptionsDlg;
- class StgCamera;
+ // foreward declare
+ class StgCanvas;
+ class Worldfile;
+ class StgWorld;
+ class StgWorldGui;
+ class StgModel;
+ class OptionsDlg;
+ class Camera;
- /** Initialize the Stage library */
- void Init( int* argc, char** argv[] );
+ /** Initialize the Stage library */
+ void Init( int* argc, char** argv[] );
- /** returns true iff Stg::Init() has been called. */
- bool InitDone();
+ /** returns true iff Stg::Init() has been called. */
+ bool InitDone();
/** Create unique identifying numbers for each type of model, and a
count of the number of types. */
@@ -103,461 +102,499 @@
} stg_model_type_t;
- /// Copyright string
- const char COPYRIGHT[] =
- "Copyright Richard Vaughan and contributors 2000-2008";
+ /// Copyright string
+ const char COPYRIGHT[] =
+ "Copyright Richard Vaughan and contributors 2000-2008";
- /// Author string
- const char AUTHORS[] =
- "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and contributors.";
+ /// Author string
+ const char AUTHORS[] =
+ "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges, Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and contributors.";
- /// Project website string
- const char WEBSITE[] = "http://playerstage.org";
+ /// Project website string
+ const char WEBSITE[] = "http://playerstage.org";
- /// Project description string
- const char DESCRIPTION[] =
- "Robot simulation library\nPart of the Player Project";
+ /// Project description string
+ const char DESCRIPTION[] =
+ "Robot simulation library\nPart of the Player Project";
- /// Project distribution license string
- const char LICENSE[] =
- "Stage robot simulation library\n" \
- "Copyright (C) 2000-2008 Richard Vaughan and contributors\n" \
- "Part of the Player Project [http://playerstage.org]\n" \
- "\n" \
- "This program is free software; you can redistribute it and/or\n" \
- "modify it under the terms of the GNU General Public License\n" \
- "as published by the Free Software Foundation; either version 2\n" \
- "of the License, or (at your option) any later version.\n" \
- "\n" \
- "This program is distributed in the hope that it will be useful,\n" \
- "but WITHOUT ANY WARRANTY; without even the implied warranty of\n" \
- "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" \
- "GNU General Public License for more details.\n" \
- "\n" \
- "You should have received a copy of the GNU General Public License\n" \
- "along with this program; if not, write to the Free Software\n" \
- "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.\n" \
- "\n" \
- "The text of the license may also be available online at\n" \
- "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";
+ /// Project distribution license string
+ const char LICENSE[] =
+ "Stage robot simulation library\n" \
+ "Copyright (C) 2000-2008 Richard Vaughan and contributors\n" \
+ "Part of the Player Project [http://playerstage.org]\n" \
+ "\n" \
+ "This program is free software; you can redistribute it and/or\n" \
+ "modify it under the terms of the GNU General Public License\n" \
+ "as published by the Free Software Foundation; either version 2\n" \
+ "of the License, or (at your option) any later version.\n" \
+ "\n" \
+ "This program is distributed in the hope that it will be useful,\n" \
+ "but WITHOUT ANY WARRANTY; without even the implied warranty of\n" \
+ "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" \
+ "GNU General Public License for more details.\n" \
+ "\n" \
+ "You should have received a copy of the GNU General Public License\n" \
+ "along with this program; if not, write to the Free Software\n" \
+ "Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.\n" \
+ "\n" \
+ "The text of the license may also be available online at\n" \
+ "http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";
- /** The maximum length of a Stage model identifier string
- */
- const uint32_t TOKEN_MAX = 64;
+ /** The maximum length of a Stage model identifier string
+ */
+ const uint32_t TOKEN_MAX = 64;
- /** Convenient constant
- */
- const double thousand = 1e3;
+ /** Convenient constant
+ */
+ const double thousand = 1e3;
- /** Convenient constant
- */
- const double million = 1e6;
+ /** Convenient constant
+ */
+ const double million = 1e6;
- /** Convenient constant
- */
- const double billion = 1e9;
+ /** Convenient constant
+ */
+ const double billion = 1e9;
- /** convert an angle in radians to degrees
- */
- inline double rtod( double r ){ return( r*180.0/M_PI ); }
+ /** convert an angle in radians to degrees
+ */
+ inline double rtod( double r ){ return( r*180.0/M_PI ); }
- /** convert an angle in degrees to radians
- */
- inline double dtor( double d ){ return( d*M_PI/180.0 ); }
+ /** convert an angle in degrees to radians
+ */
+ inline double dtor( double d ){ return( d*M_PI/180.0 ); }
- /** Normalize an angle to within +/_ M_PI
- */
- inline double normalize( double a )
- {
- //optimized version of return( atan2(sin(a), cos(a)));
- while( a < -M_PI ) a += 2.0 * M_PI;
- while( a > M_PI ) a -= 2.0 * M_PI;
- return a;
- }
+ /** Normalize an angle to within +/_ M_PI
+ */
+ inline double normalize( double a )
+ {
+ //optimized version of return( atan2(sin(a), cos(a)));
+ while( a < -M_PI ) a += 2.0 * M_PI;
+ while( a > M_PI ) a -= 2.0 * M_PI;
+ return a;
+ }
- /** take binary sign of a, either -1, or 1 if >= 0
- */
- inline int sgn( int a){ return( a<0 ? -1 : 1); }
+ /** take binary sign of a, either -1, or 1 if >= 0
+ */
+ inline int sgn( int a){ return( a<0 ? -1 : 1); }
- /** take binary sign of a, either -1, or 1 if >= 0
- */
- inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); }
+ /** take binary sign of a, either -1, or 1 if >= 0
+ */
+ inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); }
- // forward declare
- class StgModel;
+ // forward declare
+ class StgModel;
- /** Describe the image format used for saving screenshots
- */
- typedef enum {
- STG_IMAGE_FORMAT_PNG,
- STG_IMAGE_FORMAT_JPG
- } stg_image_format_t;
+ /** Describe the image format used for saving screenshots
+ */
+ typedef enum {
+ STG_IMAGE_FORMAT_PNG,
+ STG_IMAGE_FORMAT_JPG
+ } stg_image_format_t;
- /** any integer value other than this is a valid fiducial ID
- */
- enum { FiducialNone = 0 };
+ /** any integer value other than this is a valid fiducial ID */
+ enum { FiducialNone = 0 };
- /** Value that Uniquely identifies a model
- */
- typedef uint32_t stg_id_t;
+ /** Value that Uniquely identifies a model */
+ typedef uint32_t stg_id_t;
- /** Metres: floating point unit of distance
- */
- typedef double stg_meters_t;
+ /** Metres: floating point unit of distance */
+ typedef double stg_meters_t;
- /** Radians: unit of angle
- */
- typedef double stg_radians_t;
+ /** Radians: unit of angle */
+ typedef double stg_radians_t;
- /** time structure */
- typedef struct timeval stg_time_t;
+ /** time structure */
+ typedef struct timeval stg_time_t;
- /** Milliseconds: unit of (short) time */
- typedef unsigned long stg_msec_t;
+ /** Milliseconds: unit of (short) time */
+ typedef unsigned long stg_msec_t;
- /** Microseconds: unit of (very short) time */
- typedef uint64_t stg_usec_t;
+ /** Microseconds: unit of (very short) time */
+ typedef uint64_t stg_usec_t;
- /** Kilograms: unit of mass */
- typedef double stg_kg_t; // Kilograms (mass)
+ /** Kilograms: unit of mass */
+ typedef double stg_kg_t; // Kilograms (mass)
- /** Joules: unit of energy */
- typedef double stg_joules_t;
+ /** Joules: unit of energy */
+ typedef double stg_joules_t;
- /** Watts: unit of power (energy/time) */
- typedef double stg_watts_t;
+ /** Watts: unit of power (energy/time) */
+ typedef double stg_watts_t;
- /** boolean */
- typedef uint32_t stg_bool_t;
+ /** boolean */
+ typedef uint32_t stg_bool_t;
- //typedef double stg_friction_t;
+ /** 32-bit ARGB color packed 0xAARRGGBB */
+ typedef uint32_t stg_color_t;
- /** 32-bit ARGB color packed 0xAARRGGBB */
- typedef uint32_t stg_color_t;
+ /** Specify a color from its components */
+ stg_color_t stg_color_pack( double r, double g, double b, double a );
- /** Specify a color from its components */
- stg_color_t stg_color_pack( double r, double g, double b, double a );
+ /** Obtain the components of a color */
+ void stg_color_unpack( stg_color_t col,
+ double* r, double* g, double* b, double* a );
- /** Obtain the components of a color */
- void stg_color_unpack( stg_color_t col,
- double* r, double* g, double* b, double* a );
+ /** obstacle value. 0 means the model does not behave, or is sensed,
+ as an obstacle */
+ //typedef int stg_obstacle_return_t;
- /** obstacle value. 0 means the model does not behave, or is sensed,
- as an obstacle */
- //typedef int stg_obstacle_return_t;
+ /** blobfinder return value. 0 means not detected by the
+ blobfinder */
+ //typedef int stg_blob_return_t;
- /** blobfinder return value. 0 means not detected by the
- blobfinder */
- //typedef int stg_blob_return_t;
+ /** fiducial return value. 0 means not detected as a fiducial */
+ //typedef int stg_fiducial_return_t;
- /** fiducial return value. 0 means not detected as a fiducial */
- //typedef int stg_fiducial_return_t;
+ //typedef int stg_ranger_return_t;
- //typedef int stg_ranger_return_t;
+ //typedef enum { STG_GRIP_NO = 0, STG_GRIP_YES } stg_gripper_return_t;
- //typedef enum { STG_GRIP_NO = 0, STG_GRIP_YES } stg_gripper_return_t;
+ /** specify a rectangular size */
+ class stg_size_t
+ {
+ public:
+ stg_meters_t x, y, z;
- /** specify a rectangular size */
- typedef struct
- {
- stg_meters_t x, y, z;
- } stg_size_t;
+ stg_size_t( stg_meters_t x,
+ stg_meters_t y,
+ stg_meters_t z )
+ : x(x), y(y), z(z)
+ {/*empty*/}
- /** Specify a 3 axis position, in x, y and heading. */
- typedef struct
- {
- stg_meters_t x, y, z; //< location in 3 axes
- stg_radians_t a; //< rotation about the z axis.
- } stg_pose_t;
+ /** default constructor uses default non-zero values */
+ stg_size_t()
+ : x( 0.1 ), y( 0.1 ), z( 0.1 )
+ {/*empty*/}
+ };
- /** specify a 3 axis velocity in x, y and heading. */
- typedef stg_pose_t stg_velocity_t;
+ /** Specify a 3 axis position, in x, y and heading. */
+ class stg_pose_t
+ {
+ public:
+ stg_meters_t x, y, z; //< location in 3 axes
+ stg_radians_t a; //< rotation about the z axis.
- /** specify an object's basic geometry: position and rectangular
- size. */
- typedef struct
- {
- stg_pose_t pose; //< position
- stg_size_t size; //< extent
- } stg_geom_t;
+ stg_pose_t( stg_meters_t x,
+ stg_meters_t y,
+ stg_meters_t z,
+ stg_radians_t a )
+ : x(x), y(y), z(z), a(a)
+ { /*empty*/ }
- /** bound a range of values, from min to max */
- typedef struct
- {
- double min; //< smallest value in range
- double max; //< largest value in range
- } stg_bounds_t;
+ stg_pose_t() : x(0.0), y(0.0), z(0.0), a(0.0)
+ { /*empty*/ }
- /** bound a volume along the x,y,z axes */
- typedef struct
- {
- stg_bounds_t x; //< volume extent along x axis
- stg_bounds_t y; //< volume extent along y axis
- stg_bounds_t z; //< volume extent along z axis
- } stg_bounds3d_t;
+
+ static stg_pose_t Random( stg_meters_t xmin, stg_meters_t xmax,
+ stg_meters_t ymin, stg_meters_t ymax )
+ {
+ return stg_pose_t( xmin + drand48() * (xmax-xmin),
+ ymin + drand48() * (ymax-ymin),
+ 0,
+ normalize( drand48() * (2.0 * M_PI) ));
+ }
- /** bound a range of range values, from min to max
- */
- typedef struct
- {
- stg_meters_t min; //< smallest value in range
- stg_meters_t max; //< largest value in range
- } stg_range_bounds_t;
+ virtual void Print( const char* prefix )
+ {
+ printf( "%d pose [x:%.3f y:%.3f a:%.3f]\n",
+ prefix, x,y,z,a );
+ }
+ };
- /** define a three-dimensional bounding box */
- typedef struct
- {
- stg_bounds_t x, y, z;
- } stg_bbox3d_t;
+ /** return a random pose within the bounding rectangle, with z=0 and
+ angle random */
+
+
+ /** specify a 3 axis velocity in x, y and heading. */
+ class stg_velocity_t : public stg_pose_t
+ {
+ public:
+ stg_velocity_t( stg_meters_t x,
+ stg_meters_t y,
+ stg_meters_t z,
+ stg_radians_t a )
+ { /*empty*/ }
+
+ stg_velocity_t()
+ { /*empty*/ }
+
+ virtual void Print( const char* prefix )
+ {
+ printf( "%d velocity [x:%.3f y:%.3f a:%.3f]\n",
+ prefix, x,y,z,a );
+ }
+ };
- /** define a field-of-view: an angle and range bounds */
- typedef struct
- {
- stg_bounds_t range; //< min and max range of sensor
- stg_radians_t angle; //< width of viewing angle of sensor
- } stg_fov_t;
+ /** specify an object's basic geometry: position and rectangular
+ size. */
+ class stg_geom_t
+ {
+ public:
+ stg_pose_t pose; //< position
+ stg_size_t size; //< extent
+
+ virtual void Print( const char* prefix )
+ {
+ printf( "%s geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
+ prefix,
+ pose.x,
+ pose.y,
+ pose.a,
+ size.x,
+ size.y );
+ }
+ };
+ /** bound a range of values, from min to max */
+ typedef struct
+ {
+ double min; //< smallest value in range
+ double max; //< largest value in range
+ } stg_bounds_t;
- /** define a point on a 2d plane */
- typedef struct
- {
- stg_meters_t x, y;
- } stg_point_t;
+ /** bound a volume along the x,y,z axes */
+ typedef struct
+ {
+ stg_bounds_t x; //< volume extent along x axis
+ stg_bounds_t y; //< volume extent along y axis
+ stg_bounds_t z; //< volume extent along z axis
+ } stg_bounds3d_t;
- /** define a point in 3d space */
- typedef struct
- {
- float x, y, z;
- } stg_vertex_t;
+ /** bound a range of range values, from min to max */
+ typedef struct
+ {
+ stg_meters_t min; //< smallest value in range
+ stg_meters_t max; //< largest value in range
+ } stg_range_bounds_t;
- /** define vertex and its color */
- typedef struct
- {
- float x, y, z, r, g, b, a;
- } stg_colorvertex_t;
+ /** define a three-dimensional bounding box */
+ typedef struct
+ {
+ stg_bounds_t x, y, z;
+ } stg_bbox3d_t;
- /** define a point in 3d space */
- typedef struct
- {
- stg_meters_t x, y, z;
- } stg_point3_t;
+ /** define a field-of-view: an angle and range bounds */
+ typedef struct
+ {
+ stg_bounds_t range; //< min and max range of sensor
+ stg_radians_t angle; //< width of viewing angle of sensor
+ } stg_fov_t;
- /** define an integer point on the plane
- */
- typedef struct
- {
- int32_t x,y;
- } stg_point_int_t;
+ /** define a point on a 2d plane */
+ typedef struct
+ {
+ stg_meters_t x, y;
+ } stg_point_t;
- /** Create an array of [count] points. Caller must free the returned
- pointer, preferably with stg_points_destroy().
- */
- stg_point_t* stg_points_create( size_t count );
+ /** define a point in 3d space */
+ typedef struct
+ {
+ float x, y, z;
+ } stg_vertex_t;
- /** frees a point array */
- void stg_points_destroy( stg_point_t* pts );
+ /** define vertex and its color */
+ typedef struct
+ {
+ float x, y, z, r, g, b, a;
+ } stg_colorvertex_t;
- /** create an array of 4 points containing the corners of a unit
- square. */
- stg_point_t* stg_unit_square_points_create();
+ /** define a point in 3d space */
+ typedef struct
+ {
+ stg_meters_t x, y, z;
+ } stg_point3_t;
+ /** define an integer point on the plane
+ */
+ typedef struct
+ {
+ int32_t x,y;
+ } stg_point_int_t;
- /** matching function should return 0 iff the candidate block is
- acceptable */
- typedef int(*stg_line3d_func_t)(int32_t x, int32_t y, int32_t z,
- void* arg );
+ /** Create an array of [count] points. Caller must free the returned
+ pointer, preferably with stg_points_destroy().
+ */
+ stg_point_t* stg_points_create( size_t count );
+ /** frees a point array */
+ void stg_points_destroy( stg_point_t* pts );
- /** Visit every voxel along a vector from (x,y,z) to (x+dx, y+dy, z+dz
- ). Call the function for every voxel, passing in the current voxel
- coordinates followed by the two arguments. Adapted from Graphics
- Gems IV, algorithm by Cohen & Kaufman 1991 */
- int stg_line_3d( int32_t x, int32_t y, int32_t z,
- int32_t dx, int32_t dy, int32_t dz,
- stg_line3d_func_t visit_voxel,
- void* arg );
+ /** create an array of 4 points containing the corners of a unit
+ square. */
+ stg_point_t* stg_unit_square_points_create();
- /** Visit every voxel along a polygon defined by the array of
- points. Calls stg_line_3d(). */
- int stg_polygon_3d( stg_point_int_t* pts,
- unsigned int pt_count,
- stg_line3d_func_t visit_voxel,
- void* arg );
- /** return a stg_pose_t with its fields initialized by the
- parameters */
- stg_pose_t new_pose( stg_meters_t x, stg_meters_t y, stg_meters_t z, stg_radians_t a );
+ typedef uint32_t stg_movemask_t;
- /** 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);
+ const uint32_t STG_MOVE_TRANS = (1 << 0); //< bitmask for stg_movemask_t
+ const uint32_t STG_MOVE_ROT = (1 << 1); //< bitmask for stg_movemask_t
+ const uint32_t STG_MOVE_SCALE = (1 << 2); //< bitmask for stg_movemask_t
- typedef uint32_t stg_movemask_t;
- const char STG_MP_PREFIX[] = "_mp_";
- const char STG_MP_POSE[] = "_mp_pose";
- const char STG_MP_VELOCITY[] = "_mp_velocity";
- const char STG_MP_GEOM[] = "_mp_geom";
- const char STG_MP_COLOR[] = "_mp_color";
- const char STG_MP_WATTS[] = "_mp_watts";
- const char STG_MP_FIDUCIAL_RETURN[] = "_mp_fiducial_return";
- const char STG_MP_LASER_RETURN[] = "_mp_laser_return";
- const char STG_MP_OBSTACLE_RETURN[] = "_mp_obstacle_return";
- const char STG_MP_RANGER_RETURN[] = "_mp_ranger_return";
- const char STG_MP_GRIPPER_RETURN[] = "_mp_gripper_return";
- const char STG_MP_MASS[] = "_mp_mass";
+ const char STG_MP_PREFIX[] = "_mp_";
+ const char STG_MP_POSE[] = "_mp_pose";
+ const char STG_MP_VELOCITY[] = "_mp_velocity";
+ const char STG_MP_GEOM[] = "_mp_geom";
+ const char STG_MP_COLOR[] = "_mp_color";
+ const char STG_MP_WATTS[] = "_mp_watts";
+ const char STG_MP_FIDUCIAL_RETURN[] = "_mp_fiducial_return";
+ const char STG_MP_LASER_RETURN[] = "_mp_laser_return";
+ const char STG_MP_OBSTACLE_RETURN[] = "_mp_obstacle_return";
+ const char STG_MP_RANGER_RETURN[] = "_mp_ranger_return";
+ const char STG_MP_GRIPPER_RETURN[] = "_mp_gripper_return";
+ const char STG_MP_MASS[] = "_mp_mass";
- /// laser return value
- typedef enum
- {
+ /// laser return value
+ typedef enum
+ {
LaserTransparent, ///<not detected by laser model
LaserVisible, ///< detected by laser with a reflected intensity of 0
LaserBright ////< detected by laser with a reflected intensity of 1
- } stg_laser_return_t;
+ } stg_laser_return_t;
- namespace Draw
- {
- typedef struct {
- double x, y, z;
- } vertex_t;
+ namespace Draw
+ {
+ typedef struct {
+ double x, y, z;
+ } vertex_t;
- typedef struct {
- vertex_t min, max;
- } bounds3_t;
+ typedef struct {
+ vertex_t min, max;
+ } bounds3_t;
- typedef enum {
- STG_D_DRAW_POINTS,
- STG_D_DRAW_LINES,
- STG_D_DRAW_LINE_STRIP,
- STG_D_DRAW_LINE_LOOP,
- STG_D_DRAW_TRIANGLES,
- STG_D_DRAW_TRIANGLE_STRIP,
- STG_D_DRAW_TRIANGLE_FAN,
- STG_D_DRAW_QUADS,
- STG_D_DRAW_QUAD_STRIP,
- STG_D_DRAW_POLYGON,
- STG_D_PUSH,
- STG_D_POP,
- STG_D_ROTATE,
- STG_D_TRANSLATE,
- } type_t;
+ typedef enum {
+ STG_D_DRAW_POINTS,
+ STG_D_DRAW_LINES,
+ STG_D_DRAW_LINE_STRIP,
+ STG_D_DRAW_LINE_LOOP,
+ STG_D_DRAW_TRIANGLES,
+ STG_D_DRAW_TRIANGLE_STRIP,
+ STG_D_DRAW_TRIANGLE_FAN,
+ STG_D_DRAW_QUADS,
+ STG_D_DRAW_QUAD_STRIP,
+ STG_D_DRAW_POLYGON,
+ STG_D_PUSH,
+ STG_D_POP,
+ STG_D_ROTATE,
+ STG_D_TRANSLATE,
+ } type_t;
- /** the start of all stg_d structures looks like this */
- typedef struct
- {
- type_t type;
- } hdr_t;
+ /** the start of all stg_d structures looks like this */
+ typedef struct
+ {
+ type_t type;
+ } hdr_t;
- /** push is just the header, but we define it for syntax checking */
- typedef hdr_t push_t;
- /** pop is just the header, but we define it for syntax checking */
- typedef hdr_t pop_t;
+ /** push is just the header, but we define it for syntax checking */
+ typedef hdr_t push_t;
+ /** pop is just the header, but we define it for syntax checking */
+ typedef hdr_t pop_t;
- /** stg_d draw command */
- typedef struct
- {
- /** required type field */
- type_t type;
- /** number of vertices */
- size_t vert_count;
- /** array of vertex data follows at the end of this struct*/
- vertex_t verts[];
- } draw_t;
+ /** stg_d draw command */
+ typedef struct
+ {
+ /** required type field */
+ type_t type;
+ /** number of vertices */
+ size_t vert_count;
+ /** array of vertex data follows at the end of this struct*/
+ vertex_t verts[];
+ } draw_t;
- /** stg_d translate command */
- typedef struct
- {
- /** required type field */
- type_t type;
- /** distance to translate, in 3 axes */
- vertex_t vector;
- } translate_t;
+ /** stg_d translate command */
+ typedef struct
+ {
+ /** required type field */
+ type_t type;
+ /** distance to translate, in 3 axes */
+ vertex_t vector;
+ } translate_t;
- /** stg_d rotate command */
- typedef struct
- {
- /** required type field */
- type_t type;
- /** vector about which we should rotate */
- vertex_t vector;
- /** angle to rotate */
- stg_radians_t angle;
- } rotate_t;
+ /** stg_d rotate command */
+ typedef struct
+ {
+ /** required type field */
+ type_t type;
+ /** vector about which we should rotate */
+ vertex_t vector;
+ /** angle to rotate */
+ stg_radians_t angle;
+ } rotate_t;
- /** Create a draw_t object of specified type from a vertex array */
- draw_t* create( type_t type,
- vertex_t* verts,
- size_t vert_count );
+ /** Create a draw_t object of specified type from a vertex array */
+ draw_t* create( type_t type,
+ vertex_t* verts,
+ size_t vert_count );
- /** Delete the draw_t object, deallocting its memory */
- void destroy( draw_t* d );
- } // end namespace draw
+ /** Delete the draw_t object, deallocting its memory */
+ void destroy( draw_t* d );
+ } // end namespace draw
- // MACROS ------------------------------------------------------
- // Some useful macros
+ // MACROS ------------------------------------------------------
+ // Some useful macros
- /** Look up the color in the X11 database. (i.e. transform color
- name to color value). If the color is not found in the
- database, a bright red color (0xF00) will be returned instead.
- */
- stg_color_t stg_lookup_color(const char *name);
+ /** Look up the color in the X11 database. (i.e. transform color
+ name to color value). If the color is not found in the
+ database, a bright red color (0xF00) will be returned instead.
+ */
+ stg_color_t stg_lookup_color(const char *name);
- /** calculate the sum of [p1] and [p2], in [p1]'s coordinate system, and
- copy the result into result. */
- void stg_pose_sum( stg_pose_t* result, stg_pose_t* p1, stg_pose_t* p2 );
+ /** calculate the sum of [p1] and [p2], in [p1]'s coordinate system, and
+ copy the result into result. */
+ void stg_pose_sum( stg_pose_t* result, stg_pose_t* p1, stg_pose_t* p2 );
- /** returns the sum of [p1] + [p2], in [p1]'s coordinate system */
- stg_pose_t pose_sum( stg_pose_t p1, stg_pose_t p2 );
+ /** returns the sum of [p1] + [p2], in [p1]'s coordinate system */
+ stg_pose_t pose_sum( stg_pose_t p1, stg_pose_t p2 );
+
+ /** returns a new pose, with each axis scaled */
+ stg_pose_t pose_scale( stg_pose_t p1, double x, double y, double z );
- // PRETTY PRINTING -------------------------------------------------
- /** Report an error, with a standard, friendly message header */
- void stg_print_err( const char* err );
- /** Print human-readable geometry on stdout */
- void stg_print_geom( stg_geom_t* geom );
- /** Print human-readable pose on stdout */
- void stg_print_pose( stg_pose_t* pose );
- /** Print human-readable velocity on stdout */
- void stg_print_velocity( stg_velocity_t* vel );
+ // PRETTY PRINTING -------------------------------------------------
-// const uint32_t STG_SHOW_BLOCKS = (1<<0);
-// const uint32_t STG_SHOW_DATA = (1<<1);
-// const uint32_t STG_SHOW_GEOM = (1<<2);
-// const uint32_t STG_SHOW_GRID = (1<<3);
-// const uint32_t STG_SHOW_OCCUPANCY = (1<<4);
-// const uint32_t STG_SHOW_TRAILS = (1<<5);
-// const uint32_t STG_SHOW_FOLLOW = (1<<6);
-// const uint32_t STG_SHOW_CLOCK = (1<<7);
-// const uint32_t STG_SHOW_QUADTREE = (1<<8);
-// const uint32_t STG_SHOW_ARROWS = (1<<9);
-// 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);
+ /** Report an error, with a standard, friendly message header */
+ void stg_print_err( const char* err );
+ /** Print human-readable geometry on stdout */
+ void stg_print_geom( stg_geom_t* geom );
+ /** Print human-readable pose on stdout */
+ void stg_print_pose( stg_pose_t* pose );
+ /** Print human-readable velocity on stdout */
+ void stg_print_velocity( stg_velocity_t* vel );
- // forward declare
- class StgWorld;
- class StgModel;
+ // const uint32_t STG_SHOW_BLOCKS = (1<<0);
+ // const uint32_t STG_SHOW_DATA = (1<<1);
+ // const uint32_t STG_SHOW_GEOM = (1<<2);
+ // const uint32_t STG_SHOW_GRID = (1<<3);
+ // const uint32_t STG_SHOW_OCCUPANCY = (1<<4);
+ // const uint32_t STG_SHOW_TRAILS = (1<<5);
+ // const uint32_t STG_SHOW_FOLLOW = (1<<6);
+ // const uint32_t STG_SHOW_CLOCK = (1<<7);
+ // const uint32_t STG_SHOW_QUADTREE = (1<<8);
+ // const uint32_t STG_SHOW_ARROWS = (1<<9);
+ // 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;
+ class StgModel;
+ class Cell;
/** A model creator function. Each model type must define a function of this type. */
typedef StgModel* (*stg_creator_t)( StgWorld*, StgModel* );
- typedef struct
- {
- const char* token;
- stg_creator_t creator;
- } stg_typetable_entry_t;
+ typedef struct
+ {
+ const char* token;
+ stg_creator_t creator;
+ } stg_typetable_entry_t;
/** a global (to the namespace) table mapping names to model types */
extern stg_typetable_entry_t typetable[MODEL_TYPE_COUNT];
@@ -568,215 +605,150 @@
void RegisterModels();
- void gl_draw_grid( stg_bounds3d_t vol );
- void gl_pose_shift( stg_pose_t* pose );
- void gl_coord_shift( double x, double y, double z, double a );
+ void gl_draw_grid( stg_bounds3d_t vol );
+ void gl_pose_shift( stg_pose_t* pose );
+ void gl_coord_shift( double x, double y, double z, double a );
- class StgFlag
- {
- public:
- stg_color_t color;
- double size;
+ class StgFlag
+ {
+ public:
+ stg_color_t color;
+ double size;
- StgFlag( stg_color_t color, double size );
- StgFlag* Nibble( double portion );
- };
+ StgFlag( stg_color_t color, double size );
+ StgFlag* Nibble( double portion );
+ };
- /** Render a string at [x,y,z] in the current color */
- void gl_draw_string( float x, float y, float z, const char *string);
- void gl_speech_bubble( float x, float y, float z, const char* str );
- void gl_draw_octagon( float w, float h, float m );
+ /** Render a string at [x,y,z] in the current color */
+ void gl_draw_string( float x, float y, float z, const char *string);
+ void gl_speech_bubble( float x, float y, float z, const char* str );
+ void gl_draw_octagon( float w, float h, float m );
- void stg_block_list_scale( GList* blocks,
- stg_size_t* size );
- void stg_block_list_destroy( GList* list );
+ typedef int(*stg_model_callback_t)(StgModel* mod, void* user );
+ typedef int(*stg_cell_callback_t)(Cell* cell, void* user );
+
+ // return val, or minval if val < minval, or maxval if val > maxval
+ double constrain( double val, double minval, double maxval );
+
+ typedef struct
+ {
+ int enabled;
+ stg_pose_t pose;
+ stg_meters_t size; ///< rendered as a sphere with this diameter
+ stg_color_t color;
+ stg_msec_t period; ///< duration of a complete cycle
+ double duty_cycle; ///< mark/space ratio
+ } stg_blinkenlight_t;
+ typedef struct {
+ stg_pose_t pose;
+ stg_color_t color;
+ stg_usec_t time;
+ } stg_trail_item_t;
+
+ /** container for a callback function and a single argument, so
+ they can be stored together in a list with a single pointer. */
+ typedef struct
+ {
+ stg_model_callback_t callback;
+ void* arg;
+ } stg_cb_t;
- typedef int(*stg_model_callback_t)(StgModel* mod, void* user );
+ stg_cb_t* cb_create( stg_model_callback_t callback, void* arg );
+ void cb_destroy( stg_cb_t* cb );
- // return val, or minval if val < minval, or maxval if val > maxval
- double constrain( double val, double minval, double maxval );
+ /** defines a rectangle of [size] located at [pose] */
+ typedef struct
+ {
+ stg_pose_t pose;
+ stg_size_t size;
+ } stg_rotrect_t; // rotated rectangle
+ /** normalizes the set [rects] of [num] rectangles, so that they fit
+ exactly in a unit square.
+ */
+ void stg_rotrects_normalize( stg_rotrect_t* rects, int num );
- /** container for a callback function and a single argument, so
- they can be stored together in a list with a single pointer. */
- typedef struct
- {
- stg_model_callback_t callback;
- void* arg;
- } stg_cb_t;
+ /** load the image file [filename] and convert it to an array of
+ rectangles, filling in the number of rects, width and
+ height. Memory is allocated for the rectangle array [rects], so
+ the caller must free [rects].
+ */
+ int stg_rotrects_from_image_file( const char* filename,
+ stg_rotrect_t** rects,
+ unsigned int* rect_count,
+ unsigned int* widthp,
+ unsigned int* heightp );
- stg_cb_t* cb_create( stg_model_callback_t callback, void* arg );
- void cb_destroy( stg_cb_t* cb );
+
+ /** matching function should return true iff the candidate block is
+ stops the ray, false if the block transmits the ray */
+ typedef bool (*stg_ray_test_func_t)(StgModel* candidate,
+ StgModel* finder,
+ const void* arg );
+
+ // TODO - some of this needs to be implemented, the rest junked.
- /** defines a rectangle of [size] located at [pose] */
- typedef struct
- {
- stg_pose_t pose;
- stg_size_t size;
- } stg_rotrect_t; // rotated rectangle
+ /* // -------------------------------------------------------------- */
- /** normalizes the set [rects] of [num] rectangles, so that they fit
- exactly in a unit square.
- */
- void stg_rotrects_normalize( stg_rotrect_t* rects, int num );
+ /* // standard energy consumption of some devices in W. */
+ /* // */
+ /* // The MOTIONKG value is a hack to approximate cost of motion, as */
+ /* // Stage does not yet have an acceleration model. */
+ /* // */
+ /* #define STG_ENERGY_COST_LASER 20.0 // 20 Watts! (LMS200 - from SICK web site) */
+ /* #define STG_ENERGY_COST_FIDUCIAL 10.0 // 10 Watts */
+ /* #define STG_ENERGY_COST_RANGER 0.5 // 500mW (estimate) */
+ /* #define STG_ENERGY_COST_MOTIONKG 10.0 // 10 Watts per KG when moving */
+ /* #define STG_ENERGY_COST_BLOB 4.0 // 4W (estimate) */
- /** load the image file [filename] and convert it to an array of
- rectangles, filling in the number of rects, width and
- height. Memory is allocated for the rectangle array [rects], so
- the caller must free [rects].
- */
- int stg_rotrects_from_image_file( const char* filename,
- stg_rotrect_t** rects,
- unsigned int* rect_count,
- unsigned int* widthp,
- unsigned int* heightp );
+ /* typedef struct */
+ /* { */
+ /* stg_joules_t joules; // current energy stored in Joules/1000 */
+ /* stg_watts_t watts; // current power expenditure in mW (mJoules/sec) */
+ /* int charging; // 1 if we are receiving energy, -1 if we are */
+ /* // supplying energy, 0 if we are neither charging nor */
+ /* // supplying energy. */
+ /* stg_meters_t range; // the range that our charging probe hit a charger */
+ /* } stg_energy_data_t; */
- // /** matching function should return 0 iff the candidate block is
- // acceptable */
- class StgBlock;
+ /* typedef struct */
+ /* { */
+ /* stg_joules_t capacity; // maximum energy we can store (we start fully charged) */
+ /* stg_meters_t probe_range; // the length of our recharge probe */
+ /* //stg_pose_t probe_pose; // TODO - the origin of our probe */
- typedef bool (*stg_block_match_func_t)(StgBlock* candidate, StgModel* finder, const void* arg );
+ /* stg_watts_t give_rate; // give this many Watts to a probe that hits me (possibly 0) */
- // TODO - some of this needs to be implemented, the rest junked.
+ /* stg_watts_t trickle_rate; // this much energy is consumed or */
+ /* // received by this device per second as a */
+ /* // baseline trickle. Positive values mean */
+ /* // that the device is just burning energy */
+ /* // stayting alive, which is appropriate */
+ /* // for most devices. Negative values mean */
+ /* // that the device is receiving energy */
+ /* // from the environment, simulating a */
+ /* // solar cell or some other ambient energy */
+ /* // collector */
- /* // -------------------------------------------------------------- */
+ /* } stg_energy_config_t; */
- /* // standard energy consumption of some devices in W. */
- /* // */
- /* // The MOTIONKG value is a hack to approximate cost of motion, as */
- /* // Stage does not yet have an acceleration model. */
- /* // */
- /* #define STG_ENERGY_COST_LASER 20.0 // 20 Watts! (LMS200 - from SICK web site) */
- /* #define STG_ENERGY_COST_FIDUCIAL 10.0 // 10 Watts */
- /* #define STG_ENERGY_COST_RANGER 0.5 // 500mW (estimate) */
- /* #define STG_ENERGY_COST_MOTIONKG 10.0 // 10 Watts per KG when moving */
- /* #define STG_ENERGY_COST_BLOB 4.0 // 4W (estimate) */
- /* typedef struct */
- /* { */
- /* stg_joules_t joules; // current energy stored in Joules/1000 */
- /* stg_watts_t watts; // current power expenditure in mW (mJoules/sec) */
- /* int charging; // 1 if we are receiving energy, -1 if we are */
- /* // supplying energy, 0 if we are neither charging nor */
- /* // supplying energy. */
- /* stg_meters_t range; // the range that our charging probe hit a charger */
- /* } stg_energy_data_t; */
+ /* // BLINKENLIGHT ------------------------------------------------------------ */
- /* typedef struct */
- /* { */
- /* stg_joules_t capacity; // maximum energy we can store (we start fully charged) */
- /* stg_meters_t probe_range; // the length of our recharge probe */
- /* //stg_pose_t probe_pose; // TODO - the origin of our probe */
+ /* // a number of milliseconds, used for example as the blinkenlight interval */
+ /* #define STG_LIGHT_ON UINT_MAX */
+ /* #define STG_LIGHT_OFF 0 */
- /* stg_watts_t give_rate; // give this many Watts to a probe that hits me (possibly 0) */
+ //typedef int stg_interval_ms_t;
- /* stg_watts_t trickle_rate; // this much energy is consumed or */
- /* // received by this device per second as a */
- /* // baseline trickle. Positive values mean */
- /* // that the device is just burning energy */
- /* // stayting alive, which is appropriate */
- /* // for most devices. Negative values mean */
- /* // that the device is receiving energy */
- /* // from the environment, simulating a */
- /* // solar cell or some other ambient energy */
- /* // collector */
-
- /* } stg_energy_config_t; */
-
-
- /* // BLINKENLIGHT ------------------------------------------------------------ */
-
- /* // a number of milliseconds, used for example as the blinkenlight interval */
- /* #define STG_LIGHT_ON UINT_MAX */
- /* #define STG_LIGHT_OFF 0 */
-
- //typedef int stg_interval_ms_t;
-
-
- /* // TOKEN ----------------------------------------------------------------------- */
- /* // token stuff for parsing worldfiles - this may one day replace
- the worldfile c++ code */
-
- /* #define CFG_OPEN '(' */
- /* #define CFG_CLOSE ')' */
- /* #define STR_OPEN '\"' */
- /* #define STR_CLOSE '\"' */
- /* #define TPL_OPEN '[' */
- /* #define TPL_CLOSE ']' */
-
- /* typedef enum { */
- /* STG_T_NUM = 0, */
- /* STG_T_BOOLEAN, */
- /* STG_T_MODELPROP, */
- /* STG_T_WORLDPROP, */
- /* STG_T_NAME, */
- /* STG_T_STRING, */
- /* STG_T_KEYWORD, */
- /* STG_T_CFG_OPEN, */
- /* STG_T_CFG_CLOSE, */
- /* STG_T_TPL_OPEN, */
- /* STG_T_TPL_CLOSE, */
- /* } stg_token_type_t; */
-
-
-
-
- /* typedef struct stg_token */
- /* { */
- /* char* token; ///< the text of the token */
- /* stg_token_type_t type; ///< the type of the token */
- /* unsigned int line; ///< the line on which the token appears */
-
- /* struct stg_token* next; ///< linked list support */
- /* struct stg_token* child; ///< tree support */
-
- /* } stg_token_t; */
-
- /* stg_token_t* stg_token_next( stg_token_t* tokens ); */
- /* /// print [token] formatted for a human reader, with a string [prefix] */
- /* void stg_token_print( char* prefix, stg_token_t* token ); */
-
- /* /// print a token array suitable for human reader */
- /* void stg_tokens_print( stg_token_t* tokens ); */
- /* void stg_tokens_free( stg_token_t* tokens ); */
-
- /* /// create a new token structure from the arguments */
- /* stg_token_t* stg_token_create( const char* token, stg_token_type_t type, int line ); */
-
- /* /// add a token to a list */
- /* stg_token_t* stg_token_append( stg_token_t* head, */
- /* char* token, stg_token_type_t type, int line ); */
-
- /* const char* stg_token_type_string( stg_token_type_t type ); */
-
- /* const char* stg_model_type_string( stg_model_type_t type ); */
-
- /* // functions for parsing worldfiles */
- /* stg_token_t* stg_tokenize( FILE* wf ); */
- /* //StgWorld* sc_load_worldblock( stg_client_t* cli, stg_token_t** tokensptr ); */
- /* //stg_model_t* sc_load_modelblock( StgWorld* world, stg_model_t* parent, */
- /* // stg_token_t** tokensptr ); */
-
-
-
-
- //#ifdef __cplusplus
- //}
- //#endif
-
-
- // list iterator macros
+ // list iterator macros
#define LISTFUNCTION( LIST, TYPE, FUNC ) for( GList* it=LIST; it; it=it->next ) FUNC((TYPE)it->data);
-
#define LISTMETHOD( LIST, TYPE, METHOD ) for( GList* it=LIST; it; it=it->next ) ((TYPE)it->data)->METHOD();
-
#define LISTFUNCTIONARG( LIST, TYPE, FUNC, ARG ) for( GList* it=LIST; it; it=it->next ) FUNC((TYPE)it->data, ARG);
-
#define LISTMETHODARG( LIST, TYPE, METHOD, ARG ) for( GList* it=LIST; it; it=it->next ) ((TYPE)it->data)->METHOD(ARG);
-
- // Error macros - output goes to stderr
+ // Error macros - output goes to stderr
#define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
#define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
@@ -784,7 +756,7 @@
#define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_ERR5(m,a,b,c,d,e) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
- // Warning macros
+ // Warning macros
#define PRINT_WARN(m) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_WARN1(m,a) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
#define PRINT_WARN2(m,a,b) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__)
@@ -792,7 +764,7 @@
#define PRINT_WARN4(m,a,b,c,d) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_WARN5(m,a,b,c,d,e) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)
- // Message macros
+ // Message macros
#ifdef DEBUG
#define PRINT_MSG(m) printf( "Stage: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_MSG1(m,a) printf( "Stage: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
@@ -809,7 +781,7 @@
#define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m"\n", a, b, c, d, e )
#endif
- // DEBUG macros
+ // DEBUG macros
#ifdef DEBUG
#define PRINT_DEBUG(m) printf( "debug: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_DEBUG1(m,a) printf( "debug: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
@@ -826,1779 +798,1691 @@
#define PRINT_DEBUG5(m,a,b,c,d,e)
#endif
- class StgBlock;
- class StgModel;
+ class StgBlock;
+ class StgModel;
+ /** Define a callback function type that can be attached to a
+ record within a model and called whenever the record is set.*/
+ typedef int (*stg_model_callback_t)( StgModel* mod, void* user );
- stg_msec_t stg_realtime();
- stg_msec_t stg_realtime_since_start( void );
+ // ANCESTOR CLASS
+ /** Base class for StgModel and StgWorld */
+ class StgAncestor
+ {
+ friend class StgCanvas; // allow StgCanvas access to our private members
+
+ protected:
+ GList* children;
+ char* token;
+ bool debug;
+
+ public:
+
+ /** recursively call func( model, arg ) for each descendant */
+ void ForEachDescendant( stg_model_callback_t func, void* arg );
+
+ /** array contains the number of each type of child model */
+ unsigned int child_type_counts[MODEL_TYPE_COUNT];
+
+ StgAncestor();
+ virtual ~StgAncestor();
+
+ virtual void AddChild( StgModel* mod );
+ virtual void RemoveChild( StgModel* mod );
+ virtual stg_pose_t GetGlobalPose();
+
+ const char* Token()
+ { return token; }
+
+ void SetToken( const char* str )
+ { token = strdup( str ); } // teeny memory leak
+ };
+ /** raytrace sample
+ */
+ typedef struct
+ {
+ stg_pose_t pose; ///< location and direction of the ray origin
+ stg_meters_t range; ///< range to beam hit in meters
+ StgModel* mod; ///< the model struck by this beam
+ stg_color_t color; ///< the color struck by this beam
+ } stg_raytrace_result_t;
- // ANCESTOR CLASS
- /** Define a callback function type that can be attached to a
- record within a model and called whenever the record is set.*/
- typedef int (*stg_model_callback_t)( StgModel* mod, void* user );
+ const uint32_t INTERVAL_LOG_LEN = 32;
+ // defined in stage_internal.hh
+ class Region;
+ class SuperRegion;
+ class BlockGroup;
- /** Base class for StgModel and StgWorld */
- class StgAncestor
-{
- friend class StgCanvas; // allow StgCanvas access to our private members
-
- protected:
- GList* children;
-
- char* token;
- bool debug;
-
- public:
-
- /** recursively call func( model, arg ) for each descendant */
- void ForEachDescendant( stg_model_callback_t func, void* arg );
-
- /** array contains the number of each type of child model */
- unsigned int child_type_counts[MODEL_TYPE_COUNT];
-
- StgAncestor();
- virtual ~StgAncestor();
-
- // unsigned int GetNumChildrenOfType( const char* typestr );
- // void IncrementNumChildrenOfType( const char* typestr );
-
- virtual void AddChild( StgModel* mod );
- virtual void RemoveChild( StgModel* mod );
- virtual stg_pose_t GetGlobalPose();
-
- const char* Token()
- { return this->token; }
-
- void SetToken( const char* str )
+ /// %StgWorld class
+ class StgWorld : public StgAncestor
{
- this->token = strdup( str );
- }
+ friend class StgModel; // allow access to private members
+ friend class StgBlock;
+ friend class StgTime;
+ friend class StgCanvas;
-// // PURE VIRTUAL - descendents must implement
-// virtual void PushColor( stg_color_t col ) = 0;
-// virtual void PushColor( double r, double g, double b, double a ) = 0;
-// virtual void PopColor() = 0; // does nothing
-};
+ private:
+
+ static GList* world_list; ///< all the worlds that exist
+ static bool quit_all; ///< quit all worlds ASAP
+ static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids
+
+ stg_usec_t real_time_start; ///< the real time at which this world was created
+ bool destroy;
+ bool quit; ///< quit this world ASAP
+ stg_usec_t real_time_now; ///< The current real time in microseconds
+ bool dirty; ///< iff true, a gui redraw would be required
+ int total_subs; ///< the total number of subscriptions to all models
+ double ppm; ///< the resolution of the world model in pixels per meter
+ GHashTable* models_by_name; ///< the models that make up the world, indexed by name
+
+ /** StgWorld::quit is set true when this simulation time is reached */
+ stg_usec_t quit_time;
+ void LoadModel( Worldfile* wf, int entity, GHashTable* entitytable );
+ void LoadBlock( Worldfile* wf, int entity, GHashTable* entitytable );
+ void LoadBlockGroup( Worldfile* wf, int entity, GHashTable* entitytable );
-typedef struct
-{
- int enabled;
- stg_pose_t pose;
- stg_meters_t size; ///< rendered as a sphere with this diameter
- stg_color_t color;
- stg_msec_t period; ///< duration of a complete cycle
- double duty_cycle; ///< mark/space ratio
-} stg_blinkenlight_t;
+ SuperRegion* AddSuperRegion( const stg_point_int_t& coord );
+ SuperRegion* GetSuperRegion( const stg_point_int_t& coord );
+ SuperRegion* GetSuperRegionCached( const stg_point_int_t& coord);
+
+ inline Cell* GetCell( const int32_t x, const int32_t y );
+
+ void ForEachCellInPolygonGLfloat( const GLfloat pts[],
+ const uint32_t len,
+ stg_cell_callback_t cb,
+ void* cb_arg );
+
+ void ForEachCellInPolygon( const stg_point_t pts[],
+ const uint32_t pt_count,
+ stg_cell_callback_t cb,
+ void* cb_arg );
+
+ void ForEachCellInLine( const stg_point_t pt1,
+ const stg_point_t pt2,
+ stg_cell_callback_t cb,
+ void* cb_arg );
+
+ void ForEachCellInLine( stg_meters_t x1, stg_meters_t y1,
+ stg_meters_t x2, stg_meters_t y2,
+ stg_cell_callback_t cb,
+ void* cb_arg );
+
+ /** convert a distance in meters to a distance in world occupancy
+ grid pixels */
+ int32_t MetersToPixels( stg_meters_t x )
+ { return (int32_t)floor(x * ppm); };
+
+ // dummy implementations to be overloaded by GUI subclasses
+ virtual void PushColor( stg_color_t col ) { /* do nothing */ };
+ virtual void PushColor( double r, double g, double b, double a ) { /* do nothing */ };
+ virtual void PopColor(){ /* do nothing */ };
+
+ SuperRegion* CreateSuperRegion( int32_t x, int32_t y );
+ void DestroySuperRegion( SuperRegion* sr );
+
+ stg_raytrace_result_t Raytrace( stg_pose_t pose,
+ stg_meters_t range,
+ stg_ray_test_func_t func,
+ StgModel* finder,
+ const void* arg,
+ bool ztest );
+
+ void Raytrace( stg_pose_t pose,
+ stg_meters_t range,
+ stg_radians_t fov,
+ stg_ray_test_func_t func,
+ StgModel* finder,
+ const void* arg,
+ stg_raytrace_result_t* samples,
+ uint32_t sample_count,
+ bool ztest );
+
+ protected:
+
+ static void UpdateCb( StgWorld* world);
+
+ GHashTable* superregions;
+ stg_usec_t interval_sim; ///< temporal resolution: milliseconds that elapse between simulated time steps
+ stg_usec_t sim_time; ///< the current sim time in this world in ms
+ GList* ray_list;///< List of rays traced for debug visualization
+ Worldfile* wf; ///< If set, points to the worldfile used to create this world
+ bool graphics;///< true iff we have a GUI
+ stg_bounds3d_t extent; ///< Describes the 3D volume of the world
+ long unsigned int updates; ///< the number of simulated time steps executed so far
+ FileManager fileMan; ///< Used to load and save worldfiles
+ SuperRegion* sr_cached; ///< The last superregion looked up by this world
+ GList* velocity_list; ///< Models with non-zero velocity and should have their poses updated
+ GList* update_list; ///< Models that have a subscriber or controller, and need to be updated
-typedef struct
-{
- uint32_t counter;
- GSList** lists;
-} stg_bigblock_t;
-
-typedef struct
-{
- StgWorld* world;
- StgBlock* block;
-} stg_render_info_t;
-
-
-class StgBlockGrid
-{
- private:
- //stg_bigblock_t* map;
- GSList** cells;
-
- //GTrashStack* trashstack;
- uint32_t width, height;// bwidth, bheight;
-
- public:
- //uint32_t numbits;
- StgBlockGrid( uint32_t width, uint32_t height );
- ~StgBlockGrid();
- void AddBlock( uint32_t x, uint32_t y, StgBlock* block );
- void RemoveBlock( uint32_t x, uint32_t y, StgBlock* block );
- GSList* GetList( uint32_t x, uint32_t y );
- void GlobalRemoveBlock( StgBlock* block );
- void Draw( bool drawall );
-
- /** Returns the number of blocks occupying the big block, specified
- in big block coordinates, NOT in small blocks.*/
- //uint32_t BigBlockOccupancy( uint32_t bbx, uint32_t bby );
-};
-
-/** raytrace sample
- */
-typedef struct
-{
- stg_pose_t pose; ///< location and direction of the ray origin
- stg_meters_t range; ///< range to beam hit in meters
- StgBlock* block; ///< the block struck by this beam
-} stg_raytrace_sample_t;
-
-
-const uint32_t INTERVAL_LOG_LEN = 32;
-
-class Region;
-class SuperRegion;
-
-/// %StgWorld class
-class StgWorld : public StgAncestor
-{
- friend class StgModel; // allow access to private members
- friend class StgBlock;
- friend class StgTime;
- friend class StgCanvas;
+ /** Enlarge the bounding volume to include this point */
+ void Extend( stg_point3_t pt );
-private:
- static GList* world_list;
+ virtual void AddModel( StgModel* mod );
+ virtual void RemoveModel( StgModel* mod );
- /** Coordinate system stack - experimental*/
- GQueue* csstack;
-
- void PushPose();
- void PopPose();
- stg_pose_t* PeekPose();
- void ShiftPose( stg_pose_t* pose );
+ GList* GetRayList(){ return ray_list; };
+ void ClearRays();
+
+ /** store rays traced for debugging purposes */
+ void RecordRay( double x1, double y1, double x2, double y2 );
- void DrawPose();
+ /** Returns true iff the current time is greater than the time we
+ should quit */
+ bool PastQuitTime();
+
+ void StartUpdatingModel( StgModel* mod )
+ {
+ update_list = g_list_append( update_list, mod );
+ }
+
+ void StopUpdatingModel( StgModel* mod )
+ {
+ update_list = g_list_remove( update_list, mod );
+ }
+
+ void StartUpdatingModelPose( StgModel* mod )
+ {
+ velocity_list = g_list_append( velocity_list, mod );
+ }
+
+ void StopUpdatingModelPose( StgModel* mod )
+ {
+ velocity_list = g_list_remove( velocity_list, mod );
+ }
+
+ static void update_thread_entry( StgModel* mod, void* count );
+
+ GMutex* thread_mutex;
+ GCond* worker_threads_done;
+ GThreadPool *threadpool;
+ unsigned int worker_threads;
- static bool quit_all; ///< quit all worlds ASAP
- static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids
- static int AddBlockPixel( int x, int y, int z,
- stg_render_info_t* rinfo ) ; //< used as a callback by StgModel
+ public:
+ static const int DEFAULT_PPM = 50; // default resolution in pixels per meter
+ static const stg_msec_t DEFAULT_INTERVAL_SIM = 100; ///< duration of sim timestep
+ static bool UpdateAll(); //returns true when time to quit, false otherwise
- stg_usec_t real_time_start; ///< the real time at which this world was created
+ StgWorld( const char* token = "MyWorld",
+ stg_msec_t interval_sim = DEFAULT_INTERVAL_SIM,
+ double ppm = DEFAULT_PPM );
- bool quit; ///< quit this world ASAP
+ virtual ~StgWorld();
- ///< convert a distance in meters to a distance in world occupancy grid pixels
- int32_t MetersToPixels( stg_meters_t x ){ return (int32_t)floor(x * ppm) ; };
+ stg_usec_t SimTimeNow(void){ return sim_time; }
+ stg_usec_t RealTimeNow(void);
+ stg_usec_t RealTimeSinceStart(void);
- /** Called by all constructors to set up the object */
- void Initialize( const char* token,
- stg_msec_t interval_sim,
- double ppm );
+ stg_usec_t GetSimInterval(){ return interval_sim; };
- // dummy implementations to be overloaded by GUI subclasses
- virtual void PushColor( stg_color_t col ) { /* do nothing */ };
- virtual void PushColor( double r, double g, double b, double a ) { /* do nothing */ };
- virtual void PopColor(){ /* do nothing */ };
-
+ Worldfile* GetWorldFile(){ return wf; };
- /** The current real time in microseconds */
- stg_usec_t real_time_now;
+ inline virtual bool IsGUI() { return false; }
- /** StgWorld::quit is set true when this simulation time is reached */
- stg_usec_t quit_time;
+ virtual void Load( const char* worldfile_path );
+ virtual void UnLoad();
+ virtual void Reload();
+ virtual bool Save( const char* filename );
+ virtual bool Update(void);
+
+ bool TestQuit(){ return( quit || quit_all ); }
+ void Quit(){ quit = true; }
+ void QuitAll(){ quit_all = true; }
+ void CancelQuit(){ quit = false; }
+ void CancelQuitAll(){ quit_all = false; }
- bool destroy;
-
- 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
-
- bool dirty; ///< iff true, a gui redraw would be required
-
- int total_subs; ///< the total number of subscriptions to all models
- double ppm; ///< the resolution of the world model in pixels per meter
-
- void StartUpdatingModel( StgModel* mod );
- void StopUpdatingModel( StgModel* mod );
-
- SuperRegion* CreateSuperRegion( int32_t x, int32_t y );
- void DestroySuperRegion( SuperRegion* sr );
-
- void Raytrace( stg_pose_t pose,
- stg_meters_t range,
- stg_block_match_func_t func,
- StgModel* finder,
- const void* arg,
- stg_raytrace_sample_t* sample,
- bool ztest );
-
- void Raytrace( stg_pose_t pose,
- stg_meters_t range,
- stg_radians_t fov,
- stg_block_match_func_t func,
- StgModel* finder,
- const void* arg,
- stg_raytrace_sample_t* samples,
- uint32_t sample_count,
- bool ztest );
-
- void RemoveBlock( int x, int y, StgBlock* block );
-
-protected:
- GHashTable* superregions;
- GList* update_list; //< the descendants that need Update() called
- stg_usec_t interval_sim; ///< temporal resolution: milliseconds that elapse between simulated time steps
-
- static void UpdateCb( StgWorld* world);
-
- stg_usec_t sim_time; ///< the current sim time in this world in ms
- inline bool PastQuitTime() const { return (quit_time > 0) && (sim_time >= quit_time); }
+ /** Get the resolution in pixels-per-metre of the underlying
+ discrete raytracing model */
+ double Resolution(){ return ppm; };
- GList* ray_list;
- // store rays traced for debugging purposes
- void RecordRay( double x1, double y1, double x2, double y2 );
- Worldfile* wf; ///< If set, points to the worldfile used to create this world
- bool graphics;
- stg_bounds3d_t extent;
+ /** Returns a pointer to the model identified by ID, or NULL if
+ nonexistent */
+ //StgModel* GetModel( const stg_id_t id );
+
+ /** Returns a pointer to the model identified by name, or NULL if
+ nonexistent */
+ StgModel* GetModel( const char* name );
+
+ /** Return the 3D bounding box of the world, in meters */
+ stg_bounds3d_t GetExtent(){ return extent; };
+
+ /** Return the number of times the world has been updated. */
+ long unsigned int GetUpdateCount() { return updates; }
+
+ stg_point_t* LocalToGlobal( double scalex, double scaley,
+ stg_point_t pts[],
+ uint32_t pt_count );
+ };
- /** Enlarge the bounding volume to include this point */
- void Extend( stg_point3_t pt );
-
- //GHashTable* blocks;
- GArray lines;
-
- virtual void AddModel( StgModel* mod );
- virtual void RemoveModel( StgModel* mod );
-
- GList* GetRayList(){ return ray_list; };
- void ClearRays();
-
- long unsigned int updates; ///< the number of simulated time steps executed so far
-
- FileManager fileMan;
-
+class StgBlock
+{
+ friend class BlockGroup;
+ friend class StgModel;
+ friend class SuperRegion;
+ friend class StgWorld;
+ friend class StgCanvas;
public:
- static const int DEFAULT_PPM = 50; // default resolution in pixels per meter
- static const stg_msec_t DEFAULT_INTERVAL_SIM = 100; ///< duration of sim timestep
- static bool UpdateAll(); //returns true when time to quit, false otherwise
- StgWorld( const char* token = "MyWorld",
- stg_msec_t interval_sim = DEFAULT_INTERVAL_SIM,
- double ppm = DEFAULT_PPM );
+ /** Block Constructor. A model's body is a list of these
+ blocks. The point data is copied, so pts can safely be freed
+ after constructing the block.*/
+ 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 );
+
+ /** A from-file constructor */
+ StgBlock( StgModel* mod, Worldfile* wf, int entity);
+
+ ~StgBlock();
+
+ /** render the block into the world's raytrace data structure */
+ void Map();
+
+ /** remove the block from the world's raytracing data structure */
+ void UnMap();
+
+ void Draw( StgModel* mod );
+ void DrawSolid(); // draw the block in OpenGL as a solid single color
+ void DrawFootPrint(); // draw the projection of the block onto the z=0 plane
+
+ void RecordRendering( Cell* cell )
+ { g_ptr_array_add( rendered_cells, (gpointer)cell ); };
+
+ stg_point_t* Points( unsigned int *count )
+ { if( count ) *count = pt_count; return pts; };
+
+ //bool IntersectGlobalZ( stg_meters_t z )
+ //{ return( z >= global_zmin && z <= global_zmax ); }
+
+ void AddToCellArray( GPtrArray* ptrarray );
+ void RemoveFromCellArray( GPtrArray* ptrarray );
- virtual ~StgWorld();
+ void GenerateCandidateCells();
+
+// /** Prepare to render the block in a new position in global coordinates */
+// void SetPoseTentative( const stg_pose_t pose );
+
+ StgModel* TestCollision();
+
+ void SwitchToTestedCells();
+
+ void Load( Worldfile* wf, int entity );
+
+ StgModel* mod; //< model to which this block belongs
+
+ stg_color_t GetColor();
+
+ private:
+ stg_point_t* pts; //< points defining a polygon
+ size_t pt_count; //< the number of points
+
+ stg_size_t size;
- 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_bounds_t local_z; //< z extent in local coords
- void AddBlock( StgBlock* block );
- void RemoveBlock( StgBlock* block );
+ stg_color_t color;
+ bool inherit_color;
+
+ void DrawTop();
+ void DrawSides();
+
+ /** z extent in global coordinates */
+ stg_bounds_t global_z;
+
+ bool mapped;
+
+ /** an array of pointers to cells into which this block has been
+ rendered (speeds up UnMapping) */
+ GPtrArray* rendered_cells;
+
+ /** When moving a model, we test for collisions by generating, for
+ each block, a list of the cells in which it would be rendered if the
+ move were to be successful. If no collision occurs, the move is
+ allowed - the rendered cells are cleared, the potential cells are
+ written, and the pointers to the rendered and potential cells are
+ switched for next time (avoiding a memory copy).*/
+ GPtrArray* candidate_cells;
+ };
- stg_usec_t GetSimInterval(){ return interval_sim; };
- Worldfile* GetWorldFile(){ return wf; };
+ class BlockGroup
+ {
+
+ private:
+ int displaylist;
+ void BuildDisplayList( StgModel* mod );
- inline virtual bool IsGUI() { return false; }
-
- virtual void Load( const char* worldfile_path );
- virtual void UnLoad();
- virtual void Reload();
- virtual bool Save( const char* filename );
- virtual bool Update(void);
+ public:
+ GList* blocks;
+ uint32_t count;
+ stg_size_t size;
+
+ BlockGroup();
+ ~BlockGroup();
- bool TestQuit(){ return( quit || quit_all ); }
- void Quit(){ quit = true; }
- void QuitAll(){ quit_all = true; }
- void CancelQuit(){ quit = false; }
- void CancelQuitAll(){ quit_all = false; }
+ /** establish the min and max of all the blocks, so we can scale this
+ group later */
+ void CalcSize();
+ void AppendBlock( StgBlock* block );
+ void CallDisplayList( StgModel* mod );
+ void Clear() ; /** deletes all blocks from the group */
+
+ /** Returns a pointer to the first model detected to be colliding
+ with a block in this group, or NULL, if none are detected. */
+ StgModel* TestCollision();
+ void SwitchToTestedCells();
+
+ void Map();
+ void UnMap();
+
+ void DrawSolid(); // draw the block in OpenGL as a solid single color
+ void DrawFootPrint(); // draw the projection of the block onto the z=0 plane
- /** Get the resolution in pixels-per-metre of the underlying
- discrete raytracing model */
- double Resolution(){ return ppm; };
+ void LoadBitmap( StgModel* mod, const char* bitmapfile, Worldfile *wf );
+ void LoadBlock( StgModel* mod, Worldfile* wf, int entity );
+ };
- /** Returns a pointer to the model identified by ID, or NULL if
- nonexistent */
- //StgModel* GetModel( const stg_id_t id );
- /** Returns a pointer to the model identified by name, or NULL if
- nonexistent */
- StgModel* GetModel( const char* name );
+ typedef int ctrlinit_t( StgModel* mod );
+ //typedef void ctrlupdate_t( StgModel* mod );
- /** Return the 3D bounding box of the world, in meters */
- stg_bounds3d_t GetExtent(){ return extent; };
+ /// %StgModel class
+ class StgModel : public StgAncestor
+ {
+ friend class StgAncestor;
+ friend class StgWorld;
+ friend class StgWorldGui;
+ friend class StgCanvas;
+ friend class StgBlock;
+ friend class Region;
+ friend class BlockGroup;
+
+ private:
+ /** the number of models instatiated - used to assign unique IDs */
+ static uint32_t count;
+ static GHashTable* modelsbyid;
+ /** unique process-wide identifier for this model */
+ uint32_t id;
+ std::vector<Option*> drawOptions;
+ const std::vector<Option*>& getOptions() const { return drawOptions; }
- /** Return the number of times the world has been updated. */
- long unsigned int GetUpdateCount() { return updates; }
-};
+ protected:
+ static const char* typestr;
+
+ stg_pose_t pose;
+ stg_velocity_t velocity;
+ stg_watts_t watts; //< power consumed by this model
+ stg_color_t color;
+ stg_kg_t mass;
+ stg_geom_t geom;
+ stg_laser_return_t laser_return;
+ int obstacle_return;
+ int blob_return;
+ int gripper_return;
+ int ranger_return;
+ int fiducial_return;
+ int fiducial_key;
+ int boundary;
+ stg_meters_t map_resolution;
+ stg_bool_t stall;
+ StgModel* parent; //< the model that owns this one, possibly NUL
+ GArray* trail;
+ stg_pose_t global_pose;
+ bool rebuild_displaylist; //< iff true, regenerate block display list before redraw
+ bool gpose_dirty; //< set this to indicate that global pose may have changed
+ bool map_caches_are_invalid;
+ int subs; //< the number of subscriptions to this model
+ bool used; //< TRUE iff this model has been returned by GetUnusedModelOfType()
+ stg_usec_t interval; //< time between updates in us
+ stg_usec_t last_update; //< time of last update in us
+ stg_bool_t disabled; //< if non-zero, the model is disabled
+ char* say_string; //< if non-null, this string is displayed in the GUI
+ StgWorld* world; // pointer to the world in which this model exists
+ ctrlinit_t* initfunc;
+ GList* flag_list;
+ Worldfile* wf;
+ int wf_entity;
+ GPtrArray* blinkenlights;
+ int blocks_dl; //< OpenGL display list identifier
+ stg_model_type_t type;
+ BlockGroup blockgroup;
+ bool has_default_block;
+ bool on_velocity_list;
+ bool on_update_list;
+ int gui_nose;
+ int gui_grid;
+ int gui_outline;
+ int gui_mask;
+
+ /* hooks for attaching special callback functions (not used as
+ variables) */
+ char startup_hook, shutdown_hook, load_hook, save_hook, update_hook;
-typedef struct {
- stg_pose_t pose;
- stg_color_t color;
- stg_usec_t time;
-} stg_trail_item_t;
+ /** GData datalist can contain arbitrary named data items. Can be used
+ by derived model types to store properties, and for user code
+ to associate arbitrary items with a model. */
+ GData* props;
-typedef int ctrlinit_t( StgModel* mod );
-//typedef void ctrlupdate_t( StgModel* mod );
-
-/// %StgModel class
-class StgModel : public StgAncestor
-{
- friend class StgAncestor;
- friend class StgWorld;
- friend class StgWorldGui;
- friend class StgCanvas;
- friend class StgBlock;
+ /** callback functions can be attached to any field in this
+ structure. When the internal function model_change(<mod>,<field>)
+ is called, the callback is triggered */
+ GHashTable* callbacks;
-private:
- /** the number of models instatiated - used to assign unique IDs */
- static uint32_t count;
- static GHashTable* modelsbyid;
- /** unique process-wide identifier for this model */
- uint32_t id;
- std::vector<Option*> drawOptions;
- const std::vector<Option*>& getOptions() const { return drawOptions; }
+ bool data_fresh; ///< this can be set to indicate that the model has
+ ///new data that may be of interest to users. This
+ ///allows polling the model instead of adding a
+ ///data callback.
+
+ /** Thread safety flag. Iff true, Update() may be called in
+ parallel with other models. Defaults to false for safety */
+ bool thread_safe;
-protected:
- stg_pose_t pose;
- stg_velocity_t velocity;
- stg_watts_t watts; //< power consumed by this model
- stg_color_t color;
- stg_kg_t mass;
- stg_geom_t geom;
- stg_laser_return_t laser_return;
- int obstacle_return;
- int blob_return;
- int gripper_return;
- int ranger_return;
- int fiducial_return;
- int fiducial_key;
- int boundary;
- stg_meters_t map_resolution;
- stg_bool_t stall;
+ GMutex* access_mutex;
+
+
+ public:
+ void Lock()
+ {
+ if( access_mutex == NULL )
+ access_mutex = g_mutex_new();
+
+ assert( access_mutex );
+ g_mutex_lock( access_mutex );
+ }
+
+ void Unlock()
+ {
+ assert( access_mutex );
+ g_mutex_unlock( access_mutex );
+ }
+
+ protected:
- /** if non-null, this string is displayed in the GUI */
- char* say_string;
+ /// Register an Option for pickup by the GUI
+ void registerOption( Option* opt )
+ { drawOptions.push_back( opt ); }
- bool on_velocity_list;
+ /** Check to see if the current pose will yield a collision with
+ obstacles. Returns a pointer to the first entity we are in
+ collision with, or NULL if no collision exists. */
+ StgModel* TestCollision();
+
+ void CommitTestedPose();
- int gui_nose;
- int gui_grid;
- int gui_outline;
- int gui_mask;
-
- /// Register an Option for pickup by the GUI
- void registerOption( Option* opt ) { drawOptions.push_back( opt ); }
-
-
- StgModel* parent; //< the model that owns this one, possibly NULL
+ void Map();
+ void UnMap();
- /** GData datalist can contain arbitrary named data items. Can be used
- by derived model types to store properties, and for user code
- to associate arbitrary items with a model. */
- GData* props;
-
- /** callback functions can be attached to any field in this
- structure. When the internal function model_change(<mod>,<field>)
- is called, the callback is triggered */
- GHashTable* callbacks;
-
- int subs; //< the number of subscriptions to this model
- bool used; //< TRUE iff this model has been returned by GetUnusedModelOfType()
-
- stg_usec_t interval; //< time between updates in us
- stg_usec_t last_update; //< time of last update in us
-
- stg_bool_t disabled; //< if non-zero, the model is disabled
-
-// GList* d_list;
- GList* blocks; //< list of stg_block_t structs that comprise a body
-
- GArray* trail;
-
- bool rebuild_displaylist; //< iff true, regenerate block display list before redraw
-
- stg_pose_t global_pose;
-
- bool gpose_dirty; //< set this to indicate that global pose may have
- //changed
-
- bool data_fresh; ///< this can be set to indicate that the model has
- ///new data that may be of interest to users. This
- ///allows polling the model instead of adding a
- ///data callback.
-
- //stg_id_t id; // globally unique ID used as hash table key and
- // worldfile section identifier
-
- StgWorld* world; // pointer to the world in which this model exists
+ void MapWithChildren();
+ void UnMapWithChildren();
- /** 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 );
+ int TreeToPtrArray( GPtrArray* array );
- void Map();
- void UnMap();
+ /** raytraces a single ray from the point and heading identified by
+ pose, in local coords */
+ stg_raytrace_result_t Raytrace( stg_pose_t pose,
+ stg_meters_t range,
+ stg_ray_test_func_t func,
+ const void* arg,
+ bool ztest = true );
+
+ /** raytraces multiple rays around the point and heading identified
+ by pose, in local coords */
+ void Raytrace( stg_pose_t pose,
+ stg_meters_t range,
+ stg_radians_t fov,
+ stg_ray_test_func_t func,
+ const void* arg,
+ stg_raytrace_result_t* samples,
+ uint32_t sample_count,
+ bool ztest = true );
+
+ stg_raytrace_result_t Raytrace( stg_radians_t bearing,
+ stg_meters_t range,
+ stg_ray_test_func_t func,
+ const void* arg,
+ bool ztest = true );
+
+ void Raytrace( stg_radians_t bearing,
+ stg_meters_t range,
+ stg_radians_t fov,
+ stg_ray_test_func_t func,
+ const void* arg,
+ stg_raytrace_result_t* samples,
+ uint32_t sample_count,
+ bool ztest = true );
+
- void MapWithChildren();
- void UnMapWithChildren();
+ /** Causes this model and its children to recompute their global
+ position instead of using a cached pose in
+ StgModel::GetGlobalPose()..*/
+ void GPoseDirtyTree();
- int TreeToPtrArray( GPtrArray* array );
+ virtual void Startup();
+ virtual void Shutdown();
+ virtual void Update();
+ virtual void UpdatePose();
- /** raytraces a single ray from the point and heading identified by
- pose, in local coords */
- void Raytrace( stg_pose_t pose,
- stg_meters_t range,
- stg_block_match_func_t func,
- const void* arg,
- stg_raytrace_sample_t* sample,
- bool ztest = true );
+ void StartUpdating();
+ void StopUpdating();
- /** raytraces multiple rays around the point and heading identified
- by pose, in local coords */
- void Raytrace( stg_pose_t pose,
- stg_meters_t range,
- stg_radians_t fov,
- stg_block_match_func_t func,
- const void* arg,
- stg_raytrace_sample_t* samples,
- uint32_t sample_count,
- bool ztest = true );
+ StgModel* ConditionalMove( stg_pose_t newpose );
- void Raytrace( stg_radians_t bearing,
- stg_meters_t range,
- stg_block_match_func_t func,
- const void* arg,
- stg_raytrace_sample_t* sample,
- bool ztest = true );
+ stg_meters_t ModelHeight();
- void Raytrace( stg_radians_t bearing,
- stg_meters_t range,
- stg_radians_t fov,
- stg_block_match_func_t func,
- const void* arg,
- stg_raytrace_sample_t* samples,
- uint32_t sample_count,
- bool ztest = true );
-
-
- /** Causes this model and its children to recompute their global
- position instead of using a cached pose in
- StgModel::GetGlobalPose()..*/
- void GPoseDirtyTree();
-
- virtual void Startup();
- virtual void Shutdown();
- virtual void Update();
- virtual void UpdatePose();
-
- void DrawBlocksTree();
- virtual void DrawBlocks();
- virtual void DrawStatus( StgCamera* cam );
- void DrawStatusTree( StgCamera* cam );
-
- void DrawOriginTree();
- void DrawOrigin();
-
- void PushLocalCoords();
- void PopCoords();
-
- ///Draw the image stored in texture_id above the model
- void DrawImage( uint32_t texture_id, Stg::StgCamera* cam, float alpha );
-
- ///Returns the max height of the model
- stg_meters_t ModelHeight();
-
- // static wrapper for DrawBlocks()
- static void DrawBlocks( gpointer dummykey,
- StgModel* mod,
- void* arg );
-
- virtual void DrawPicker();
- virtual void DataVisualize( StgCamera* cam );
-
- virtual void DrawSelected(void);
-
- void DrawTrailFootprint();
- void DrawTrailBlocks();
- void DrawTrailArrows();
- void DrawGrid();
- void UpdateIfDue();
+ bool UpdateDue( void );
+ void UpdateIfDue();
- /* hooks for attaching special callback functions (not used as
- variables) */
- char startup_hook, shutdown_hook, load_hook, save_hook, update_hook;
+ void DrawBlocksTree();
+ virtual void DrawBlocks();
+ virtual void DrawStatus( Camera* cam );
+ void DrawStatusTree( Camera* cam );
+
+ void DrawOriginTree();
+ void DrawOrigin();
+
+ void PushLocalCoords();
+ void PopCoords();
+
+ ///Draw the image stored in texture_id above the model
+ void DrawImage( uint32_t texture_id, Camera* cam, float alpha );
+
+
+ // static wrapper for DrawBlocks()
+ static void DrawBlocks( gpointer dummykey,
+ StgModel* mod,
+ void* arg );
+
+ virtual void DrawPicker();
+ virtual void DataVisualize( Camera* cam );
+
+ virtual void DrawSelected(void);
+
+ void DrawTrailFootprint();
+ void DrawTrailBlocks();
+ void DrawTrailArrows();
+ void DrawGrid();
+
- ctrlinit_t* initfunc;
+ void DrawBlinkenlights();
- GList* flag_list;
-
- Worldfile* wf;
- int wf_entity;
-
- GPtrArray* blinkenlights;
- void DrawBlinkenlights();
-
- /** OpenGL display list identifier */
- int blocks_dl;
-
- /** Compile the display list for this model */
- //void BuildDisplayList();
-
- stg_model_type_t type;
- static const char* typestr;
-
- void DataVisualizeTree( StgCamera* cam );
+ void DataVisualizeTree( Camera* cam );
- virtual void PushColor( stg_color_t col )
- { world->PushColor( col ); }
+ virtual void PushColor( stg_color_t col )
+ { world->PushColor( col ); }
- virtual void PushColor( double r, double g, double b, double a )
- { world->PushColor( r,g,b,a ); }
+ virtual void PushColor( double r, double g, double b, double a )
+ { world->PushColor( r,g,b,a ); }
- virtual void PopColor(){ world->PopColor(); }
+ virtual void PopColor(){ world->PopColor(); }
- void DrawFlagList();
-
- void PushMyPose();
- void PopPose();
- void ShiftPose( stg_pose_t* pose );
- void ShiftToTop();
+ void DrawFlagList();
+ stg_point_t* LocalToGlobal( double scalex,
+ double scaley,
+ stg_point_t pts[],
+ uint32_t pt_count );
-public:
- void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax,
- stg_meters_t ymin, stg_meters_t ymax );
+ void DrawPose( stg_pose_t pose );
+
+ public:
+ void RecordRenderPoint( GSList** head, GSList* link,
+ unsigned int* c1, unsigned int* c2 );
+
+ void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax,
+ stg_meters_t ymin, stg_meters_t ymax );
- /** Look up a model pointer by a unique model ID */
- static StgModel* LookupId( uint32_t id )
- { return (StgModel*)g_hash_table_lookup( modelsbyid, (void*)id ); }
+ /** Look up a model pointer by a unique model ID */
+ static StgModel* LookupId( uint32_t id )
+ { return (StgModel*)g_hash_table_lookup( modelsbyid, (void*)id ); }
- StgModel( StgWorld* world, StgModel* parent, stg_model_type_t type = MODEL_TYPE_PLAIN );
- virtual ~StgModel();
+ /** Constructor */
+ StgModel( StgWorld* world,
+ StgModel* parent,
+ stg_model_type_t type = MODEL_TYPE_PLAIN );
+
+ /** Destructor */
+ virtual ~StgModel();
- void Say( const char* str );
+ void Say( const char* str );
- void Load( Worldfile* wf, int wf_entity )
- {
+ void Load( Worldfile* wf, int wf_entity )
+ {
/** Set the worldfile and worldfile entity ID - must be called
- before Load() */
+ before Load() */
SetWorldfile( wf, wf_entity );
Load(); // call virtual load
- }
+ }
- /** Set the worldfile and worldfile entity ID */
- void SetWorldfile( Worldfile* wf, int wf_entity )
- { this->wf = wf; this->wf_entity = wf_entity; }
+ /** Set the worldfile and worldfile entity ID */
+ void SetWorldfile( Worldfile* wf, int wf_entity )
+ { this->wf = wf; this->wf_entity = wf_entity; }
- /** configure a model by reading from the current world file */
- virtual void Load();
+ /** configure a model by reading from the current world file */
+ virtual void Load();
- /** save the state of the model to the current world file */
- virtual void Save();
+ /** save the state of the model to the current world file */
+ virtual void Save();
- /** Should be called after all models are loaded, to do any last-minute setup */
- void Init();
+ /** Should be called after all models are loaded, to do any last-minute setup */
+ void Init();
+ void InitRecursive();
+
+ void AddFlag( StgFlag* flag );
+ void RemoveFlag( StgFlag* flag );
- void AddFlag( StgFlag* flag );
- void RemoveFlag( StgFlag* flag );
+ void PushFlag( StgFlag* flag );
+ StgFlag* PopFlag();
- void PushFlag( StgFlag* flag );
- StgFlag* PopFlag();
+ int GetFlagCount(){ return g_list_length( flag_list ); }
- int GetFlagCount(){ return g_list_length( flag_list ); }
-
- void AddBlinkenlight( stg_blinkenlight_t* b )
- {
+ void AddBlinkenlight( stg_blinkenlight_t* b )
+ {
g_ptr_array_add( this->blinkenlights, b );
- }
+ }
- void ClearBlinkenlights()
- {
+ void ClearBlinkenlights()
+ {
g_ptr_array_set_size( this->blinkenlights, 0 );
- }
+ }
- void Enable(){ disabled = false; };
- void Disable(){ disabled = true; };
+ void Enable(){ disabled = false; };
+ void Disable(){ disabled = true; };
- // Load a control program for this model from an external library
- void LoadControllerModule( char* lib );
+ // Load a control program for this model from an external library
+ void LoadControllerModule( char* lib );
- // call this to ensure the GUI window is redrawn
- void NeedRedraw();
+ // call this to ensure the GUI window is redrawn
+ void NeedRedraw();
- void AddBlock( stg_point_t* pts,
- size_t pt_count,
- stg_meters_t zmin,
- stg_meters_t zmax,
- stg_color_t color,
- bool inherit_color );
+ void LoadBlock( Worldfile* wf, int entity );
+
+ // void AddBlock( stg_point_t* pts,
+ // size_t pt_count,
+ // stg_meters_t zmin,
+ // stg_meters_t zmax,
+ // stg_color_t color,
+ // bool inherit_color );
+
+ void AddBlockRect( stg_meters_t x, stg_meters_t y,
+ stg_meters_t dx, stg_meters_t dy,
+ stg_meters_t dz );
- void AddBlockRect( double x, double y,
- double width, double height );
+
+ /** remove all blocks from this model, freeing their memory */
+ void ClearBlocks();
- /** remove all blocks from this model, freeing their memory */
- void ClearBlocks();
+ //const char* TypeStr(){ return this->typestr; }
+ StgModel* Parent(){ return this->parent; }
+ StgModel* GetModel( const char* name );
+ int GuiMask(){ return this->gui_mask; };
+ inline StgWorld* GetWorld(){ return this->world; }
- //const char* TypeStr(){ return this->typestr; }
- StgModel* Parent(){ return this->parent; }
- StgModel* GetModel( const char* name );
- int GuiMask(){ return this->gui_mask; };
- inline StgWorld* GetWorld(){ return this->world; }
+ /// return the root model of the tree containing this model
+ StgModel* Root()
+ { return( parent ? parent->Root() : this ); }
- /// return the root model of the tree containing this model
- StgModel* Root()
- { return( parent ? parent->Root() : this ); }
+ bool ObstacleReturn(){ return obstacle_return; }
+ stg_laser_return_t GetLaserReturn(){ return laser_return; }
+ int GetRangerReturn(){ return ranger_return; }
+ int FiducialReturn(){ return fiducial_return; }
+ int FiducialKey(){ return fiducial_key; }
- bool ObstacleReturn(){ return obstacle_return; }
- stg_laser_return_t GetLaserReturn(){ return laser_return; }
- int GetRangerReturn(){ return ranger_return; }
- int FiducialReturn(){ return fiducial_return; }
- int FiducialKey(){ return fiducial_key; }
+ bool IsAntecedent( StgModel* testmod );
- bool IsAntecedent( StgModel* testmod );
+ // returns true if model [testmod] is a descendent of model [mod]
+ bool IsDescendent( StgModel* testmod );
- // returns true if model [testmod] is a descendent of model [mod]
- bool IsDescendent( StgModel* testmod );
+ bool IsRelated( StgModel* mod2 );
- bool IsRelated( StgModel* mod2 );
+ /** get the pose of a model in the global CS */
+ stg_pose_t GetGlobalPose();
- /** get the pose of a model in the global CS */
- stg_pose_t GetGlobalPose();
+ /** get the velocity of a model in the global CS */
+ stg_velocity_t GetGlobalVelocity();
- /** get the velocity of a model in the global CS */
- stg_velocity_t GetGlobalVelocity();
+ /* set the velocity of a model in the global coordinate system */
+ void SetGlobalVelocity( stg_velocity_t gvel );
- /* set the velocity of a model in the global coordinate system */
- void SetGlobalVelocity( stg_velocity_t gvel );
+ /** subscribe to a model's data */
+ void Subscribe();
- /** subscribe to a model's data */
- void Subscribe();
+ /** unsubscribe from a model's data */
+ void Unsubscribe();
- /** unsubscribe from a model's data */
- void Unsubscribe();
+ /** set the pose of model in global coordinates */
+ void SetGlobalPose( stg_pose_t gpose );
- /** set the pose of model in global coordinates */
- void SetGlobalPose( stg_pose_t gpose );
+ /** set a model's velocity in its parent's coordinate system */
+ void SetVelocity( stg_velocity_t vel );
- /** set a model's velocity in its parent's coordinate system */
- void SetVelocity( stg_velocity_t vel );
+ /** set a model's pose in its parent's coordinate system */
+ void SetPose( stg_pose_t pose );
- /** set a model's pose in its parent's coordinate system */
- void SetPose( stg_pose_t pose );
+ /** add values to a model's pose in its parent's coordinate system */
+ void AddToPose( stg_pose_t pose );
- /** add values to a model's pose in its parent's coordinate system */
- void AddToPose( stg_pose_t pose );
+ /** add values to a model's pose in its parent's coordinate system */
+ void AddToPose( double dx, double dy, double dz, double da );
- /** add values to a model's pose in its parent's coordinate system */
- void AddToPose( double dx, double dy, double dz, double da );
+ /** set a model's geometry (size and center offsets) */
+ void SetGeom( stg_geom_t src );
- /** set a model's geometry (size and center offsets) */
- void SetGeom( stg_geom_t src );
+ /** set a model's geometry (size and center offsets) */
+ void SetFiducialReturn( int fid );
- /** set a model's geometry (size and center offsets) */
- void SetFiducialReturn( int fid );
+ /** set a model's fiducial key: only fiducial finders with a
+ matching key can detect this model as a fiducial. */
+ void SetFiducialKey( int key );
- /** set a model's fiducial key: only fiducial finders with a
- matching key can detect this model as a fiducial. */
- void SetFiducialKey( int key );
+ stg_color_t GetColor(){ return color; }
- stg_color_t GetColor(){ return color; }
+ // stg_laser_return_t GetLaserReturn(){ return laser_return; }
- // stg_laser_return_t GetLaserReturn(){ return laser_return; }
+ /** Change a model's parent - experimental*/
+ int SetParent( StgModel* newparent);
- /** Change a model's parent - experimental*/
- int SetParent( StgModel* newparent);
+ /** Get a model's geometry - it's size and local pose (offset from
+ origin in local coords) */
+ stg_geom_t GetGeom(){ return geom; }
- /** Get a model's geometry - it's size and local pose (offset from
- origin in local coords) */
- stg_geom_t GetGeom(){ return geom; }
+ /** Get the pose of a model in its parent's coordinate system */
+ stg_pose_t GetPose(){ return pose; }
- /** Get the pose of a model in its parent's coordinate system */
- stg_pose_t GetPose(){ return pose; }
+ /** Get a model's velocity (in its local reference frame) */
+ stg_velocity_t GetVelocity(){ return velocity; }
- /** Get a model's velocity (in its local reference frame) */
- stg_velocity_t GetVelocity(){ return velocity; }
+ // guess what these do?
+ void SetColor( stg_color_t col );
+ void SetMass( stg_kg_t mass );
+ void SetStall( stg_bool_t stall );
+ void SetGripperReturn( int val );
+ void SetLaserReturn( stg_laser_return_t val );
+ void SetObstacleReturn( int val );
+ void SetBlobReturn( int val );
+ void SetRangerReturn( int val );
+ void SetBoundary( int val );
+ void SetGuiNose( int val );
+ void SetGuiMask( int val );
+ void SetGuiGrid( int val );
+ void SetGuiOutline( int val );
+ void SetWatts( stg_watts_t watts );
+ void SetMapResolution( stg_meters_t res );
- // guess what these do?
- void SetColor( stg_color_t col );
- void SetMass( stg_kg_t mass );
- void SetStall( stg_bool_t stall );
- void SetGripperReturn( int val );
- void SetLaserReturn( stg_laser_return_t val );
- void SetObstacleReturn( int val );
- void SetBlobReturn( int val );
- void SetRangerReturn( int val );
- void SetBoundary( int val );
- void SetGuiNose( int val );
- void SetGuiMask( int val );
- void SetGuiGrid( int val );
- void SetGuiOutline( int val );
- void SetWatts( stg_watts_t watts );
- void SetMapResolution( stg_meters_t res );
+ bool DataIsFresh(){ return this->data_fresh; }
- bool DataIsFresh(){ return this->data_fresh; }
+ /* attach callback functions to data members. The function gets
+ called when the member is changed using SetX() accessor method */
- /* attach callback functions to data members. The function gets
- called when the member is changed using SetX() accessor method */
+ void AddCallback( void* address,
+ stg_model_callback_t cb,
+ void* user );
- void AddCallback( void* address,
- stg_model_callback_t cb,
- void* user );
+ int RemoveCallback( void* member,
+ stg_model_callback_t callback );
- int RemoveCallback( void* member,
- stg_model_callback_t callback );
+ void CallCallbacks( void* address );
- void CallCallbacks( void* address );
+ /* wrappers for the generic callback add & remove functions that hide
+ some implementation detail */
- /* wrappers for the generic callback add & remove functions that hide
- some implementation detail */
+ void AddStartupCallback( stg_model_callback_t cb, void* user )
+ { AddCallback( &startup_hook, cb, user ); };
- void AddStartupCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &startup_hook, cb, user ); };
+ void RemoveStartupCallback( stg_model_callback_t cb )
+ { RemoveCallback( &startup_hook, cb ); };
- void RemoveStartupCallback( stg_model_callback_t cb )
- { RemoveCallback( &startup_hook, cb ); };
+ void AddShutdownCallback( stg_model_callback_t cb, void* user )
+ { AddCallback( &shutdown_hook, cb, user ); };
- void AddShutdownCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &shutdown_hook, cb, user ); };
+ void RemoveShutdownCallback( stg_model_callback_t cb )
+ { RemoveCallback( &shutdown_hook, cb ); }
- void RemoveShutdownCallback( stg_model_callback_t cb )
- { RemoveCallback( &shutdown_hook, cb ); }
+ void AddLoadCallback( stg_model_callback_t cb, void* user )
+ { AddCallback( &load_hook, cb, user ); }
- void AddLoadCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &load_hook, cb, user ); }
+ void RemoveLoadCallback( stg_model_callback_t cb )
+ { RemoveCallback( &load_hook, cb ); }
- void RemoveLoadCallback( stg_model_callback_t cb )
- { RemoveCallback( &load_hook, cb ); }
+ void AddSaveCallback( stg_model_callback_t cb, void* user )
+ { AddCallback( &save_hook, cb, user ); }
- void AddSaveCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &save_hook, cb, user ); }
+ void RemoveSaveCallback( stg_model_callback_t cb )
+ { RemoveCallback( &save_hook, cb ); }
- void RemoveSaveCallback( stg_model_callback_t cb )
- { RemoveCallback( &save_hook, cb ); }
+ void AddUpdateCallback( stg_model_callback_t cb, void* user )
+ { AddCallback( &update_hook, cb, user ); }
- void AddUpdateCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &update_hook, cb, user ); }
+ void RemoveUpdateCallback( stg_model_callback_t cb )
+ { RemoveCallback( &update_hook, cb ); }
- void RemoveUpdateCallback( stg_model_callback_t cb )
- { RemoveCallback( &update_hook, cb ); }
+ /** named-property interface
+ */
+ void* GetProperty( char* key );
- /** named-property interface
- */
- void* GetProperty( char* key );
-
- /** @brief Set a named property of a Stage model.
+ /** @brief Set a named property of a Stage model.
- Set a property of a Stage model.
+ Set a property of a Stage model.
- This function can set both predefined and user-defined
- properties of a model. Predefined properties are intrinsic to
- every model, such as pose and color. Every supported predefined
- properties has its identifying string defined as a preprocessor
- macro in stage.h. Users should use the macro instead of a
- hard-coded string, so that the compiler can help you to avoid
- mis-naming properties.
+ This function can set both predefined and user-defined
+ properties of a model. Predefined properties are intrinsic to
+ every model, such as pose and color. Every supported predefined
+ properties has its identifying string defined as a preprocessor
+ macro in stage.h. Users should use the macro instead of a
+ hard-coded string, so that the compiler can help you to avoid
+ mis-naming properties.
- User-defined properties allow the user to attach arbitrary data
- pointers to a model. User-defined property data is not copied,
- so the original pointer must remain valid. User-defined property
- names are simple strings. Names beginning with an underscore
- ('_') are reserved for internal libstage use: users should not
- use names beginning with underscore (at risk of causing very
- weird behaviour).
+ User-defined properties allow the user to attach arbitrary data
+ pointers to a model. User-defined property data is not copied,
+ so the original pointer must remain valid. User-defined property
+ names are simple strings. Names beginning with an underscore
+ ('_') are reserved for internal libstage use: users should not
+ use names beginning with underscore (at risk of causing very
+ weird behaviour).
- Any callbacks registered for the named property will be called.
+ Any callbacks registered for the named property will be called.
- Returns 0 on success, or a positive error code on failure.
+ Returns 0 on success, or a positive error code on failure.
- *CAUTION* The caller is responsible for making sure the pointer
- points to data of the correct type for the property, so use
- carefully. Check the docs or the equivalent
- stg_model_set_<property>() function definition to see the type
- of data required for each property.
+ *CAUTION* The caller is responsible for making sure the pointer
+ points to data of the correct type for the property, so use
+ carefully. Check the docs or the equivalent
+ stg_model_set_<property>() function definition to see the type
+ of data required for each property.
*/
- int SetProperty( char* key, void* data );
- void UnsetProperty( char* key );
+ int SetProperty( char* key, void* data );
+ void UnsetProperty( char* key );
- virtual void Print( char* prefix );
- virtual const char* PrintWithPose();
+ virtual void Print( char* prefix );
+ virtual const char* PrintWithPose();
- /** Convert a pose in the world coordinate system into a model's
- local coordinate system. Overwrites [pose] with the new
- coordinate. */
- void GlobalToLocal( stg_pose_t* pose );
+ /** Convert a pose in the world coordinate system into a model's
+ local coordinate system. Overwrites [pose] with the new
+ coordinate. */
+ stg_pose_t GlobalToLocal( const stg_pose_t pose );
- /** Convert a pose in the model's local coordinate system into the
- world coordinate system. Overwrites [pose] with the new
- coordinate. */
- //void LocalToGlobal( stg_pose_t* pose );
+ /** Convert a pose in the model's local coordinate system into the
+ world coordinate system. Overwrites [pose] with the new
+ coordinate. */
+ //void LocalToGlobal( stg_pose_t* pose );
- /** Return the global pose (i.e. pose in world coordinates) of a
- pose specified in the model's local coordinate system */
- stg_pose_t LocalToGlobal( stg_pose_t pose );
+ /** Return the global pose (i.e. pose in world coordinates) of a
+ pose specified in the model's local coordinate system */
+ stg_pose_t LocalToGlobal( const stg_pose_t pose );
- /** Return the 3d point in world coordinates of a 3d point
- specified in the model's local coordinate system */
- stg_point3_t LocalToGlobal( stg_point3_t local );
+ /** Return the 3d point in world coordinates of a 3d point
+ specified in the model's local coordinate system */
+ stg_point3_t LocalToGlobal( const stg_point3_t local );
- /** returns the first descendent of this model that is unsubscribed
- and has the type indicated by the string */
- StgModel* GetUnsubscribedModelOfType( stg_model_type_t type );
+ /** returns the first descendent of this model that is unsubscribed
+ and has the type indicated by the string */
+ StgModel* GetUnsubscribedModelOfType( const stg_model_type_t type );
- /** returns the first descendent of this model that is unused
- and has the type indicated by the string. This model is tagged as used. */
- StgModel* GetUnusedModelOfType( stg_model_type_t type );
+ /** returns the first descendent of this model that is unused
+ and has the type indicated by the string. This model is tagged as used. */
+ StgModel* GetUnusedModelOfType( const stg_model_type_t type );
- // Iff true, model may output some debugging visualizations and other info
- //bool debug;
+ // Iff true, model may output some debugging visualizations and other info
+ //bool debug;
- /** Returns the value of the model's stall boolean, which is true
- iff the model has crashed into another model */
- bool Stalled(){ return this->stall; }
-};
+ /** Returns the value of the model's stall boolean, which is true
+ iff the model has crashed into another model */
+ bool Stalled(){ return this->stall; }
+ };
-// BLOCKS
-class StgBlock
-{
-public:
+ // BLOCKS
- /** Block Constructor. A model's body is a list of these
- blocks. The point data is copied, so pts can safely be freed
- after constructing the block.*/
- StgBlock( StgModel* mod,
- stg_point_t* pts,
- size_t pt_count,
- stg_meters_t height,
- stg_meters_t z_offset,
- stg_color_t color,
- bool inherit_color );
-
- ~StgBlock();
-
- void Map();
- void UnMap(); // draw the block into the world
-
- void DrawGlobal(); // draw the block in OpenGL using pts_global coords
- void Draw(); // draw the block in OpenGL
- //void Draw2D(); // draw the block in OpenGL
- void DrawSolid(); // draw the block in OpenGL as a solid single color
- void DrawFootPrint(); // draw the projection of the block onto the z=0 plane
-
- static void ScaleList( GList* blocks, stg_size_t* size );
- void RecordRenderPoint( GSList** head, GSList* link, unsigned int* c1, unsigned int* c2 );
-
- StgModel* Model(){ return mod; };
-
- stg_point_t* Points( unsigned int *count )
- { if( count ) *count = pt_count; return pts; };
-
- bool IntersectGlobalZ( stg_meters_t z )
- { return( z >= global_zmin && z <= global_zmax ); }
-
- stg_color_t Color()
- { return( inherit_color ? mod->GetColor() : color ); }
-
-private:
- stg_point_t* pts; //< points defining a polygon
- size_t pt_count; //< the number of points
- stg_meters_t zmin;
- stg_meters_t zmax;
-
- stg_meters_t global_zmin;
- stg_meters_t global_zmax;
-
- StgModel* mod; //< model to which this block belongs
-
- stg_point_int_t* pts_global_pixels; //< points defining a polygon in global coords
-
- stg_color_t color;
- bool inherit_color;
-
- GArray* rendered_points;
-
- inline void DrawTop();
- inline void DrawSides();
-
- inline void PushColor( stg_color_t col )
- { mod->PushColor( col ); }
-
- inline void PushColor( double r, double g, double b, double a )
- { mod->PushColor( r,g,b,a ); }
-
- inline void PopColor()
- { mod->PopColor(); }
-};
-
-// COLOR STACK CLASS
-class GlColorStack
-{
- public:
- GlColorStack();
- ~GlColorStack();
-
- void Push( GLdouble col[4] );
- void Push( double r, double g, double b, double a );
- void Push( double r, double g, double b );
- void Push( stg_color_t col );
-
- void Pop();
-
- unsigned int Length()
- { return g_queue_get_length( colorstack ); }
-
- private:
- GQueue* colorstack;
-};
-
-class StgCamera
-{
- protected:
- float _pitch; //left-right (about y)
- float _yaw; //up-down (about x)
- float _x, _y, _z;
+ class Camera
+ {
+ protected:
+ float _pitch; //left-right (about y)
+ float _yaw; //up-down (about x)
+ float _x, _y, _z;
- public:
- StgCamera() : _pitch( 0 ), _yaw( 0 ), _x( 0 ), _y( 0 ), _z( 0 ) { }
- virtual ~StgCamera() { }
+ public:
+ Camera() : _pitch( 0 ), _yaw( 0 ), _x( 0 ), _y( 0 ), _z( 0 ) { }
+ virtual ~Camera() { }
- virtual void Draw( void ) const = 0;
- virtual void SetProjection( void ) const = 0;
+ virtual void Draw( void ) const = 0;
+ virtual void SetProjection( void ) const = 0;
- inline float yaw( void ) const { return _yaw; }
- inline float pitch( void ) const { return _pitch; }
+ inline float yaw( void ) const { return _yaw; }
+ inline float pitch( void ) const { return _pitch; }
- inline float x( void ) const { return _x; }
- inline float y( void ) const { return _y; }
- inline float z( void ) const { return _z; }
+ inline float x( void ) const { return _x; }
+ inline float y( void ) const { return _y; }
+ inline float z( void ) const { return _z; }
- virtual void reset() = 0;
- virtual void Load( Worldfile* wf, int sec ) = 0;
+ virtual void reset() = 0;
+ virtual void Load( Worldfile* wf, int sec ) = 0;
- //TODO data should be passed in somehow else. (at least min/max stuff)
- //virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0;
-};
+ //TODO data should be passed in somehow else. (at least min/max stuff)
+ //virtual void SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) const = 0;
+ };
-class StgPerspectiveCamera : public StgCamera
-{
- private:
- float _z_near;
- float _z_far;
- float _vert_fov;
- float _horiz_fov;
- float _aspect;
+ class PerspectiveCamera : public Camera
+ {
+ private:
+ float _z_near;
+ float _z_far;
+ float _vert_fov;
+ float _horiz_fov;
+ float _aspect;
- public:
- StgPerspectiveCamera( void );
+ public:
+ PerspectiveCamera( void );
- virtual void Draw( void ) const;
- virtual void SetProjection( void ) const;
- //void SetProjection( float aspect ) const;
- void update( void );
+ virtual void Draw( void ) const;
+ virtual void SetProjection( void ) const;
+ //void SetProjection( float aspect ) const;
+ void update( void );
- void strafe( float amount );
- void forward( float amount );
+ void strafe( float amount );
+ void forward( float amount );
- 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; }
- void move( float x, float y, float z );
- inline void setFov( float horiz_fov, float vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; }
- ///update vertical fov based on window aspect and current horizontal fov
- inline void setAspect( float aspect ) {
- //std::cout << "aspect: " << aspect << " vert: " << _vert_fov << " => " << aspect * _vert_fov << std::endl;
@@ Diff output truncated at 100000 characters. @@
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|