math.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_lib/math.h>
00002 
00003 namespace vigir_footstep_planning
00004 {
00005 bool pointWithinPolygon(int x, int y, const std::vector<std::pair<int, int> >& edges)
00006 {
00007   int cn = 0;
00008 
00009   // loop through all edges of the polygon
00010   for(unsigned int i = 0; i < edges.size(); ++i)
00011   {
00012     unsigned int i_plus = (i + 1) % edges.size();
00013     if ((edges[i].second <= y && edges[i_plus].second > y) ||
00014         (edges[i].second > y && edges[i_plus].second <= y))
00015     {
00016       if (fabs(edges[i_plus].second - edges[i].second) > FLOAT_CMP_THR)
00017       {
00018         float vt = (float)(y - edges[i].second) / (edges[i_plus].second - edges[i].second);
00019         if (x < edges[i].first + vt * (edges[i_plus].first - edges[i].first))
00020           cn++;
00021       }
00022     }
00023   }
00024   return cn & 1;
00025 }
00026 
00027 void getDeltaStep(const msgs::Foot& current, const msgs::Foot& next, geometry_msgs::Pose& dstep)
00028 {
00029   tf::Pose current_tf;
00030   tf::poseMsgToTF(current.pose, current_tf);
00031 
00032   tf::Pose next_tf;
00033   tf::poseMsgToTF(next.pose, next_tf);
00034 
00035   tf::Pose dstep_tf;
00036   getDeltaStep(current_tf, next_tf, dstep_tf);
00037   tf::poseTFToMsg(dstep_tf, dstep);
00038 
00039   // adjust for the left foot
00040   if (current.foot_index != next.foot_index && current.foot_index == msgs::Foot::LEFT)
00041   {
00042     dstep.position.y = -dstep.position.y;
00043 
00044     double r, p, y;
00045     dstep_tf.getBasis().getRPY(r, p, y);
00046     dstep.orientation = tf::createQuaternionMsgFromRollPitchYaw(-r, p, -y);
00047 
00048 //    double dyaw = angles::shortest_angular_distance(current.getYaw(), next.getYaw());
00049 //    dy = -dy;
00050 //    dyaw = -dyaw;
00051   }
00052 }
00053 
00054 void getDeltaStep(const tf::Pose& current, const tf::Pose& next, tf::Pose& dstep)
00055 {
00056   // reconstruct step primitive (current -> next, must be mirrored if current is left foot)
00057   dstep = current.inverse() * next;
00058 }
00059 
00060 void quaternionToNormalYaw(const geometry_msgs::Quaternion& q, geometry_msgs::Vector3& n, double& yaw)
00061 {
00062   tf::Quaternion q_tf;
00063   tf::quaternionMsgToTF(q, q_tf);
00064 
00065   double r, p, y;
00066   tf::Matrix3x3(q_tf).getRPY(r, p, y);
00067 
00068   RPYToNormal(r, p, y, n);
00069 
00070   yaw = y;
00071 }
00072 
00073 void quaternionToNormal(const geometry_msgs::Quaternion& q, geometry_msgs::Vector3& n)
00074 {
00075   tf::Quaternion q_tf;
00076   tf::quaternionMsgToTF(q, q_tf);
00077 
00078   double r, p, y;
00079   tf::Matrix3x3(q_tf).getRPY(r, p, y);
00080 
00081   RPYToNormal(r, p, y, n);
00082 }
00083 
00084 void normalToQuaternion(const geometry_msgs::Vector3& n, double yaw, geometry_msgs::Quaternion& q)
00085 {
00086   double r, p;
00087   normalToRP(n, yaw, r, p);
00088   q = tf::createQuaternionMsgFromRollPitchYaw(r, p, yaw);
00089 }
00090 
00091 void RPYToNormal(double r, double p, double y, geometry_msgs::Vector3& n)
00092 {
00093   double sin_roll = sin(r);
00094   double sin_pitch = sin(p);
00095   double sin_yaw = sin(y);
00096   double cos_yaw = cos(y);
00097 
00098   // rotate around z axis
00099   n.x = cos_yaw*sin_pitch + sin_yaw*sin_roll;
00100   n.y = sin_yaw*sin_pitch - cos_yaw*sin_roll;
00101   n.z = sqrt(1.0 - n.x*n.x + n.y*n.y);
00102 }
00103 
00104 void normalToRP(const geometry_msgs::Vector3& n, double yaw, double& r, double& p)
00105 {
00106   // inverse rotation around z axis
00107   double sin_yaw = sin(-yaw);
00108   double cos_yaw = cos(-yaw);
00109 
00110   r = -asin(sin_yaw*n.x + cos_yaw*n.y);
00111   p = asin(cos_yaw*n.x - sin_yaw*n.y);
00112 }
00113 }


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