00001
00002
00003
00004
00005
00006
00007
00008
00009 #ifndef PLAN_H
00010 #define PLAN_H
00011
00012 #ifdef __cplusplus
00013 extern "C" {
00014 #endif
00015
00016
00017 typedef struct _plan_cell_t
00018 {
00019
00020 unsigned short ci, cj;
00021
00022
00023 char occ_state;
00024
00025
00026 float occ_dist;
00027
00028
00029 float plan_cost;
00030
00031
00032 struct _plan_cell_t *plan_next;
00033
00034 } plan_cell_t;
00035
00036
00037
00038 typedef struct
00039 {
00040
00041 int size_x, size_y;
00042
00043
00044 int min_x, min_y, max_x, max_y;
00045
00046
00047
00048 double origin_x, origin_y;
00049
00050
00051 double scale;
00052
00053
00054 double des_min_radius, abs_min_radius;
00055
00056
00057 double max_radius;
00058
00059
00060 double dist_penalty;
00061
00062
00063 plan_cell_t *cells;
00064 unsigned char* marks;
00065 size_t marks_size;
00066
00067
00068 int queue_start, queue_len, queue_size;
00069 plan_cell_t **queue;
00070
00071
00072 int waypoint_count, waypoint_size;
00073 plan_cell_t **waypoints;
00074 } plan_t;
00075
00076
00077
00078 plan_t *plan_alloc(double abs_min_radius, double des_min_radius,
00079 double max_radius, double dist_penalty);
00080
00081
00082 void plan_free(plan_t *plan);
00083
00084
00085 void plan_reset(plan_t *plan);
00086
00087 #if 0
00088
00089 int plan_load_occ(plan_t *plan, const char *filename, double scale);
00090 #endif
00091
00092 void plan_set_bounds(plan_t* plan, int min_x, int min_y, int max_x, int max_y);
00093
00094 void plan_set_bbox(plan_t* plan, double padding, double min_size,
00095 double x0, double y0, double x1, double y1);
00096
00097 int plan_check_inbounds(plan_t* plan, double x, double y);
00098
00099
00100 void plan_update_cspace(plan_t *plan, const char* cachefile);
00101
00102
00103 void plan_update_plan(plan_t *plan, double gx, double gy);
00104
00105
00106 void plan_update_waypoints(plan_t *plan, double px, double py);
00107
00108
00109 int plan_get_waypoint(plan_t *plan, int i, double *px, double *py);
00110
00111
00112 void plan_convert_waypoint(plan_t* plan, plan_cell_t *waypoint,
00113 double *px, double *py);
00114
00115 #if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
00116
00117
00118
00119 int plan_write_cspace(plan_t *plan, const char* fname, unsigned int* hash);
00120
00121
00122
00123
00124 int plan_read_cspace(plan_t *plan, const char* fname, unsigned int* hash);
00125
00126
00127
00128 void plan_md5(unsigned int* digest, plan_t* plan);
00129 #endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
00130
00131
00132
00133
00134
00135
00136
00137
00138 #define PLAN_WXGX(plan, i) ((plan)->origin_x + (i) * (plan)->scale)
00139 #define PLAN_WYGY(plan, j) ((plan)->origin_y + (j) * (plan)->scale)
00140
00141
00142
00143
00144 #define PLAN_GXWX(plan, x) ((int)(((x) - (plan)->origin_x) / (plan)->scale + 0.5))
00145 #define PLAN_GYWY(plan, y) ((int)(((y) - (plan)->origin_y) / (plan)->scale + 0.5))
00146
00147
00148 #define PLAN_VALID(plan, i, j) ((i >= 0) && (i < plan->size_x) && (j >= 0) && (j < plan->size_y))
00149
00150 #define PLAN_VALID_BOUNDS(plan, i, j) ((i >= plan->min_x) && (i <= plan->max_x) && (j >= plan->min_y) && (j <= plan->max_y))
00151
00152
00153 #define PLAN_INDEX(plan, i, j) ((i) + (j) * plan->size_x)
00154
00155 #ifdef __cplusplus
00156 }
00157 #endif
00158
00159 #endif