Resolves #22, #23, #24, #25 and #26.

6 files changed, 112 insertions, 49 deletions

diff --git a/ArcWelder/arc_welder.cpp b/ArcWelder/arc_welder.cpp
index d7b6120..710c3cd 100644
--- a/ArcWelder/arc_welder.cpp
+++ b/ArcWelder/arc_welder.cpp
@@ -203,8 +203,8 @@ arc_welder_results results;
 		<< ", g90_91_influences_extruder: " << (p_source_position_->get_g90_91_influences_extruder() ? "True" : "False")
 		<< ", allow_3d_arcs: " << (allow_3d_arcs_ ? "True" : "False")
 		<< ", allow_dynamic_precision: " << (allow_dynamic_precision_ ? "True" : "False")
-		<< ", default_xyz_precision: " << std::setprecision(0) << (current_arc_.get_xyz_precision())
-		<< ", default_e_precision: " << std::setprecision(0) << (current_arc_.get_e_precision());
+		<< ", default_xyz_precision: " << std::setprecision(0) << static_cast<double>(current_arc_.get_xyz_precision())
+		<< ", default_e_precision: " << std::setprecision(0) << static_cast<double>(current_arc_.get_e_precision());
 	p_logger_->log(logger_type_, INFO, stream.str());
 
 
diff --git a/ArcWelder/arc_welder.h b/ArcWelder/arc_welder.h
index 9e6d74a..4e86426 100644
--- a/ArcWelder/arc_welder.h
+++ b/ArcWelder/arc_welder.h
@@ -37,6 +37,8 @@
 #include "unwritten_command.h"
 #include "logger.h"
 #include <cmath>
+#include <iomanip>
+#include <sstream>
 
 #ifdef _MSC_VER
 #define _CRT_SECURE_NO_WARNINGS
diff --git a/ArcWelder/segmented_arc.cpp b/ArcWelder/segmented_arc.cpp
index 0618917..fe212ff 100644
--- a/ArcWelder/segmented_arc.cpp
+++ b/ArcWelder/segmented_arc.cpp
@@ -27,7 +27,8 @@
 #include "utilities.h"
 #include "segmented_shape.h"
 #include <iostream>
-#include <iomanip>
+//#include <iomanip>
+//#include <sstream>
 #include <stdio.h>
 #include <cmath>
 
@@ -213,7 +214,7 @@ bool segmented_arc::try_add_point_internal_(point p, double pd)
   double previous_shape_length = original_shape_length_;
   original_shape_length_ += pd;
   arc original_arc = current_arc_;
