rendered paste body? nurbs.patch
? rs274ngc/nurbs_additional_functions.cc
Index: nml_intf/canon.hh
===================================================================
RCS file: /cvs/emc2/src/emc/nml_intf/canon.hh,v
retrieving revision 1.17
diff -u -r1.17 canon.hh
--- nml_intf/canon.hh 5 Nov 2006 19:34:44 -0000 1.17
+++ nml_intf/canon.hh 7 May 2007 15:50:21 -0000
@@ -18,6 +18,7 @@
#define CANON_HH
#include <stdio.h> // FILE
+#include <vector>
/*
This is the header file that all applications that use the
@@ -63,6 +64,18 @@
#define OFF 0
#define ON 1
+typedef struct { /* type for NURBS control points */
+ double X,
+ Y,
+ W;
+ } CONTROL_POINT;
+
+typedef struct {
+ double X,
+ Y;
+ } PLANE_POINT;
+
+
typedef int CANON_PLANE;
#define CANON_PLANE_XY 1
#define CANON_PLANE_YZ 2
@@ -392,6 +405,36 @@
double a_position, double b_position,
double c_position);
+/* Additional functions needed to calculate nurbs points */
+
+extern std::vector<unsigned int> knot_vector_creator(unsigned int n, unsigned int k);
+extern double Nmix(unsigned int i, unsigned int k, double u,
+ std::vector<unsigned int> knot_vector);
+extern double Rden(double u, unsigned int k,
+ std::vector<CONTROL_POINT> nurbs_control_points,
+ std::vector<unsigned int> knot_vector);
+extern PLANE_POINT nurbs_point(double u, unsigned int k,
+ std::vector<CONTROL_POINT> nurbs_control_points,
+ std::vector<unsigned int> knot_vector);
+extern double alpha_finder(double dx, double dy);
+
+/* Canon calls */
+
+extern void NURBS_FEED(std::vector<CONTROL_POINT> nurbs_control_points, unsigned int k);
+/* Move at the feed rate along an approximation of a NURBS with a variable number
+ * of control points
+ */
+
+extern void SPLINE_FEED(double x1, double y1, double x2, double y2, double x3, double y3);
+/* Move at the feed rate along an approximation of the cubic
+ * bezier spline with control points x1y1 x2y2 and endpoint x3y3
+ */
+extern void SPLINE_FEED(double x1, double y1, double x2, double y2);
+/* Move at the feed rate along an approximation of the conic
+ * spline with control point x1y1 and endpoint x2y2
+ */
+
+
/* Move at existing feed rate so that at any time during the move,
all axes have covered the same proportion of their required motion.
The meanings of the parameters is the same as for STRAIGHT_TRAVERSE.*/
Index: rs274ngc/Submakefile
===================================================================
RCS file: /cvs/emc2/src/emc/rs274ngc/Submakefile,v
retrieving revision 1.7
diff -u -r1.7 Submakefile
--- rs274ngc/Submakefile 6 Mar 2007 03:19:09 -0000 1.7
+++ rs274ngc/Submakefile 7 May 2007 15:50:21 -0000
@@ -13,6 +13,7 @@
interp_read.cc \
interp_write.cc \
interp_o_word.cc \
+ nurbs_additional_functions.cc \
rs274ngc_errors.cc \
rs274ngc_pre.cc)
USERSRCS += $(LIBRS274SRCS)
Index: rs274ngc/gcodemodule.cc
===================================================================
RCS file: /cvs/emc2/src/emc/rs274ngc/gcodemodule.cc,v
retrieving revision 1.14
diff -u -r1.14 gcodemodule.cc
--- rs274ngc/gcodemodule.cc 9 Mar 2007 17:23:57 -0000 1.14
+++ rs274ngc/gcodemodule.cc 7 May 2007 15:50:21 -0000
@@ -175,6 +175,68 @@
Py_XDECREF(result);
}
+static double TO_PROG_LEN(double p) {
+ if(metric) return p*25.4;
+ return p;
+}
+static double FROM_PROG_LEN(double p) {
+ if(metric) return p/25.4;
+ return p;
+}
+
+void NURBS_FEED(std::vector<CONTROL_POINT> nurbs_control_points, unsigned int k) {
+ double u = 0.0;
+ unsigned int n = nurbs_control_points.size() - 1;
+ double umax = n - k + 2;
+ unsigned int div = nurbs_control_points.size()*15;
+ std::vector<unsigned int> knot_vector = knot_vector_creator(n, k);
+ PLANE_POINT P1;
+ while (u+umax/div < umax) {
+ PLANE_POINT P1 = nurbs_point(u+umax/div,k,nurbs_control_points,knot_vector);
+ STRAIGHT_FEED(P1.X,P1.Y, _pos_z, _pos_a, _pos_b, _pos_c);
+ u = u + umax/div;
+ }
+ P1.X = nurbs_control_points[n].X;
+ P1.Y = nurbs_control_points[n].Y;
+ STRAIGHT_FEED(P1.X,P1.Y, _pos_z, _pos_a, _pos_b, _pos_c);
+ knot_vector.clear();
+}
+
+void SPLINE_FEED(double x1, double y1, double x2, double y2) {
+ double x0 = TO_PROG_LEN(_pos_x),
+ y0 = TO_PROG_LEN(_pos_y);
+
+ fprintf(stderr, "SPLINE_FEED(conic): %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n",
+ x0,y0,x1,y1,x2,y2);
+
+ for(int i=1; i<=100; i++) {
+ double t = i / 100.;
+ double t2 = t*t;
+ double t1 = 2*t*(1-t);
+ double t0 = (1-t)*(1-t);
+ double x = x0*t0 + x1*t1 + x2*t2;
+ double y = y0*t0 + y1*t1 + y2*t2;
+ STRAIGHT_FEED(x,y, _pos_z, _pos_a, _pos_b, _pos_c);
+ }
+}
+
+void SPLINE_FEED(double x1, double y1, double x2, double y2, double x3, double y3) {
+ double x0 = TO_PROG_LEN(_pos_x),
+ y0 = TO_PROG_LEN(_pos_y);
+
+ for(int i=1; i<=100; i++) { double t = i / 100.;
+ double t3 = t*t*t;
+ double t2 = 3*t*t*(1-t);
+ double t1 = 3*t*(1-t)*(1-t);
+ double t0 = (1-t)*(1-t)*(1-t);
+ double x = x0*t0 + x1*t1 + x2*t2 + x3*t3;
+ double y = y0*t0 + y1*t1 + y2*t2 + y3*t3;
+ STRAIGHT_FEED(x,y, _pos_z, _pos_a, _pos_b, _pos_c);
+ }
+}
+
+
+
void ARC_FEED(double first_end, double second_end, double first_axis,
double second_axis, int rotation, double axis_end_point,
double a_position, double b_position, double c_position) {
Index: rs274ngc/interp_array.cc
===================================================================
RCS file: /cvs/emc2/src/emc/rs274ngc/interp_array.cc,v
retrieving revision 1.12
diff -u -r1.12 interp_array.cc
--- rs274ngc/interp_array.cc 30 Dec 2006 23:12:03 -0000 1.12
+++ rs274ngc/interp_array.cc 7 May 2007 15:50:22 -0000
@@ -75,7 +75,7 @@
const int Interp::_gees[] = {
/* 0 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,
/* 20 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1,
-/* 40 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,
+/* 40 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1, 1, 0,-1,-1,-1,-1,-1,-1,
/* 60 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,
/* 80 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,
/* 100 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,
Index: rs274ngc/interp_check.cc
===================================================================
RCS file: /cvs/emc2/src/emc/rs274ngc/interp_check.cc,v
retrieving revision 1.13
diff -u -r1.13 interp_check.cc
--- rs274ngc/interp_check.cc 30 Apr 2007 01:18:10 -0000 1.13
+++ rs274ngc/interp_check.cc 7 May 2007 15:50:23 -0000
@@ -81,12 +81,12 @@
setup_pointer settings) //!< pointer to machine settings
{
static char name[] = "check_g_codes";
- int mode0;
+ int mode0, mode1;
int p_int;
mode0 = block->g_modes[0];
-
- if (mode0 == -1) {
+ mode1 = block->g_modes[1];
+ if ((mode0 == -1) && (settings->motion_mode != G_5_2)) {
} else if (mode0 == G_4) {
CHK((block->p_number == -1.0), NCE_DWELL_TIME_MISSING_WITH_G4);
} else if (mode0 == G_10) {
@@ -96,6 +96,12 @@
NCE_P_VALUE_NOT_AN_INTEGER_WITH_G10_L2);
CHK(((p_int < 1) || (p_int > 9)), NCE_P_VALUE_OUT_OF_RANGE_WITH_G10_L2);
} else if (mode0 == G_28) {
+ } else if (mode0 == G_28) {
+ } else if ((mode1 != G_5_2) && (mode0 != G_5_3)) {
+ CHKF(((mode1 != G_5_2) && (mode0 != G_5_3) && (mode1 != -1)), (
+ "Can use only G5.2 or G5.3 after G5.2"));
+ } else if (mode1 == G_5_2){
+ } else if (mode0 == G_5_3){
} else if (mode0 == G_30) {
} else if (mode0 == G_53) {
CHK(((block->motion_to_be != G_0) && (block->motion_to_be != G_1)),
@@ -260,12 +266,14 @@
}
if (block->i_flag == ON) { /* could still be useless if yz_plane arc */
- CHK(((motion != G_2) && (motion != G_3) && (motion != G_76) && (motion != G_87) && (block->g_modes[8] != G_43)),
+ CHK(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) &&
+ (motion != G_76) && (motion != G_87) && (block->g_modes[8] != G_43)),
NCE_I_WORD_WITH_NO_G2_OR_G3_G76_OR_G87_TO_USE_IT);
}
if (block->j_flag == ON) { /* could still be useless if xz_plane arc */
- CHK(((motion != G_2) && (motion != G_3) && (motion != G_76) && (motion != G_87)),
+ CHK(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) &&
+ (motion != G_76) && (motion != G_87)),
NCE_J_WORD_WITH_NO_G2_OR_G3_G76_OR_G87_TO_USE_IT);
}
@@ -290,7 +298,7 @@
if (block->l_number != -1) {
CHK((((motion < G_81) || (motion > G_89)) &&
(block->g_modes[0] != G_10) &&
- (motion != G_76)),
+ (motion != G_76) && (motion != G_5_2)),
NCE_L_WORD_WITH_NO_CANNED_CYCLE_OR_G10);
}
@@ -308,12 +316,13 @@
(block->m_modes[9] != 53) &&
(block->user_m != 1) &&
(motion != G_82) && (motion != G_86) &&
- (motion != G_88) && (motion != G_89) && (motion != G_76)),
+ (motion != G_88) && (motion != G_89) && (motion != G_76) && (motion != G_5) &&
+ (motion != G_5_2)),
NCE_P_WORD_WITH_NO_G4_G10_G64_G76_G82_G86_G88_G89);
}
if (block->q_number != -1.0) {
- CHK((motion != G_83) && (block->user_m != 1) && motion != G_76,
+ CHK((motion != G_5) && (motion != G_83) && (block->user_m != 1) && motion != G_76,
NCE_Q_WORD_WITH_NO_G83);
}
Index: rs274ngc/interp_convert.cc
===================================================================
RCS file: /cvs/emc2/src/emc/rs274ngc/interp_convert.cc,v
retrieving revision 1.71
diff -u -r1.71 interp_convert.cc
--- rs274ngc/interp_convert.cc 30 Apr 2007 01:45:23 -0000 1.71
+++ rs274ngc/interp_convert.cc 7 May 2007 15:50:28 -0000
@@ -58,6 +58,158 @@
/****************************************************************************/
+/*! convert_nurbs
+ *
+ * Returned value: int
+ * Returns a rs274ngc error code, or INTERP_OK if everything is OK.
+ *
+ * Side effects: Generates a nurbs move and updates the position of the tool
+ */
+
+static unsigned int nurbs_order;
+static std::vector<CONTROL_POINT> nurbs_control_points;
+
+int Interp::convert_nurbs(int mode,
+ block_pointer block, //!< pointer to a block of RS274 instructions
+ setup_pointer settings) //!< pointer to machine settings
+{
+ static char name[] = "convert_nurbs";
+ unsigned int i;
+ double end_z, AA_end, BB_end, CC_end;
+ CONTROL_POINT CP;
+
+ if (mode == G_5_2) {
+ CHKF((((block->x_flag) && !(block->y_flag)) || (!(block->x_flag) && (block->y_flag))), (
+ "You must specify both X and Y coordinates for Control Points"));
+ CHKF((!(block->x_flag) && !(block->y_flag) && (block->p_number > 0) &&
+ (!nurbs_control_points.empty())), (
+ "Can specify P without X and Y only for the first control point"));
+
+ CHKF(((block->p_number <= 0) && (!nurbs_control_points.empty())), (
+ "Must specify positive weight P for every Control Point"));
+ if (settings->feed_mode == UNITS_PER_MINUTE) {
+ CHKF((settings->feed_rate == 0.0), (
+ "Cannot make a NURBS with 0 feedrate"));
+ }
+ if (block->l_number != -1 && block->l_number > 3) {
+ nurbs_order = block->l_number;
+ }
+ if (nurbs_control_points.empty()) {
+ CP.X = settings->current_x;
+ CP.Y = settings->current_y;
+ if (!(block->x_flag) && !(block->y_flag) && (block->p_number > 0)) {
+ CP.W = block->p_number;
+ } else {
+ CP.W = 1;
+ }
+ nurbs_order = 3;
+ nurbs_control_points.push_back(CP);
+ }
+ if ((block->x_flag) && (block->y_flag)) {
+ find_ends(block, settings, &CP.X, &CP.Y, &end_z, &AA_end, &BB_end, &CC_end);
+ CP.W = block->p_number;
+ nurbs_control_points.push_back(CP);
+ }
+
+//for (i=0;i<nurbs_control_points.size();i++){
+// printf( "X %8.4f, Y %8.4f, W %8.4f\n",
+// nurbs_control_points[i].X,
+// nurbs_control_points[i].Y,
+// nurbs_control_points[i].W);
+//
+// }
+// printf("*-----------------------------------------*\n");
+ settings->motion_mode = mode;
+ }
+
+ else if (mode == G_5_3){
+ CHKF((settings->motion_mode != G_5_2), (
+ "Cannot use G.3 without G.2 first"));
+ CHKF((nurbs_control_points.size()<nurbs_order), (
+ "You must specify a number of control points at least equal to \
+the order L = %d", nurbs_order));
+ NURBS_FEED(nurbs_control_points, nurbs_order);
+ //printf("hello\n");
+ nurbs_control_points.clear();
+ //printf("%d\n", nurbs_control_points.size());
+ settings->motion_mode = -1;
+ }
+ return INTERP_OK;
+}
+
+ /****************************************************************************/
+
+/*! convert_spline
+ *
+ * Returned value: int
+ * Returns a rs274ngc error code, or INTERP_OK if everything is OK.
+ *
+ * Side effects: Generates a spline move and updates the position of the tool
+ */
+int Interp::convert_spline(int mode,
+ block_pointer block, //!< pointer to a block of RS274 instructions
+ setup_pointer settings) //!< pointer to machine settings
+{
+ static char name[] = "convert_spline";
+ double x1, y1, x2, y2, x3, y3;
+ double end_z, AA_end, BB_end, CC_end;
+
+
+ CHKF((settings->cutter_comp_side != OFF), (
+ "Cannot convert spline with cutter radius compensation"));
+
+ if (settings->feed_mode == UNITS_PER_MINUTE) {
+ CHK((settings->feed_rate == 0.0),
+ NCE_CANNOT_MAKE_ARC_WITH_ZERO_FEED_RATE);
+ } else if (settings->feed_mode == INVERSE_TIME) {
+ CHK((block->f_number == -1.0),
+ NCE_F_WORD_MISSING_WITH_INVERSE_TIME_ARC_MOVE);
+ }
+
+ CHKF((settings->plane != CANON_PLANE_XY),
+ ("Splines must be in the XY plane"));
+ //Error (for now): Splines must be in XY plane
+
+ CHKF((block->z_flag == ON || block->a_flag == ON || block->b_flag == ON
+ || block->c_flag == ON),
+ ("Splines may not have motion in Z, A, B, or C"));
+
+ if(mode == G_5_1) {
+ CHKF(block->i_flag != ON || block->j_flag != ON,
+ ("Must specify both I and J with G5.1"));
+ x1 = settings->current_x + block->i_number;
+ y1 = settings->current_y + block->j_number;
+ find_ends(block, settings, &x2, &y2, &end_z, &AA_end, &BB_end, &CC_end);
+ SPLINE_FEED(x1,y1,x2,y2);
+ settings->current_x = x2;
+ settings->current_y = y2;
+ } else {
+ if(block->i_flag != ON || block->j_flag != ON) {
+ CHKF(block->i_flag == ON || block->j_flag == ON,
+ ("Must specify both I and J, or neither"));
+ x1 = settings->current_x + settings->cycle_i;
+ y1 = settings->current_y + settings->cycle_j;
+ } else {
+ x1 = settings->current_x + block->i_number;
+ y1 = settings->current_y + block->j_number;
+ }
+ find_ends(block, settings, &x3, &y3, &end_z, &AA_end, &BB_end, &CC_end);
+
+ x2 = x3 + block->p_number;
+ y2 = y3 + block->q_number;
+
+ SPLINE_FEED(x1, y1, x2, y2, x3, y3);
+
+ settings->cycle_i = -block->p_number;
+ settings->cycle_j = -block->q_number;
+ settings->current_x = x3;
+ settings->current_y = y3;
+ }
+ return INTERP_OK;
+}
+
+/****************************************************************************/
+
/*! convert_arc
Returned Value: int
@@ -2022,6 +2174,8 @@
} else if ((code == G_92) || (code == G_92_1) ||
(code == G_92_2) || (code == G_92_3)) {
CHP(convert_axis_offsets(code, block, settings));
+ } else if (code == G_5_3) {
+ CHP(convert_nurbs(code, block, settings));
} else if ((code == G_4) || (code == G_53)); /* handled elsewhere */
else
ERM(NCE_BUG_CODE_NOT_G4_G10_G28_G30_G53_OR_G92_SERIES);
@@ -2071,6 +2225,10 @@
settings->motion_mode = G_80;
} else if ((motion > G_80) && (motion < G_90)) {
CHP(convert_cycle(motion, block, settings));
+ } else if ((motion == G_5) || (motion == G_5_1)) {
+ CHP(convert_spline(motion, block, settings));
+ } else if (motion == G_5_2) {
+ CHP(convert_nurbs(motion, block, settings));
} else
ERM(NCE_BUG_UNKNOWN_MOTION_CODE);
Index: rs274ngc/interp_internal.hh
===================================================================
RCS file: /cvs/emc2/src/emc/rs274ngc/interp_internal.hh,v
retrieving revision 1.23
diff -u -r1.23 interp_internal.hh
--- rs274ngc/interp_internal.hh 30 Apr 2007 01:18:09 -0000 1.23
+++ rs274ngc/interp_internal.hh 7 May 2007 15:50:29 -0000
@@ -127,6 +127,10 @@
#define G_2 20
#define G_3 30
#define G_4 40
+#define G_5 50
+#define G_5_1 51
+#define G_5_2 52
+#define G_5_3 53
#define G_10 100
#define G_17 170
#define G_18 180
@@ -443,6 +447,14 @@
return error_code; \
} else
+#define CHKF(bad, error_args) if (bad) { \
+ setError error_args; \
+ _setup.stack_index = 0; \
+ strcpy(_setup.stack[_setup.stack_index++], name); \
+ _setup.stack[_setup.stack_index][0] = 0; \
+ return NCE_VARIABLE; \
+ } else
+
#define CHP(try_this) \
if ((status = (try_this)) != INTERP_OK) { \
if (_setup.stack_index < 49) \
Index: rs274ngc/rs274ngc.hh
===================================================================
RCS file: /cvs/emc2/src/emc/rs274ngc/rs274ngc.hh,v
retrieving revision 1.23
diff -u -r1.23 rs274ngc.hh
--- rs274ngc/rs274ngc.hh 9 Mar 2007 17:23:57 -0000 1.23
+++ rs274ngc/rs274ngc.hh 7 May 2007 15:50:30 -0000
@@ -24,6 +24,7 @@
#include <stdio.h>
#include "canon.hh"
#include "interp_internal.hh"
+#include "emc.hh"
#define DEBUG_EMC
@@ -160,6 +161,8 @@
int check_m_codes(block_pointer block);
int check_other_codes(block_pointer block);
int close_and_downcase(char *line);
+ int convert_nurbs(int move, block_pointer block, setup_pointer settings);
+ int convert_spline(int move, block_pointer block, setup_pointer settings);
int convert_arc(int move, block_pointer block, setup_pointer settings);
int convert_arc2(int move, block_pointer block,
setup_pointer settings, double *current1,
Index: sai/saicanon.cc
===================================================================
RCS file: /cvs/emc2/src/emc/sai/saicanon.cc,v
retrieving revision 1.5
diff -u -r1.5 saicanon.cc
--- sai/saicanon.cc 15 Mar 2007 16:19:57 -0000 1.5
+++ sai/saicanon.cc 7 May 2007 15:50:31 -0000
@@ -426,6 +426,42 @@
/* Machining Functions */
+void NURBS_FEED(
+std::vector<CONTROL_POINT> nurbs_control_points, unsigned int k)
+{
+ fprintf(_outfile, "%5d ", _line_number++);
+ print_nc_line_number();
+ fprintf(_outfile, "NURBS_FEED(%d\n",
+ nurbs_control_points.size());
+
+ _program_position_x = nurbs_control_points[nurbs_control_points.size()].X;
+ _program_position_y = nurbs_control_points[nurbs_control_points.size()].Y;
+}
+
+void SPLINE_FEED(
+double x1, double y1, double x2, double y2, double x3, double y3)
+{
+ fprintf(_outfile, "%5d ", _line_number++);
+ print_nc_line_number();
+ fprintf(_outfile, "SPLINE_FEED(%.4f, %.4f, %.4f, %.4f, %.4f, %.4f\n",
+ x1, y1, x2, y2, x3, y3);
+
+ _program_position_x = x3;
+ _program_position_y = y3;
+}
+
+void SPLINE_FEED(
+double x1, double y1, double x2, double y2)
+{
+ fprintf(_outfile, "%5d ", _line_number++);
+ print_nc_line_number();
+ fprintf(_outfile, "SPLINE_FEED(%.4f, %.4f, %.4f, %.4f\n",
+ x1, y1, x2, y2);
+
+ _program_position_x = x2;
+ _program_position_y = y2;
+}
+
void ARC_FEED(
double first_end, double second_end,
double first_axis, double second_axis, int rotation, double axis_end_point
Index: task/emccanon.cc
===================================================================
RCS file: /cvs/emc2/src/emc/task/emccanon.cc,v
retrieving revision 1.83
diff -u -r1.83 emccanon.cc
--- task/emccanon.cc 5 May 2007 01:48:57 -0000 1.83
+++ task/emccanon.cc 7 May 2007 15:50:33 -0000
@@ -794,6 +794,188 @@
/* Machining Functions */
+/* Spline and NURBS additional functions; */
+
+static double max(double a, double b) {
+ if(a < b) return b;
+ return a;
+}
+static void unit(double *x, double *y) {
+ double h = hypot(*x, *y);
+ if(h != 0) { *x/=h; *y/=h; }
+}
+
+static void
+arc(double x0, double y0, double x1, double y1, double dx, double dy) {
+ double small = 0.000001;
+ double x = x1-x0, y=y1-y0;
+ double den = 2 * (y*dx - x*dy);
+ double r = -(x*x+y*y)/den;
+ double i = dy*r, j = -dx*r;
+ double cx = x1+i, cy=y1+j;
+ if (fabs(den) > small) {
+ ARC_FEED(x1, y1, cx, cy, r<0 ? 1 : -1,
+ TO_PROG_LEN(canonEndPoint.z), TO_PROG_ANG(canonEndPoint.a),
+ TO_PROG_ANG(canonEndPoint.b), TO_PROG_ANG(canonEndPoint.c));
+ } else {
+ STRAIGHT_FEED(x1,y1,
+ TO_PROG_LEN(canonEndPoint.z), TO_PROG_ANG(canonEndPoint.a),
+ TO_PROG_ANG(canonEndPoint.b), TO_PROG_ANG(canonEndPoint.c));
+ }
+}
+
+static int
+biarc(double p0x, double p0y, double tsx, double tsy,
+ double p4x, double p4y, double tex, double tey, double r=1.0) {
+ unit(&tsx, &tsy);
+ unit(&tex, &tey);
+
+ double vx = p0x - p4x, vy = p0y - p4y;
+ double c = vx*vx + vy*vy;
+ double b = 2 * (vx * (r*tsx + tex) + vy * (r*tsy + tey));
+ double a = 2 * r * (tsx * tex + tsy * tey - 1);
+
+ double discr = b*b - 4*a*c;
+ if(discr < 0) return 0;
+
+ double disq = sqrt(discr);
+ double beta1 = (-b-disq) / 2 / a;
+ double beta2 = (-b+disq) / 2 / a;
+
+ if(beta1 > 0 && beta2 > 0)
+ return 0;
+ double beta = max(beta1, beta2);
+ double alpha = beta * r;
+ double ab = alpha + beta;
+ double p1x = p0x + alpha * tsx, p1y = p0y + alpha * tsy,
+ p3x = p4x - beta * tex, p3y = p4y - beta * tey,
+ p2x = (p1x*beta + p3x*alpha) / ab,
+ p2y = (p1y*beta + p3y*alpha) / ab;
+ double tmx = p3x-p2x, tmy = p3y-p2y;
+ unit(&tmx, &tmy);
+
+ arc(p0x, p0y, p2x, p2y, tsx, tsy);
+ arc(p2x, p2y, p4x, p4y, tmx, tmy);
+ return 1;
+}
+
+
+/* Canon calls */
+
+void NURBS_FEED(std::vector<CONTROL_POINT> nurbs_control_points, unsigned int k) {
+ double u = 0.0;
+ unsigned int n = nurbs_control_points.size() - 1;
+ double umax = n - k + 2;
+ unsigned int div = nurbs_control_points.size()*15;
+ double dxs,dys,dx1,dy1,dx2,dy2,dxe,dye,alpha1,alpha2, alphaM;
+ std::vector<unsigned int> knot_vector = knot_vector_creator(n, k);
+ PLANE_POINT P0, P1, P2;
+
+ P0 = nurbs_point(u,k,nurbs_control_points,knot_vector);
+ P1 = nurbs_point(u+umax/div,k,nurbs_control_points,knot_vector);
+ dxs = nurbs_control_points[1].X-nurbs_control_points[0].X;
+ dys = nurbs_control_points[1].Y-nurbs_control_points[0].Y;
+ unit(&dxs,&dys);
+ u = u + umax/div;
+ while (u+umax/div <= umax) {
+ P2= nurbs_point(u+umax/div,k,nurbs_control_points,knot_vector);
+ dx1 = P1.X-P0.X;
+ dy1 = P1.Y-P0.Y;
+ dx2 = P2.X-P1.X;
+ dy2 = P2.Y-P1.Y;
+ alpha1 = alpha_finder(dx1,dy1);
+ alpha2 = alpha_finder(dx2,dy2);
+ if (alpha2 > alpha1 + M_PI)
+ alphaM = (alpha1+alpha2)/2 + M_PI;
+ else
+ alphaM = (alpha1+alpha2)/2;
+ dxe = cos(alphaM);
+ dye = sin(alphaM);
+ unit(&dxe,&dye);
+ biarc(P0.X,P0.Y,dxs,dys,P1.X,P1.Y,dxe,dye);
+ //printf("___________________________________________\n");
+ //printf("X %8.4f Y %8.4f\n", P0.X, P0.Y);
+ dxs = dxe;
+ dys = dye;
+ P0 = P1;
+ P1 = P2;
+ //printf("u = %f\n", u);
+ u = u + umax/div;
+ }
+ P1.X = nurbs_control_points[n].X;
+ P1.Y = nurbs_control_points[n].Y;
+ dxe = nurbs_control_points[n].X - nurbs_control_points[n-1].X;
+ dye = nurbs_control_points[n].Y - nurbs_control_points[n-1].Y;
+ unit(&dxe,&dye);
+ biarc(P0.X,P0.Y,dxs,dys,P1.X,P1.Y,dxe,dye);
+ printf("parameters: n = %d, umax = %f, div= %d, u = %f, k = %d\n",n,umax,div,u,k);
+ knot_vector.clear();
+}
+
+void SPLINE_FEED(double x1, double y1, double x2, double y2) {
+ flush_segments();
+
+ double x0 = TO_PROG_LEN(canonEndPoint.x);
+ double y0 = TO_PROG_LEN(canonEndPoint.y);
+ double xx0 = 2*(x1-x0), xx1 = 2*(x2-x1),
+ yy0 = 2*(y1-y0), yy1 = 2*(y2-y1),
+ ox = x0, oy = y0, odx = xx0, ody = yy0;
+
+#define N 2
+ for(int i=1; i<=N; i++) {
+ double t = i * 1. / N;
+ double u = 1. / N;
+ double t0 = (1-t)*(1-t);
+ double t1 = 2*t*(1-t);
+ double t2 = t*t;
+ double q0 = (1-t);
+ double q1 = t;
+
+perturb:
+ double x = x0*t0 + x1*t1 + x2*t2;
+ double y = y0*t0 + y1*t1 + y2*t2;
+ double dx = xx0*q0 + xx1*q1;
+ double dy = yy0*q0 + yy1*q1;
+ if(!biarc(ox, oy, odx, ody, x, y, dx, dy)) {
+ t = t - u; u /= -2; goto perturb;
+ }
+ ox = x; oy = y; odx = dx; ody = dy;
+ }
+}
+
+void SPLINE_FEED(double x1, double y1, double x2, double y2, double x3, double y3) {
+ flush_segments();
+
+ double x0 = TO_PROG_LEN(canonEndPoint.x);
+ double y0 = TO_PROG_LEN(canonEndPoint.y);
+ double xx0 = 3*(x1-x0), xx1 = 3*(x2-x1), xx2 = 3*(x3-x2),
+ yy0 = 3*(y1-y0), yy1 = 3*(y2-y1), yy2 = 3*(y3-y2),
+ ox = x0, oy = y0, odx = xx0, ody = yy0;
+
+#define N 4
+ for(int i=1; i<=N; i++) {
+ double t = i * 1. / N;
+ double u = 1. / N;
+ double t3 = t*t*t;
+ double t2 = 3*t*t*(1-t);
+ double t1 = 3*t*(1-t)*(1-t);
+ double t0 = (1-t)*(1-t)*(1-t);
+ double q0 = (1-t)*(1-t);
+ double q1 = 2*t*(1-t);
+ double q2 = t*t;
+
+perturb:
+ double x = x0*t0 + x1*t1 + x2*t2 + x3*t3;
+ double y = y0*t0 + y1*t1 + y2*t2 + y3*t3;
+ double dx = xx0*q0 + xx1*q1 + xx2*q2;
+ double dy = yy0*q0 + yy1*q1 + yy2*q2;
+ if(!biarc(ox, oy, odx, ody, x, y, dx, dy)) {
+ t = t - u; u /= -2; goto perturb;
+ }
+ ox = x; oy = y; odx = dx; ody = dy;
+ }
+}
+
void ARC_FEED(double first_end, double second_end,
double first_axis, double second_axis, int rotation,
double axis_end_point, double a, double b, double c)