Continue moving math calls to utilities

17 files changed, 287 insertions, 280 deletions

diff --git a/ArcWelder/arc_welder.cpp b/ArcWelder/arc_welder.cpp
index b966b62..fe129b4 100644
--- a/ArcWelder/arc_welder.cpp
+++ b/ArcWelder/arc_welder.cpp
@@ -495,7 +495,7 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end, bool is_reprocess
       // Calculate R
       if (r == 0)
       {
-        r = std::sqrt(i * i + j * j);
+        r = utilities::sqrt(i * i + j * j);
       }
       // Now we know the radius and the chord length;
       movement_length_mm = utilities::get_arc_distance(p_pre_pos->x, p_pre_pos->y, p_pre_pos->z, p_cur_pos->x, p_cur_pos->y, p_cur_pos->z, i, j, r, p_cur_pos->command.command == "G2");
@@ -535,7 +535,7 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end, bool is_reprocess
     mm_extruded_per_mm_travel = extruder_current.e_relative / movement_length_mm;
     if (previous_extrusion_rate_ > 0)
     {
-      extrusion_rate_change_percent = std::fabs(utilities::get_percent_change(previous_extrusion_rate_, mm_extruded_per_mm_travel));
+      extrusion_rate_change_percent = utilities::abs(utilities::get_percent_change(previous_extrusion_rate_, mm_extruded_per_mm_travel));
     }
   }
   if (previous_extrusion_rate_ != 0 && utilities::greater_than(extrusion_rate_change_percent, extrusion_rate_variance_percent_))
diff --git a/ArcWelder/segmented_arc.cpp b/ArcWelder/segmented_arc.cpp
index 6f238a5..527ad3a 100644
--- a/ArcWelder/segmented_arc.cpp
+++ b/ArcWelder/segmented_arc.cpp
@@ -240,10 +240,12 @@ bool segmented_arc::try_add_point_internal_(printer_point p)
       // Apply firmware compensation
       // See how many arcs will be interpolated
       double circumference = 2.0 * PI_DOUBLE * current_arc_.radius;
