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; }