diff --git a/common/cpp/robocin/geometry/mathematics.h b/common/cpp/robocin/geometry/mathematics.h index 7d6b8bfd..2f5d9651 100644 --- a/common/cpp/robocin/geometry/mathematics.h +++ b/common/cpp/robocin/geometry/mathematics.h @@ -1,27 +1,15 @@ #ifndef ROBOCIN_MATHEMATICS_H #define ROBOCIN_MATHEMATICS_H +#include "robocin/geometry/point2d.h" +#include "robocin/geometry/line.h" + #include #include #include #include namespace mathematics { -/*! - * @tparam T arithmetic type. - * @return Returns true if T is float and the absolute value of f is within 0.00001f of 0.0. - * @return Returns true if T is double and the absolute value of d is within 0.000000000001 of - * 0.0. - * @return Returns true if T is an integer type and equals to 0. - */ -template -[[maybe_unused]] constexpr bool isNull(const T& value) noexcept { - if constexpr (std::is_floating_point_v) { - return (fabsf(value - 1.0F) < 0.00001F); - } else { - return value == 0; - } -} /*! * @tparam T arithmetic type. @@ -35,69 +23,113 @@ template template [[maybe_unused]] constexpr T map(const T& value, const T& l_lower, const T& l_higher, const T& r_lower, const T& r_higher) { - if (isNull(l_higher - l_lower)) { + if (robocin::Point2D::fuzzyIsNull(l_higher - l_lower)) { throw std::runtime_error("'l_lower' equals to 'l_higher'."); } return (value - l_lower) * (r_higher - r_lower) / (l_higher - l_lower) + r_lower; } -static constexpr bool isNullable(double f) { return isNull(f) or not std::isnormal(f); } +////////////////////////////////////////////////////// adapting /*! - * @tparam PT Requires '.x' and '.y' members and '.cross()' member function. + * @tparam T arithmetic type. * @param a, b, c, d lines (a, b) and (c, d). * @return Determines if lines through (a, b) and (c, d) are parallel. */ -template -constexpr bool linesParallel(const PT& a, const PT& b, const PT& c, const PT& d) { - return isNullable((b - a).cross(c - d)); +template +constexpr bool linesParallel(const robocin::Point2D& a, + const robocin::Point2D& b, + const robocin::Point2D& c, + const robocin::Point2D& d) { + return robocin::Point2D::fuzzyIsNull(robocin::Point2D{b - a}.cross(c - d)); +} + +/*! + * @tparam T arithmetic type. + * @param a, b lines a and b. + * @return Determines if lines through (a, b) and (c, d) are parallel. + */ +template +constexpr bool linesParallel(const robocin::Line& a, + const robocin::Line& b) { + return linesParallel(a.p1(),a.p2(),b.p1(),b.p2()); } + /*! - * @tparam PT Requires '.x' and '.y' members and '.cross()' member function. + * @tparam T arithmetic type. * @param a, b, c, d lines (a, b) and (c, d). * @return Determines if lines through (a, b) and (c, d) are collinear. */ -template -constexpr bool linesCollinear(const PT& a, const PT& b, const PT& c, const PT& d) { - if (!linesParallel(a, b, c, d)) { +template +constexpr bool linesCollinear(const robocin::Point2D& a, + const robocin::Point2D& b, + const robocin::Point2D& c, + const robocin::Point2D& d) { + if (!linesParallel(a, b, c, d)) { return false; } - if (!isNullable((a - b).cross(a - c))) { + if (!robocin::Point2D::fuzzyIsNull(robocin::Point2D{a - b}.cross(a - c))) { return false; } - if (!isNullable((c - d).cross(c - a))) { + if (!robocin::Point2D::fuzzyIsNull(robocin::Point2D{c - d}.cross(c - a))) { return false; } return true; } /*! - * @tparam PT Requires '.x' and '.y' members and '.cross()', '.distanceSquaredTo()' and '.dot()' - * member functions. + * @tparam T arithmetic type. + * @param a and b are lines. + * @return Determines if lines through (a, b) and (c, d) are collinear. + */ +template +constexpr bool linesCollinear(const robocin::Line& a, + const robocin::Line& b) { + return linesCollinear(a.p1(),a.p2(),b.p1(),b.p2()); +} +/*! + * @tparam T arithmetic type. * @param a, b, c, d lines (a, b) and (c, d). * @return Determines if line segment from a to b intersects with line segment from c to d. */ -template -constexpr bool segmentsIntersect(const PT& a, const PT& b, const PT& c, const PT& d) { - if (linesCollinear(a, b, c, d)) { - if (isNullable(a.distanceSquaredTo(c)) || isNullable(a.distanceSquaredTo(d)) - || isNullable(b.distanceSquaredTo(c)) || isNullable(b.distanceSquaredTo(d))) { +template +constexpr bool segmentsIntersect(const robocin::Point2D& a, + const robocin::Point2D& b, + const robocin::Point2D& c, + const robocin::Point2D& d) { + if (linesCollinear(a, b, c, d)) { + if (robocin::Point2D::fuzzyIsNull(a.distanceSquaredTo(c)) + || robocin::Point2D::fuzzyIsNull(a.distanceSquaredTo(d)) + || robocin::Point2D::fuzzyIsNull(b.distanceSquaredTo(c)) + || robocin::Point2D::fuzzyIsNull(b.distanceSquaredTo(d))) { return true; } - if ((c - a).dot(c - b) > 0 && (d - a).dot(d - b) > 0 && (c - b).dot(d - b) > 0) { + if (robocin::Point2D{c - a}.dot(c - b) > 0 && robocin::Point2D{d - a}.dot(d - b) > 0 + && robocin::Point2D{c - b}.dot(d - b) > 0) { return false; } return true; } - if ((d - a).cross(b - a) * (c - a).cross(b - a) > 0) { + if (robocin::Point2D{d - a}.cross(b - a) * robocin::Point2D{c - a}.cross(b - a) > 0) { return false; } - if ((a - c).cross(d - c) * (b - c).cross(d - c) > 0) { + if (robocin::Point2D{a - c}.cross(d - c) * robocin::Point2D{b - c}.cross(d - c) > 0) { return false; } return true; } +/*! + * @tparam T arithmetic type. + * @param a, b, c, d lines (a, b) and (c, d). + * @return Determines if line segment from a to b intersects with line segment from c to d. + */ +template +constexpr bool segmentsIntersect(const robocin::Line& a, + const robocin::Line& b) { + return segmentsIntersect(a.p1(),a.p2(),b.p1(),b.p2()); +} + } // namespace mathematics #endif // ROBOCIN_MATHEMATICS_H diff --git a/common/cpp/robocin/geometry/point2d.h b/common/cpp/robocin/geometry/point2d.h index eca30a08..9f13588b 100644 --- a/common/cpp/robocin/geometry/point2d.h +++ b/common/cpp/robocin/geometry/point2d.h @@ -5,6 +5,7 @@ #include #include #include +#include namespace robocin { @@ -254,6 +255,66 @@ struct Point2D { return result; } + /*! + * @param a, b form the line (a, b) + * @return Returns the distance between the point and its projection onto line defined by a and + * b. + * @note assuming a != b. + */ + constexpr auto distanceToLine(const Point2D& a, const Point2D& b) { + return distanceTo(projectOntoLine(a, b)); + } + + /*! + * @param a, b form the line (a, b) + * @return Returns the distance between the point and its projection onto segment defined by a and + * b. + * @note assuming a != b. + */ + constexpr auto distanceToSegment(const Point2D& a, const Point2D& b) { + return distanceTo(projectOntoSegment(a, b)); + } + + /*! + * @return Determines if point is in a possibly non-convex polygon (by William Randolph Franklin); + * returns 1 for strictly interior points, 0 for strictly exterior points, and 0 or 1 for the + * remaining points. + * @note It's possible to convert this into an *exact* test using integer arithmetic by taking + * care of the division appropriately (making sure to deal with signs properly) and then by + * writing exact tests for checking point on polygon boundary. + * @note Be careful: Because of this, this function allows integer point types. + */ + + bool pointInPolygon(const std::vector& polygon) { + int n = static_cast(polygon.size()); + bool c = false; + for (int i = 0; i < n; ++i) { + int j = (i + 1) % n; + if ((polygon[i].y <= y && y < polygon[j].y) || (polygon[j].y <= y && y < polygon[i].y)) { + if (x < (polygon[i].x + + (polygon[j].x - polygon[i].x) * (y - polygon[i].y) + / (polygon[j].y - polygon[i].y))) { + c = !c; + } + } + } + return c; + } + + /*! + * @return Determines if point is on the boundary of a polygon. + */ + bool pointOnPolygon(const std::vector& polygon) { + int n = static_cast(polygon.size()); + for (int i = 0; i < n; ++i) { + if (auto proj = projectedOntoSegment(polygon[i], polygon[(i + 1) % n]); + fuzzyIsNull(distanceSquaredTo(proj))) { + return true; + } + } + return false; + } + // Streams: friend inline std::istream& operator>>(std::istream& is, Point2D& point) { return is >> point.x >> point.y;