-  if (arc::try_create_arc(points_, current_arc_, original_shape_length_, max_radius_mm_, resolution_mm_, path_tolerance_percent_, min_arc_segments_, mm_per_arc_segment_, get_xyz_precision(), allow_3d_arcs_))
+  if (arc::try_create_arc(points_, current_arc_, original_shape_length_, max_radius_mm_, resolution_mm_, path_tolerance_percent_, min_arc_segments_, mm_per_arc_segment_, get_xyz_tolerance(), allow_3d_arcs_))
   {
     // See how many arcs will be interpolated
     bool abort_arc = false;
@@ -235,7 +236,10 @@ bool segmented_arc::try_add_point_internal_(point p, double pd)
     {
       abort_arc = true;
     }
-
+    if (!abort_arc && current_arc_.length < get_xyz_tolerance())
+    {
+      abort_arc = true;
+    }
     if (abort_arc)
     {
       // This arc has been cancelled either due to firmware correction,
@@ -271,31 +275,94 @@ std::string segmented_arc::get_shape_gcode_relative(double f)
 
 std::string segmented_arc::get_shape_gcode_(bool has_e, double e, double f) const
 {
-  char buf[100];
   std::string gcode;
+  // Calculate gcode size
+  bool has_f = utilities::greater_than_or_equal(f, 1);
+  bool has_z = allow_3d_arcs_ && !utilities::is_equal(
+    current_arc_.start_point.z, current_arc_.end_point.z, get_xyz_tolerance()
+  );
+  gcode.reserve(96);
 
+  if (current_arc_.angle_radians < 0)
+  {
+    gcode += "G2";
+  }
+  else
+  {
+    gcode += "G3";
+
+  }
+  // Add X, Y, I and J
+  gcode += " X";
+  gcode += utilities::dtos(current_arc_.end_point.x, get_xyz_precision());
+  
+  gcode += " Y";
+  gcode += utilities::dtos(current_arc_.end_point.y, get_xyz_precision());
+  
+  if (has_z)
+  {
+    gcode += " Z";
+    gcode += utilities::dtos(current_arc_.end_point.z, get_xyz_precision());
+  }
 
+  // Output I and J, but do NOT check for 0.  
+  // Simplify 3d has issues visualizing G2/G3 with 0 for I or J
+  // and until it is fixed, it is not worth the hassle.
   double i = current_arc_.get_i();
+  gcode += " I";
+  gcode += utilities::dtos(i, get_xyz_precision());
+
   double j = current_arc_.get_j();
-  // Here is where the performance part kicks in (these are expensive calls) that makes things a bit ugly.
-  // there are a few cases we need to take into consideration before choosing our sprintf string
-  // create the XYZ portion
+  gcode += " J";
+  gcode += utilities::dtos(j, get_xyz_precision());
+
+  // Add E if it appears
+  if (has_e)
+  {
+    gcode += " E";
+    gcode += utilities::dtos(e, get_e_precision());
+  }
+
+  // Add F if it appears
+  if (has_f)
+  {
+    gcode += " F";
+    gcode += utilities::dtos(f, 0);
+  }
+
+  return gcode;
+
+}
+
+/*
+* This is an older implementation using ostringstream.  It is substantially slower.
+* Keep this around in case there are problems with the custom dtos function
+std::string segmented_arc::get_shape_gcode_(bool has_e, double e, double f) const
+{
+  static std::ostringstream gcode;
+  gcode.str("");
+  gcode << std::fixed;
 
   if (current_arc_.angle_radians < 0)
   {
-    gcode = "G2";
+    gcode << "G2";
   }
   else
   {
-    gcode = "G3";
+    gcode << "G3";
 
   }
+  gcode << std::setprecision(get_xyz_precision());
   // Add X, Y, I and J
-  gcode += " X";
-  gcode += utilities::to_string(current_arc_.end_point.x, get_xyz_precision(), buf, false);
+  if (!utilities::is_zero(current_arc_.end_point.x, get_xyz_precision()))
+  {
+    gcode << " X" << current_arc_.end_point.x;
+  }
 
-  gcode += " Y";
-  gcode += utilities::to_string(current_arc_.end_point.y, get_xyz_precision(), buf, false);
+  if (!utilities::is_zero(current_arc_.end_point.y, get_xyz_precision()))
+  {
+    gcode << " Y" << current_arc_.end_point.y;
+  }
 
   if (allow_3d_arcs_)
   {
@@ -305,37 +372,34 @@ std::string segmented_arc::get_shape_gcode_(bool has_e, double e, double f) cons
     if (!utilities::is_equal(z_initial, z_final, get_xyz_tolerance()))
     {
       // The z axis has changed within the precision of the gcode coordinates
-      gcode += " Z";
-      gcode += utilities::to_string(current_arc_.end_point.z, get_xyz_precision(), buf, false);
+      gcode << " Z" << current_arc_.end_point.z;
     }
   }
 
-  if (!utilities::is_zero(i, get_xyz_tolerance()))
-  {
-    gcode += " I";
-    gcode += utilities::to_string(i, get_xyz_precision(), buf, false);
-  }
-  if (!utilities::is_zero(j, get_xyz_tolerance()))
-  {
-    gcode += " J";
-    gcode += utilities::to_string(j, get_xyz_precision(), buf, false);
-  }
+  // Output I and J, but do NOT check for 0.
+  // Simplify 3d has issues visualizing G2/G3 with 0 for I or J
+  // and until it is fixed, it is not worth the hassle.
+  double i = current_arc_.get_i();
+  gcode << " I" << i;
+
+  double j = current_arc_.get_j();
+  gcode << " J" << j;
 
   // Add E if it appears
   if (has_e)
   {
-    gcode += " E";
-    gcode += utilities::to_string(e, get_e_precision(), buf, false);
+    gcode << std::setprecision(get_e_precision());
+    gcode << " E" << e;
   }
 
   // Add F if it appears
   if (utilities::greater_than_or_equal(f, 1))
   {
-    gcode += " F";
-    gcode += utilities::to_string(f, 0, buf, true);
+    gcode << std::setprecision(0);
+    gcode << " F" << f;
   }
 
-  return gcode;
+  return gcode.str();
 
 }
-
+*/
\ No newline at end of file
diff --git a/ArcWelder/segmented_arc.h b/ArcWelder/segmented_arc.h
index 36d20da..52833b3 100644
--- a/ArcWelder/segmented_arc.h
+++ b/ArcWelder/segmented_arc.h
@@ -25,9 +25,6 @@
 
 #pragma once
 #include "segmented_shape.h"
-#include <iomanip>
-#include <sstream>
-
 #define GCODE_CHAR_BUFFER_SIZE 1000
 
 class segmented_arc :
@@ -63,9 +60,6 @@ public:
 private:
 	bool try_add_point_internal_(point p, double pd);
 	std::string get_shape_gcode_(bool has_e, double e, double f) const;
-	std::string get_g1(double x, double y, double z, double e, double f, bool has_z);
-	std::string interpolate_arc(double f, bool is_relative, double start_e = 0);
-	//circle arc_circle_;
 	arc current_arc_;
 	double max_radius_mm_;
 	int min_arc_segments_;
diff --git a/ArcWelder/segmented_shape.cpp b/ArcWelder/segmented_shape.cpp
index f505d59..217154d 100644
--- a/ArcWelder/segmented_shape.cpp
+++ b/ArcWelder/segmented_shape.cpp
@@ -94,7 +94,9 @@ bool segment::get_closest_perpendicular_point(const point& p1, const point& p2,
 {
   // [(Cx - Ax)(Bx - Ax) + (Cy - Ay)(By - Ay)] / [(Bx - Ax) ^ 2 + (By - Ay) ^ 2]
   double num = (c.x - p1.x) * (p2.x - p1.x) + (c.y - p1.y) * (p2.y - p1.y);
-  double denom = (std::pow((p2.x - p1.x), 2) + std::pow((p2.y - p1.y), 2));
+  double x_dif = p2.x - p1.x;
+  double y_dif = p2.y - p1.y;
+  double denom = (x_dif * x_dif) + (y_dif * y_dif);
   double t = num / denom;
 
   // We're considering this a failure if t == 0 or t==1 within our tolerance.  In that case we hit the endpoint, which is OK.
@@ -198,7 +200,7 @@ bool circle::try_create_circle(const point& p1, const point& p2, const point& p3
   return true;
 }
 
-bool circle::try_create_circle(const array_list<point>& points, const double max_radius, const double resolution_mm, const int xyz_precision, bool allow_3d_arcs, bool check_middle_only, circle& new_circle)
+bool circle::try_create_circle(const array_list<point>& points, const double max_radius, const double resolution_mm, const double xyz_tolerance, bool allow_3d_arcs, bool check_middle_only, circle& new_circle)
 {
   int middle_index = points.count() / 2;
   int check_index;
@@ -210,7 +212,7 @@ bool circle::try_create_circle(const array_list<point>& points, const double max
     // Check the index
     if (circle::try_create_circle(points[0], points[check_index], points[points.count() - 1], max_radius, new_circle))
     {
-      if (!new_circle.is_over_deviation(points, resolution_mm, xyz_precision, allow_3d_arcs))
+      if (!new_circle.is_over_deviation(points, resolution_mm, xyz_tolerance, allow_3d_arcs))
       {
         return true;
       }
@@ -269,7 +271,7 @@ point circle::get_closest_point(const point& p) const
   return point(px, py, pz, 0);
 }
 
-bool circle::is_over_deviation(const array_list<point>& points, const double resolution_mm, const int xyz_precision, const bool allow_3d_arcs)
+bool circle::is_over_deviation(const array_list<point>& points, const double resolution_mm, const double xyz_tolerance, const bool allow_3d_arcs)
 {
   // We need to ensure that the Z steps are constand per linear travel unit
   double z_step_per_distance = 0;
@@ -289,7 +291,7 @@ bool circle::is_over_deviation(const array_list<point>& points, const double res
       if (z_step_per_distance == 0) {
         z_step_per_distance = current_z_stepper_distance;
       }
-      if (!utilities::is_equal(z_step_per_distance, current_z_stepper_distance, std::pow(10.0, -1.0 * xyz_precision)))
+      if (!utilities::is_equal(z_step_per_distance, current_z_stepper_distance, xyz_tolerance))
       {
         // The z step is uneven, can't create arc				
         return true;
@@ -471,11 +473,11 @@ bool arc::try_create_arc(
   double path_tolerance_percent,
   int min_arc_segments,
   double mm_per_arc_segment,
-  int xyz_precision,
+  double xyz_tolerance,
   bool allow_3d_arcs)
 {
   circle test_circle;
-  if (circle::try_create_circle(points, max_radius_mm, resolution_mm, xyz_precision, allow_3d_arcs, false, test_circle))
+  if (circle::try_create_circle(points, max_radius_mm, resolution_mm, xyz_tolerance, allow_3d_arcs, false, test_circle))
   {
     // We could save a bit of processing power and do our firmware compensation here, but we won't be able to track statistics for this easily.
     // moved check to segmented_arc.cpp
diff --git a/ArcWelder/segmented_shape.h b/ArcWelder/segmented_shape.h
index 3b8e807..1070625 100644
--- a/ArcWelder/segmented_shape.h
+++ b/ArcWelder/segmented_shape.h
@@ -33,6 +33,7 @@
 // The minimum theta value allowed between any two arc in order for an arc to be
 // created.  This prevents sign calculation issues for very small values of theta
 #define DEFAULT_XYZ_PRECISION 3
+#define DEFAULT_XYZ_TOLERANCE 0.001
 #define DEFAULT_E_PRECISION 5
 #define ARC_LENGTH_PERCENT_TOLERANCE_DEFAULT 0.05  // one percent
 struct point
@@ -109,7 +110,7 @@ struct circle {
 
 	static bool try_create_circle(const point &p1, const point &p2, const point &p3, const double max_radius, circle& new_circle);
 	
-	static bool try_create_circle(const array_list<point>& points, const double max_radius, const double resolutino_mm, const int xyz_precision, bool allow_3d_arcs, bool check_middle_only, circle& new_circle);
+	static bool try_create_circle(const array_list<point>& points, const double max_radius, const double resolutino_mm, const double xyz_tolerance, bool allow_3d_arcs, bool check_middle_only, circle& new_circle);
 
 	double get_radians(const point& p1, const point& p2) const;
 
@@ -117,7 +118,7 @@ struct circle {
 
 	point get_closest_point(const point& p) const;
 
-	bool is_over_deviation(const array_list<point>& points, const double resolution_mm, const int xyz_precision, const bool allow_3d_arcs);
+	bool is_over_deviation(const array_list<point>& points, const double resolution_mm, const double xyz_tolerance, const bool allow_3d_arcs);
 };
 
 #define DEFAULT_RESOLUTION_MM 0.05
@@ -177,7 +178,7 @@ struct arc : circle
 		double path_tolerance_percent = ARC_LENGTH_PERCENT_TOLERANCE_DEFAULT,
 		int min_arc_segments = DEFAULT_MIN_ARC_SEGMENTS,
 		double mm_per_arc_segment = DEFAULT_MM_PER_ARC_SEGMENT,
-		int xyz_precision = DEFAULT_XYZ_PRECISION,
+		double xyz_tolerance = DEFAULT_XYZ_TOLERANCE,
 		bool allow_3d_arcs = DEFAULT_ALLOW_3D_ARCS);
 };
 double distance_from_segment(segment s, point p);