38 #ifndef COSTMAP_2D_COSTMAP_MATH_H_ 39 #define COSTMAP_2D_COSTMAP_MATH_H_ 44 #include <geometry_msgs/Point.h> 47 inline double sign(
double x)
49 return x < 0.0 ? -1.0 : 1.0;
55 return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0);
58 inline double distance(
double x0,
double y0,
double x1,
double y1)
60 return hypot(x1 - x0, y1 - y0);
63 double distanceToLine(
double pX,
double pY,
double x0,
double y0,
double x1,
double y1);
65 bool intersects(std::vector<geometry_msgs::Point>& polygon,
float testx,
float testy);
67 bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2);
69 #endif // COSTMAP_2D_COSTMAP_MATH_H_ bool intersects(std::vector< geometry_msgs::Point > &polygon, float testx, float testy)
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
double sign(double x)
Return -1 if x < 0, +1 otherwise.
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
double sign0(double x)
Same as sign(x) but returns 0 if x is 0.
double distance(double x0, double y0, double x1, double y1)