double
Line2D::distanceTo(Point2D &p) {
/*dist = |A*x + B*y + C|
---------------
sqrt(A^2 + B^2)*/
if(this->v(0) == 0.0 && this->v(1) == 0.0)
return GEOMETRY_INFINITE;
return abs(this->v(0)*p.getPoint()(0) + this->v(1)*p.getPoint()(1) + this->v(2))/(sqrt(GEOMETRY_SQUARE(this->v(0)) + GEOMETRY_SQUARE(this->v(1))));
}
Eigen::Vector3d
Line2D::getLine(Point2D &p1, Point2D &p2) {
return this->getLine(p1.getPoint()(0), p1.getPoint()(1), p2.getPoint()(0), p2.getPoint()(1));
}
Line2D
Line2D::getNormalLine(Point2D &p) {
return this->getNormalLine(p.getPoint()(0), p.getPoint()(1));
}
bool
Line2D::hasPoint(Point2D &p) {
return this->v(0) * p.getPoint()(0) + this->v(1) * p.getPoint()(1) + this->v(2) == 0;
}
