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 #include <footstep_planner/helper.h>
00025
00026 namespace footstep_planner
00027 {
00028 bool
00029 collision_check(double x, double y, double theta, double height,
00030 double width, int accuracy,
00031 const gridmap_2d::GridMap2D& distance_map)
00032 {
00033 double d = distance_map.distanceMapAt(x, y);
00034 if (d < 0.0)
00035 return true;
00036 d -= distance_map.getResolution();
00037
00038 double r_o = sqrt(width*width + height*height) / 2.0;
00039
00040 if (d >= r_o)
00041 return false;
00042 else if (accuracy == 0)
00043 return false;
00044
00045 double h_half = height / 2.0f;
00046 double w_half = width / 2.0f;
00047 double r_i = std::min(w_half, h_half);
00048
00049 if (d <= r_i)
00050 return true;
00051 else if (accuracy == 1)
00052 return true;
00053
00054 double h_new;
00055 double w_new;
00056 double delta_x;
00057 double delta_y;
00058 if (width < height)
00059 {
00060 double h_clear = sqrt(d*d - w_half*w_half);
00061 h_new = h_half - h_clear;
00062 w_new = width;
00063 delta_x = h_clear + h_new/2.0;
00064 delta_y = 0.0;
00065 }
00066 else
00067 {
00068 double w_clear = sqrt(d*d - h_half*h_half);
00069 h_new = height;
00070 w_new = w_half - w_clear;
00071 delta_x = 0.0;
00072 delta_y = w_clear + w_new/2.0;
00073 }
00074 double theta_cos = cos(theta);
00075 double theta_sin = sin(theta);
00076 double x_shift = theta_cos*delta_x - theta_sin*delta_y;
00077 double y_shift = theta_sin*delta_x + theta_cos*delta_y;
00078
00079 return (collision_check(x+x_shift, y+y_shift, theta, h_new, w_new,
00080 accuracy, distance_map) ||
00081 collision_check(x-x_shift, y-y_shift, theta, h_new, w_new,
00082 accuracy, distance_map));
00083 }
00084
00085
00086 bool
00087 pointWithinPolygon(int x, int y, const std::vector<std::pair<int, int> >& edges)
00088 {
00089 int cn = 0;
00090
00091
00092 for(unsigned int i = 0; i < edges.size() - 1; ++i)
00093 {
00094 if ((edges[i].second <= y && edges[i + 1].second > y) ||
00095 (edges[i].second > y && edges[i + 1].second <= y))
00096 {
00097 float vt = (float)(y - edges[i].second) /
00098 (edges[i + 1].second - edges[i].second);
00099 if (x < edges[i].first + vt * (edges[i + 1].first - edges[i].first))
00100 {
00101 ++cn;
00102 }
00103 }
00104 }
00105 return cn & 1;
00106 }
00107 }