footstep_planner: footstep_planner: helper.cpp Source File

00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/src/helper.cpp $
00002 // SVN $Id: helper.cpp 3810 2013-01-20 18:57:24Z garimort@informatik.uni-freiburg.de $
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 }