-      int num_segments = (int)std::floor(circumference / min_arc_segments_);
+      // TODO: Should this be ceil?
+      int num_segments = (int)utilities::floor(circumference / min_arc_segments_);
       if (num_segments < min_arc_segments_) {
         //num_segments = (int)std::ceil(circumference/approximate_length) * (int)std::ceil(approximate_length / mm_per_arc_segment);
-        num_segments = (int)std::floor(circumference / original_shape_length_);
+        // TODO: Should this be ceil?
+        num_segments = (int)utilities::floor(circumference / original_shape_length_);
         if (num_segments < min_arc_segments_) {
           abort_arc = true;
           num_firmware_compensations_++; 
diff --git a/ArcWelder/segmented_shape.cpp b/ArcWelder/segmented_shape.cpp
index 447cfbe..dd422a4 100644
--- a/ArcWelder/segmented_shape.cpp
+++ b/ArcWelder/segmented_shape.cpp
@@ -83,7 +83,7 @@ point point::get_midpoint(point p1, point p2)
 
 bool point::is_near_collinear(const point& p1, const point& p2, const point& p3, double tolerance)
 {
-  return fabs((p1.y - p2.y) * (p1.x - p3.x) - (p1.y - p3.y) * (p1.x - p2.x)) <= 1e-9;
+  return utilities::abs((p1.y - p2.y) * (p1.x - p3.x) - (p1.y - p3.y) * (p1.x - p2.x)) <= 1e-9;
 }
 
 double point::cartesian_distance(const point& p1, const point& p2)
@@ -124,7 +124,7 @@ bool segment::get_closest_perpendicular_point(const point& p1, const point& p2,
 #pragma region Vector Functions
 double vector::get_magnitude()
 {
-  return sqrt(x * x + y * y + z * z);
+  return utilities::sqrt(x * x + y * y + z * z);
 }
 
 double vector::cross_product_magnitude(vector v1, vector v2)
@@ -144,9 +144,9 @@ double vector::cross_product_magnitude(vector v1, vector v2)
 // Users of this code must verify correctness for their application.
 // dot product (3D) which allows vector operations in arguments
 #define dot(u,v)   ((u).x * (v).x + (u).y * (v).y + (u).z * (v).z)
-#define dotxy(u,v)   ((u).x * (v).x + (u).y * (v).y)
-#define norm(v)     sqrt(dot(v,v))     // norm = length of  vector
-#define d(u,v)      norm(u-v)          // distance = norm of difference
+//#define dotxy(u,v)   ((u).x * (v).x + (u).y * (v).y)
+//#define norm(v)     utilities::sqrt(dot(v,v))     // norm = length of  vector
+//#define d(u,v)      norm(u-v)          // distance = norm of difference
 
 #pragma endregion Distance Calculation Source
 
@@ -259,7 +259,7 @@ bool circle::try_create_circle(const array_list<printer_point>& points, const do
 
 double circle::get_polar_radians(const point& p1) const
 {
-  double polar_radians = atan2(p1.y - center.y, p1.x - center.x);
+  double polar_radians = utilities::atan2(p1.y - center.y, p1.x - center.x);
   if (polar_radians < 0)
     polar_radians = (2.0 * PI_DOUBLE) + polar_radians;
   return polar_radians;
@@ -290,7 +290,7 @@ bool circle::get_deviation_sum_squared(const array_list<printer_point>& points,
         return false;
       }
     }
-    double deviation = std::fabs(distance_from_center - radius);
+    double deviation = utilities::abs(distance_from_center - radius);
     total_deviation += deviation * deviation;
     if (deviation > resolution_mm)
     {
@@ -305,7 +305,7 @@ bool circle::get_deviation_sum_squared(const array_list<printer_point>& points,
     if (segment::get_closest_perpendicular_point(points[index], points[index + 1], center, point_to_test))
     {
       double distance = utilities::get_cartesian_distance(point_to_test.x, point_to_test.y, center.x, center.y);
-      double deviation = std::fabs(distance - radius);
+      double deviation = utilities::abs(distance - radius);
       total_deviation += deviation * deviation;
       if (deviation > resolution_mm)
       {
@@ -346,7 +346,7 @@ bool circle::is_over_deviation(const array_list<printer_point>& points, const do
           return true;
         }
       }
-      if (std::fabs(distance_from_center - radius) > resolution_mm)
+      if (utilities::abs(distance_from_center - radius) > resolution_mm)
       {
         return true;
       }
@@ -357,7 +357,7 @@ bool circle::is_over_deviation(const array_list<printer_point>& points, const do
     if (segment::get_closest_perpendicular_point(current_point, points[index + 1], center, point_to_test))
     {
       double distance = utilities::get_cartesian_distance(point_to_test.x, point_to_test.y, center.x, center.y);
-      if (std::fabs(distance - radius) > resolution_mm)
+      if (utilities::abs(distance - radius) > resolution_mm)
       {
        return true;
       }
@@ -457,7 +457,7 @@ bool arc::try_create_arc(
     // see if an arc moving in the opposite direction had the correct length.
 
     // Find the rest of the angle across the circle
-    double test_radians = std::fabs(angle_radians - 2 * PI_DOUBLE);
+    double test_radians = utilities::abs(angle_radians - 2 * PI_DOUBLE);
     // Calculate the length of that arc
     double test_arc_length = c.radius * test_radians;
     if (allow_3d_arcs)
@@ -667,7 +667,7 @@ bool arc::ray_intersects_segment(const point rayOrigin, const point rayDirection
   vector v3 = vector(-rayDirection.y, rayDirection.x, 0);
 
   double dot = dot(v2, v3);
-  if (std::fabs(dot) < 0.000001)
+  if (utilities::abs(dot) < 0.000001)
     return false;
 
   double t1 = vector::cross_product_magnitude(v2, v1) / dot;
@@ -727,7 +727,7 @@ void segmented_shape::set_xyz_precision(unsigned char precision)
 
 void segmented_shape::set_xyz_tolerance_from_precision()
 {
-  xyz_tolerance_ = std::pow(10.0, -1.0 * static_cast<double>(xyz_precision_));
+  xyz_tolerance_ = utilities::pow(10, -1.0 * static_cast<double>(xyz_precision_));
 }
 
 void segmented_shape::reset_precision()
diff --git a/ArcWelderInverseProcessor/firmware.h b/ArcWelderInverseProcessor/firmware.h
index a11b057..80d5d07 100644
--- a/ArcWelderInverseProcessor/firmware.h
+++ b/ArcWelderInverseProcessor/firmware.h
@@ -5,7 +5,6 @@
 #include <iomanip>
 #include <algorithm>
 
-#define M_PI       3.14159265358979323846   // pi
 #define DEFAULT_FIRMWARE_TYPE firmware_types::MARLIN_2
 #define LATEST_FIRMWARE_VERSION_NAME "LATEST_RELEASE"
 #define DEFAULT_FIRMWARE_VERSION_NAME LATEST_FIRMWARE_VERSION_NAME
diff --git a/ArcWelderInverseProcessor/marlin_1.cpp b/ArcWelderInverseProcessor/marlin_1.cpp
index 6269645..0cca4e9 100644
--- a/ArcWelderInverseProcessor/marlin_1.cpp
+++ b/ArcWelderInverseProcessor/marlin_1.cpp
@@ -156,7 +156,7 @@ void marlin_1::plan_arc_1_1_9_1(const float(&cart)[MARLIN_XYZE], // Destination
 	// Radius vector from center to current location
 	float r_P = -offset[0], r_Q = -offset[1];
 
-	const float radius = HYPOT(r_P, r_Q),
+	const float radius = utilities::hypotf(r_P, r_Q),
 		center_P = current_position[p_axis] - r_P,
 		center_Q = current_position[q_axis] - r_Q,
 		rt_X = cart[p_axis] - center_P,
@@ -165,19 +165,19 @@ void marlin_1::plan_arc_1_1_9_1(const float(&cart)[MARLIN_XYZE], // Destination
 		extruder_travel = cart[E_CART] - current_position[E_CART];
 
 	// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
-	float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);
-	if (angular_travel < 0) angular_travel += RADIANS(360);
-	if (clockwise) angular_travel -= RADIANS(360);
+	float angular_travel = (float)utilities::atan2((double)r_P * rt_Y - (double)r_Q * rt_X, (double)r_P * rt_X + (double)r_Q * rt_Y);
+	if (angular_travel < 0) angular_travel += utilities::radiansf(360.0f);
+	if (clockwise) angular_travel -= utilities::radiansf(360.0f);
 
 	// Make a circle if the angular rotation is 0 and the target is current position
 	if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis])
-		angular_travel = RADIANS(360);
+		angular_travel = utilities::radiansf(360.0f);
 
 	const float flat_mm = radius * angular_travel,
-		mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm);
+		mm_of_travel = linear_travel ? utilities::hypotf(flat_mm, linear_travel) : utilities::absf(flat_mm);
 	if (mm_of_travel < 0.001f) return;
 
-	uint16_t segments = (uint16_t)FLOOR(mm_of_travel / (float)(args_.mm_per_arc_segment));
+	uint16_t segments = (uint16_t)utilities::floorf(mm_of_travel / (float)(args_.mm_per_arc_segment));
 	NOLESS(segments, 1);
 
 	/**
@@ -212,7 +212,7 @@ void marlin_1::plan_arc_1_1_9_1(const float(&cart)[MARLIN_XYZE], // Destination
 		linear_per_segment = linear_travel / segments,
 		extruder_per_segment = extruder_travel / segments,
 		sin_T = theta_per_segment,
-		cos_T = 1 - 0.5f * sq(theta_per_segment); // Small angle approximation
+		cos_T = 1 - 0.5f * utilities::sqf(theta_per_segment); // Small angle approximation
 
 	// Initialize the linear axis
 	raw[l_axis] = current_position[l_axis];
@@ -273,42 +273,10 @@ void marlin_1::plan_arc_1_1_9_1(const float(&cart)[MARLIN_XYZE], // Destination
 	COPY(current_position, cart);
 }
 
-// Marlin Function Defs
-float marlin_1::HYPOT(float x, float y)
-{
-	return (float)utilities::hypot(x, y);
-}
-
-float marlin_1::ATAN2(float x, float y)
-{
-	return (float)utilities::atan2(x, y);
-}
-
-float marlin_1::RADIANS(float x)
-{
-	return (x * (float)M_PI) / 180;
-}
-
-float marlin_1::ABS(float x)
-{
-	return (float)utilities::abs((double)x);
-}
-
-float marlin_1::FLOOR(float x)
-{
-	return (float)utilities::floor((double)x);
-}
-
-float marlin_1::NOLESS(uint16_t x, uint16_t y)
+void marlin_1::NOLESS(uint16_t &x, uint16_t y)
 {
 	if (x < y)
-		return y;
-	return x;
-}
-
-float marlin_1::sq(float x)
-{
-	return x * x;
+		x = y;
 }
 
 float marlin_1::MMS_SCALED(float x)
diff --git a/ArcWelderInverseProcessor/marlin_1.h b/ArcWelderInverseProcessor/marlin_1.h
index 9579bfb..0fc11c4 100644
--- a/ArcWelderInverseProcessor/marlin_1.h
+++ b/ArcWelderInverseProcessor/marlin_1.h
@@ -86,13 +86,7 @@ private:
   
   plan_arc_func plan_arc_;
   // Marlin Function Defs
-  float HYPOT(float x, float y);
-  float ATAN2(float x, float y);
-  float RADIANS(float x);
-  float ABS(float x);
-  float FLOOR(float x);
-  float NOLESS(uint16_t x, uint16_t y);
-  float sq(float x);
+  void NOLESS(uint16_t& x, uint16_t y);
   float MMS_SCALED(float x);
   void COPY(float target[MARLIN_XYZE], const float (&source)[MARLIN_XYZE]);
   bool buffer_line_kinematic(const float (&cart)[MARLIN_XYZE], double fr_mm_s, int active_extruder);
diff --git a/ArcWelderInverseProcessor/marlin_2.cpp b/ArcWelderInverseProcessor/marlin_2.cpp
index 4893dd1..afd17a6 100644
--- a/ArcWelderInverseProcessor/marlin_2.cpp
+++ b/ArcWelderInverseProcessor/marlin_2.cpp
@@ -168,7 +168,7 @@ void marlin_2::plan_arc_2_0_9_1(
   rvec[0] = - offset[X_AXIS];
   rvec[1] = - offset[Y_AXIS];
 
-  const float radius = HYPOT(rvec[0], rvec[1]),
+  const float radius = utilities::hypotf(rvec[0], rvec[1]),
     center_P = current_position[p_axis] - rvec[0],
     center_Q = current_position[q_axis] - rvec[1],
     rt_X = cart[p_axis] - center_P,
@@ -183,25 +183,25 @@ void marlin_2::plan_arc_2_0_9_1(
   // Do a full circle if starting and ending positions are "identical"
   if (NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {
     // Preserve direction for circles
-    angular_travel = clockwise ? -RADIANS(360) : RADIANS(360);
+    angular_travel = clockwise ? -utilities::radiansf(360.0f) : utilities::radiansf(360.0f);
   }
   else {
     // Calculate the angle
-    angular_travel = ATAN2(rvec[0] * rt_Y - rvec[1] * rt_X, rvec[0] * rt_X + rvec[1] * rt_Y);
+    angular_travel = utilities::atan2f(rvec[0] * rt_Y - rvec[1] * rt_X, rvec[0] * rt_X + rvec[1] * rt_Y);
 
     // Angular travel too small to detect? Just return.
     if (!angular_travel) return;
 
     // Make sure angular travel over 180 degrees goes the other way around.
     switch (((angular_travel < 0) << 1) | (int)clockwise) {
-    case 1: angular_travel -= RADIANS(360); break; // Positive but CW? Reverse direction.
-    case 2: angular_travel += RADIANS(360); break; // Negative but CCW? Reverse direction.
+    case 1: angular_travel -= utilities::radiansf(360.0f); break; // Positive but CW? Reverse direction.
+    case 2: angular_travel += utilities::radiansf(360.0f); break; // Negative but CCW? Reverse direction.
     }
 
     if (args_.min_arc_segments > 1)
     {
-      min_segments = (uint16_t)CEIL(min_segments * ABS(angular_travel) / RADIANS(360));
-      min_segments = (uint16_t)NOLESS(min_segments, 1U);
+      min_segments = (uint16_t)utilities::ceilf(min_segments * utilities::absf(angular_travel) / utilities::radiansf(360.0f));
+      NOLESS(min_segments, 1U);
     }
   }
 
@@ -210,7 +210,7 @@ void marlin_2::plan_arc_2_0_9_1(
   float extruder_travel = cart[E_AXIS] - current_position[E_AXIS];
 
   const float flat_mm = radius * angular_travel,
-    mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm);
+    mm_of_travel = linear_travel ? utilities::hypotf(flat_mm, linear_travel) : utilities::absf(flat_mm);
   if (mm_of_travel < 0.001f) return;
 
   const float scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
@@ -219,17 +219,17 @@ void marlin_2::plan_arc_2_0_9_1(
   float seg_length = (float)args_.mm_per_arc_segment;
   if (args_.arc_segments_per_r > 0)
   {
-    seg_length = constrain((float)args_.mm_per_arc_segment * radius, (float)args_.mm_per_arc_segment, (float)args_.arc_segments_per_r);
+    seg_length = utilities::constrainf((float)args_.mm_per_arc_segment * radius, (float)args_.mm_per_arc_segment, (float)args_.arc_segments_per_r);
   }
   else if (args_.arc_segments_per_sec > 0)
   {
-    seg_length = _MAX(scaled_fr_mm_s * RECIPROCAL((float)args_.arc_segments_per_sec), (float)args_.mm_per_arc_segment);
+    seg_length = utilities::maxf(scaled_fr_mm_s * utilities::reciprocalf((float)args_.arc_segments_per_sec), (float)args_.mm_per_arc_segment);
   }
 
   // Divide total travel by nominal segment length
-  uint16_t segments = (uint16_t)FLOOR(mm_of_travel / seg_length);
-  //uint16_t segments = FLOOR(mm_of_travel / seg_length);
-  segments = (uint16_t)NOLESS(segments, min_segments);         // At least some segments
+  uint16_t segments = (uint16_t)utilities::floorf(mm_of_travel / seg_length);
+  //uint16_t segments = utilities::floorf(mm_of_travel / seg_length);
+  NOLESS(segments, min_segments);         // At least some segments
   seg_length = mm_of_travel / segments;
 
   /**
@@ -261,7 +261,7 @@ void marlin_2::plan_arc_2_0_9_1(
    // Vector rotation matrix values
   float raw[MARLIN_2_XYZE];
   const float theta_per_segment = angular_travel / segments,
-    sq_theta_per_segment = sq(theta_per_segment),
+    sq_theta_per_segment = utilities::sqf(theta_per_segment),
     sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
     cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
 
@@ -296,8 +296,8 @@ void marlin_2::plan_arc_2_0_9_1(
       // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
       // To reduce stuttering, the sin and cos could be computed at different times.
       // For now, compute both at the same time.
-      const float cos_Ti = COS(i * theta_per_segment);
-      const float sin_Ti = SIN(i * theta_per_segment);
+      const float cos_Ti = (float)utilities::cos((i * (double)theta_per_segment));
+      const float sin_Ti = (float)utilities::sin((i * (double)theta_per_segment));
       rvec[0] = -offset[0] * cos_Ti + offset[1] * sin_Ti;
       rvec[1] = -offset[0] * sin_Ti - offset[1] * cos_Ti;
     }
@@ -375,7 +375,7 @@ void marlin_2::plan_arc_2_0_9_2(
   rvec[0] = -offset[X_AXIS];
   rvec[1] = -offset[Y_AXIS];
 
-  const float radius = HYPOT(rvec[0], rvec[1]),
+  const float radius = utilities::hypotf(rvec[0], rvec[1]),
     center_P = current_position[p_axis] - rvec[0],
     center_Q = current_position[q_axis] - rvec[1],
     rt_X = cart[p_axis] - center_P,
@@ -390,28 +390,28 @@ void marlin_2::plan_arc_2_0_9_2(
   // Do a full circle if starting and ending positions are "identical"
   if (NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {
     // Preserve direction for circles
-    angular_travel = clockwise ? -RADIANS(360) : RADIANS(360);
-    abs_angular_travel = RADIANS(360);
+    angular_travel = clockwise ? -utilities::radiansf(360.0f) : utilities::radiansf(360.0f);
+    abs_angular_travel = utilities::radiansf(360.0f);
     min_segments = min_circle_segments;
   }
   else {
     // Calculate the angle
-    angular_travel = ATAN2(rvec[0] * rt_Y - rvec[1] * rt_X, rvec[0] * rt_X + rvec[1] * rt_Y);
+    angular_travel = utilities::atan2f(rvec[0] * rt_Y - rvec[1] * rt_X, rvec[0] * rt_X + rvec[1] * rt_Y);
 
     // Angular travel too small to detect? Just return.
     if (!angular_travel) return;
 
     // Make sure angular travel over 180 degrees goes the other way around.
     switch (((angular_travel < 0) << 1) | (int)clockwise) {
-    case 1: angular_travel -= RADIANS(360); break; // Positive but CW? Reverse direction.
-    case 2: angular_travel += RADIANS(360); break; // Negative but CCW? Reverse direction.
+    case 1: angular_travel -= utilities::radiansf(360.0f); break; // Positive but CW? Reverse direction.
+    case 2: angular_travel += utilities::radiansf(360.0f); break; // Negative but CCW? Reverse direction.
     }
 
-    abs_angular_travel = ABS(angular_travel);
+    abs_angular_travel = utilities::absf(angular_travel);
 
     // Apply minimum segments to the arc
-    const float portion_of_circle = abs_angular_travel / RADIANS(360);  // Portion of a complete circle (0 < N < 1)
-    min_segments = (uint16_t)CEIL((min_circle_segments)*portion_of_circle);     // Minimum segments for the arc
+    const float portion_of_circle = abs_angular_travel / utilities::radiansf(360.0f);  // Portion of a complete circle (0 < N < 1)
+    min_segments = (uint16_t)utilities::ceilf((min_circle_segments)*portion_of_circle);     // Minimum segments for the arc
   }
 
   float travel_L = cart[Z_AXIS] - start_L;
@@ -429,19 +429,19 @@ void marlin_2::plan_arc_2_0_9_2(
   // Get the nominal segment length based on settings
   float nominal_segment_mm;
   if (args_.arc_segments_per_sec > 0) {
-    nominal_segment_mm = constrain(scaled_fr_mm_s * RECIPROCAL((float)args_.arc_segments_per_sec), (float)args_.get_min_arc_segment_mm(), (float)args_.get_max_arc_segment_mm());
+    nominal_segment_mm = utilities::constrainf(scaled_fr_mm_s * utilities::reciprocalf((float)args_.arc_segments_per_sec), (float)args_.get_min_arc_segment_mm(), (float)args_.get_max_arc_segment_mm());
   }
   else {
     nominal_segment_mm = (float)args_.get_max_arc_segment_mm();
   }
   // Number of whole segments based on the nominal segment length
-  const float nominal_segments = _MAX(FLOOR(flat_mm / nominal_segment_mm), min_segments);
+  const float nominal_segments = utilities::maxf(utilities::floorf(flat_mm / nominal_segment_mm), min_segments);
 
   // A new segment length based on the required minimum
-  const float segment_mm = constrain(flat_mm / nominal_segments, (float)args_.get_min_arc_segment_mm(), (float)args_.get_max_arc_segment_mm());
+  const float segment_mm = utilities::constrainf(flat_mm / nominal_segments, (float)args_.get_min_arc_segment_mm(), (float)args_.get_max_arc_segment_mm());
 
   // The number of whole segments in the arc, ignoring the remainder
-  uint16_t segments = (uint16_t)FLOOR(flat_mm / segment_mm);
+  uint16_t segments = (uint16_t)utilities::floorf(flat_mm / segment_mm);
 
   // Are the segments now too few to reach the destination?
   const float segmented_length = segment_mm * segments;
@@ -478,7 +478,7 @@ void marlin_2::plan_arc_2_0_9_2(
    // Vector rotation matrix values
   float raw[MARLIN_2_XYZE];
   const float theta_per_segment = proportion * angular_travel / segments,
-    sq_theta_per_segment = sq(theta_per_segment),
+    sq_theta_per_segment = utilities::sqf(theta_per_segment),
     sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
     cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
 
@@ -516,8 +516,8 @@ void marlin_2::plan_arc_2_0_9_2(
       // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
       // To reduce stuttering, the sin and cos could be computed at different times.
       // For now, compute both at the same time.
-      const float cos_Ti = COS(i * theta_per_segment);
-      const float sin_Ti = SIN(i * theta_per_segment);
+      const float cos_Ti = (float)utilities::cos(i * (double)theta_per_segment);
+      const float sin_Ti = (float)utilities::sin(i * (double)theta_per_segment);
       rvec[0] = -offset[0] * cos_Ti + offset[1] * sin_Ti;
       rvec[1] = -offset[0] * sin_Ti - offset[1] * cos_Ti;
     }
@@ -549,97 +549,26 @@ void marlin_2::plan_arc_2_0_9_2(
   COPY(current_position, raw);
 }
 // Marlin Function Defs
-float marlin_2::HYPOT(float x, float y)
+void marlin_2::NOLESS(uint16_t& x, uint16_t y)
 {
-  return (float)utilities::hypot(x, y);
+    if (x < y)
+        x = y;
 }
-
-float marlin_2::ATAN2(float x, float y)
-{
-  return (float)utilities::atan2(x, y);
-}
-
-float marlin_2::RADIANS(float x)
-{
-  return (x * (float)M_PI) / 180;
-}
-
-float marlin_2::ABS(float x)
-{
-  return (float)utilities::abs(x);
-}
-
-float marlin_2::FLOOR(float x)
-{
-  return (float)utilities::floor(x);
-}
-
-float marlin_2::COS(float x)
-{
-    return (float)utilities::cos(x);
-}
-
-float marlin_2::SIN(float x)
-{
-    return (float)utilities::sin(x);
-}
-
-float marlin_2::NOLESS(uint16_t x, uint16_t y)
-{
-  if (x < y)
-    return y;
-  return x;
-}
-
-float marlin_2::sq(float x)
-{
-  return x * x;
-}
-
 float marlin_2::MMS_SCALED(float x)
 {
   // No scaling
   return x;
 }
 
-bool marlin_2::WITHIN(float N, float L, float H)
-{
-  return ((N) >= (L) && (N) <= (H));
-}
 bool marlin_2::NEAR_ZERO(float x)
 {
-  return WITHIN(x, -0.000001f, 0.000001f);
+  return utilities::withinf(x, -0.000001f, 0.000001f);
 }
 bool marlin_2::NEAR(float x, float y)
 {
   return NEAR_ZERO((x)-(y));
 }
 
-float marlin_2::CEIL(float x)
-{
-  return (float)utilities::ceil(x);
-}
-
-float marlin_2::constrain(float value, float arg_min, float arg_max)
-{
-  return ((value) < (arg_min) ? (arg_min) : ((value) > (arg_max) ? (arg_max) : (value)));
-}
-
-float marlin_2::_MAX(float x, float y)
-{
-  if (x>y) return x;
-  return y;
-}
-float marlin_2::_MIN(float x, float y)
-{
-  if (x < y) return x;
-  return y;
-}
-
-float marlin_2::RECIPROCAL(float x)
-{
-  return (float)1.0/x;
-}
 void marlin_2::COPY(float target[MARLIN_2_XYZE], const float(&source)[MARLIN_2_XYZE])
 {
   // This is a slow copy, but speed isn't much of an issue here.
diff --git a/ArcWelderInverseProcessor/marlin_2.h b/ArcWelderInverseProcessor/marlin_2.h
index f9735ec..2abea56 100644
--- a/ArcWelderInverseProcessor/marlin_2.h
+++ b/ArcWelderInverseProcessor/marlin_2.h
@@ -84,24 +84,10 @@ private:
   plan_arc_func plan_arc_;
 
   // Marlin Function Defs
-  float HYPOT(float x, float y);
-  float ATAN2(float x, float y);
-  float RADIANS(float x);
-  float COS(float x);
-  float SIN(float s);
-  float ABS(float x);
-  float FLOOR(float x);
-  float NOLESS(uint16_t x, uint16_t y);
-  float sq(float x);
+  void NOLESS(uint16_t& x, uint16_t y);
   float MMS_SCALED(float x);
   bool NEAR_ZERO(float x);
   bool NEAR(float x, float y);
-  bool WITHIN(float N, float L, float H);
-  float CEIL(float x);
-  float constrain(float value, float arg_min, float arg_max);
-  float _MAX(float x, float y);
-  float _MIN(float x, float y);
-  float RECIPROCAL(float x);
   void COPY(float target[MARLIN_2_XYZE], const float(&source)[MARLIN_2_XYZE]);
 };
 
diff --git a/ArcWelderInverseProcessor/prusa.cpp b/ArcWelderInverseProcessor/prusa.cpp
index ddd8a53..34ee82b 100644
--- a/ArcWelderInverseProcessor/prusa.cpp
+++ b/ArcWelderInverseProcessor/prusa.cpp
@@ -117,7 +117,7 @@ std::string prusa::interpolate_arc(firmware_position& target, double i, double j
   float prusa_radius = static_cast<float>(r);
   if (prusa_radius != 0)
   {
-    prusa_radius = (float)utilities::hypot(prusa_offset[X_AXIS], prusa_offset[Y_AXIS]); // Compute arc radius for mc_arc
+    prusa_radius = utilities::hypotf(prusa_offset[X_AXIS], prusa_offset[Y_AXIS]); // Compute arc radius for mc_arc
   }
   float prusa_f = static_cast<float>(target.f);
   
@@ -175,21 +175,21 @@ void prusa::mc_arc_3_10_0(float* position, float* target, float* offset, float f
   float rt_axis1 = target[axis_1] - center_axis1;
 
   // CCW angle between position and target from circle center. Only one atan2() trig computation required.
-  float angular_travel = atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
-  if (angular_travel < 0) { angular_travel += 2 * (float)M_PI; }
-  if (isclockwise) { angular_travel -= 2 * (float)M_PI; }
+  float angular_travel = (float)utilities::atan2((double)r_axis0 * rt_axis1 - (double)r_axis1 * rt_axis0, (double)r_axis0 * rt_axis0 + (double)r_axis1 * rt_axis1);
+  if (angular_travel < 0) { angular_travel += 2.0f * PI_FLOAT; }
+  if (isclockwise) { angular_travel -= 2.0f * PI_FLOAT; }
 
   //20141002:full circle for G03 did not work, e.g. G03 X80 Y80 I20 J0 F2000 is giving an Angle of zero so head is not moving
   //to compensate when start pos = target pos && angle is zero -> angle = 2Pi
   if (position[axis_0] == target[axis_0] && position[axis_1] == target[axis_1] && angular_travel == 0)
   {
-    angular_travel += 2 * (float)M_PI;
+    angular_travel += 2.0f * PI_FLOAT;
   }
   //end fix G03
 
-  float millimeters_of_travel = (float)utilities::hypot(angular_travel * radius, fabs(linear_travel));
+  float millimeters_of_travel = (float)utilities::hypot((double)angular_travel * radius, utilities::absf(linear_travel));
   if (millimeters_of_travel < 0.001) { return; }
-  uint16_t segments = (uint16_t)floor(millimeters_of_travel / cs.mm_per_arc_segment);
+  uint16_t segments = (uint16_t)utilities::floorf(millimeters_of_travel / cs.mm_per_arc_segment);
   if (segments == 0) segments = 1;
 
   /*
@@ -255,8 +255,8 @@ void prusa::mc_arc_3_10_0(float* position, float* target, float* offset, float f
     else {
       // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
       // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
-      cos_Ti = cos(i * theta_per_segment);
-      sin_Ti = sin(i * theta_per_segment);
+      cos_Ti = (float)utilities::cos(i * (double)theta_per_segment);
+      sin_Ti = (float)utilities::sin(i * (double)theta_per_segment);
       r_axis0 = -offset[axis_0] * cos_Ti + offset[axis_1] * sin_Ti;
       r_axis1 = -offset[axis_0] * sin_Ti - offset[axis_1] * cos_Ti;
       count = 0;
@@ -325,14 +325,14 @@ void prusa::mc_arc_3_11_0(float* position, float* target, float* offset, float f
   uint8_t n_arc_correction = cs.n_arc_correction;
 
   // CCW angle between position and target from circle center. Only one atan2() trig computation required.
-  float angular_travel_total = atan2(r_axis_x * rt_y - r_axis_y * rt_x, r_axis_x * rt_x + r_axis_y * rt_y);
-  if (angular_travel_total < 0) { angular_travel_total += 2 * (float)M_PI; }
+  float angular_travel_total = (float)utilities::atan2((double)r_axis_x * rt_y - (double)r_axis_y * rt_x, (double)r_axis_x * rt_x + (double)r_axis_y * rt_y);
+  if (angular_travel_total < 0) { angular_travel_total += 2.0f * PI_FLOAT; }
 
   if (cs.min_arc_segments > 0)
   {
     // 20200417 - FormerLurker - Implement MIN_ARC_SEGMENTS if it is defined - from Marlin 2.0 implementation
     // Do this before converting the angular travel for clockwise rotation
-    mm_per_arc_segment = radius * ((2.0f * (float)M_PI) / cs.min_arc_segments);
+    mm_per_arc_segment = radius * ((2.0f * PI_FLOAT) / cs.min_arc_segments);
   }
   if (cs.arc_segments_per_sec > 0)
   {
@@ -356,23 +356,23 @@ void prusa::mc_arc_3_11_0(float* position, float* target, float* offset, float f
   }
 
   // Adjust the angular travel if the direction is clockwise
-  if (isclockwise) { angular_travel_total -= 2 * (float)M_PI; }
+  if (isclockwise) { angular_travel_total -= 2.0f * PI_FLOAT; }
 
   //20141002:full circle for G03 did not work, e.g. G03 X80 Y80 I20 J0 F2000 is giving an Angle of zero so head is not moving
   //to compensate when start pos = target pos && angle is zero -> angle = 2Pi
   if (position[X_AXIS] == target[X_AXIS] && position[Y_AXIS] == target[Y_AXIS] && angular_travel_total == 0)
   {
-    angular_travel_total += 2 * (float)M_PI;
+    angular_travel_total += 2.0f * PI_FLOAT;
   }
   //end fix G03
 
   // 20200417 - FormerLurker - rename millimeters_of_travel to millimeters_of_travel_arc to better describe what we are
   // calculating here
-  const float millimeters_of_travel_arc = hypot(angular_travel_total * radius, fabs(travel_z));
+  const float millimeters_of_travel_arc = (float)utilities::hypot(angular_travel_total * (double)radius, utilities::fabs(travel_z));
   if (millimeters_of_travel_arc < 0.001) { return; }
 
   // Calculate the number of arc segments
-  uint16_t segments = static_cast<uint16_t>(ceil(millimeters_of_travel_arc / mm_per_arc_segment));
+  uint16_t segments = static_cast<uint16_t>(utilities::ceilf(millimeters_of_travel_arc / mm_per_arc_segment));
 
   /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
      and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
@@ -409,7 +409,7 @@ void prusa::mc_arc_3_11_0(float* position, float* target, float* offset, float f
     for (uint16_t i = 1; i < segments; i++) {
       if (n_arc_correction-- == 0) {
         // Calculate the actual position for r_axis_x and r_axis_y
-        const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
+        const float cos_Ti = (float)utilities::cos(i * (double)theta_per_segment), sin_Ti = (float)utilities::sin(i * (double)theta_per_segment);
         r_axis_x = -offset[X_AXIS] * cos_Ti + offset[Y_AXIS] * sin_Ti;
         r_axis_y = -offset[X_AXIS] * sin_Ti - offset[Y_AXIS] * cos_Ti;
         // reset n_arc_correction
diff --git a/ArcWelderInverseProcessor/repetier.cpp b/ArcWelderInverseProcessor/repetier.cpp
index 7d5798c..e5c34ec 100644
--- a/ArcWelderInverseProcessor/repetier.cpp
+++ b/ArcWelderInverseProcessor/repetier.cpp
@@ -119,7 +119,7 @@ std::string repetier::interpolate_arc(firmware_position& target, double i, doubl
   float repetier_radius = static_cast<float>(r);
   if (repetier_radius != 0)
   {
-    repetier_radius = (float)utilities::hypot(repetier_offset[X_AXIS], repetier_offset[Y_AXIS]); // Compute arc radius for mc_arc
+    repetier_radius = utilities::hypotf(repetier_offset[X_AXIS], repetier_offset[Y_AXIS]); // Compute arc radius for mc_arc
   }
   float repetier_f = static_cast<float>(target.f);
 
@@ -175,12 +175,12 @@ void repetier::arc_1_0_5(float* position, float* target, float* offset, float ra
   if (repetier_is_close(position[X_AXIS], target[X_AXIS]) && repetier_is_close(position[Y_AXIS], target[Y_AXIS]))
   {
     // Preserve direction for circles
-    angular_travel = isclockwise ? -2.0f * (float)M_PI : 2.0f * (float)M_PI;
+    angular_travel = isclockwise ? -2.0f * PI_FLOAT : 2.0f * PI_FLOAT;
   }
   else 
   {
     // CCW angle between position and target from circle center. Only one atan2() trig computation required.
-    angular_travel = (float)utilities::atan2((double)(r_axis0 * rt_axis1) - (double)(r_axis1 * rt_axis0), (double)(r_axis0 * rt_axis0) + (double)(r_axis1 * rt_axis1));
+    angular_travel = (float)utilities::atan2((double)r_axis0 * rt_axis1 - (double)r_axis1 * rt_axis0, (double)r_axis0 * rt_axis0 + (double)r_axis1 * rt_axis1);
     
     // No need to draw an arc if there is no angular travel
     if (!angular_travel) return;
@@ -190,12 +190,12 @@ void repetier::arc_1_0_5(float* position, float* target, float* offset, float ra
     {
       if (isclockwise)
       {
-        angular_travel -= 2.0f * (float)M_PI;
+        angular_travel -= 2.0f * PI_FLOAT;
       }
     }
     else if (!isclockwise)
     {
-      angular_travel += 2.0f * (float)M_PI;
+      angular_travel += 2.0f * PI_FLOAT;
     }
   }
 
@@ -204,7 +204,7 @@ void repetier::arc_1_0_5(float* position, float* target, float* offset, float ra
   if (linear_travel)
   {
     // If we have any Z motion, add this to the total mm of travel.
-    millimeters_of_travel = (float)utilities::hypot(millimeters_of_travel, linear_travel);
+    millimeters_of_travel = utilities::hypotf(millimeters_of_travel, linear_travel);
   }
 
   if (millimeters_of_travel < 0.001f) {
@@ -287,8 +287,8 @@ void repetier::arc_1_0_5(float* position, float* target, float* offset, float ra
     else {
       // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
       // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
-      cos_Ti = (float)utilities::cos(i * theta_per_segment);
-      sin_Ti = (float)utilities::sin(i * theta_per_segment);
+      cos_Ti = (float)utilities::cos(i * (double)theta_per_segment);
+      sin_Ti = (float)utilities::sin(i * (double)theta_per_segment);
       r_axis0 = -offset[0] * cos_Ti + offset[1] * sin_Ti;
       r_axis1 = -offset[0] * sin_Ti - offset[1] * cos_Ti;
       count = 0;
@@ -347,21 +347,21 @@ void repetier::arc_1_0_4(float* position, float* target, float* offset, float ra
   long etarget = Printer::destinationSteps[E_AXIS];
   */
   // CCW angle between position and target from circle center. Only one atan2() trig computation required.
-  float angular_travel = (float)utilities::atan2((double)(r_axis0 * rt_axis1) - (double)(r_axis1 * rt_axis0), (double)(r_axis0 * rt_axis0) + (double)(r_axis1 * rt_axis1));
+  float angular_travel = (float)utilities::atan2((double)r_axis0 * rt_axis1 - (double)r_axis1 * rt_axis0, (double)r_axis0 * rt_axis0 + (double)r_axis1 * rt_axis1);
   if ((!isclockwise && angular_travel <= 0.00001) || (isclockwise && angular_travel < -0.000001)) {
-    angular_travel += 2.0f * (float)M_PI;
+    angular_travel += 2.0f * PI_FLOAT;
   }
   if (isclockwise) {
-    angular_travel -= 2.0f * (float)M_PI;
+    angular_travel -= 2.0f * PI_FLOAT;
   }
 
-  float millimeters_of_travel = (float)utilities::fabs(angular_travel) * radius; //hypot(angular_travel*radius, fabs(linear_travel));
+  float millimeters_of_travel = (float)utilities::fabs(angular_travel) * radius;
   if (millimeters_of_travel < 0.001f) {
     return; // treat as succes because there is nothing to do;
   }
   //uint16_t segments = (radius>=BIG_ARC_RADIUS ? floor(millimeters_of_travel/MM_PER_ARC_SEGMENT_BIG) : floor(millimeters_of_travel/MM_PER_ARC_SEGMENT));
   // Increase segment size if printing faster then computation speed allows
-  uint16_t segments = (uint16_t)(feedrate > 60.0f ? utilities::floor(millimeters_of_travel / utilities::min(static_cast<float>(args_.mm_per_arc_segment), feedrate * 0.01666f * static_cast<float>(args_.mm_per_arc_segment))) : utilities::floor(millimeters_of_travel / static_cast<float>(args_.mm_per_arc_segment)));
+  uint16_t segments = (uint16_t)(feedrate > 60.0f ? utilities::floorf(millimeters_of_travel / utilities::minf(static_cast<float>(args_.mm_per_arc_segment), feedrate * 0.01666f * static_cast<float>(args_.mm_per_arc_segment))) : utilities::floorf(millimeters_of_travel / static_cast<float>(args_.mm_per_arc_segment)));
   if (segments == 0)
     segments = 1;
   /*
@@ -426,8 +426,8 @@ void repetier::arc_1_0_4(float* position, float* target, float* offset, float ra
     else {
       // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
       // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
-      cos_Ti = (float)utilities::cos(i * theta_per_segment);
-      sin_Ti = (float)utilities::sin(i * theta_per_segment);
+      cos_Ti = (float)utilities::cos(i * (double)theta_per_segment);
+      sin_Ti = (float)utilities::sin(i * (double)theta_per_segment);
       r_axis0 = -offset[0] * cos_Ti + offset[1] * sin_Ti;
       r_axis1 = -offset[0] * sin_Ti - offset[1] * cos_Ti;
       count = 0;
@@ -444,14 +444,6 @@ void repetier::arc_1_0_4(float* position, float* target, float* offset, float ra
   moveToReal(target[X_AXIS], target[Y_AXIS], position[Z_AXIS], target[E_AXIS]);
 }
 
-float repetier::min(float x, float y)
-{
-  if (x < y)
-  {
-    return x;
-  }
-  return y;
-}
 
 //void repetier::buffer_line_kinematic(float x, float y, float z, const float& e, float feed_rate, uint8_t extruder, const float* gcode_target)
 void repetier::moveToReal(float x, float y, float z, float e)
diff --git a/ArcWelderInverseProcessor/repetier.h b/ArcWelderInverseProcessor/repetier.h
index 6b4bcb3..0c381c5 100644
--- a/ArcWelderInverseProcessor/repetier.h
+++ b/ArcWelderInverseProcessor/repetier.h
@@ -33,7 +33,6 @@ private:
   // Note that trailing underscore are sometimes dropped to keep the ported function as close as possible to the original
   float feedrate;
   // Repetier Function Defs
-  float min(float x, float y);
   void moveToReal(float x, float y, float z, float e);
 };
 
diff --git a/ArcWelderInverseProcessor/smoothieware.cpp b/ArcWelderInverseProcessor/smoothieware.cpp
index bf17934..a747489 100644
--- a/ArcWelderInverseProcessor/smoothieware.cpp
+++ b/ArcWelderInverseProcessor/smoothieware.cpp
@@ -105,23 +105,23 @@ bool smoothieware::append_arc_2021_06_19(SmoothiewareGcode* gcode, const float t
   //check for condition where atan2 formula will fail due to everything canceling out exactly
   if ((this->machine_position[this->plane_axis_0] == target[this->plane_axis_0]) && (this->machine_position[this->plane_axis_1] == target[this->plane_axis_1])) {
     if (is_clockwise) { // set angular_travel to -2pi for a clockwise full circle
-      angular_travel = (-2 * (float)PI);
+      angular_travel = (-2 * PI_FLOAT);
     }
     else { // set angular_travel to 2pi for a counterclockwise full circle
-      angular_travel = (2 * (float)PI);
+      angular_travel = (2 * PI_FLOAT);
     }
   }
   else {
     // Patch from GRBL Firmware - Christoph Baumann 04072015
     // CCW angle between position and target from circle center. Only one atan2() trig computation required.
     // Only run if not a full circle or angular travel will incorrectly result in 0.0f
-    angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
+    angular_travel = (float)utilities::atan2((double)r_axis0 * rt_axis1 - (double)r_axis1 * rt_axis0, (double)r_axis0 * rt_axis0 + (double)r_axis1 * rt_axis1);
     if (plane_axis_2 == Y_AXIS) { is_clockwise = !is_clockwise; }  //Math for XZ plane is reverse of other 2 planes
     if (is_clockwise) { // adjust angular_travel to be in the range of -2pi to 0 for clockwise arcs
-      if (angular_travel > 0) { angular_travel -= (2 * (float)PI); }
+      if (angular_travel > 0) { angular_travel -= (2 * PI_FLOAT); }
     }
     else {  // adjust angular_travel to be in the range of 0 to 2pi for counterclockwise arcs
-      if (angular_travel < 0) { angular_travel += (2 * (float)PI); }
+      if (angular_travel < 0) { angular_travel += (2 * PI_FLOAT); }
     }
   }
 
@@ -135,7 +135,7 @@ bool smoothieware::append_arc_2021_06_19(SmoothiewareGcode* gcode, const float t
 
 
   // Find the distance for this gcode
-  float millimeters_of_travel = (float)utilities::hypot(angular_travel * radius, utilities::fabsf(linear_travel));
+  float millimeters_of_travel = (float)utilities::hypot((double)angular_travel * radius, utilities::fabsf(linear_travel));
 
   // We don't care about non-XYZ moves ( for example the extruder produces some of those )
   if (millimeters_of_travel < 0.000001F) {
@@ -144,8 +144,8 @@ bool smoothieware::append_arc_2021_06_19(SmoothiewareGcode* gcode, const float t
 
   // limit segments by maximum arc error
   float arc_segment = (float)args_.mm_per_arc_segment;
-  if ((args_.mm_max_arc_error > 0) && (2 * radius > args_.mm_max_arc_error)) {
-    float min_err_segment = 2 * (float)utilities::sqrtf((args_.mm_max_arc_error * (2 * radius - args_.mm_max_arc_error)));
+  if ((args_.mm_max_arc_error > 0) && (2.0 * (double)radius > args_.mm_max_arc_error)) {
+    float min_err_segment = 2 * (float)utilities::sqrt((args_.mm_max_arc_error * (2.0 * (double)radius - args_.mm_max_arc_error)));
     if (args_.mm_per_arc_segment < min_err_segment) {
       arc_segment = min_err_segment;
     }
@@ -158,7 +158,7 @@ bool smoothieware::append_arc_2021_06_19(SmoothiewareGcode* gcode, const float t
 
   // Figure out how many segments for this gcode
   // TODO for deltas we need to make sure we are at least as many segments as requested, also if mm_per_line_segment is set we need to use the
-  uint16_t segments = (uint16_t)floorf(millimeters_of_travel / arc_segment);
+  uint16_t segments = (uint16_t)utilities::floorf(millimeters_of_travel / arc_segment);
   bool moved = false;
 
   if (segments > 1) {
@@ -223,8 +223,8 @@ bool smoothieware::append_arc_2021_06_19(SmoothiewareGcode* gcode, const float t
       else {
         // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
         // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
-        cos_Ti = (float)utilities::cosf(i * theta_per_segment);
-        sin_Ti = (float)utilities::sinf(i * theta_per_segment);
+        cos_Ti = (float)utilities::cos(i * (double)theta_per_segment);
+        sin_Ti = (float)utilities::sin(i * (double)theta_per_segment);
         r_axis0 = -offset[this->plane_axis_0] * cos_Ti + offset[this->plane_axis_1] * sin_Ti;
         r_axis1 = -offset[this->plane_axis_0] * sin_Ti - offset[this->plane_axis_1] * cos_Ti;
         count = 0;
diff --git a/ArcWelderInverseProcessor/smoothieware.h b/ArcWelderInverseProcessor/smoothieware.h
index 61461c6..77d54bd 100644
--- a/ArcWelderInverseProcessor/smoothieware.h
+++ b/ArcWelderInverseProcessor/smoothieware.h
@@ -57,7 +57,6 @@ private:
   static const int plane_axis_1 = AxisEnum::Y_AXIS;
   static const int plane_axis_2 = AxisEnum::Z_AXIS;
   static const int plane_axis_3 = AxisEnum::E_AXIS;
-  const double PI = M_PI;
   SmoothiewareGcode gcode_;
   SmoothiewareKernel *THEKERNEL;
   float feed_rate;
diff --git a/ArcWelderTest/ArcWelderTest.cpp b/ArcWelderTest/ArcWelderTest.cpp
index 23588fd..31ca28f 100644
--- a/ArcWelderTest/ArcWelderTest.cpp
+++ b/ArcWelderTest/ArcWelderTest.cpp
@@ -34,7 +34,7 @@ int main(int argc, char* argv[])
 int run_tests(int argc, char* argv[])
 {
 
-	_CrtMemState state;
+	_CrtMemState state1, state2, state3;
 	// This line will take a snapshot
 	// of the memory allocated at this point.
 	_CrtSetReportMode(_CRT_WARN, _CRTDBG_MODE_FILE);
@@ -46,7 +46,7 @@ int run_tests(int argc, char* argv[])
 
 	//std::string filename = argv[1];
 	unsigned int num_runs = 1;
-	_CrtMemCheckpoint(&state);
+	_CrtMemCheckpoint(&state1);
 
 	auto start = std::chrono::high_resolution_clock::now();
 	for (unsigned int index = 0; index < num_runs; index++)
@@ -94,7 +94,11 @@ int run_tests(int argc, char* argv[])
 
 	}
 	auto end = std::chrono::high_resolution_clock::now();
-	_CrtMemDumpAllObjectsSince(&state);
+	_CrtMemCheckpoint(&state2);
+	if (_CrtMemDifference(&state3, &state1, &state2)) {
+		_CrtMemDumpStatistics(&state3);
+	}
+	//_CrtMemDumpAllObjectsSince(&state);
 	std::chrono::duration<double> diff = end - start;
 	std::cout << "Tests completed in " << diff.count() << " seconds";
 	//std::cout << "Has Memory Leak = " << has_leak << ".\r\n";
diff --git a/GcodeProcessorLib/gcode_position.cpp b/GcodeProcessorLib/gcode_position.cpp
index 444a879..daf66e4 100644
--- a/GcodeProcessorLib/gcode_position.cpp
+++ b/GcodeProcessorLib/gcode_position.cpp
@@ -616,7 +616,7 @@ void gcode_position::update(parsed_command& command, const long file_line_number
 			{
 				double r;
 				r = snapshot_x_max_; // good stand in for radius
-				const double dist = sqrt(p_current_pos->x*p_current_pos->x + p_current_pos->y*p_current_pos->y);
+				const double dist = utilities::sqrt(p_current_pos->x*p_current_pos->x + p_current_pos->y*p_current_pos->y);
 				is_in_bounds = utilities::less_than_or_equal(dist, r);
 
 			}
diff --git a/GcodeProcessorLib/utilities.cpp b/GcodeProcessorLib/utilities.cpp
index c1ac9fc..c53ddf8 100644
--- a/GcodeProcessorLib/utilities.cpp
+++ b/GcodeProcessorLib/utilities.cpp
@@ -38,11 +38,11 @@ namespace utilities {
 
 bool utilities::is_zero(double x, double tolerance)
 {
-	return fabs(x) < tolerance;
+	return utilities::abs(x) < tolerance;
 }
 bool utilities::is_zero(double x)
 {
-	return fabs(x) < ZERO_TOLERANCE;
+	return utilities::abs(x) < ZERO_TOLERANCE;
 }
 
 int utilities::round_up_to_int(double x, double tolerance)
@@ -57,13 +57,13 @@ int utilities::round_up_to_int(double x)
 
 bool utilities::is_equal(double x, double y, double tolerance)
 {
-	double abs_difference = fabs(x - y);
+	double abs_difference = utilities::abs(x - y);
 	return abs_difference < tolerance;
 }
 
 bool utilities::is_equal(double x, double y)
 {
-	double abs_difference = fabs(x - y);
+	double abs_difference = utilities::abs(x - y);
 	return abs_difference < ZERO_TOLERANCE;
 }
 
@@ -114,7 +114,7 @@ double utilities::get_cartesian_distance(double x1, double y1, double x2, double
 	double xdif = x1 - x2;
 	double ydif = y1 - y2;
 	double dist_squared = xdif * xdif + ydif * ydif;
-	return sqrt(dist_squared);
+	return utilities::sqrt(dist_squared);
 }
 
 double utilities::get_cartesian_distance(double x1, double y1, double z1, double x2, double y2, double z2)
@@ -124,30 +124,30 @@ double utilities::get_cartesian_distance(double x1, double y1, double z1, double
 	double ydif = y1 - y2;
 	double zdif = z1 - z2;
 	double dist_squared = xdif * xdif + ydif * ydif + zdif * zdif;
-	return sqrt(dist_squared);
+	return utilities::sqrt(dist_squared);
 }
 
 double utilities::get_arc_distance(double x1, double y1, double z1, double x2, double y2, double z2, double i, double j, double r, bool is_clockwise)
 {
 	double center_x = x1 - i;
 	double center_y = y1 - j;
-	double radius = hypot(i, j);
+	double radius = utilities::hypot(i, j);
 	double z_dist = z2 - z1;
 	double rt_x = x2 - center_x;
 	double rt_y = y2 - center_y;
-	double angular_travel_total = atan2(i * rt_y - j * rt_x, i * rt_x + j * rt_y);
-	if (angular_travel_total < 0) { angular_travel_total += (double)(2.0 * PI_DOUBLE); }
+	double angular_travel_total = utilities::atan2(i * rt_y - j * rt_x, i * rt_x + j * rt_y);
+	if (angular_travel_total < 0) { angular_travel_total += 2.0 * PI_DOUBLE; }
 	// Adjust the angular travel if the direction is clockwise
-	if (is_clockwise) { angular_travel_total -= (float)(2 * PI_DOUBLE); }
+	if (is_clockwise) { angular_travel_total -= 2.0 * PI_DOUBLE; }
 	// Full circle fix.
 	if (x1 == x2 && y1 == y2 && angular_travel_total == 0)
 	{
-		angular_travel_total += (float)(2 * PI_DOUBLE);
+		angular_travel_total += 2.0 * PI_DOUBLE;
 	}
 
 	// 20200417 - FormerLurker - rename millimeters_of_travel to millimeters_of_travel_arc to better describe what we are
 	// calculating here
-	return hypot(angular_travel_total * radius, fabs(z_dist));
+	return utilities::hypot(angular_travel_total * radius, utilities::abs(z_dist));
 
 }
 
@@ -298,7 +298,7 @@ std::string utilities::get_percent_change_string(int v1, int v2, int precision)
 
 int utilities::get_num_digits(int x)
 {
-	x = (int)abs(x);
+	x = utilities::abs(x);
 	return (x < 10 ? 1 :
 		(x < 100 ? 2 :
 			(x < 1000 ? 3 :
@@ -314,8 +314,8 @@ int utilities::get_num_digits(int x)
 int utilities::get_num_digits(double x, int precision)
 {
 	return get_num_digits(
-		(int)ceil(x * pow(10, (double)precision) - .4999999999999)
-		/ pow(10, (double)precision)
+		(int)utilities::ceil(x * utilities::pow(10, precision) - .4999999999999)
+		/ utilities::pow(10, precision)
 	);
 }
 
@@ -398,7 +398,20 @@ double utilities::hypot(double x, double y)
 	}
 	if (y == 0.0) return x;
 	y /= x;
-	return x * sqrt(1.0 + y * y);
+	return x * utilities::sqrt(1.0 + y * y);
+}
+
+float utilities::hypotf(float x, float y)
+{
+	if (x < 0.0f) x = -x;
+	if (y < 0.0f) y = -y;
+	if (x < y) {
+		float tmp = x;
+		x = y; y = tmp;
+	}
+	if (y == 0.0f) return x;
+	y /= x;
+	return x * utilities::sqrtf(1.0f + y * y);
 }
 
 double utilities::atan2(double y, double x)
@@ -406,7 +419,7 @@ double utilities::atan2(double y, double x)
 	return std::atan2(y, x);
 }
 
-double utilities::atan2f(double y, double x)
+float utilities::atan2f(float y, float x)
 {
 	return std::atan2(y, x);
 }
@@ -416,7 +429,7 @@ double utilities::floor(double x)
 	return std::floor(x);
 }
 
-double utilities::floorf(double x)
+float utilities::floorf(float x)
 {
 	return std::floor(x);
 }
@@ -426,22 +439,27 @@ double utilities::ceil(double x)
 	return std::ceil(x);
 }
 
+float utilities::ceilf(float x)
+{
+	return std::ceil(x);
+}
+
 double utilities::cos(double x)
 {
 	return std::cos(x);
 }
 
-double utilities::sin(double x)
+float utilities::cosf(float x)
 {
-	return std::sin(x);
+	return std::cos(x);
 }
 
-double utilities::cosf(double x)
+double utilities::sin(double x)
 {
-	return std::cos(x);
+	return std::sin(x);
 }
 
-double utilities::sinf(double x)
+float utilities::sinf(float x)
 {
 	return std::sin(x);
 }
@@ -451,12 +469,22 @@ double utilities::abs(double x)
 	return std::abs(x);
 }
 
+int utilities::abs(int x)
+{
+	return std::abs(x);
+}
+
+float utilities::absf(float x)
+{
+	return std::abs(x);
+}
+
 double utilities::fabs(double x)
 {
 	return std::fabs(x);
 }
 
-double utilities::fabsf(double x)
+float utilities::fabsf(float x)
 {
 	return std::fabs(x);
 }
@@ -466,7 +494,7 @@ double utilities::sqrt(double x)
 	return std::sqrt(x);
 }
 
-double utilities::sqrtf(double x)
+float utilities::sqrtf(float x)
 {
 	return std::sqrt(x);
 }
@@ -476,11 +504,81 @@ double utilities::pow(int e, double x)
 	return std::pow(e, x);
 }
 
-double utilities::min(float x, float y)
+double utilities::pow(int e, int x)
+{
+	return std::pow(e, x);
+}
+
+double utilities::min(double x, double y)
+{
+	return std::min(x, y);
+}
+
+float utilities::minf(float x, float y)
 {
 	return std::min(x, y);
 }
 
+double utilities::max(double x, double y)
+{
+	return std::max(x, y);
+}
+
+float utilities::maxf(float x, float y)
+{
+	return std::max(x, y);
+}
+
+double utilities::radians(double x)
+{
+	return (x * PI_DOUBLE) / 180.0;
+}
+
+float utilities::radiansf(float x)
+{
+	return (x * PI_FLOAT) / 180.0f;
+}
+
+double utilities::sq(double x)
+{
+	return x * x;
+}
+
+float utilities::sqf(float x)
+{
+	return x * x;
+}
+
+bool utilities::within(double value, double min, double max)
+{
+	return ((value) >= (min) && (value) <= (max));
+}
+
+bool utilities::withinf(float value, float min, float max)
+{
+	return ((value) >= (min) && (value) <= (max));
+}
+
+double utilities::constrain(double value, double arg_min, double arg_max)
+{
+	return ((value) < (arg_min) ? (arg_min) : ((value) > (arg_max) ? (arg_max) : (value)));
+}
+
+float utilities::constrainf(float value, float arg_min, float arg_max)
+{
+	return ((value) < (arg_min) ? (arg_min) : ((value) > (arg_max) ? (arg_max) : (value)));
+}
+
+double utilities::reciprocal(double x)
+{
+	return 1.0 / x;
+}
+
+float utilities::reciprocalf(float x)
+{
+	return 1.0f / x;
+}
+
 void* utilities::memcpy(void* dest, const void* src, size_t n)
 {
 	return std::memcpy(dest, src, n);
diff --git a/GcodeProcessorLib/utilities.h b/GcodeProcessorLib/utilities.h
index 7e2f80b..88e8c40 100644
--- a/GcodeProcessorLib/utilities.h
+++ b/GcodeProcessorLib/utilities.h
@@ -38,6 +38,7 @@
 // retract enough while wiping.
 #define ZERO_TOLERANCE 0.000005
 #define PI_DOUBLE 3.14159265358979323846264338327950288
+#define PI_FLOAT 3.14159265358979323846264338327950288f
 
 namespace utilities{
 	extern const std::string WHITESPACE_;
@@ -107,38 +108,74 @@ namespace utilities{
 	bool get_temp_file_path_for_file(const std::string& file_path, std::string& temp_file_path);
 
 	double hypot(double x, double y);
+	
+	float hypotf(float x, float y);
 
 	double atan2(double y, double x);
 
-	double atan2f(double y, double x);
+	float atan2f(float y, float x);
 
 	double floor(double x);
 
-	double floorf(double x);
+	float floorf(float x);
 
 	double ceil(double x);
+	
+	float ceilf(float x);
 
 	double cos(double x);
 
-	double sin(double x);
+	float cosf(float x);
 
-	double cosf(double x);
+	double sin(double x);
 
-	double sinf(double x);
+	float sinf(float x);
 
 	double abs(double x);
 
+	int abs(int x);
+
+	float absf(float x);
+
 	double fabs(double x);
 
-	double fabsf(double x);
+	float fabsf(float x);
 
 	double sqrt(double x);
 
-	double sqrtf(double x);
+	float sqrtf(float x);
 
 	double pow(int e, double x);
 
-	double min(float x, float y);
+	double pow(int e, int x);
+
+	double min(double x, double y);
+	
+	float minf(float x, float y);
+	
+	double max(double x, double y);
+	
+	float maxf(float x, float y);
+
+	double radians(double x);
+	
+	float radiansf(float x);
+
+	double sq(double x);
+
+	float sqf(float x);
+
+	bool within(double n, double l, double h);
+
+	bool withinf(float n, float l, float h);
+
+	double constrain(double value, double arg_min, double arg_max);
+
+	float constrainf(float value, float arg_min, float arg_max);
+
+	double reciprocal(double x);
+
+	float reciprocalf(float x);
 
 	void* memcpy(void* dest, const void* src, size_t n);