helper.cpp
Go to the documentation of this file.
00001 /*
00002  * A footstep planner for humanoid robots
00003  *
00004  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
00005  * http://www.ros.org/wiki/footstep_planner
00006  *
00007  *
00008  * This program is free software: you can redistribute it and/or modify
00009  * it under the terms of the GNU General Public License as published by
00010  * the Free Software Foundation, version 3.
00011  *
00012  * This program is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00021 #include <footstep_planner/helper.h>
00022 
00023 namespace footstep_planner
00024 {
00025 bool
00026 collision_check(double x, double y, double theta, double height,
00027                 double width, int accuracy,
00028                 const gridmap_2d::GridMap2D& distance_map)
00029 {
00030   double d = distance_map.distanceMapAt(x, y);
00031   if (d < 0.0) // if out of bounds => collision
00032     return true;
00033   d -= distance_map.getResolution();
00034 
00035   double r_o = sqrt(width*width + height*height) / 2.0;
00036 
00037   if (d >= r_o)
00038     return false;
00039   else if (accuracy == 0)
00040     return false;
00041 
00042   double h_half = height / 2.0f;
00043   double w_half = width / 2.0f;
00044   double r_i = std::min(w_half, h_half);
00045 
00046   if (d <= r_i)
00047     return true;
00048   else if (accuracy == 1)
00049     return true;
00050 
00051   double h_new;
00052   double w_new;
00053   double delta_x;
00054   double delta_y;
00055   if (width < height)
00056   {
00057     double h_clear = sqrt(d*d - w_half*w_half);
00058     h_new = h_half - h_clear;
00059     w_new = width;
00060     delta_x = h_clear + h_new/2.0;
00061     delta_y = 0.0;
00062   }
00063   else // footWidth >= footHeight
00064   {
00065     double w_clear = sqrt(d*d - h_half*h_half);
00066     h_new = height;
00067     w_new = w_half - w_clear;
00068     delta_x = 0.0;
00069     delta_y = w_clear + w_new/2.0;
00070   }
00071   double theta_cos = cos(theta);
00072   double theta_sin = sin(theta);
00073   double x_shift = theta_cos*delta_x - theta_sin*delta_y;
00074   double y_shift = theta_sin*delta_x + theta_cos*delta_y;
00075 
00076   return (collision_check(x+x_shift, y+y_shift, theta, h_new, w_new,
00077                           accuracy, distance_map) ||
00078           collision_check(x-x_shift, y-y_shift, theta, h_new, w_new,
00079                           accuracy, distance_map));
00080 }
00081 
00082 
00083 bool
00084 pointWithinPolygon(int x, int y, const std::vector<std::pair<int, int> >& edges)
00085 {
00086   int cn = 0;
00087 
00088   // loop through all edges of the polygon
00089   for(unsigned int i = 0; i < edges.size() - 1; ++i)
00090   {
00091     if ((edges[i].second <= y && edges[i + 1].second > y) ||
00092         (edges[i].second > y && edges[i + 1].second <= y))
00093     {
00094       float vt = (float)(y - edges[i].second) /
00095         (edges[i + 1].second - edges[i].second);
00096       if (x < edges[i].first + vt * (edges[i + 1].first - edges[i].first))
00097       {
00098         ++cn;
00099       }
00100     }
00101   }
00102   return cn & 1;
00103 }
00104 }


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Mon Oct 6 2014 00:41:41