Add more functions to deal with Point2D (#95)

This PR adds the following functions to the geometry namespace: 
- distanceToSegment
- distanceToLine
- pointInPolygon
- pointOnPolygon 
- segmentsIntersect

All of those functions are used throughout the code to deal with the
robot's navigation

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 <algorithm>
 #include <cmath>
 #include <stdexcept>
 #include <type_traits>
 
 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 <class T>
-[[maybe_unused]] constexpr bool isNull(const T& value) noexcept {
-  if constexpr (std::is_floating_point_v<T>) {
-    return (fabsf(value - 1.0F) < 0.00001F);
-  } else {
-    return value == 0;
-  }
-}
 
 /*!
  * @tparam T arithmetic type.
@@ -35,69 +23,113 @@ template <class T>
 template <class T>
 [[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<T>::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 <class PT>
-constexpr bool linesParallel(const PT& a, const PT& b, const PT& c, const PT& d) {
-  return isNullable((b - a).cross(c - d));
+template <class T>
+constexpr bool linesParallel(const robocin::Point2D<T>& a,
+                             const robocin::Point2D<T>& b,
+                             const robocin::Point2D<T>& c,
+                             const robocin::Point2D<T>& d) {
+  return robocin::Point2D<T>::fuzzyIsNull(robocin::Point2D<T>{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 <class T>
+constexpr bool linesParallel(const robocin::Line<T>& a,
+                             const robocin::Line<T>& 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 <class PT>
-constexpr bool linesCollinear(const PT& a, const PT& b, const PT& c, const PT& d) {
-  if (!linesParallel<PT>(a, b, c, d)) {
+template <class T>
+constexpr bool linesCollinear(const robocin::Point2D<T>& a,
+                             const robocin::Point2D<T>& b,
+                             const robocin::Point2D<T>& c,
+                             const robocin::Point2D<T>& d) {
+  if (!linesParallel<T>(a, b, c, d)) {
     return false;
   }
-  if (!isNullable((a - b).cross(a - c))) {
+  if (!robocin::Point2D<T>::fuzzyIsNull(robocin::Point2D<T>{a - b}.cross(a - c))) {
     return false;
   }
-  if (!isNullable((c - d).cross(c - a))) {
+  if (!robocin::Point2D<T>::fuzzyIsNull(robocin::Point2D<T>{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 <class T>
+constexpr bool linesCollinear(const robocin::Line<T>& a,
+                              const robocin::Line<T>& 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 <class PT>
-constexpr bool segmentsIntersect(const PT& a, const PT& b, const PT& c, const PT& d) {
-  if (linesCollinear<PT>(a, b, c, d)) {
-    if (isNullable(a.distanceSquaredTo(c)) || isNullable(a.distanceSquaredTo(d))
-        || isNullable(b.distanceSquaredTo(c)) || isNullable(b.distanceSquaredTo(d))) {
+template <class T>
+constexpr bool segmentsIntersect(const robocin::Point2D<T>& a,
+                             const robocin::Point2D<T>& b,
+                             const robocin::Point2D<T>& c,
+                             const robocin::Point2D<T>& d) {
+  if (linesCollinear<T>(a, b, c, d)) {
+    if (robocin::Point2D<T>::fuzzyIsNull(a.distanceSquaredTo(c))
+        || robocin::Point2D<T>::fuzzyIsNull(a.distanceSquaredTo(d))
+        || robocin::Point2D<T>::fuzzyIsNull(b.distanceSquaredTo(c))
+        || robocin::Point2D<T>::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<T>{c - a}.dot(c - b) > 0 && robocin::Point2D<T>{d - a}.dot(d - b) > 0
+        && robocin::Point2D<T>{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<T>{d - a}.cross(b - a) * robocin::Point2D<T>{c - a}.cross(b - a) > 0) {
     return false;
   }
-  if ((a - c).cross(d - c) * (b - c).cross(d - c) > 0) {
+  if (robocin::Point2D<T>{a - c}.cross(d - c) * robocin::Point2D<T>{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 <class T>
+constexpr bool segmentsIntersect(const robocin::Line<T>& a,
+                                 const robocin::Line<T>& b) {
+  return segmentsIntersect(a.p1(),a.p2(),b.p1(),b.p2());
+}
+
 } // namespace mathematics
 
 #endif // ROBOCIN_MATHEMATICS_H

common/cpp/robocin/geometry/point2d.h

@@ -5,6 +5,7 @@
 #include <ostream>
 #include <stdexcept>
 #include <type_traits>
+#include <vector>
 
 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<Point2D>& polygon) {
+    int n = static_cast<int>(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<Point2D>& polygon) {
+    int n = static_cast<int>(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;