Compare floats with epsilon instead of equality check with 0 (UBC-Thunderbots#3509)

Co-authored-by: Apeiros-46B <Apeiros-46B@users.noreply.github.com>

Author: Apeiros-46B

src/software/ai/navigator/obstacle/trajectory_obstacle.hpp

@@ -1,10 +1,13 @@
 #pragma once
 
+#include <cmath>
+
 #include "software/ai/navigator/obstacle/geom_obstacle.hpp"
 #include "software/ai/navigator/trajectory/trajectory_path.h"
 #include "software/geom/algorithms/contains.h"
 #include "software/geom/algorithms/distance.h"
 #include "software/geom/algorithms/intersects.h"
+#include "software/geom/geom_constants.h"
 
 template <typename GEOM_TYPE>
 class TrajectoryObstacle : public GeomObstacle<GEOM_TYPE>
@@ -40,7 +43,7 @@ TrajectoryObstacle<GEOM_TYPE>::TrajectoryObstacle(const GEOM_TYPE& geom,
 template <typename GEOM_TYPE>
 bool TrajectoryObstacle<GEOM_TYPE>::contains(const Point& p, const double t_sec) const
 {
-    if (t_sec == 0)
+    if (std::abs(t_sec) < FIXED_EPSILON)
     {
         return ::contains(this->geom_, p);
     }
@@ -56,7 +59,7 @@ bool TrajectoryObstacle<GEOM_TYPE>::contains(const Point& p, const double t_sec)
 template <typename GEOM_TYPE>
 double TrajectoryObstacle<GEOM_TYPE>::distance(const Point& p, const double t_sec) const
 {
-    if (t_sec == 0)
+    if (std::abs(t_sec) < FIXED_EPSILON)
     {
         return ::distance(this->geom_, p);
     }
@@ -73,7 +76,7 @@ template <typename GEOM_TYPE>
 double TrajectoryObstacle<GEOM_TYPE>::signedDistance(const Point& p,
                                                      const double t_sec) const
 {
-    if (t_sec == 0)
+    if (std::abs(t_sec) < FIXED_EPSILON)
     {
         return ::signedDistance(this->geom_, p);
     }
@@ -90,7 +93,7 @@ template <typename GEOM_TYPE>
 bool TrajectoryObstacle<GEOM_TYPE>::intersects(const Segment& segment,
                                                const double t_sec) const
 {
-    if (t_sec == 0)
+    if (std::abs(t_sec) < FIXED_EPSILON)
     {
         return ::intersects(this->geom_, segment);
     }

src/software/geom/algorithms/step_along_perimeter.cpp

@@ -1,10 +1,12 @@
 #include "software/geom/algorithms/step_along_perimeter.h"
 
 #include <algorithm>
+#include <cmath>
 #include <vector>
 
 #include "software/geom/algorithms/closest_point.h"
 #include "software/geom/algorithms/distance.h"
+#include "software/geom/geom_constants.h"
 #include "software/geom/segment.h"
 
 
@@ -21,7 +23,7 @@ Point stepAlongPerimeter(const Polygon& polygon, const Point& start,
     // finds the point closest to start point on the segment
     Point closest_start = closestPoint(start, polygon_segments[start_segment_index]);
 
-    if (travel_distance == 0.0)
+    if (std::abs(travel_distance) < FIXED_EPSILON)
     {
         return closest_start;
     }

src/software/geom/angle.h

@@ -439,7 +439,7 @@ inline constexpr double Angle::toDegrees() const
 
 inline constexpr Angle Angle::mod(Angle divisor) const
 {
-    if (divisor.toRadians() == 0)
+    if (divisor.toRadians() < FIXED_EPSILON)
     {
         return Angle::fromRadians(toRadians());
     }