helper.cpp
Go to the documentation of this file.
00001 // SVN $HeadURL$
00002 // SVN $Id$
00003 
00004 /*
00005  * A footstep planner for humanoid robots
00006  *
00007  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
00008  * http://www.ros.org/wiki/footstep_planner
00009  *
00010  *
00011  * This program is free software: you can redistribute it and/or modify
00012  * it under the terms of the GNU General Public License as published by
00013  * the Free Software Foundation, version 3.
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, see <http://www.gnu.org/licenses/>.
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) // if out of bounds => collision
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 // footWidth >= footHeight
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   // loop through all edges of the polygon
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Tue Oct 15 2013 10:06:51