diff --git a/CMakeLists.txt b/CMakeLists.txt index 210d8b5c57..4df609698e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,6 @@ # Copyright (c) 2022 Ultimaker B.V. # CuraEngine is released under the terms of the AGPLv3 or higher. +# Modified by BigRep GmbH cmake_policy(SET CMP0091 NEW) # For MSVC flags, will be ignored on non-Windows OS's cmake_minimum_required(VERSION 3.20) @@ -114,6 +115,7 @@ set(engine_SRCS # Except main.cpp. src/pathPlanning/LinePolygonsCrossings.cpp src/pathPlanning/NozzleTempInsert.cpp src/pathPlanning/SpeedDerivatives.cpp + src/pathPlanning/ArcPathCalculator.cpp src/plugins/converters.cpp diff --git a/include/LayerPlan.h b/include/LayerPlan.h index 0ff9ffe19e..077deafbcb 100644 --- a/include/LayerPlan.h +++ b/include/LayerPlan.h @@ -1,5 +1,6 @@ // Copyright (c) 2024 UltiMaker // CuraEngine is released under the terms of the AGPLv3 or higher +// Modified by BigRep GmbH #ifndef LAYER_PLAN_H #define LAYER_PLAN_H @@ -780,6 +781,15 @@ class LayerPlan : public NoCopy * \return the combing boundary or an empty Polygons if no combing is required */ Polygons computeCombBoundary(const CombBoundary boundary_type); + + /*! + * Find the previous position at which the print head was before the current position + * + * \param paths The planned paths for the extrusion, travels etc. + * \param path_idx The current path index + * \return The previous position (2D) of the print head + */ + Point2LL getPreviousPosition(const std::vector& paths, const int path_idx) const; }; } // namespace cura diff --git a/include/PrintFeature.h b/include/PrintFeature.h index 699dcf7b29..9c1e1228f4 100644 --- a/include/PrintFeature.h +++ b/include/PrintFeature.h @@ -1,10 +1,12 @@ +// Modified by BigRep GmbH + #ifndef PRINT_FEATURE #define PRINT_FEATURE namespace cura { -enum class PrintFeatureType: unsigned char +enum class PrintFeatureType : unsigned char { NoneType = 0, // used to mark unspecified jumps in polygons. libArcus depends on it OuterWall = 1, @@ -18,14 +20,13 @@ enum class PrintFeatureType: unsigned char MoveRetraction = 9, SupportInterface = 10, PrimeTower = 11, - NumPrintFeatureTypes = 12 // this number MUST be the last one because other modules will + MoveGradualZHop = 12, + NumPrintFeatureTypes = 13 // this number MUST be the last one because other modules will // use this symbol to get the total number of types, which can // be used to create an array or so }; - - } // namespace cura #endif // PRINT_FEATURE diff --git a/include/gcodeExport.h b/include/gcodeExport.h index 5edd5dcdda..a8799953af 100644 --- a/include/gcodeExport.h +++ b/include/gcodeExport.h @@ -1,5 +1,6 @@ // Copyright (c) 2023 UltiMaker // CuraEngine is released under the terms of the AGPLv3 or higher +// Modified by BigRep GmbH #ifndef GCODEEXPORT_H #define GCODEEXPORT_H @@ -482,6 +483,14 @@ class GCodeExport : public NoCopy void writeUnretractionAndPrime(); void writeRetraction(const RetractionConfig& config, bool force = false, bool extruder_switch = false); + /*! + * Prepare for a retraction, while handling extruded_volume_at_previous_n_retractions and evaluating if a retraction should be performed + * + * \param Retraction configuration + * \return true if a retraction should be performed + */ + bool handleRetractionLimitation(const RetractionConfig& config); + /*! * Start a z hop with the given \p hop_height. * @@ -490,6 +499,26 @@ class GCodeExport : public NoCopy */ void writeZhopStart(const coord_t hop_height, Velocity speed = 0.0); + /*! + * Start a spiral z hop with the given \p hop_height. + * + * \param previous_position The previous point + * \param target The next target point in the extrusion + * \param hop_height The height to move above the current layer + * \param xy_speed The current speed in xy-direction + * \param printer_bounding_box Used to determine if a spiral leaves the build volume + * \param force_retraction if a retraction should be enforced + * \return An ordered vector of xyz-points along the hopping arc and the velocity with which it should travel towards this point + */ + std::vector> writeSpiralZhopStart( + const Point2LL previous_position, + const Point2LL target, + const coord_t hop_height, + const Velocity xy_speed, + const AABB3D& printer_bounding_box, + const RetractionConfig& config, + bool force_retraction = false); + /*! * End a z hop: go back to the layer height * diff --git a/include/pathPlanning/ArcPathCalculator.h b/include/pathPlanning/ArcPathCalculator.h new file mode 100644 index 0000000000..3b7c9ed29f --- /dev/null +++ b/include/pathPlanning/ArcPathCalculator.h @@ -0,0 +1,182 @@ +// Copyright (c) 2023 BigRep GmbH +// Released under the terms of the AGPLv3 or higher. +// Author(s): Nadine Mikkelsen, Matthew McMillan (plaintoothpaste) + +#ifndef PATH_PLANNING_ARC_PATH_CALCULATOR_H +#define PATH_PLANNING_ARC_PATH_CALCULATOR_H + +#include +#include + +#include "../settings/types/Velocity.h" +#include "../utils/AABB3D.h" +#include "../utils/Point2LL.h" +#include "../utils/Point3LL.h" + + +namespace cura +{ + +// The value to which vectors will be normalized (because the vectors/points can only be normalized to integer value lengths) +// Since usually the engine is calculating in micros this was set to one millimeter +constexpr coord_t normalization_value = 1000; + +/*! + * Class to generate an arc path (possibly a spiral with multiple turns) + * where the beginning and the end are tangential to the previous movement done and + * the following travel toward the next planned position. While the radius is always given + * the turns of the spiral are calculated and added as required to keep a constant xy speed + * and to not move faster in z direction then the provided z speed threshold. + * Calculations are done for a discretized arc (depending on the given step size for the linear segments). + * For each gradual z-hop one arc path is used. + */ +class ArcPath +{ +public: + const Point2LL start_; //!< The beginning of the arc + const Point2LL end_; //!< The end point of the arc + const coord_t radius_; //!< the radius of the underlying circle + const Point2LL circle_center_; //!< The center of the underlying circle + const coord_t z_increase_; //!< The z difference from the beginning to the end of the arc + const Velocity xy_speed_; //!< The speed along the xy-plane, with which the arc should be traveled + const Velocity z_speed_; //!< The speed along the z-axis, with which the arc should be traveled + const coord_t discretization_step_size_; //!< The discretization step size + const coord_t n_discrete_steps_; //!< The amount of discrete steps (at least) required for arc discretization + const bool is_clockwise_; //!< Whether the arc is spanned clockwise or counter clockwise + const int n_turns_; //!< The number of turns to be taken, one turn is only the arc going from start to end and higher values mean, that full turns are required as well, going + //!< from start to start + + /* + * \brief Factory method, which creates an arc path where the start is tangential to the previous movement done and the end leaves the arc tangential towards a provided target + * position. + * + * \param previous The previous position from which the print head is coming from + * \param current The position at which the print head is right now + * \param target The target position to which the print head should go + * \param hop_height The distance in z direction by which the print head should be raised + * \param radius The desired radius of the arc + * \param xy_speed The speed in xy-direction with which the print head was traveling from the previous position to the current one + * \param z_speed_limit A threshold value in how fast the print head should maximally be raised in z direction + * \param discretization_step_size The xy-distance of one step for approximating the arc linearly (used for calculating the length of the arc when determining the speed in z + * direction) \return The arc path object + */ + static ArcPath calculate( + const Point2LL previous, + const Point2LL current, + const Point2LL target, + const coord_t hop_height, + const coord_t radius, + const Velocity xy_speed, + const Velocity z_speed_limit, + const coord_t discretization_step_size); + + /* + * \brief Checks if the arc is inside some bounding box + * + * \param bounding_box The bounding to consider + * \return Evaluates true, if the arc leaves the bounding box + */ + bool isOutOfBounds(const AABB3D& bounding_box) const; + + /* + * \brief Discretizes an arc given the xy-step size provided when it is constructed and computes the velocity with which each point on the arc should be approached + * + * \param z_start The current z height + * \return Vector of pairs including a 3d position and a velocity for approaching it. The first value is not the arc start, because this is assumed to be the current position. + */ + std::vector> getDiscreteArc(const coord_t z_start) const; + + ArcPath() = delete; + +private: + /* + * \brief The constructor for an arc path, which is private because the factory method is the intended way to create this object. + * + * \param start_ The beginning of the arc + * \param end_ The end point of the arc + * \param radius_ The radius of the underlying circle + * \param circle_center_ The center of the underlying circle + * \param z_increase_ The z difference from the beginning to the end of the arc + * \param xy_speed_ The speed along the xy-plane, with which the arc should be traveled + * \param z_speed_ The speed along the z-axis, with which the arc should be traveled + * \param step_size The discretization step size + * \param n_discrete_steps_ The amount of discrete steps (at least) required for arc discretization + * \param is_clockwise_ Whether the arc is spanned clockwise or counter clockwise + * \param n_turns_ The number of turns to be taken, one turn is only the arc going from start to end and higher values mean, that full turns are required as well, going from + start to start + */ + ArcPath( + const Point2LL start, + const Point2LL end, + const coord_t radius, + const Point2LL circle_center, + const coord_t z_increase, + const Velocity xy_speed, + const Velocity z_speed, + const coord_t discretization_step_size, + const coord_t n_discrete_steps, + const bool is_clockwise, + const int n_turns); + + /*! + * \brief Converts an angle to a range of 0 to 2pi. + * + * \param angle Value to be converted + * \return Angle inside [0,2pi] + */ + static float convertToLimitedAngle(const float angle); + + /*! + * \brief Calculates the x component of the coordinate of a specific tangent (depending on the provided function). + * The tangent is calculated for a given target point outside the circle. + * + * \param r Circle radius + * \param d Distance between the circle center and the target point + * \param x X component of the target point + * \param y Y component of the target point + * \param func Can be subtraction or sum, depending on which tangent, on which side of the angle, should be computed. Use the sum to get the right tangent (viewed from the + * target point) and vice versa. \return Resulting x-component of the tangent + */ + static coord_t calcTangentX(const coord_t r, const coord_t d, const coord_t x, const coord_t y, const std::function func); + + /*! + * \brief Calculates the y-component of the coordinate of a specific tangent (depending on the provided function). + * The tangent is calculated for a given target point outside the circle. + * + * \param r Circle radius + * \param d Distance between the circle center and the target point + * \param x X component of the target point + * \param y Y component of the target point + * \param func Can be subtraction or sum, depending on which tangent, on which side of the angle, should be computed. Use the sum to get the right tangent (viewed from the + * target point) and vice versa. \return Resulting y-component of the tangent + */ + static coord_t calcTangentY(const coord_t r, const coord_t d, const coord_t x, const coord_t y, std::function func); + + /* + * \brief Calculate the angle of an arc. + * + * \param arc_start The starting point of the arc + * \param arc_end The end point of the arc + * \param circle_center The center of the circle on which the arc is based + * \param is_clockwise Whether or not the arc is turning clockwise or counter clockwise. + * \return The angle of the spanned arc in radians. + */ + static float calcArcAngle(const Point2LL arc_start, const Point2LL arc_end, const Point2LL circle_center, const bool is_clockwise); + + /* + * \brief Calculates the number of steps that have to be (at least) done to discretize the arc with a given step size. + * + * \param arc_start The starting point of the arc + * \param arc_end The end point of the arc + * \param radius The radius of the underlying circle + * \param step_size The size of the linear discretization steps + * \param circle_center The center of the underlying circle + * \param is_clockwise If the arc is spanned clockwise or count clockwise + * \return The minimal number of steps required for arc discretization + */ + static int + calcNumberOfSteps(const Point2LL arc_start, const Point2LL arc_end, const coord_t radius, const coord_t step_size, const Point2LL circle_center, const bool is_clockwise); +}; +} // namespace cura + +#endif // PATH_PLANNING_ARC_PATH_CALCULATOR_H \ No newline at end of file diff --git a/src/LayerPlan.cpp b/src/LayerPlan.cpp index 0acaf8345c..a11d8b94c1 100644 --- a/src/LayerPlan.cpp +++ b/src/LayerPlan.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2023 UltiMaker // CuraEngine is released under the terms of the AGPLv3 or higher +// Modified by BigRep GmbH #include "LayerPlan.h" @@ -238,6 +239,48 @@ Polygons LayerPlan::computeCombBoundary(const CombBoundary boundary_type) return comb_boundary; } +Point2LL LayerPlan::getPreviousPosition(const std::vector& paths, const int path_idx) const +{ + Point2LL starting_position = layer_start_pos_per_extruder_[last_planned_extruder_->extruder_nr_]; + + if (path_idx == 0) + { + return starting_position; + } + + // the current position will be the last point in the last path + // for the previous point we need to fetch what ever point was there before + size_t steps_back = 1; + size_t n_prev_pts = paths[path_idx - steps_back].points.size(); + + while (n_prev_pts == 0) + { + ++steps_back; + n_prev_pts = paths[path_idx - steps_back].points.size(); + } + + if (path_idx - steps_back < 0) + { + return starting_position; + } + + if (n_prev_pts == 1) + { + ++steps_back; + n_prev_pts = paths[path_idx - steps_back].points.size(); + while (n_prev_pts == 0 && path_idx - steps_back < 0) + { + ++steps_back; + n_prev_pts = paths[path_idx - steps_back].points.size(); + } + return path_idx - steps_back >= 0 ? paths[path_idx - steps_back].points[n_prev_pts - 1] : starting_position; + } + else // n_prev_pts > 1 + { + return path_idx - steps_back >= 0 ? paths[path_idx - steps_back].points[n_prev_pts - 2] : starting_position; + } +} + void LayerPlan::setIsInside(bool _is_inside) { is_inside_ = _is_inside; @@ -1965,6 +2008,8 @@ void LayerPlan::writeGCode(GCodeExport& gcode) const bool jerk_enabled = mesh_group_settings.get("jerk_enabled"); const bool jerk_travel_enabled = mesh_group_settings.get("jerk_travel_enabled"); std::shared_ptr current_mesh; + // used for spiral z-hop + double last_speed = 0; for (size_t extruder_plan_idx = 0; extruder_plan_idx < extruder_plans_.size(); extruder_plan_idx++) { @@ -2140,14 +2185,55 @@ void LayerPlan::writeGCode(GCodeExport& gcode) if (path.retract) { retraction_config = path.mesh ? &path.mesh->retraction_wipe_config : retraction_config; - gcode.writeRetraction(retraction_config->retraction_config); - insertTempOnTime(extruder_plan.getRetractTime(path), path_idx); + + const bool retraction_while_spiral = Application::getInstance().current_slice_->scene.settings.get("retract_during_spiral"); + const bool do_spiral_z_hop = Application::getInstance().current_slice_->scene.settings.get("z_hop_type") == "spiral"; + // options: + // perform no z hop with retraction before + // spiral z hop with retraction before + // spiral z hop with retraction in spiral + // vertical hop with retraction before + + // if the retraction is not done during the spiral, do a normal retraction before + if (! do_spiral_z_hop || ! retraction_while_spiral) + { + gcode.writeRetraction(retraction_config->retraction_config); + insertTempOnTime(extruder_plan.getRetractTime(path), path_idx); + } + if (path.perform_z_hop) { - gcode.writeZhopStart(z_hop_height); - z_hop_height = retraction_config->retraction_config.zHop; // back to normal z hop + if (do_spiral_z_hop) + { + Point2LL previous_point = getPreviousPosition(paths, path_idx); + const double xy_speed = last_speed == 0 ? path.config.getSpeed().value * path.speed_factor : last_speed; + const std::vector> spiral_points + = gcode.writeSpiralZhopStart(previous_point, path.points[0], z_hop_height, xy_speed, storage_.machine_size, retraction_config->retraction_config); + + const auto current_pos = Point3LL{ gcode.getPositionXY().X, gcode.getPositionXY().Y, gcode.getPositionZ() }; + // The spiral point vector might be empty, in the case where no spiral was possible (i.e. when it would have been outside of the printer bounds) + double total_spiral_time = spiral_points.size() > 0 ? (current_pos - spiral_points[0].first).vSizeMM() / spiral_points[0].second : 0; + + for (size_t pt_idx = 0; pt_idx < spiral_points.size(); ++pt_idx) + { + const auto& [point, velocity] = spiral_points[pt_idx]; + if (pt_idx > 0) + { + total_spiral_time += (point - spiral_points[pt_idx - 1].first).vSizeMM() / velocity; + } + // Send this to the optimized layer plan for the gcode preview + communication->sendLineTo(path.config.type, Point2LL{ point.x_, point.y_ }, path.getLineWidthForLayerView(), path.config.getLayerThickness(), velocity); + } + insertTempOnTime(total_spiral_time, path_idx); + z_hop_height = retraction_config->retraction_config.zHop; // back to normal z hop + } + else // vertical hop with retraction before + { + gcode.writeZhopStart(z_hop_height); + z_hop_height = retraction_config->retraction_config.zHop; // back to normal z hop + } } - else + else // perform no z hop with retraction before { gcode.writeZhopEnd(); if (z_ > 0 && path.z_offset != 0) @@ -2176,6 +2262,7 @@ void LayerPlan::writeGCode(GCodeExport& gcode) // for some movements such as prime tower purge, the speed may get changed by this factor speed *= path.speed_factor; + last_speed = speed; // This seems to be the best location to place this, but still not ideal. if (path.mesh != current_mesh) @@ -2241,6 +2328,7 @@ void LayerPlan::writeGCode(GCodeExport& gcode) insertTempOnTime(time, path_idx); const double extrude_speed = speed * path.speed_back_pressure_factor; + last_speed = extrude_speed; communication->sendLineTo(path.config.type, path.points[point_idx], path.getLineWidthForLayerView(), path.config.getLayerThickness(), extrude_speed); gcode.writeExtrusion(path.points[point_idx], extrude_speed, path.getExtrusionMM3perMM(), path.config.type, update_extrusion_offset); @@ -2278,6 +2366,7 @@ void LayerPlan::writeGCode(GCodeExport& gcode) gcode.setZ(std::round(z_ + layer_thickness_ * length / totalLength)); const double extrude_speed = speed * spiral_path.speed_back_pressure_factor; + last_speed = extrude_speed; communication->sendLineTo( spiral_path.config.type, spiral_path.points[point_idx], diff --git a/src/gcodeExport.cpp b/src/gcodeExport.cpp index 8a3cc11538..ef80a6ff4d 100644 --- a/src/gcodeExport.cpp +++ b/src/gcodeExport.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2023 UltiMaker // CuraEngine is released under the terms of the AGPLv3 or higher +// Modified by BigRep GmbH #include "gcodeExport.h" @@ -17,6 +18,7 @@ #include "Slice.h" #include "WipeScriptConfig.h" #include "communication/Communication.h" //To send layer view data. +#include "pathPlanning/ArcPathCalculator.h" #include "settings/types/LayerIndex.h" #include "utils/Date.h" #include "utils/string.h" // MMtoStream, PrecisionedDouble @@ -609,6 +611,9 @@ void GCodeExport::writeTypeComment(const PrintFeatureType& type) case PrintFeatureType::PrimeTower: *output_stream_ << ";TYPE:PRIME-TOWER" << new_line_; break; + case PrintFeatureType::MoveGradualZHop: + *output_stream_ << ";TYPE:GRADUAL-Z-HOP" << new_line_; + break; case PrintFeatureType::MoveCombing: case PrintFeatureType::MoveRetraction: case PrintFeatureType::NoneType: @@ -1139,6 +1144,35 @@ void GCodeExport::writeUnretractionAndPrime() } } +bool GCodeExport::handleRetractionLimitation(const RetractionConfig& config) +{ // handle retraction limitation + + ExtruderTrainAttributes& extruder_attributes = extruder_attr_[current_extruder_]; + const double current_extruded_volume = getCurrentExtrudedVolume(); + std::deque& extruded_volume_at_previous_n_retractions = extruder_attributes.extruded_volume_at_previous_n_retractions_; + while (extruded_volume_at_previous_n_retractions.size() > config.retraction_count_max && ! extruded_volume_at_previous_n_retractions.empty()) + { + // extruder switch could have introduced data which falls outside the retraction window + // also the retraction_count_max could have changed between the last retraction and this + extruded_volume_at_previous_n_retractions.pop_back(); + } + if (config.retraction_count_max <= 0) + { + return false; + } + if (extruded_volume_at_previous_n_retractions.size() == config.retraction_count_max + && current_extruded_volume < extruded_volume_at_previous_n_retractions.back() + config.retraction_extrusion_window * extruder_attributes.filament_area_) + { + return false; + } + extruded_volume_at_previous_n_retractions.push_front(current_extruded_volume); + if (extruded_volume_at_previous_n_retractions.size() == config.retraction_count_max + 1) + { + extruded_volume_at_previous_n_retractions.pop_back(); + } + return true; +} + void GCodeExport::writeRetraction(const RetractionConfig& config, bool force, bool extruder_switch) { ExtruderTrainAttributes& extr_attr = extruder_attr_[current_extruder_]; @@ -1164,29 +1198,10 @@ void GCodeExport::writeRetraction(const RetractionConfig& config, bool force, bo return; } - { // handle retraction limitation - double current_extruded_volume = getCurrentExtrudedVolume(); - std::deque& extruded_volume_at_previous_n_retractions = extr_attr.extruded_volume_at_previous_n_retractions_; - while (extruded_volume_at_previous_n_retractions.size() > config.retraction_count_max && ! extruded_volume_at_previous_n_retractions.empty()) - { - // extruder switch could have introduced data which falls outside the retraction window - // also the retraction_count_max could have changed between the last retraction and this - extruded_volume_at_previous_n_retractions.pop_back(); - } - if (! force && config.retraction_count_max <= 0) - { - return; - } - if (! force && extruded_volume_at_previous_n_retractions.size() == config.retraction_count_max - && current_extruded_volume < extruded_volume_at_previous_n_retractions.back() + config.retraction_extrusion_window * extr_attr.filament_area_) - { - return; - } - extruded_volume_at_previous_n_retractions.push_front(current_extruded_volume); - if (extruded_volume_at_previous_n_retractions.size() == config.retraction_count_max + 1) - { - extruded_volume_at_previous_n_retractions.pop_back(); - } + const bool perform_retraction = handleRetractionLimitation(config); + if (! force && ! perform_retraction) + { + return; } if (extr_attr.machine_firmware_retract_) @@ -1246,6 +1261,139 @@ void GCodeExport::writeZhopStart(const coord_t hop_height, Velocity speed /*= 0* } } +std::vector> GCodeExport::writeSpiralZhopStart( + const Point2LL previous_position, + const Point2LL target, + const coord_t hop_height, + const Velocity xy_speed, + const AABB3D& printer_bounding_box, + const RetractionConfig& config, + bool force_retraction /* = false */) +{ + if (hop_height <= 0) + { + return {}; + } + + const bool retraction_while_spiral = Application::getInstance().current_slice_->scene.settings.get("retract_during_spiral"); + const auto radius = Application::getInstance().current_slice_->scene.settings.get("spiral_radius"); + + const Point2LL current_position = getPositionXY(); + const ExtruderTrain& extruder = Application::getInstance().current_slice_->scene.extruders[current_extruder_]; + const auto z_speed = extruder.settings_.get("speed_z_hop"); + assert(z_speed > 0.0 && "Z hop speed should be positive."); + const coord_t step_size = Application::getInstance().current_slice_->scene.current_mesh_group->settings.get("minimal_segment_length") * 1000; + is_z_hopped_ = hop_height; + const ArcPath arc = ArcPath::calculate(previous_position, current_position, target, hop_height, radius, xy_speed, z_speed, step_size); + + if (! arc.isOutOfBounds(printer_bounding_box)) + { + const coord_t start_z = getPositionZ(); + std::vector> arc_points = arc.getDiscreteArc(start_z); + + // TODO: Do continuous arc movement using G2 / G3 commands (incl. retraction while spiral) + + // Basic parameter setup for the retraction during the spiral + // The first n_full_retraction_steps it will do a retraction where the retraction speed is the same as set in the UI. + // The retraction amount for each discrete arc segment is set in such way, so that the desired arc velocity and the desired retraction speed are compatible. + // Most likely these calculation will not result in an integer division without a rest there will be one segment, following the full retraction segments, where you need to + // retract a little rest. However, for this little rest, the retraction speed will not be equal to the one set in the UI. + ExtruderTrainAttributes& extruder_attributes = extruder_attr_[current_extruder_]; + const coord_t z_per_step = arc_points[0].first.z_ - start_z; // Move this into the for loop when using non linear z increase + const auto step_size_3d = static_cast(sqrt(step_size * step_size + z_per_step * z_per_step)); + const double new_retraction_e_amount = mmToE(config.distance); + const double retraction_diff_e_amount = extruder_attributes.retraction_e_amount_current_ - new_retraction_e_amount; + const double retraction_speed = ((retraction_diff_e_amount < 0.0) ? config.speed : extruder_attributes.last_retraction_prime_speed_); + double retraction_per_step = 0; + + size_t n_full_retraction_steps = 0; // for which you have a "correct" retraction speed as set in the UI + double remaining_retraction = 0; + const bool not_retraction = std::abs(retraction_diff_e_amount) < 0.000001 || (! force_retraction && ! handleRetractionLimitation(config)); + if (not_retraction) + { + // in case there should not be a retraction keep these at zero (only added to be explicit) + remaining_retraction = 0; + n_full_retraction_steps = 0; + } + else + { + const int retraction_sign = retraction_diff_e_amount / std::abs(retraction_diff_e_amount); + // units: retraction_per_step as e value, step_size_3d is in microns, retraction_speed in mm/sec, arc_points[0].second in mm/sec + retraction_per_step = mmToE( + retraction_sign * retraction_speed * (INT2MM(step_size_3d) / arc_points[0].second)); // For different / non linear velocity per step move this into the for loop + n_full_retraction_steps = floor(retraction_diff_e_amount / retraction_per_step); + // assert(theory_n_retraction_steps > 1 && "Retraction speed is faster then movement!"); + remaining_retraction = retraction_diff_e_amount - n_full_retraction_steps * retraction_per_step; + } + + writeTypeComment(PrintFeatureType::MoveGradualZHop); + for (size_t idx = 0; idx < arc_points.size(); ++idx) + { + const std::pair pt = arc_points[idx]; + if (retraction_while_spiral && idx < n_full_retraction_steps) + { + // spiral segments and retractions with full retraction speed + // TODO: BitsFromBytes and G10 (flavor REPETIER) retraction not implemented (not able to test those) + *output_stream_ << "G1"; + writeFXYZE(pt.second, pt.first.x_, pt.first.y_, pt.first.z_, current_e_value_ + retraction_per_step, PrintFeatureType::MoveGradualZHop); + } + else if (retraction_while_spiral && remaining_retraction != 0 && idx == n_full_retraction_steps) + { + // spiral segments and slower retraction (for remainder of the division or very fast or very small retractions) + *output_stream_ << "G1"; + writeFXYZE(pt.second, pt.first.x_, pt.first.y_, pt.first.z_, current_e_value_ + remaining_retraction, PrintFeatureType::MoveGradualZHop); + } + else + { + // spiral segments without retraction + *output_stream_ << "G0"; + writeFXYZE(pt.second, pt.first.x_, pt.first.y_, pt.first.z_, current_e_value_, PrintFeatureType::MoveGradualZHop); + } + } + + // if the spiral was not large enough to do the full retraction then do a normal retraction for the remaining retraction afterwards + if (arc_points.size() <= n_full_retraction_steps) + { + const double retraction_done = retraction_per_step * arc_points.size(); + remaining_retraction = retraction_diff_e_amount - retraction_done; + if (std::abs(remaining_retraction) >= 0.000001) + { + // just like in the standard retraction + current_e_value_ += remaining_retraction; + const double output_e = (relative_extrusion_) ? retraction_diff_e_amount : current_e_value_; + *output_stream_ << "G1 F" << PrecisionedDouble{ 1, retraction_speed * 60 } << " " << extruder_attributes.extruder_character_ << PrecisionedDouble{ 5, output_e } + << new_line_; + + current_speed_ = retraction_speed; + estimate_calculator_.plan( + TimeEstimateCalculator::Position(INT2MM(current_position_.x_), INT2MM(current_position_.y_), INT2MM(current_position_.z_), eToMm(current_e_value_)), + current_speed_, + PrintFeatureType::MoveRetraction); + } + } + + // required for doing the un-retract (e.g. in the beginning of the next extrusion) + if (retraction_while_spiral) + { + extruder_attributes.last_retraction_prime_speed_ = config.primeSpeed; + extruder_attributes.retraction_e_amount_current_ + = new_retraction_e_amount; // suppose that for UM2 the retraction amount in the firmware is equal to the provided amount + extruder_attributes.prime_volume_ += config.prime_volume; + } + + return arc_points; + } + + // if the spiral would cross the printer bounds do a vertical z-hop + // do not forget to do the retraction, when it was supposed to be done during the spiral + if (retraction_while_spiral) + { + writeRetraction(config); + } + writeZhopStart(hop_height); + return std::vector>{}; +} + void GCodeExport::writeZhopEnd(Velocity speed /*= 0*/) { if (is_z_hopped_) diff --git a/src/pathPlanning/ArcPathCalculator.cpp b/src/pathPlanning/ArcPathCalculator.cpp new file mode 100644 index 0000000000..e04c59dec6 --- /dev/null +++ b/src/pathPlanning/ArcPathCalculator.cpp @@ -0,0 +1,230 @@ +// Copyright (c) 2023 BigRep GmbH +// Released under the terms of the AGPLv3 or higher. +// Author(s): Nadine Mikkelsen, Matthew McMillan (plaintoothpaste) + +#include "pathPlanning/ArcPathCalculator.h" + +namespace cura +{ + +ArcPath ArcPath::calculate( + const Point2LL previous, + const Point2LL current, + const Point2LL target, + const coord_t hop_height, + const coord_t radius, + const Velocity xy_speed, + const Velocity z_speed_limit, + const coord_t discretization_step_size) +{ + assert(radius != 0 && "Arc radius should be larger then zero."); + const Point2LL last_direction = current - previous; + const Point2LL center_vec = normal(last_direction, radius); + auto orthogonal_last_vec = Point2LL{ -last_direction.Y, last_direction.X }; + orthogonal_last_vec + = normal(orthogonal_last_vec, normalization_value); // normalization to length 1 results in (0,0) since it is a ClipperLib::IntPoint, normalize to 1 mm hence 1000 um + const Point2LL target_previous_vec = normal(target - previous, normalization_value); + // When calculating the dot product of vectors which are not normalized keep in mind, that it involves squaring the magnitude of the dot product + // Check if it is parallel and inverse direction + const bool inverse_direction = dot(normal(last_direction, normalization_value), normal(target - current, normalization_value)) == -(normalization_value * normalization_value); + const float dot_product = dot(orthogonal_last_vec, target_previous_vec) / static_cast(normalization_value * normalization_value); + + // Step 1: Determine the nature of the underlying circle: + // On which side of the previous movement the circle should be located and + // in which direction should we travel the arc (clockwise?) and + // which (of two possible) tangents should be used as exit point from the arc + std::function tangent_func; + bool clockwise_rotation; + Point2LL circle_center; + if ((dot_product == 0) && ! inverse_direction) // colinear edge case + { + clockwise_rotation = true; + circle_center = Point2LL{ center_vec.Y, -center_vec.X }; + } + else if (dot_product > 0) // circle is on the left side + { + clockwise_rotation = false; + circle_center = Point2LL{ -center_vec.Y, center_vec.X }; + tangent_func = [](coord_t a, coord_t b) -> coord_t + { + return a - b; + }; + } + else // circle is on the right side + { + clockwise_rotation = true; + circle_center = Point2LL{ center_vec.Y, -center_vec.X }; + tangent_func = [](coord_t a, coord_t b) -> coord_t + { + return a + b; + }; + } + circle_center = circle_center + current; + + // Step 2: Calculate the tangent point at which we should leave the arc + const coord_t dist_to_target = vSize(circle_center - target); + Point2LL arc_end; + if (dist_to_target == radius) // edge case: the target position is located exactly on the circle + { + arc_end = target; + } + else if (((dot_product == 0) && ! inverse_direction) || dist_to_target < radius) // edge case: points are colinear or the target point is inside the circle + { + arc_end = current; + } + else + { + const Point2LL centered_target = target - circle_center; + const coord_t tangent_x = calcTangentX(radius, dist_to_target, centered_target.X, centered_target.Y, tangent_func); + const coord_t tangent_y = calcTangentY(radius, dist_to_target, centered_target.X, centered_target.Y, tangent_func); + arc_end = Point2LL{ tangent_x, tangent_y } + circle_center; + } + + // Step 3: Determine if the arc is sufficient to raise the print head in z-direction without exceeding the z-speed threshold + // Add spiral turns when the z-speed would be too high + // Since in many cases the arc will be linearly approximated for or by the printer board, this calculation is done by assuming linear discretization steps + int n_discrete_steps = calcNumberOfSteps(current, arc_end, radius, discretization_step_size, circle_center, clockwise_rotation); + float z_height_per_segment = static_cast(hop_height) / n_discrete_steps; + const int n_steps_full_turn = calcNumberOfSteps(current, current, radius, discretization_step_size, circle_center, clockwise_rotation); + int n_turns = 1; + auto z_speed = Velocity{ 0 }; + while (true) + { + // Velocity is always defined in mm/sec and the distance in um (micro meter), but the unit conversion here is mathematically redundant + z_speed = (z_height_per_segment * (xy_speed /* *1000 */ / discretization_step_size)) /* /1000 */; + if (z_speed <= z_speed_limit) // z speed is not too high + { + break; + } + ++n_turns; + n_discrete_steps += n_steps_full_turn; + z_height_per_segment = static_cast(hop_height) / n_discrete_steps; + } + + return ArcPath(current, arc_end, radius, circle_center, hop_height, xy_speed, z_speed, discretization_step_size, n_discrete_steps, clockwise_rotation, n_turns); +} + +bool ArcPath::isOutOfBounds(const AABB3D& bounding_box) const +{ + return circle_center_.Y - radius_ <= bounding_box.min_.y_ || circle_center_.X - radius_ <= bounding_box.min_.x_ || circle_center_.Y + radius_ >= bounding_box.max_.y_ + || circle_center_.X + radius_ >= bounding_box.max_.x_; +} + +std::vector> ArcPath::getDiscreteArc(const coord_t z_start) const +{ + const coord_t z_end = z_start + z_increase_; + assert(z_end >= z_start && "The start height of a z-hop needs to be less then the end height."); + // Calculate the angle difference on the circle between two different steps of the discretization + const Point2LL centered_start = normal(start_ - circle_center_, normalization_value); + const float angle_to_start + = convertToLimitedAngle(std::atan2(centered_start.Y / static_cast(normalization_value), centered_start.X / static_cast(normalization_value))); + const float arc_angle = calcArcAngle(start_, end_, circle_center_, is_clockwise_); + const float angle_per_step = (arc_angle + (n_turns_ - 1) * 2 * std::numbers::pi) / n_discrete_steps_; + const float height_per_step = static_cast(z_end - z_start) / n_discrete_steps_; + const Velocity vel = sqrt(xy_speed_ * xy_speed_ + z_speed_ * z_speed_); + + // This vector is not including the current position, which is the start point of the arc + std::vector> arc_points{}; + arc_points.reserve(n_discrete_steps_); + // For each discretization rotate the starting position by an increasing angle and keep a constant velocity for each step + for (size_t i = 1; i < n_discrete_steps_; ++i) + { + const auto angle = is_clockwise_ ? convertToLimitedAngle(angle_to_start - i * angle_per_step) : convertToLimitedAngle(angle_to_start + i * angle_per_step); + auto pt = Point3LL{ static_cast(cos(angle) * radius_ + circle_center_.X), + static_cast(sin(angle) * radius_ + circle_center_.Y), + static_cast(z_start + i * height_per_step) }; + arc_points.emplace_back(pt, vel); + } + arc_points.emplace_back(Point3LL{ end_.X, end_.Y, z_end }, vel); + return arc_points; +} + +ArcPath::ArcPath( + const Point2LL start, + const Point2LL end, + const coord_t radius, + const Point2LL circle_center, + const coord_t z_increase, + const Velocity xy_speed, + const Velocity z_speed, + const coord_t discretization_step_size, + const coord_t n_discrete_steps, + const bool is_clockwise, + const int n_turns) + : start_(start) + , end_(end) + , radius_(radius) + , circle_center_(circle_center) + , z_increase_(z_increase) + , xy_speed_(xy_speed) + , z_speed_(z_speed) + , discretization_step_size_(discretization_step_size) + , n_discrete_steps_(n_discrete_steps) + , is_clockwise_(is_clockwise) + , n_turns_(n_turns) +{ +} + +float ArcPath::convertToLimitedAngle(const float angle) +{ + float properAngle = angle; + while (properAngle < 0) + { + properAngle = 2 * std::numbers::pi - abs(properAngle); + } + while (properAngle > 2 * static_cast(std::numbers::pi)) + { + properAngle = properAngle - 2 * std::numbers::pi; + } + return properAngle; +} + + +coord_t ArcPath::calcTangentX(const coord_t r, const coord_t d, const coord_t x, const coord_t y, const std::function func) +{ + // for the tangent calculation see: https://en.wikipedia.org/wiki/Tangent_lines_to_circles + const float d_2 = d * d; + const float r_2 = r * r; + return func(x * (r_2 / d_2), (r / d_2) * sqrt(d_2 - r_2) * (-y)); +} + +coord_t ArcPath::calcTangentY(const coord_t r, const coord_t d, const coord_t x, const coord_t y, const std::function func) +{ + // for the tangent calculation see: https://en.wikipedia.org/wiki/Tangent_lines_to_circles + const float d_2 = d * d; + const float r_2 = r * r; + return func(y * (r_2 / d_2), ((r * x) / d_2) * sqrt(d_2 - r_2)); +} + +float ArcPath::calcArcAngle(const Point2LL arc_start, const Point2LL arc_end, const Point2LL circle_center, const bool is_clockwise) +{ + // if the arc start and end point coincide it should be full circle instead of a zero length arc + if (arc_start == arc_end) + { + return 2.0 * std::numbers::pi; + } + const Point2LL centered_start = normal(arc_start - circle_center, normalization_value); + const Point2LL centered_end = normal(arc_end - circle_center, normalization_value); + // atan2 range is normally in the interval of [-pi,pi] for further calculations shift the range to [0,2*pi] to always calculate with positive angles + const float angle_to_start + = convertToLimitedAngle(std::atan2(centered_start.Y / static_cast(normalization_value), centered_start.X / static_cast(normalization_value))); + const float angle_to_end + = convertToLimitedAngle(std::atan2(centered_end.Y / static_cast(normalization_value), centered_end.X / static_cast(normalization_value))); + return convertToLimitedAngle(is_clockwise ? angle_to_start - angle_to_end : angle_to_end - angle_to_start); +} + +int ArcPath::calcNumberOfSteps( + const Point2LL arc_start, + const Point2LL arc_end, + const coord_t radius, + const coord_t step_size, + const Point2LL circle_center, + const bool is_clockwise) +{ + const float arc_angle = calcArcAngle(arc_start, arc_end, circle_center, is_clockwise); + const float step_angle = std::acos(1 - pow(step_size, 2) / (2 * pow(radius, 2))); + const int n_steps = floor(arc_angle / step_angle); + return n_steps > 0 ? n_steps : 1; +} + +} // namespace cura \ No newline at end of file diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 1e36893cee..41f61bbbec 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,5 +1,6 @@ # Copyright (c) 2022 Ultimaker B.V. # CuraEngine is released under the terms of the AGPLv3 or higher. +# Modified by BigRep GmbH message(STATUS "Building tests...") include(GoogleTest) @@ -40,6 +41,10 @@ set(TESTS_SRC_UTILS UnionFindTest ) +set(TESTS_SRC_PATHPLANNING + ArcPathCalculatorTest + ) + foreach (test ${TESTS_SRC_BASE}) add_executable(${test} main.cpp ${test}.cpp) add_test(NAME ${test} COMMAND "${test}" WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}") @@ -70,3 +75,9 @@ foreach (test ${TESTS_SRC_UTILS}) add_test(NAME ${test} COMMAND "${test}" WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}") target_link_libraries(${test} PRIVATE _CuraEngine test_helpers GTest::gtest GTest::gmock clipper::clipper) endforeach () + +foreach (test ${TESTS_SRC_PATHPLANNING}) + add_executable(${test} main.cpp pathPlanning/${test}.cpp) + add_test(NAME ${test} COMMAND "${test}" WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}") + target_link_libraries(${test} PRIVATE _CuraEngine test_helpers GTest::gtest GTest::gmock clipper::clipper) +endforeach () diff --git a/tests/pathPlanning/ArcPathCalculatorTest.cpp b/tests/pathPlanning/ArcPathCalculatorTest.cpp new file mode 100644 index 0000000000..4820a9c680 --- /dev/null +++ b/tests/pathPlanning/ArcPathCalculatorTest.cpp @@ -0,0 +1,494 @@ +// Copyright (c) 2023 BigRep GmbH +#include "pathPlanning/ArcPathCalculator.h" + +#include + +namespace cura +{ +class ArcPathCalculation_GeneralCreationTest : public testing::Test +{ +public: + const Point2LL previous_position = { 0, 0 }; + const Point2LL current_position = { 80'000, 80'000 }; + const Point2LL target_position = { 10'000, 10'000 }; + const coord_t hop_height = 1'000; + const coord_t radius = 10'000; + const Velocity xy_speed = Velocity{ 1'000 }; + const Velocity z_speed = Velocity{ 500 }; + const coord_t step_size = 1; + const coord_t n_discretization_steps = 10; + const coord_t error_margin = 1; +}; + +TEST_F(ArcPathCalculation_GeneralCreationTest, CenterCorrectDistance) +{ + const ArcPath arc = ArcPath::calculate(previous_position, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(current_position - arc.end_) - 2 * radius, error_margin); +} + + +TEST_F(ArcPathCalculation_GeneralCreationTest, CorrectTangentPoint_PositiveValues) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X + radius, current_position.Y + 2 * radius }; + const auto tangent_point = Point2LL{ current_position.X + radius, current_position.Y + radius }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LE(vSize(arc.end_ - tangent_point), error_margin); + EXPECT_FALSE(arc.is_clockwise_); +} + +TEST_F(ArcPathCalculation_GeneralCreationTest, CorrectTangentPoint_FromPositiveToNegativeValues) +{ + const auto current_position = Point2LL{ 20, 10 }; + const auto target_position = Point2LL{ current_position.X + radius, current_position.Y - 2 * radius }; + const auto tangent_point = Point2LL{ current_position.X + radius, current_position.Y - radius }; + const ArcPath arc = ArcPath::calculate({ 10, 10 }, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LE(vSize(arc.end_ - tangent_point), error_margin); + EXPECT_TRUE(arc.is_clockwise_); +} + +TEST_F(ArcPathCalculation_GeneralCreationTest, CorrectTangentPoint_FromNegativeToPositiveValues) +{ + const auto current_position = Point2LL{ -10, -10 }; + const auto target_position = Point2LL{ current_position.X + radius, current_position.Y + 2 * radius }; + const auto tangent_point = Point2LL{ current_position.X + radius, current_position.Y + radius }; + const ArcPath arc = ArcPath::calculate({ -20, -10 }, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LE(vSize(arc.end_ - tangent_point), error_margin); + EXPECT_FALSE(arc.is_clockwise_); +} + +TEST_F(ArcPathCalculation_GeneralCreationTest, CorrectTangentPoint_OnlyNegativeValues) +{ + const auto current_position = Point2LL{ -20'000, -10'000 }; + const auto target_position = Point2LL{ current_position.X - radius, current_position.Y - 2 * radius }; + const auto tangent_point = Point2LL{ current_position.X - radius, current_position.Y - radius }; + const ArcPath arc = ArcPath::calculate({ -10'000, -10'000 }, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LE(vSize(arc.end_ - tangent_point), error_margin); + EXPECT_FALSE(arc.is_clockwise_); +} + + +TEST_F(ArcPathCalculation_GeneralCreationTest, DirectionTest_VerticalClockwise) +{ + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, { 10'000, 20'000 }, { 15'000, 20'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_TRUE(arc.is_clockwise_); +} + +TEST_F(ArcPathCalculation_GeneralCreationTest, DirectionTest_VerticalCounterClockwise) +{ + const ArcPath arc = ArcPath::calculate({ 15'000, 10'000 }, { 15'000, 20'000 }, { 10'000, 20'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_FALSE(arc.is_clockwise_); +} + +TEST_F(ArcPathCalculation_GeneralCreationTest, DirectionTest_HorizontalClockwise) +{ + const ArcPath arc = ArcPath::calculate({ 10'000, 15'000 }, { 20'000, 15'000 }, { 20'000, 10'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_TRUE(arc.is_clockwise_); +} + +TEST_F(ArcPathCalculation_GeneralCreationTest, DirectionTest_HorizontalCounterClockwise) +{ + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, { 20'000, 10'000 }, { 20'000, 15'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_FALSE(arc.is_clockwise_); +} + +TEST_F(ArcPathCalculation_GeneralCreationTest, DirectionTest_DiagonalClockwise) +{ + const ArcPath arc = ArcPath::calculate({ 1000, 1000 }, { 2000, 2000 }, { 2000, 1000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_TRUE(arc.is_clockwise_); +} + +TEST_F(ArcPathCalculation_GeneralCreationTest, DirectionTest_DiagonalCounterClockwise) +{ + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, { 20'000, 20'000 }, { 10'000, 20'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_FALSE(arc.is_clockwise_); +} + +TEST_F(ArcPathCalculation_GeneralCreationTest, TunaroundTest_SingleTurn) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X + radius, current_position.Y + 2 * radius }; + const double arc_length = (2 * std::numbers::pi * radius) / 4; + const Velocity high_z_speed = 1.2 * hop_height / (arc_length / xy_speed); + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, radius, xy_speed, high_z_speed, step_size); + EXPECT_EQ(arc.n_turns_, 1); +} + +TEST_F(ArcPathCalculation_GeneralCreationTest, TunaroundTest_TwoTurnsLowSpeed) +{ + const auto current_position = Point2LL{ 20'000, 20'000 }; + const auto target_position = Point2LL{ current_position.X + radius, current_position.Y + 2 * radius }; + const double circumference = 2 * std::numbers::pi * radius; + const double arc_length = circumference / 4.0; + const Velocity low_z_speed = 0.5 * hop_height / (arc_length / xy_speed); + const double minimal_arc_length = xy_speed * (hop_height / low_z_speed); + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, radius, xy_speed, low_z_speed, step_size); + EXPECT_GE(arc.n_turns_, minimal_arc_length / circumference); +} + + +class ArcPathCalculation_EdgeCaseTest : public testing::Test +{ +public: + const Point2LL previous_position = { 10'000, 10'000 }; + const Point2LL current_position = { 10'000, 20'000 }; + const Point2LL target_position = { 15'000, 20'000 }; + const coord_t hop_height = 1'000; + const coord_t radius = 10'000; + const Velocity xy_speed = Velocity{ 100 }; + const Velocity z_speed = Velocity{ 50 }; + const coord_t step_size = 1; + const AABB3D bounding_box = AABB3D({ 0, 0, 0 }, { 100'000, 100'000, 100'000 }); + const coord_t error_margin = 1; +}; + + +TEST_F(ArcPathCalculation_EdgeCaseTest, ZeroHopHeightTest) +{ + const ArcPath arc = ArcPath::calculate(previous_position, current_position, target_position, 0, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(current_position - arc.end_) - 2 * radius, error_margin); + EXPECT_TRUE(arc.is_clockwise_); +} + + +TEST_F(ArcPathCalculation_EdgeCaseTest, ZeroRadiusTest) +{ + ASSERT_DEATH(ArcPath::calculate(previous_position, current_position, target_position, hop_height, 0, xy_speed, z_speed, step_size), "Arc radius should be larger then zero."); +} + + +TEST_F(ArcPathCalculation_EdgeCaseTest, BoundingBoxTest_IsInside) +{ + const ArcPath arc = ArcPath::calculate({ 0, 0 }, { 50'000, 50'000 }, { 0, 0 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_FALSE(arc.isOutOfBounds(bounding_box)) << "Failed when testing travel moves and arc are valid and inside the printer bounds."; +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, BoundingBoxTest_IsOutsideMinY) +{ + const ArcPath arc = ArcPath::calculate({ 50'000, 50'000 }, { 50'000, 0 }, { 70'000, 50'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_TRUE(arc.isOutOfBounds(bounding_box)); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, BoundingBoxTest_IsOutsideMaxY) +{ + const ArcPath arc = ArcPath::calculate({ 50'000, 5000 }, { 50'000, 100'000 }, { 70'000, 50'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_TRUE(arc.isOutOfBounds(bounding_box)); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, BoundingBoxTest_IsOutsideMinX) +{ + const ArcPath arc = ArcPath::calculate({ 50'000, 50'000 }, { 0, 50'000 }, { 50'000, 70'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_TRUE(arc.isOutOfBounds(bounding_box)); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, BoundingBoxTest_IsOutsideMaxX) +{ + const ArcPath arc = ArcPath::calculate({ 50'000, 5000 }, { 100'000, 50'000 }, { 50'000, 70'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_TRUE(arc.isOutOfBounds(bounding_box)); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, BoundingBoxTest_DirectlyOnBounds) +{ + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, { 10'000, 100 }, { 11'000, 10'000 }, hop_height, 100, xy_speed, z_speed, step_size); + EXPECT_TRUE(arc.isOutOfBounds(bounding_box)); +} + + +TEST_F(ArcPathCalculation_EdgeCaseTest, ShortTravelTest_TargetPointOutsideCircle) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X, current_position.Y + 3 * radius }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_TRUE((arc.end_ != current_position) && (arc.end_ != target_position)) << "Failed when testing the end point for a travel move which is longer then the circle diameter."; +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ShortTravelTest_TargetPointOnCircle) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X, current_position.Y + 2 * radius }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(arc.end_ - target_position), error_margin) << "Failed when testing the end point for a travel move which is the exact same length as the circle diameter."; +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ShortTravelTest_TargetPointInsideCircle) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X, current_position.Y + 50 }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(arc.end_ - current_position), error_margin) << "Failed when testing the end point for a travel move which is short then the circle diameter."; +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ShortTravelTest_TargetPointIsCurrentPosition) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, current_position, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(arc.end_ - current_position), error_margin); + EXPECT_GE(arc.n_turns_, 1); +} + + +TEST_F(ArcPathCalculation_EdgeCaseTest, ColinearLineTest_LeftToRightLine) +{ + const auto current_position = Point2LL{ 15'000, 10'000 }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, { 30'000, 10'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(arc.end_ - current_position), error_margin); + EXPECT_TRUE(arc.is_clockwise_); + EXPECT_NEAR(arc.circle_center_.X, current_position.X, error_margin); + EXPECT_NEAR(arc.circle_center_.Y, current_position.Y - radius, error_margin); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ColinearLineTest_LeftToRightLineCurrentIsCenter) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, { 30'000, 10'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(arc.end_ - current_position), error_margin); + EXPECT_TRUE(arc.is_clockwise_); + EXPECT_NEAR(arc.circle_center_.X, current_position.X, error_margin); + EXPECT_NEAR(arc.circle_center_.Y, current_position.Y - radius, error_margin); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ColinearLineTest_RightToLeftLine) +{ + const auto current_position = Point2LL{ 15'000, 10'000 }; + const ArcPath arc = ArcPath::calculate({ 30'000, 10'000 }, current_position, { 10'000, 10'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(arc.end_ - current_position), error_margin); + EXPECT_TRUE(arc.is_clockwise_); + EXPECT_NEAR(arc.circle_center_.X, current_position.X, error_margin); + EXPECT_NEAR(arc.circle_center_.Y, current_position.Y + radius, error_margin); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ColinearLineTest_OppositeDirections) +{ + const auto current_position = Point2LL{ 10'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X + radius, current_position.Y }; + const auto previous_position = Point2LL{ current_position.X + radius / 3, current_position.Y }; + const ArcPath arc = ArcPath::calculate(previous_position, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + const auto target_tangent = Point2LL{ arc.circle_center_.X + radius, arc.circle_center_.Y }; + const auto target_circle_center = Point2LL{ current_position.X, current_position.Y + radius }; + EXPECT_LE(vSize(arc.circle_center_ - target_circle_center), error_margin); + EXPECT_LE(vSize(arc.end_ - target_tangent), error_margin); + EXPECT_TRUE(arc.is_clockwise_); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ColinearLineTest_BottomToTopLine) +{ + const auto current_position = Point2LL{ 10'000, 15'000 }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, { 10'000, 30'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(arc.end_ - current_position), error_margin); + EXPECT_TRUE(arc.is_clockwise_); + EXPECT_NEAR(arc.circle_center_.Y, current_position.Y, error_margin); + EXPECT_NEAR(arc.circle_center_.X, current_position.X + radius, error_margin); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ColinearLineTest_TopToBottomLine) +{ + const auto current_position = Point2LL{ 10'000, 15'000 }; + const ArcPath arc = ArcPath::calculate({ 10'000, 30'000 }, current_position, { 10'000, 10'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(arc.end_ - current_position), error_margin); + EXPECT_TRUE(arc.is_clockwise_); + EXPECT_NEAR(arc.circle_center_.Y, current_position.Y, error_margin); + EXPECT_NEAR(arc.circle_center_.X, current_position.X - radius, error_margin); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ColinearLineTest_DiagonalRisingLineLeftToRight) +{ + const auto current_position = Point2LL{ 15'000, 15'000 }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, { 30'000, 30'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(arc.end_ - current_position), error_margin); + EXPECT_TRUE(arc.is_clockwise_); + const coord_t xy_offset = radius * std::sin(std::numbers::pi / 4); + EXPECT_NEAR(arc.circle_center_.Y, current_position.Y - xy_offset, error_margin); + EXPECT_NEAR(arc.circle_center_.X, current_position.X + xy_offset, error_margin); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ColinearLineTest_DiagonalRisingLineRightToLeft) +{ + const auto current_position = Point2LL{ 15'000, 15'000 }; + const ArcPath arc = ArcPath::calculate({ 30'000, 30'000 }, current_position, { 10'000, 10'000 }, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(arc.end_ - current_position), error_margin); + EXPECT_TRUE(arc.is_clockwise_); + const coord_t xy_offset = radius * std::sin(std::numbers::pi / 4); + EXPECT_NEAR(arc.circle_center_.Y, current_position.Y + xy_offset, error_margin); + EXPECT_NEAR(arc.circle_center_.X, current_position.X - xy_offset, error_margin); +} + + +TEST_F(ArcPathCalculation_EdgeCaseTest, ColinearAndHorizontalAndInRadiusDistance) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X + 2 * radius, current_position.Y }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + EXPECT_LT(vSize(arc.end_ - current_position), error_margin); + EXPECT_TRUE(arc.is_clockwise_); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ColinearAndZeroRadiusTest) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X + 2 * radius, current_position.Y }; + ASSERT_DEATH(ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, 0, xy_speed, z_speed, step_size), "Arc radius should be larger then zero."); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, ColinearAndOutOfBoundingBox) +{ + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, { 10'000, 10 }, { 10'000, 5 }, hop_height, 100, xy_speed, z_speed, step_size); + EXPECT_TRUE(arc.isOutOfBounds(bounding_box)); +} + +TEST_F(ArcPathCalculation_EdgeCaseTest, OutOfBoundingBoxAndInCicleRadius) +{ + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, { 10'000, 100 }, { 11'000, 150 }, hop_height, 200, xy_speed, z_speed, step_size); + EXPECT_TRUE(arc.isOutOfBounds(bounding_box)); +} + +class ArcPathCalculation_DiscretizationTest : public testing::Test +{ +public: + const Point2LL previous_position = { 0, 0 }; + const Point2LL current_position = { 80'000, 80'000 }; + const Point2LL target_position = { 10'000, 10'000 }; + const coord_t z_height = 10; + const coord_t hop_height = 10; + const coord_t radius = 1'000; + const Velocity xy_speed = Velocity{ 100 }; + const Velocity z_speed = Velocity{ 50 }; + const coord_t step_size = 100; + const coord_t error_margin = 1; +}; + +TEST_F(ArcPathCalculation_DiscretizationTest, CorrectRadius) +{ + const ArcPath arc = ArcPath::calculate(previous_position, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + const std::vector> arc_points = arc.getDiscreteArc(z_height); + for (size_t idx = 0; idx < arc_points.size(); ++idx) + { + EXPECT_LE(vSize(Point2LL{ arc_points[idx].first.x_, arc_points[idx].first.y_ } - arc.circle_center_) - radius, error_margin) + << "The " << idx << "th point in the arc had a wrong distance to the origin"; + } +} + +TEST_F(ArcPathCalculation_DiscretizationTest, CorrectOrdering) +{ + const coord_t step_size = 100; + const ArcPath arc = ArcPath::calculate(previous_position, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + const std::vector> arc_points = arc.getDiscreteArc(z_height); + double last_distance = -error_margin; + bool sign_switch = false; + for (auto& pt : arc_points) + { + coord_t distance_to_start = vSize(current_position - Point2LL{ pt.first.x_, pt.first.y_ }); + if (! sign_switch && distance_to_start <= last_distance) + { + last_distance += error_margin; + sign_switch = true; + } + if (! sign_switch) + { + EXPECT_GT(distance_to_start, last_distance); + last_distance = distance_to_start - error_margin; + } + else + { + EXPECT_LT(distance_to_start, last_distance); + last_distance = distance_to_start + error_margin; + } + } +} + +TEST_F(ArcPathCalculation_DiscretizationTest, ZeroHopHeightTest) +{ + const ArcPath arc = ArcPath::calculate(previous_position, current_position, target_position, 0, radius, xy_speed, z_speed, step_size); + std::vector> arc_points = arc.getDiscreteArc(z_height); + + for (auto& pt : arc_points) + { + EXPECT_NEAR(pt.first.z_, z_height, error_margin); + } +} + + +TEST_F(ArcPathCalculation_DiscretizationTest, ShortTravelTest_TargetPointOnCircle) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X, current_position.Y + 2 * radius }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + std::vector> arc_points = arc.getDiscreteArc(z_height); + EXPECT_NEAR(vSize(Point2LL{ arc_points[0].first.x_, arc_points[0].first.y_ } - current_position), step_size, error_margin) + << "Fails when checking that the first point should be step_size far away from the current position."; + EXPECT_LT(vSize(Point2LL{ arc_points.back().first.x_, arc_points.back().first.y_ } - target_position), error_margin) + << "Fails when checking that the last point should be the target position."; + EXPECT_GE(arc_points.size(), 2) << "Fails when checking that there should be at 3 points in the discritzed arc."; +} + +TEST_F(ArcPathCalculation_DiscretizationTest, ShortTravelTest_TargetPointInsideCircle) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X, current_position.Y + radius }; + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, radius, xy_speed, z_speed, step_size); + std::vector> arc_points = arc.getDiscreteArc(z_height); + EXPECT_NEAR(vSize(Point2LL{ arc_points[0].first.x_, arc_points[0].first.y_ } - current_position), step_size, error_margin) + << "Fails when checking that the first point should be step_size far away from the current position."; + EXPECT_LT(vSize(Point2LL{ arc_points.back().first.x_, arc_points.back().first.y_ } - current_position), error_margin) + << "Fails when checking that the last point should be the current position."; + EXPECT_GE(arc_points.size(), 2) << "Fails when checking that there should be at 3 points in the discritzed arc."; +} + + +TEST_F(ArcPathCalculation_DiscretizationTest, TravelendsCloserThanStepSize) +{ + const coord_t low_hop_height = 1; + const auto current_position = Point2LL{ 10'000, 20'000 }; + const auto target_position = Point2LL{ current_position.X + radius, current_position.Y + radius + 2 }; + const ArcPath arc = ArcPath::calculate( + { 10'000, 10'000 }, + current_position, + target_position, + low_hop_height, + radius, + xy_speed, + Velocity{ 1000 }, + 2 * vSize(target_position - current_position)); + std::vector> arc_points = arc.getDiscreteArc(z_height); + EXPECT_EQ(arc_points.size(), 1); + EXPECT_LT(vSize(Point2LL{ arc_points[0].first.x_, arc_points[0].first.y_ } - arc.end_), error_margin); +} + + +TEST_F(ArcPathCalculation_DiscretizationTest, SpeedCalculationTest_SingleArc) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X + radius, current_position.Y + 2 * radius }; + const double arc_length = (2 * std::numbers::pi * radius) / 4; + const coord_t total_distance = arc_length + hop_height; + const Velocity high_z_speed = 1.2 * hop_height / (arc_length / xy_speed); + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, radius, xy_speed, high_z_speed, step_size); + const std::vector> arc_points = arc.getDiscreteArc(z_height); + + for (auto& pt : arc_points) + { + EXPECT_EQ(arc_points[0].second, pt.second) << "The speed does not stay constant over the arc."; + } + EXPECT_NEAR(arc_points[0].second * (arc_length / total_distance), xy_speed, error_margin); + EXPECT_LE(arc_points[0].second * (hop_height / total_distance), high_z_speed, error_margin); +} + +TEST_F(ArcPathCalculation_DiscretizationTest, SpeedCalculationTest_MultipleTurnArounds) +{ + const auto current_position = Point2LL{ 20'000, 10'000 }; + const auto target_position = Point2LL{ current_position.X + radius, current_position.Y + 2 * radius }; + const double circumference = 2 * std::numbers::pi * radius; + const double arc_length = circumference / 4; + const Velocity low_z_speed = 0.5 * hop_height / (arc_length / xy_speed); + const ArcPath arc = ArcPath::calculate({ 10'000, 10'000 }, current_position, target_position, hop_height, radius, xy_speed, low_z_speed, step_size); + const std::vector> arc_points = arc.getDiscreteArc(z_height); + const coord_t total_distance = arc.n_turns_ * circumference + arc_length + hop_height; + + for (auto& pt : arc_points) + { + EXPECT_EQ(arc_points[0].second, pt.second) << "The speed does not stay constant over the arc."; + } + EXPECT_NEAR(arc_points[0].second * ((arc.n_turns_ * circumference + arc_length) / total_distance), xy_speed, error_margin); + EXPECT_LE(arc_points[0].second * (hop_height / total_distance), low_z_speed, error_margin); +} + +} // namespace cura \ No newline at end of file