stage.h
Go to the documentation of this file.00001 #ifndef STG_H 00002 #define STG_H 00003 /* 00004 * Stage : a multi-robot simulator. 00005 * 00006 * Copyright (C) 2001-2004 Richard Vaughan, Andrew Howard and Brian 00007 * Gerkey for the Player/Stage Project 00008 * http://playerstage.sourceforge.net 00009 * 00010 * This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU General Public License for more details. 00019 * 00020 * You should have received a copy of the GNU General Public License 00021 * along with this program; if not, write to the Free Software 00022 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00023 * 00024 */ 00025 00026 /* File: stage.h 00027 * Desc: External header file for the Stage library 00028 * Authors: Richard Vaughan vaughan@sfu.ca 00029 * Andrew Howard ahowards@usc.edu 00030 * Brian Gerkey gerkey@stanford.edu 00031 * Date: 1 June 2003 00032 * CVS: $Id: stage.h,v 1.158 2005/08/11 19:56:54 rtv Exp $ 00033 */ 00034 00035 00043 #include <stdlib.h> 00044 #include <stdio.h> 00045 #include <sys/socket.h> 00046 #include <netdb.h> 00047 #include <string.h> 00048 #include <unistd.h> 00049 #include <stdint.h> // for portable int types eg. uint32_t 00050 #include <sys/types.h> 00051 #include <sys/time.h> 00052 #include <assert.h> 00053 #include <pthread.h> 00054 #include <semaphore.h> 00055 00056 #include <glib.h> // we use GLib's data structures extensively 00057 #include <rtk.h> // and graphics stuff pulled from Andrew Howard's RTK2 library 00058 00059 #ifdef __cplusplus 00060 extern "C" { 00061 #endif 00062 00063 #include "config.h" 00064 #include "replace.h" 00065 00104 // TODO - fix this up 00105 #define FiducialNone 0 00106 00109 #define RangecomNone 0 00110 #define RangecomImpede 1 00111 #define RangecomPoint 2 00112 00113 // Basic self-describing measurement types. All packets with real 00114 // measurements are specified in these terms so changing types here 00115 // should work throughout the code If you change these, be sure to 00116 // change the byte-ordering macros below accordingly. 00117 00119 typedef int stg_id_t; 00120 00122 typedef double stg_meters_t; 00123 00125 typedef double stg_radians_t; 00126 00128 typedef unsigned long stg_msec_t; 00129 00131 typedef double stg_kg_t; // Kilograms (mass) 00132 00134 typedef double stg_joules_t; 00135 00137 typedef double stg_watts_t; 00138 00140 typedef int stg_bool_t; 00141 00142 //typedef double stg_friction_t; 00143 00145 typedef uint32_t stg_color_t; 00146 00149 typedef int stg_obstacle_return_t; 00150 00153 typedef int stg_blob_return_t; 00154 00156 typedef int stg_fiducial_return_t; 00157 00159 typedef int stg_rangecom_return_t; 00160 00161 typedef int stg_ranger_return_t; 00162 00163 typedef enum { STG_GRIP_NO = 0, STG_GRIP_YES } stg_gripper_return_t; 00164 00167 typedef struct 00168 { 00169 stg_meters_t x, y; 00170 } stg_size_t; 00171 00175 typedef struct 00176 { 00177 stg_meters_t x, y, a; 00178 } stg_pose_t; 00179 00182 typedef stg_pose_t stg_velocity_t; 00183 00187 typedef struct 00188 { 00189 stg_pose_t pose; 00190 stg_size_t size; 00191 } stg_geom_t; 00192 00194 typedef struct 00195 { 00196 double min; //< smallest value in range 00197 double max; //< largest value in range 00198 } stg_bounds_t; 00199 00201 typedef struct 00202 { 00203 stg_bounds_t range; //< min and max range of sensor 00204 stg_radians_t angle; //< width of viewing angle of sensor 00205 } stg_fov_t; 00206 00207 00208 // PRETTY PRINTING ------------------------------------------------- 00209 00216 void stg_print_geom( stg_geom_t* geom ); 00217 00219 void stg_err( const char* err ); 00220 00223 // ENERGY -------------------------------------------------------------- 00224 00226 typedef struct 00227 { 00229 stg_joules_t stored; 00230 00232 stg_joules_t capacity; 00233 00235 stg_joules_t input_joules; 00236 00238 stg_joules_t output_joules; 00239 00241 stg_watts_t input_watts; 00242 00244 stg_watts_t output_watts; 00245 00247 stg_bool_t charging; 00248 00250 stg_meters_t range; 00251 } stg_energy_data_t; 00252 00254 typedef struct 00255 { 00257 stg_joules_t capacity; 00258 00260 stg_watts_t give; 00261 00264 stg_watts_t take; 00265 00267 stg_meters_t probe_range; 00268 } stg_energy_config_t; 00269 00270 // there is currently no energy command packet 00271 00272 // BLINKENLIGHT ------------------------------------------------------- 00273 00274 //typedef struct 00275 //{ 00276 //int enable; 00277 //stg_msec_t period; 00278 //} stg_blinkenlight_t; 00279 00280 // GRIPPER ------------------------------------------------------------ 00281 00282 // Possible Gripper return values 00283 //typedef enum 00284 //{ 00285 // GripperDisabled = 0, 00286 // GripperEnabled 00287 //} stg_gripper_return_t; 00288 00289 // GUIFEATURES ------------------------------------------------------- 00290 00291 // Movement masks for figures 00292 #define STG_MOVE_TRANS (1 << 0) 00293 #define STG_MOVE_ROT (1 << 1) 00294 #define STG_MOVE_SCALE (1 << 2) 00295 00296 typedef int stg_movemask_t; 00297 00298 /* typedef struct */ 00299 /* { */ 00300 /* uint8_t show_data; */ 00301 /* uint8_t show_cfg; */ 00302 /* uint8_t show_cmd; */ 00303 00304 /* uint8_t nose; */ 00305 /* uint8_t grid; */ 00306 /* //uint8_t boundary; */ 00307 /* uint8_t outline; */ 00308 /* stg_movemask_t movemask; */ 00309 /* } stg_guifeatures_t; */ 00310 00311 00312 // LASER ------------------------------------------------------------ 00313 00315 typedef enum 00316 { 00317 LaserTransparent, 00318 LaserVisible, 00319 LaserBright 00320 } stg_laser_return_t; 00321 00322 00325 stg_msec_t stg_timenow( void ); 00326 00332 int stg_init( int argc, char** argv ); 00333 00334 00336 int stg_quit_test( void ); 00337 00340 void stg_quit_request( void ); 00341 00342 00343 // UTILITY STUFF ---------------------------------------------------- 00344 00353 const char* stg_version_string( void ); 00354 00359 stg_color_t stg_lookup_color(const char *name); 00360 00363 void stg_pose_sum( stg_pose_t* result, stg_pose_t* p1, stg_pose_t* p2 ); 00364 00365 // POINTS --------------------------------------------------------- 00366 00373 typedef struct 00374 { 00375 stg_meters_t x, y; 00376 } stg_point_t; 00377 00380 stg_point_t* stg_points_create( size_t count ); 00381 00383 void stg_points_destroy( stg_point_t* pts ); 00384 00388 // POLYGONS --------------------------------------------------------- 00389 00397 typedef struct 00398 { 00400 GArray* points; 00401 00403 stg_bool_t filled; 00404 00406 stg_color_t color; 00407 } stg_polygon_t; 00408 00409 00411 stg_polygon_t* stg_polygons_create( int count ); 00412 00414 void stg_polygons_destroy( stg_polygon_t* p, size_t count ); 00415 00418 stg_polygon_t* stg_unit_polygon_create( void ); 00419 00423 void stg_polygon_set_points( stg_polygon_t* poly, stg_point_t* pts, size_t count ); 00426 void stg_polygon_append_points( stg_polygon_t* poly, stg_point_t* pts, size_t count ); 00427 00430 void stg_polygons_normalize( stg_polygon_t* polys, int num, 00431 double width, double height ); 00432 00434 void stg_polygon_print( stg_polygon_t* poly ); 00435 00437 void stg_polygons_print( stg_polygon_t* polys, unsigned int count ); 00438 00443 stg_polygon_t* stg_polygons_from_image_file( const char* filename, 00444 size_t* poly_count ); 00445 00448 // end util documentation group 00452 // end property typedefs ------------------------------------------------- 00453 00454 00455 // forward declare struct types from player_internal.h 00456 struct _stg_model; 00457 struct _stg_matrix; 00458 struct _gui_window; 00459 struct _stg_world; 00460 00463 typedef struct _stg_world stg_world_t; 00464 00465 00471 typedef struct _stg_model stg_model_t; // defined in stage_internal.h 00472 00473 // MODEL -------------------------------------------------------- 00474 00475 // group the docs of all the model types 00481 #define STG_PROPNAME_MAX 128 00482 00483 00484 00485 00486 00491 typedef int (*stg_property_callback_t)(stg_model_t* mod, char* propname, void* data, size_t len, void* userdata ); 00492 00493 00500 typedef int(*stg_model_initializer_t)(stg_model_t*); 00501 00502 00505 typedef struct 00506 { 00507 stg_property_callback_t callback; 00508 void* arg; 00509 } stg_cbarg_t; 00510 00512 stg_model_t* stg_model_create( stg_world_t* world, 00513 stg_model_t* parent, 00514 stg_id_t id, 00515 char* token, 00516 stg_model_initializer_t initializer ); 00517 00519 void stg_model_destroy( stg_model_t* mod ); 00520 00522 void stg_model_get_global_pose( stg_model_t* mod, stg_pose_t* pose ); 00523 00525 //void stg_model_get_global_velocity( stg_model_t* mod, stg_velocity_t* gvel ); 00526 00527 /* set the velocity of a model in the global coordinate system */ 00528 //void stg_model_set_global_velocity( stg_model_t* mod, stg_velocity_t* gvel ); 00529 00531 void stg_model_subscribe( stg_model_t* mod ); 00532 00534 void stg_model_unsubscribe( stg_model_t* mod ); 00535 00537 void stg_model_load( stg_model_t* mod ); 00538 00540 void stg_model_save( stg_model_t* mod ); 00541 00543 //const char* stg_model_type_string( stg_model_type_t type ); 00544 00545 // SET properties - use these to set props, don't set them directly 00546 00548 int stg_model_set_global_pose( stg_model_t* mod, stg_pose_t* gpose ); 00549 00551 int stg_model_set_velocity( stg_model_t* mod, stg_velocity_t* vel ); 00552 00553 // TODO? 00556 //void stg_model_lock( stg_model_t* mod ); 00557 00559 //void stg_model_unlock( stg_model_t* mod ); 00560 00562 int stg_model_set_parent( stg_model_t* mod, stg_model_t* newparent); 00563 00564 void stg_model_get_geom( stg_model_t* mod, stg_geom_t* dest ); 00565 void stg_model_get_velocity( stg_model_t* mod, stg_velocity_t* dest ); 00566 00567 void stg_model_set_property( stg_model_t* mod, 00568 const char* prop, 00569 void* data, 00570 size_t len ); 00571 00574 void* stg_model_get_property( stg_model_t* mod, 00575 const char* prop, 00576 size_t* len ); 00577 00580 void* stg_model_get_property_fixed( stg_model_t* mod, 00581 const char* name, 00582 size_t size ); 00583 00584 void stg_model_property_refresh( stg_model_t* mod, const char* propname ); 00585 00586 00589 stg_polygon_t* stg_model_get_polygons( stg_model_t* mod, size_t* poly_count ); 00590 void stg_model_set_polygons( stg_model_t* mod, 00591 stg_polygon_t* polys, 00592 size_t poly_count ); 00593 00594 // get a copy of the property data - caller must free it 00595 //int stg_model_copy_property_data( stg_model_t* mod, const char* prop, 00596 // void** data ); 00597 00598 int stg_model_add_property_callback( stg_model_t* mod, const char* prop, 00599 stg_property_callback_t, void* user ); 00600 00601 int stg_model_remove_property_callback( stg_model_t* mod, const char* prop, 00602 stg_property_callback_t ); 00603 00604 int stg_model_remove_property_callbacks( stg_model_t* mod, const char* prop ); 00605 00608 void stg_model_print( stg_model_t* mod ); 00609 00612 int stg_model_is_antecedent( stg_model_t* mod, stg_model_t* testmod ); 00613 00616 int stg_model_is_descendent( stg_model_t* mod, stg_model_t* testmod ); 00617 00621 int stg_model_is_related( stg_model_t* mod1, stg_model_t* mod2 ); 00622 00624 stg_model_t* stg_model_root( stg_model_t* mod ); 00625 00627 GPtrArray* stg_model_array_from_tree( stg_model_t* root ); 00628 00630 int stg_model_startup( stg_model_t* mod ); 00631 00633 int stg_model_shutdown( stg_model_t* mod ); 00634 00637 int stg_model_update( stg_model_t* model ); 00638 00642 void stg_model_global_to_local( stg_model_t* mod, stg_pose_t* pose ); 00643 00647 void stg_model_local_to_global( stg_model_t* mod, stg_pose_t* pose ); 00648 00649 00653 void stg_model_add_property_toggles( stg_model_t* mod, 00654 const char* propname, 00655 stg_property_callback_t callback_on, 00656 void* arg_on, 00657 stg_property_callback_t callback_off, 00658 void* arg_off, 00659 const char* label, 00660 int enabled ); 00661 00662 int stg_model_fig_clear_cb( stg_model_t* mod, void* data, size_t len, 00663 void* userp ); 00664 // end model doc group 00666 00667 // WORLD -------------------------------------------------------- 00668 00679 stg_world_t* stg_world_create( stg_id_t id, 00680 const char* token, 00681 int sim_interval, 00682 int real_interval, 00683 double ppm, 00684 double width, 00685 double height ); 00686 00690 stg_world_t* stg_world_create_from_file( const char* worldfile_path ); 00691 00694 void stg_world_destroy( stg_world_t* world ); 00695 00698 void stg_world_stop( stg_world_t* world ); 00699 00702 void stg_world_start( stg_world_t* world ); 00703 00710 int stg_world_update( stg_world_t* world, int sleepflag ); 00711 00713 void stg_world_load( stg_world_t* mod ); 00714 00716 void stg_world_save( stg_world_t* mod ); 00717 00720 void stg_world_print( stg_world_t* world ); 00721 00724 void stg_world_set_interval_real( stg_world_t* world, unsigned int val ); 00725 00729 void stg_world_set_interval_sim( stg_world_t* world, unsigned int val ); 00730 00733 stg_model_t* stg_world_get_model( stg_world_t* world, stg_id_t mid ); 00734 00736 stg_model_t* stg_world_model_name_lookup( stg_world_t* world, const char* name ); 00737 00738 00739 00743 void stg_world_add_property_callback( stg_world_t* world, 00744 char* propname, 00745 stg_property_callback_t callback, void* 00746 userdata ); 00747 00752 void stg_world_remove_property_callback( stg_world_t* world, 00753 char* propname, 00754 stg_property_callback_t callback ); 00755 00759 // BLOBFINDER MODEL -------------------------------------------------------- 00760 00765 #define STG_BLOB_CHANNELS_MAX 16 00766 00769 typedef struct 00770 { 00771 int channel_count; // 0 to STG_BLOBFINDER_CHANNELS_MAX 00772 stg_color_t channels[STG_BLOB_CHANNELS_MAX]; 00773 int scan_width; 00774 int scan_height; 00775 stg_meters_t range_max; 00776 stg_radians_t pan, tilt, zoom; 00777 } stg_blobfinder_config_t; 00778 00781 typedef struct 00782 { 00783 int channel; 00784 stg_color_t color; 00785 int xpos, ypos; // all values are in pixels 00786 //int width, height; 00787 int left, top, right, bottom; 00788 int area; 00789 stg_meters_t range; 00790 } stg_blobfinder_blob_t; 00791 00794 stg_model_t* stg_blobfinder_create( stg_world_t* world, 00795 stg_model_t* parent, 00796 stg_id_t id, 00797 char* token ); 00800 // LASER MODEL -------------------------------------------------------- 00801 00808 typedef struct 00809 { 00810 uint32_t range; 00811 char reflectance; 00812 } stg_laser_sample_t; 00813 00816 typedef struct 00817 { 00818 stg_radians_t fov; 00819 stg_meters_t range_max; 00820 stg_meters_t range_min; 00821 00824 int samples; 00825 } stg_laser_config_t; 00826 00829 void stg_print_laser_config( stg_laser_config_t* slc ); 00830 00833 stg_model_t* stg_laser_create( stg_world_t* world, 00834 stg_model_t* parent, 00835 stg_id_t id, 00836 char* token ); 00837 00840 size_t stg_model_get_data_laser( stg_model_t* mod, 00841 stg_laser_sample_t* data, 00842 size_t max_samples ); 00845 // GRIPPER MODEL -------------------------------------------------------- 00846 00851 typedef enum { 00852 STG_GRIPPER_PADDLE_OPEN = 0, // default state 00853 STG_GRIPPER_PADDLE_CLOSED, 00854 STG_GRIPPER_PADDLE_OPENING, 00855 STG_GRIPPER_PADDLE_CLOSING, 00856 } stg_gripper_paddle_state_t; 00857 00858 typedef enum { 00859 STG_GRIPPER_LIFT_DOWN = 0, // default state 00860 STG_GRIPPER_LIFT_UP, 00861 STG_GRIPPER_LIFT_UPPING, // verbed these to match the paddle state 00862 STG_GRIPPER_LIFT_DOWNING, 00863 } stg_gripper_lift_state_t; 00864 00865 typedef enum { 00866 STG_GRIPPER_CMD_NOP = 0, // default state 00867 STG_GRIPPER_CMD_OPEN, 00868 STG_GRIPPER_CMD_CLOSE, 00869 STG_GRIPPER_CMD_UP, 00870 STG_GRIPPER_CMD_DOWN 00871 } stg_gripper_cmd_type_t; 00872 00875 typedef struct 00876 { 00877 stg_size_t paddle_size; 00878 00879 stg_gripper_paddle_state_t paddles; 00880 stg_gripper_lift_state_t lift; 00881 00882 double paddle_position; 00883 double lift_position; 00884 00885 stg_meters_t inner_break_beam_inset; 00886 stg_meters_t outer_break_beam_inset; 00887 stg_bool_t paddles_stalled; // true iff some solid object stopped 00888 // the paddles closing or opening 00889 00890 GSList *grip_stack; 00891 int grip_stack_size; 00892 00893 } stg_gripper_config_t; 00894 00897 typedef struct 00898 { 00899 stg_gripper_cmd_type_t cmd; 00900 int arg; 00901 } stg_gripper_cmd_t; 00902 00903 00906 typedef struct 00907 { 00908 stg_gripper_paddle_state_t paddles; 00909 stg_gripper_lift_state_t lift; 00910 00911 double paddle_position; 00912 double lift_position; 00913 00914 stg_bool_t inner_break_beam; 00915 stg_bool_t outer_break_beam; 00916 00917 stg_bool_t left_paddle_contact[3]; 00918 stg_bool_t right_paddle_contact[3]; 00919 00920 stg_bool_t paddles_stalled; // true iff some solid object stopped 00921 // the paddles closing or opening 00922 00923 int stack_count; 00924 00925 } stg_gripper_data_t; 00926 00927 00930 void stg_print_gripper_config( stg_gripper_config_t* slc ); 00931 00934 stg_model_t* stg_gripper_create( stg_world_t* world, 00935 stg_model_t* parent, 00936 stg_id_t id, 00937 char* token ); 00940 // FIDUCIAL MODEL -------------------------------------------------------- 00941 00948 typedef struct 00949 { 00950 stg_meters_t max_range_anon; 00951 stg_meters_t max_range_id; 00952 stg_meters_t min_range; 00953 stg_radians_t fov; // field of view 00954 stg_radians_t heading; // center of field of view 00955 00956 } stg_fiducial_config_t; 00957 00960 typedef struct 00961 { 00962 stg_meters_t range; // range to the target 00963 stg_radians_t bearing; // bearing to the target 00964 stg_pose_t geom; // size and relative angle of the target 00965 int id; // the identifier of the target, or -1 if none can be detected. 00966 00967 } stg_fiducial_t; 00968 00971 stg_model_t* stg_fiducial_create( stg_world_t* world, 00972 stg_model_t* parent, 00973 stg_id_t id, 00974 char* token ); 00977 // RANGECOM MODEL ------------------------------------------------------ 00984 typedef struct 00985 { 00986 stg_meters_t max_range; 00987 stg_meters_t wall_penalty; 00988 } stg_rangecom_config_t; 00989 00993 typedef struct 00994 { 00995 stg_meters_t range; 00996 stg_meters_t bearing; 00997 stg_model_t *model; 00998 char name[32]; 00999 } stg_rangecom_t; 01000 01003 stg_model_t * stg_rangecom_create ( stg_world_t* world, 01004 stg_model_t* parent, 01005 stg_id_t id, 01006 char *token ); 01009 // HACKBONE MODEL ------------------------------------------------------ 01014 typedef struct 01015 { 01016 int max_hops; 01017 stg_model_t* rangecom; 01018 } stg_hackbone_config_t; 01019 01021 typedef struct 01022 { 01023 stg_model_t *model; 01024 stg_model_t *firsthop; 01025 int hops; 01026 } stg_hackbone_t; 01027 01030 stg_model_t * stg_hackbone_create ( stg_world_t* world, 01031 stg_model_t* parent, 01032 stg_id_t id, 01033 char *token ); 01034 01037 // RANGER MODEL -------------------------------------------------------- 01038 01044 typedef struct 01045 { 01046 stg_pose_t pose; 01047 stg_size_t size; 01048 stg_bounds_t bounds_range; 01049 stg_radians_t fov; 01050 } stg_ranger_config_t; 01051 01052 typedef struct 01053 { 01054 stg_meters_t range; 01055 //double error; // TODO 01056 } stg_ranger_sample_t; 01057 01060 stg_model_t* stg_ranger_create( stg_world_t* world, 01061 stg_model_t* parent, 01062 stg_id_t id, 01063 char* token ); 01066 // POSITION MODEL -------------------------------------------------------- 01067 01072 //#define STG_MM_POSITION_RESETODOM 77 01073 01074 typedef enum 01075 { STG_POSITION_CONTROL_VELOCITY, STG_POSITION_CONTROL_POSITION } 01076 stg_position_control_mode_t; 01077 01078 #define STG_POSITION_CONTROL_DEFAULT STG_POSITION_CONTROL_VELOCITY 01079 01080 typedef enum 01081 { STG_POSITION_LOCALIZATION_GPS, STG_POSITION_LOCALIZATION_ODOM } 01082 stg_position_localization_mode_t; 01083 01084 #define STG_POSITION_LOCALIZATION_DEFAULT STG_POSITION_LOCALIZATION_GPS 01085 01087 typedef enum 01088 { STG_POSITION_DRIVE_DIFFERENTIAL, STG_POSITION_DRIVE_OMNI } 01089 stg_position_drive_mode_t; 01090 01091 #define STG_POSITION_DRIVE_DEFAULT STG_POSITION_DRIVE_DIFFERENTIAL 01092 01094 typedef struct 01095 { 01096 stg_meters_t x,y,a; 01097 stg_position_control_mode_t mode; 01098 } stg_position_cmd_t; 01099 01101 typedef struct 01102 { 01103 stg_pose_t pose; 01104 stg_pose_t pose_error; 01105 stg_pose_t origin; 01106 stg_velocity_t velocity; 01107 stg_velocity_t integration_error; 01108 stg_bool_t stall; 01109 stg_position_localization_mode_t localization; 01110 } stg_position_data_t; 01111 01113 typedef int stg_position_stall_t; 01114 01116 stg_model_t* stg_position_create( stg_world_t* world, stg_model_t* parent, stg_id_t id, char* token ); 01117 01119 void stg_model_position_set_odom( stg_model_t* mod, stg_pose_t* odom ); 01120 01123 // end the group of all models 01128 // MACROS ------------------------------------------------------ 01129 // Some useful macros 01130 01131 01132 01149 #define PRECISION 100000.0 01150 01152 #define EQ(A,B) ((lrint(A*PRECISION))==(lrint(B*PRECISION))) 01153 01155 #define LT(A,B) ((lrint(A*PRECISION))<(lrint(B*PRECISION))) 01156 01158 #define GT(A,B) ((lrint(A*PRECISION))>(lrint(B*PRECISION))) 01159 01161 #define GTE(A,B) ((lrint(A*PRECISION))>=(lrint(B*PRECISION))) 01162 01164 #define LTE(A,B) ((lrint(A*PRECISION))<=(lrint(B*PRECISION))) 01165 01168 #ifndef TRUE 01169 #define TRUE 1 01170 #endif 01171 01172 #ifndef FALSE 01173 #define FALSE 0 01174 #endif 01175 01176 #define MILLION 1e6 01177 #define BILLION 1e9 01178 01179 #ifndef M_PI 01180 #define M_PI 3.14159265358979323846 01181 #endif 01182 01183 #ifndef TWOPI 01184 #define TWOPI (2.0*M_PI) 01185 #endif 01186 01187 #ifndef RTOD 01188 01189 #define RTOD(r) ((r) * 180.0 / M_PI) 01190 #endif 01191 01192 #ifndef DTOR 01193 01194 #define DTOR(d) ((d) * M_PI / 180.0) 01195 #endif 01196 01197 #ifndef NORMALIZE 01198 01199 #define NORMALIZE(z) atan2(sin(z), cos(z)) 01200 #endif 01201 01202 01203 #ifdef __cplusplus 01204 } 01205 #endif 01206 01207 // end documentation group libstage 01210 #endif
Generated on Thu Aug 11 13:08:10 2005 for Stage by
