32 double distanceToLine(
double pX,
double pY,
double x0,
double y0,
double x1,
double y1)
39 double dot = A * C + B * D;
40 double len_sq = C * C + D * D;
41 double param = dot / len_sq;
64 bool intersects(std::vector<geometry_msgs::Point>& polygon,
float testx,
float testy)
67 int i, j, nvert = polygon.size();
68 for (i = 0, j = nvert - 1; i < nvert; j = i++)
70 float yi = polygon[i].y, yj = polygon[j].y, xi = polygon[i].x, xj = polygon[j].x;
72 if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi))
78 bool intersects_helper(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
80 for (
unsigned int i = 0; i < polygon1.size(); i++)
86 bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
bool intersects(std::vector< geometry_msgs::Point > &polygon, float testx, float testy)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val)
TFSIMD_FORCE_INLINE const tfScalar & y() const
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
bool intersects_helper(std::vector< geometry_msgs::Point > &polygon1, std::vector< geometry_msgs::Point > &polygon2)
double distance(double x0, double y0, double x1, double y1)
TFSIMD_FORCE_INLINE const tfScalar & x() const
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)