geometry.cpp
Go to the documentation of this file.
00001 /*
00002  * geometry.cpp
00003  *
00004  *  Created on: Apr 11, 2013
00005  *      Author: jorge
00006  */
00007 
00008 #include "yocs_math_toolkit/common.hpp"
00009 #include "yocs_math_toolkit/geometry.hpp"
00010 
00011 
00012 namespace mtk
00013 {
00014 
00015 
00016 double wrapAngle(double a)
00017 {
00018   a = fmod(a + M_PI, 2*M_PI);
00019   if (a < 0.0)
00020       a += 2.0*M_PI;
00021   return a - M_PI;
00022 }
00023 
00024 double roll(const tf::Transform& tf)
00025 {
00026   double roll, pitch, yaw;
00027   tf::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw);
00028   return roll;
00029 }
00030 
00031 double roll(geometry_msgs::Pose pose)
00032 {
00033   tf::Transform tf;
00034   pose2tf(pose, tf);
00035   return roll(tf);
00036 }
00037 
00038 double roll(geometry_msgs::PoseStamped pose)
00039 {
00040   return roll(pose.pose);
00041 }
00042 
00043 double pitch(const tf::Transform& tf)
00044 {
00045   double roll, pitch, yaw;
00046   tf::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw);
00047   return pitch;
00048 }
00049 
00050 double pitch(geometry_msgs::Pose pose)
00051 {
00052   tf::Transform tf;
00053   pose2tf(pose, tf);
00054   return pitch(tf);
00055 }
00056 
00057 double pitch(geometry_msgs::PoseStamped pose)
00058 {
00059   return pitch(pose.pose);
00060 }
00061 
00062 
00063 double distance2D(double x, double y)
00064 {
00065   return std::sqrt(std::pow(x, 2) + std::pow(y, 2));
00066 }
00067 
00068 double distance2D(const tf::Point& p)
00069 {
00070   return std::sqrt(std::pow(p.x(), 2) + std::pow(p.y(), 2));
00071 }
00072 
00073 double distance2D(double ax, double ay, double bx, double by)
00074 {
00075   return std::sqrt(std::pow(ax - bx, 2) + std::pow(ay - by, 2));
00076 }
00077 
00078 double distance2D(const tf::Point& p1, const tf::Point& p2)
00079 {
00080   return std::sqrt(std::pow(p2.x() - p1.x(), 2) + std::pow(p2.y() - p1.y(), 2));
00081 }
00082 
00083 double distance2D(geometry_msgs::Point a, geometry_msgs::Point b)
00084 {
00085   return distance2D(tf::Vector3(a.x, a.y, a.z), tf::Vector3(b.x, b.y, b.z));
00086 }
00087 
00088 double distance2D(geometry_msgs::Pose a, geometry_msgs::Pose b)
00089 {
00090   return distance2D(a.position, b.position);
00091 }
00092 
00093 double distance2D(const tf::Transform& a, const tf::Transform& b)
00094 {
00095   return distance2D(a.getOrigin(), b.getOrigin());
00096 }
00097 
00098 
00099 double distance3D(double x, double y, double z)
00100 {
00101   return std::sqrt(std::pow(x, 2) + std::pow(y, 2) + std::pow(z, 2));
00102 }
00103 
00104 double distance3D(const tf::Point& p)
00105 {
00106   return std::sqrt(std::pow(p.x(), 2) + std::pow(p.y(), 2) + std::pow(p.z(), 2));
00107 }
00108 
00109 double distance3D(double ax, double ay, double az, double bx, double by, double bz)
00110 {
00111   return std::sqrt(std::pow(ax - bx, 2) + std::pow(ay - by, 2) + std::pow(az - bz, 2));
00112 }
00113 
00114 double distance3D(const tf::Point& p1, const tf::Point& p2)
00115 {
00116   return std::sqrt(std::pow(p2.x() - p1.x(), 2) + std::pow(p2.y() - p1.y(), 2) + std::pow(p2.z() - p1.z(), 2));
00117 }
00118 
00119 double distance3D(geometry_msgs::Point a, geometry_msgs::Point b)
00120 {
00121   return distance3D(tf::Vector3(a.x, a.y, a.z), tf::Vector3(b.x, b.y, b.z));
00122 }
00123 
00124 double distance3D(geometry_msgs::Pose a, geometry_msgs::Pose b)
00125 {
00126   return distance3D(a.position, b.position);
00127 }
00128 
00129 double distance3D(const tf::Transform& a, const tf::Transform& b)
00130 {
00131   return distance3D(a.getOrigin(), b.getOrigin());
00132 }
00133 
00134 
00135 double heading(const tf::Vector3& a, const tf::Vector3& b)
00136 {
00137   return std::atan2(b.y() - a.y(), b.x() - a.x());
00138 }
00139 
00140 double heading(geometry_msgs::Point a, geometry_msgs::Point b)
00141 {
00142   return heading(tf::Vector3(a.x, a.y, a.z), tf::Vector3(b.x, b.y, b.z));
00143 }
00144 
00145 double heading(geometry_msgs::Pose a, geometry_msgs::Pose b)
00146 {
00147   return heading(a.position, b.position);
00148 }
00149 
00150 double heading(const tf::Transform& a, const tf::Transform& b)
00151 {
00152   return heading(a.getOrigin(), b.getOrigin());
00153 }
00154 
00155 double minAngle(const tf::Quaternion& a, const tf::Quaternion& b)
00156 {
00157   return tf::angleShortestPath(a, b);
00158 }
00159 
00160 double minAngle(geometry_msgs::Quaternion a, geometry_msgs::Quaternion b)
00161 {
00162   return minAngle(tf::Quaternion(a.x, a.y, a.z, a.w), tf::Quaternion(b.x, b.y, b.z, b.w));
00163 }
00164 
00165 double minAngle(geometry_msgs::Pose a, geometry_msgs::Pose b)
00166 {
00167   return minAngle(a.orientation, b.orientation);
00168 }
00169 
00170 double minAngle(const tf::Transform& a, const tf::Transform& b)
00171 {
00172   return minAngle(a.getRotation(), b.getRotation());
00173 }
00174 
00175 bool sameFrame(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b)
00176 {
00177   return sameFrame(a.header.frame_id, b.header.frame_id);
00178 }
00179 
00180 bool sameFrame(const std::string& frame_a, const std::string& frame_b)
00181 {
00182   if (frame_a.length() == 0 && frame_b.length() == 0)
00183   {
00184     ROS_WARN("Comparing two empty frame ids (considered as the same frame)");
00185     return true;
00186   }
00187 
00188   if (frame_a.length() == 0 || frame_b.length() == 0)
00189   {
00190     ROS_WARN("Comparing %s%s with an empty frame id (considered as different frames)",
00191              frame_a.c_str(), frame_b.c_str());
00192     return false;
00193   }
00194 
00195   int start_a = frame_a.at(0) == '/' ? 1 : 0;
00196   int start_b = frame_b.at(0) == '/' ? 1 : 0;
00197 
00198   return frame_a.compare(start_a, frame_a.length(), frame_b, start_b, frame_b.length()) == 0;
00199 }
00200 
00201 double pointSegmentDistance(double px, double py, double s1x, double s1y, double s2x, double s2y)
00202 {
00203   // Return minimum distance between line segment s1-s2 and point p
00204 
00205   double l = distance2D(s1x, s1y, s2x, s2y);    // i.e. |p2 - p1|^2
00206   if (l == 0.0)
00207     return distance2D(px, py, s1x, s1y);        // s1 == s2 case
00208 
00209   // Consider the line extending the segment, parameterized as s1 + t (s2 - s1).
00210   // We find projection of point p onto the line.
00211   // It falls where t = [(p - s1) . (s2 - s1)] / |s2 - s1|^2
00212   double t = (- s1x * (s2x - s1x) - s1y * (s2y - s1y)) / l;
00213   if (t < 0.0)                           // Beyond the s1 end of the segment
00214     return distance2D(s1x, s1y);
00215 
00216   if (t > 1.0)                           // Beyond the s2 end of the segment
00217     return distance2D(s2x, s2y);
00218 
00219   // Projection falls on the segment
00220   return distance2D(s1x + t * (s2x - s1x), s1y + t * (s2y - s1y));
00221 }
00222 
00223 bool raySegmentIntersection(double r1x, double r1y, double r2x, double r2y,
00224                             double s1x, double s1y, double s2x, double s2y,
00225                             double& ix, double& iy, double& distance)
00226 {
00227   double r, s, d;
00228   // Make sure the lines aren't parallel
00229   if ((r2y - r1y) / (r2x - r1x) != (s2y - s1y) / (s2x - s1x))
00230   {
00231     d = (((r2x - r1x) * (s2y - s1y)) - (r2y - r1y) * (s2x - s1x));
00232     if (d != 0)
00233     {
00234       r = (((r1y - s1y) * (s2x - s1x)) - (r1x - s1x) * (s2y - s1y)) / d;
00235       s = (((r1y - s1y) * (r2x - r1x)) - (r1x - s1x) * (r2y - r1y)) / d;
00236       if (r >= 0)
00237       {
00238         if (s >= 0 && s <= 1)
00239         {
00240           ix = r1x + r * (r2x - r1x);
00241           iy = r1y + r * (r2y - r1y);
00242           distance = distance2D(ix, iy);
00243           return true;
00244         }
00245       }
00246     }
00247   }
00248   return false;
00249 }
00250 
00251 bool rayCircleIntersection(double rx, double ry, double cx, double cy, double radius,
00252                            double& ix, double& iy, double& distance)
00253 {
00254   double a = rx * rx + ry * ry;
00255   double bBy2 = rx * cx + ry * cy;
00256   double c = cx * cx + cy * cy - radius * radius;
00257 
00258   double pBy2 = bBy2 / a;
00259   double q = c / a;
00260 
00261   double discriminant = pBy2 * pBy2 - q;
00262   if (discriminant < 0)
00263     return false;
00264 
00265   // if disc == 0 ... dealt with later
00266   double tmpSqrt = std::sqrt(discriminant);
00267   double abScalingFactor1 = -pBy2 + tmpSqrt;
00268   double abScalingFactor2 = -pBy2 - tmpSqrt;
00269 
00270   ix = - rx * abScalingFactor1;
00271   iy = - ry * abScalingFactor1;
00272   distance = distance2D(ix, iy);
00273 
00274   // discard the backward-pointing half of the ray
00275   if ((ix*rx < 0.0) && (iy*ry < 0.0))
00276     return false;
00277 
00278   if (discriminant == 0)  // abScalingFactor1 == abScalingFactor2
00279     return true;
00280 
00281   // Check if the second intersection point is close (naively inefficient)
00282   double i2x = - rx * abScalingFactor2;
00283   double i2y = - ry * abScalingFactor2;
00284   double distance2 = distance2D(i2x, i2y);
00285 
00286   if (distance2 < distance)
00287   {
00288     ix = i2x;
00289     iy = i2y;
00290     distance = distance2;
00291   }
00292 
00293   return true;
00294 }
00295 
00296 
00297 } /* namespace mtk */


yocs_math_toolkit
Author(s): Jorge Santos
autogenerated on Thu Jun 6 2019 21:47:25