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 --- 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 - 10 files changed, 91 insertions(+), 225 deletions(-) (limited to 'ArcWelderInverseProcessor') 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; -- cgit v1.2.3