yocs_math_toolkit: geometry.hpp Source File

Go to the documentation of this file.
00001 /*
00002  * common.hpp
00003  *
00004  *  Created on: Apr 11, 2013
00005  *      Author: jorge
00006  */
00007 
00008 #ifndef GEOMETRY_HPP_
00009 #define GEOMETRY_HPP_
00010 
00011 
00012 #include <tf/tf.h>
00013 
00014 #include <geometry_msgs/Pose.h>
00015 #include <geometry_msgs/PoseStamped.h>
00016 
00017 
00018 namespace mtk
00019 {
00020 
00021 double wrapAngle(double a);
00022 
00023 double roll(const tf::Transform& tf);
00024 double roll(geometry_msgs::Pose pose);
00025 double roll(geometry_msgs::PoseStamped pose);
00026 
00027 double pitch(const tf::Transform& tf);
00028 double pitch(geometry_msgs::Pose pose);
00029 double pitch(geometry_msgs::PoseStamped pose);
00030 
00031 double distance2D(double ax, double ay, double bx, double by);
00032 double distance2D(geometry_msgs::Point a, geometry_msgs::Point b = geometry_msgs::Point());
00033 double distance2D(geometry_msgs::Pose a, geometry_msgs::Pose b = geometry_msgs::Pose());
00034 double distance2D(const tf::Vector3& a, const tf::Vector3& b = tf::Vector3());
00035 double distance2D(const tf::Transform& a, const tf::Transform& b = tf::Transform());
00036 
00037 double distance3D(double ax, double ay, double az, double bx, double by, double bz);
00038 double distance3D(geometry_msgs::Point a, geometry_msgs::Point b = geometry_msgs::Point());
00039 double distance3D(geometry_msgs::Pose a, geometry_msgs::Pose b = geometry_msgs::Pose());
00040 double distance3D(const tf::Vector3& a, const tf::Vector3& b);
00041 double distance3D(const tf::Transform& a, const tf::Transform& b);
00042 
00043 double heading(geometry_msgs::Point a, geometry_msgs::Point b = geometry_msgs::Point());
00044 double heading(geometry_msgs::Pose a, geometry_msgs::Pose b = geometry_msgs::Pose());
00045 double heading(const tf::Vector3& a, const tf::Vector3& b);
00046 double heading(const tf::Transform& a, const tf::Transform& b);
00047 
00048 double minAngle(geometry_msgs::Quaternion a, geometry_msgs::Quaternion b);
00049 double minAngle(geometry_msgs::Pose a, geometry_msgs::Pose b);
00050 double minAngle(const tf::Quaternion& a, const tf::Quaternion& b);
00051 double minAngle(const tf::Transform& a, const tf::Transform& b);
00052 
00053 double pointSegmentDistance(double px, double py, double s1x, double s1y, double s2x, double s2y);
00054 
00055 bool raySegmentIntersection(double r1x, double r1y, double r2x, double r2y,
00056                             double s1x, double s1y, double s2x, double s2y,
00057                             double& ix, double& iy, double& distance);
00058 bool rayCircleIntersection(double rx, double ry, double cx, double cy, double radius,
00059                            double& ix, double& iy, double& distance);
00060 
00061 } /* namespace mtk */
00062 
00063 #endif /* GEOMETRY_HPP_ */