26 lines
793 B
C++
26 lines
793 B
C++
#include "dock_planner/utils/line_common.h"
|
|
|
|
double vda_5050_utils::distanceFromPointToLine(double x0, double y0, double x1, double y1, double x2, double y2)
|
|
{
|
|
double A = y2 - y1;
|
|
double B = -(x2 - x1);
|
|
double C = -A * x1 - B * y1;
|
|
|
|
return fabs(A * x0 + B * y0 + C) / sqrt(A * A + B * B);
|
|
}
|
|
|
|
bool vda_5050_utils::isPointOnSegment(double x0, double y0, double x1, double y1, double x2, double y2, const double tolerance)
|
|
{
|
|
double crossProduct = distanceFromPointToLine(x0, y0, x1, y1, x2, y2);
|
|
if (fabs(crossProduct) > fabs(tolerance))
|
|
return false;
|
|
|
|
// Kiểm tra tọa độ của P có nằm giữa A và B không
|
|
if (x0 >= std::min(x1, x2) && x0 <= std::max(x1, x2) &&
|
|
y0 >= std::min(y1, y2) && y0 <= std::max(y1, y2))
|
|
{
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
} |