math.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2017, Alexander Stumpf, TU Darmstadt
00003 // Based on http://wiki.ros.org/footstep_planner by Johannes Garimort and Armin Hornung
00004 // All rights reserved.
00005 
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00014 //       group, TU Darmstadt nor the names of its contributors may be used to
00015 //       endorse or promote products derived from this software without
00016 //       specific prior written permission.
00017 
00018 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00022 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00023 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00024 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00025 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00026 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00027 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 //=================================================================================================
00029 
00030 #ifndef VIGIR_FOOTSTEP_PLANNING_LIB_MATH_H__
00031 #define VIGIR_FOOTSTEP_PLANNING_LIB_MATH_H__
00032 
00033 #define DEBUG_HASH 0
00034 #define DEBUG_TIME 0
00035 
00036 #include <math.h>
00037 
00038 #include <tf/tf.h>
00039 #include <angles/angles.h>
00040 
00041 #include <vigir_footstep_planning_msgs/footstep_planning_msgs.h>
00042 
00043 
00044 
00045 namespace vigir_footstep_planning
00046 {
00047 static const double TWO_PI = 2 * M_PI;
00048 
00049 static const double FLOAT_CMP_THR = 0.0001;
00050 
00052 static const int cvMmScale = 1000;
00053 
00054 enum Leg { RIGHT=0, LEFT=1, NOLEG=2 };
00055 
00056 
00061 inline double euclidean_distance_sq(int x1, int y1, int x2, int y2)
00062 {
00063   // note: do *not* use pow() to square!
00064   return (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
00065 }
00066 
00068 inline double euclidean_distance_sq(double x1, double y1, double x2, double y2)
00069 {
00070   // note: do *not* use pow() to square!
00071   return (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
00072 }
00073 
00074 inline double euclidean_distance_sq(int x1, int y1, int z1, int x2, int y2, int z2)
00075 {
00076   // note: do *not* use pow() to square!
00077   return (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2);
00078 }
00079 
00081 inline double euclidean_distance_sq(double x1, double y1, double z1, double x2, double y2, double z2)
00082 {
00083   // note: do *not* use pow() to square!
00084   return (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2);
00085 }
00086 
00088 inline double euclidean_distance(int x1, int y1, int x2, int y2)
00089 {
00090   return sqrt(double(euclidean_distance_sq(x1, y1, x2, y2)));
00091 }
00092 
00094 inline double euclidean_distance(double x1, double y1, double x2, double y2)
00095 {
00096   return sqrt(euclidean_distance_sq(x1, y1, x2, y2));
00097 }
00098 
00100 inline double euclidean_distance(int x1, int y1, int z1, int x2, int y2, int z2)
00101 {
00102   return sqrt(double(euclidean_distance_sq(x1, y1, z1, x2, y2, z2)));
00103 }
00104 
00106 inline double euclidean_distance(double x1, double y1, double z1, double x2, double y2, double z2)
00107 {
00108   return sqrt(euclidean_distance_sq(x1, y1, z1, x2, y2, z2));
00109 }
00110 
00111 inline double parabol(double x, double y, double a_inv, double b_inv)
00112 {
00113   return x*x*a_inv + y*y*b_inv;
00114 }
00115 
00117 inline double grid_cost(int x1, int y1, int x2, int y2, float cell_size)
00118 {
00119   int x = abs(x1 - x2);
00120   int y = abs(y1 - y2);
00121 
00122   if (x + y > 1)
00123     return M_SQRT2 * cell_size;
00124   else
00125     return cell_size;
00126 }
00127 
00128 
00129 inline double pround(double x, double prec)
00130 {
00131   return ::round(x/prec)*prec;
00132 }
00133 
00134 inline double pceil(double x, double prec)
00135 {
00136   return ::ceil(x/prec)*prec;
00137 }
00138 
00139 inline double pfloor(double x, double prec)
00140 {
00141   return ::floor(x/prec)*prec;
00142 }
00143 
00144 
00146 // TODO: check consistency for negative values
00147 inline int disc_val(double length, double cell_size)
00148 {
00149   //return int(floor((length / cell_size) + 0.5));
00150   return static_cast<int>(round(length / cell_size));
00151 }
00152 
00153 
00158 // TODO: check consistency for negative values
00159 inline double cont_val(int length, double cell_size)
00160 {
00161   //return double(length * cell_size);
00162   return static_cast<double>(length) * cell_size;
00163 }
00164 
00165 
00167 inline int angle_state_2_cell(double angle, double angle_bin_size)
00168 {
00169 //  double bin_size_half = M_PI / angle_bin_num;
00170 //  return int(angles::normalize_angle_positive(angle + bin_size_half) /
00171 //             TWO_PI * angle_bin_num);
00172   //return int(angles::normalize_angle_positive(angle + 0.5*angle_bin_size) / angle_bin_size);
00173   return disc_val(angle, angle_bin_size);
00174 }
00175 
00176 
00178 inline double angle_cell_2_state(int angle, double angle_bin_size)
00179 {
00180 //  double bin_size = TWO_PI / angle_bin_num;
00181 //  return angle * bin_size;
00182   //return double(angle * angle_bin_size);
00183   return cont_val(angle, angle_bin_size);
00184 }
00185 
00186 
00191 inline int state_2_cell(float value, float cell_size)
00192 {
00193   //return value >= 0 ? int(value / cell_size) : int(value / cell_size) - 1;
00194   return disc_val(value, cell_size);
00195 }
00196 
00197 
00202 inline double cell_2_state(int value, double cell_size)
00203 {
00204   //return (double(value) + 0.5) * cell_size;
00205   return cont_val(value, cell_size);
00206 }
00207 
00208 
00210 inline unsigned int int_hash(int key)
00211 {
00212   key += (key << 12);
00213   key ^= (key >> 22);
00214   key += (key << 4);
00215   key ^= (key >> 9);
00216   key += (key << 10);
00217   key ^= (key >> 2);
00218   key += (key << 7);
00219   key ^= (key >> 12);
00220   return key;
00221 }
00222 
00223 
00229 inline unsigned int calc_hash_tag(int x, int y, int theta, int leg, unsigned int hash_pred, unsigned int hash_succ, int max_hash_size)
00230 {
00231   return int_hash((int_hash(x) << 3) + (int_hash(y) << 2) + (int_hash(theta) << 1) +
00232                   (int_hash(leg))) % max_hash_size;
00233 
00234 //  return int_hash((int_hash(x) << 4) + (int_hash(y) << 3) + (int_hash(theta) << 2) +
00235 //                  (int_hash(leg) << 1)/* + hash_pred + hash_succ*/) % max_hash_size;
00236 }
00237 
00245 bool pointWithinPolygon(int x, int y, const std::vector<std::pair<int, int> >& edges);
00246 
00247 void getDeltaStep(const msgs::Foot& current, const msgs::Foot& next, geometry_msgs::Pose& dstep);
00248 void getDeltaStep(const tf::Pose& current, const tf::Pose& next, tf::Pose& dstep);
00249 
00250 void quaternionToNormalYaw(const geometry_msgs::Quaternion& q, geometry_msgs::Vector3& n, double& yaw);
00251 void quaternionToNormal(const geometry_msgs::Quaternion& q, geometry_msgs::Vector3& n);
00252 void normalToQuaternion(const geometry_msgs::Vector3& n, double yaw, geometry_msgs::Quaternion& q);
00253 
00254 void RPYToNormal(double r, double p, double y, geometry_msgs::Vector3& n);
00255 void normalToRP(const geometry_msgs::Vector3& n, double yaw, double& r, double& p);
00256 }
00257 
00258 #endif


vigir_footstep_planning_lib
Author(s): Alexander Stumpf
autogenerated on Sat Jul 15 2017 02:47:56