Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #ifndef FOOTSTEP_PLANNER_HELPER_H_
00025 #define FOOTSTEP_PLANNER_HELPER_H_
00026
00027 #define DEBUG_HASH 0
00028 #define DEBUG_TIME 0
00029
00030
00031 #include <gridmap_2d/GridMap2D.h>
00032 #include <angles/angles.h>
00033 #include <tf/tf.h>
00034
00035 #include <math.h>
00036
00037
00038 namespace footstep_planner
00039 {
00040 static const double TWO_PI = 2 * M_PI;
00041
00042 static const double FLOAT_CMP_THR = 0.0001;
00043
00044 enum Leg { RIGHT=0, LEFT=1, NOLEG=2 };
00045
00046
00051 inline double euclidean_distance_sq(int x1, int y1, int x2, int y2)
00052 {
00053
00054 return (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
00055 }
00056
00057
00059 inline double euclidean_distance(int x1, int y1, int x2, int y2)
00060 {
00061 return sqrt(double(euclidean_distance_sq(x1, y1, x2, y2)));
00062 }
00063
00064
00066 inline double euclidean_distance(double x1, double y1, double x2, double y2)
00067 {
00068 return sqrt(euclidean_distance_sq(x1, y1, x2, y2));
00069 }
00070
00071
00073 inline double euclidean_distance_sq(double x1, double y1, double x2,
00074 double y2)
00075 {
00076
00077 return (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
00078 }
00079
00080
00082 inline double grid_cost(int x1, int y1, int x2, int y2, float cell_size)
00083 {
00084 int x = abs(x1 - x2);
00085 int y = abs(y1 - y2);
00086
00087 if (x + y > 1)
00088 return M_SQRT2 * cell_size;
00089 else
00090 return cell_size;
00091 }
00092
00093
00095 inline int angle_state_2_cell(double angle, int angle_bin_num)
00096 {
00097 double bin_size_half = M_PI / angle_bin_num;
00098 return int(angles::normalize_angle_positive(angle + bin_size_half) /
00099 TWO_PI * angle_bin_num);
00100 }
00101
00102
00104 inline double angle_cell_2_state(int angle, int angle_bin_num)
00105 {
00106 double bin_size = TWO_PI / angle_bin_num;
00107 return angle * bin_size;
00108 }
00109
00110
00115 inline int state_2_cell(float value, float cell_size)
00116 {
00117 return value >= 0 ? int(value / cell_size) : int(value / cell_size) - 1;
00118 }
00119
00120
00125 inline double cell_2_state(int value, double cell_size)
00126 {
00127 return (double(value) + 0.5) * cell_size;
00128 }
00129
00130
00132
00133 inline int disc_val(double length, double cell_size)
00134 {
00135 return int(floor((length / cell_size) + 0.5));
00136 }
00137
00138
00143
00144 inline double cont_val(int length, double cell_size)
00145 {
00146 return double(length * cell_size);
00147 }
00148
00149
00151 inline unsigned int int_hash(int key)
00152 {
00153 key += (key << 12);
00154 key ^= (key >> 22);
00155 key += (key << 4);
00156 key ^= (key >> 9);
00157 key += (key << 10);
00158 key ^= (key >> 2);
00159 key += (key << 7);
00160 key ^= (key >> 12);
00161 return key;
00162 }
00163
00164
00169 inline unsigned int calc_hash_tag(int x, int y, int theta, int leg,
00170 int max_hash_size)
00171 {
00172 return int_hash((int_hash(x) << 3) + (int_hash(y) << 2) +
00173 (int_hash(theta) << 1) + (int_hash(leg)))
00174 % max_hash_size;
00175 }
00176
00177
00179 inline int round(double r)
00180 {
00181 return (r > 0.0) ? floor(r + 0.5) : ceil(r - 0.5);
00182 }
00183
00184
00203 bool collision_check(double x, double y, double theta,
00204 double height, double width, int accuracy,
00205 const gridmap_2d::GridMap2D& distance_map);
00206
00207
00215 bool pointWithinPolygon(int x, int y,
00216 const std::vector<std::pair<int, int> >& edges);
00217 }
00218 #endif