From 00df4c1dd984e294108fd77359af6f8771143fc7 Mon Sep 17 00:00:00 2001 From: FormerLurker Date: Sat, 13 Nov 2021 10:49:44 -0600 Subject: Continue moving math calls to utilities --- ArcWelder/arc_welder.cpp | 4 +- ArcWelder/segmented_arc.cpp | 6 +- ArcWelder/segmented_shape.cpp | 26 ++--- ArcWelderInverseProcessor/firmware.h | 1 - ArcWelderInverseProcessor/marlin_1.cpp | 52 ++-------- ArcWelderInverseProcessor/marlin_1.h | 8 +- ArcWelderInverseProcessor/marlin_2.cpp | 143 +++++++-------------------- ArcWelderInverseProcessor/marlin_2.h | 16 +-- ArcWelderInverseProcessor/prusa.cpp | 34 +++---- ArcWelderInverseProcessor/repetier.cpp | 38 +++----- ArcWelderInverseProcessor/repetier.h | 1 - ArcWelderInverseProcessor/smoothieware.cpp | 22 ++--- ArcWelderInverseProcessor/smoothieware.h | 1 - ArcWelderTest/ArcWelderTest.cpp | 10 +- GcodeProcessorLib/gcode_position.cpp | 2 +- GcodeProcessorLib/utilities.cpp | 150 ++++++++++++++++++++++++----- GcodeProcessorLib/utilities.h | 53 ++++++++-- 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& 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& 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& 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& 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& 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(xyz_precision_)); + xyz_tolerance_ = utilities::pow(10, -1.0 * static_cast(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 #include -#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(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(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(ceil(millimeters_of_travel_arc / mm_per_arc_segment)); + uint16_t segments = static_cast(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(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(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(args_.mm_per_arc_segment), feedrate * 0.01666f * static_cast(args_.mm_per_arc_segment))) : utilities::floor(millimeters_of_travel / static_cast(args_.mm_per_arc_segment))); + uint16_t segments = (uint16_t)(feedrate > 60.0f ? utilities::floorf(millimeters_of_travel / utilities::minf(static_cast(args_.mm_per_arc_segment), feedrate * 0.01666f * static_cast(args_.mm_per_arc_segment))) : utilities::floorf(millimeters_of_travel / static_cast(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 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); -- cgit v1.2.3