interp_arc.cc
1 /******************************************************************** 2 * Description: interp_arc.cc 3 * 4 * Derived from a work by Thomas Kramer 5 * 6 * Author: 7 * License: GPL Version 2 8 * System: Linux 9 * 10 * Copyright (c) 2004 All rights reserved. 11 * 12 * Last change: 13 ********************************************************************/ 14 #ifndef _GNU_SOURCE 15 #define _GNU_SOURCE 16 #endif 17 #include <unistd.h> 18 #include <stdio.h> 19 #include <stdlib.h> 20 #include <math.h> 21 #include <string.h> 22 #include <ctype.h> 23 #include <sys/types.h> 24 #include <sys/stat.h> 25 #include <libintl.h> 26 #include <rtapi_math.h> 27 #include "rs274ngc.hh" 28 #include "rs274ngc_return.hh" 29 #include "rs274ngc_interp.hh" 30 #include "interp_internal.hh" 31 32 #define _(s) gettext(s) 33 34 char Interp::arc_axis1(int plane) { 35 switch(plane) { 36 case CANON_PLANE_XY: return 'X'; 37 case CANON_PLANE_XZ: return 'Z'; 38 case CANON_PLANE_YZ: return 'Y'; 39 default: return '!'; 40 } 41 } 42 43 char Interp::arc_axis2(int plane) { 44 switch(plane) { 45 case CANON_PLANE_XY: return 'Y'; 46 case CANON_PLANE_XZ: return 'X'; 47 case CANON_PLANE_YZ: return 'Z'; 48 default: return '!'; 49 } 50 } 51 52 53 /***********************************************************************/ 54 55 /*! arc_data_comp_ijk 56 57 Returned Value: int 58 If any of the following errors occur, this returns the error code shown. 59 Otherwise, it returns INTERP_OK. 60 1. The two calculable values of the radius differ by more than 61 tolerance: NCE_RADIUS_TO_END_OF_ARC_DIFFERS_FROM_RADIUS_TO_START 62 2. move is not G_2 or G_3: NCE_BUG_CODE_NOT_G2_OR_G3 63 64 Side effects: 65 This finds and sets the values of center_x, center_y, and turn. 66 67 Called by: convert_arc_comp1 68 69 This finds the center coordinates and number of full or partial turns 70 counterclockwise of a helical or circular arc in ijk-format in the XY 71 plane. The center is computed easily from the current point and center 72 offsets, which are given. It is checked that the end point lies one 73 tool radius from the arc. 74 75 76 */ 77 78 int Interp::arc_data_comp_ijk(int move, //!<either G_2 (cw arc) or G_3 (ccw arc) 79 int plane, //!<active plane 80 int side, //!<either RIGHT or LEFT 81 double tool_radius, //!<radius of the tool 82 double current_x, //!<first coordinate of current point 83 double current_y, //!<second coordinate of current point 84 double end_x, //!<first coordinate of arc end point 85 double end_y, //!<second coordinate of arc end point 86 int ij_absolute, //!<how to interpret i/j numbers 87 double i_number, //!<first coordinate of center (abs or incr) 88 double j_number, //!<second coordinate of center (abs or incr) 89 int p_number, 90 double *center_x, //!<pointer to first coordinate of center of arc 91 double *center_y, //!<pointer to second coordinate of center of arc 92 int *turn, //!<pointer to number of full or partial circles CCW 93 double radius_tolerance, //!<minimum radius tolerance 94 double spiral_abs_tolerance, //!<tolerance of start and end radius difference 95 double spiral_rel_tolerance) 96 { 97 double arc_radius; 98 double radius2; 99 char a = arc_axis1(plane), b = arc_axis2(plane); 100 101 if ( ij_absolute ) { 102 *center_x = (i_number); 103 *center_y = (j_number); 104 } else { 105 *center_x = (current_x + i_number); 106 *center_y = (current_y + j_number); 107 } 108 arc_radius = hypot((*center_x - current_x), (*center_y - current_y)); 109 radius2 = hypot((*center_x - end_x), (*center_y - end_y)); 110 CHKS(((arc_radius < radius_tolerance) || (radius2 < radius_tolerance)), 111 _("Zero-radius arc: " 112 "start=(%c%.4f,%c%.4f) center=(%c%.4f,%c%.4f) end=(%c%.4f,%c%.4f) r1=%.4f r2=%.4f"), 113 a, current_x, b, current_y, 114 a, *center_x, b, *center_y, 115 a, end_x, b, end_y, arc_radius, radius2); 116 117 double abs_err = fabs(arc_radius - radius2); 118 double rel_err = abs_err / std::max(arc_radius, radius2); 119 120 CHKS((abs_err > spiral_abs_tolerance * 100.0) || 121 (rel_err > spiral_rel_tolerance && abs_err > spiral_abs_tolerance), 122 _("Radius to end of arc differs from radius to start: " 123 "start=(%c%.4f,%c%.4f) center=(%c%.4f,%c%.4f) end=(%c%.4f,%c%.4f) " 124 "r1=%.4f r2=%.4f abs_err=%.4g rel_err=%.4f%%"), 125 a, current_x, b, current_y, 126 a, *center_x, b, *center_y, 127 a, end_x, b, end_y, arc_radius, radius2, 128 abs_err, rel_err*100); 129 130 CHKS(((arc_radius <= tool_radius) && (((side == LEFT) && (move == G_3)) || 131 ((side == RIGHT) && (move == G_2)))), 132 NCE_TOOL_RADIUS_NOT_LESS_THAN_ARC_RADIUS_WITH_COMP); 133 134 /* This catches an arc too small for the tool, also */ 135 if (move == G_2) 136 *turn = -1 * p_number; 137 else if (move == G_3) 138 *turn = 1 * p_number; 139 else 140 ERS(NCE_BUG_CODE_NOT_G2_OR_G3); 141 return INTERP_OK; 142 } 143 144 /****************************************************************************/ 145 146 /*! arc_data_comp_r 147 148 Returned Value: int 149 If any of the following errors occur, this returns the error code shown. 150 Otherwise, it returns INTERP_OK. 151 1. The arc radius is too small to reach the end point: 152 NCE_RADIUS_TOO_SMALL_TO_REACH_END_POINT 153 2. The arc radius is not greater than the tool_radius, but should be: 154 NCE_TOOL_RADIUS_NOT_LESS_THAN_ARC_RADIUS_WITH_COMP 155 3. An imaginary value for offset would be found, which should never 156 happen if the theory is correct: NCE_BUG_IN_TOOL_RADIUS_COMP 157 158 Side effects: 159 This finds and sets the values of center_x, center_y, and turn. 160 161 Called by: convert_arc_comp1 162 163 This finds the center coordinates and number of full or partial turns 164 counterclockwise of a helical or circular arc (call it arc1) in 165 r-format in the XY plane. Arc2 is constructed so that it is tangent 166 to a circle whose radius is tool_radius and whose center is at the 167 point (current_x, current_y) and passes through the point (end_x, 168 end_y). Arc1 has the same center as arc2. The radius of arc1 is one 169 tool radius larger or smaller than the radius of arc2. 170 171 If the value of the big_radius argument is negative, that means [NCMS, 172 page 21] that an arc larger than a semicircle is to be made. 173 Otherwise, an arc of a semicircle or less is made. 174 175 The algorithm implemented here is to construct a line L from the 176 current point to the end point, and a perpendicular to it from the 177 center of the arc which intersects L at point P. Since the distance 178 from the end point to the center and the distance from the current 179 point to the center are known, two equations for the length of the 180 perpendicular can be written. The right sides of the equations can be 181 set equal to one another and the resulting equation solved for the 182 length of the line from the current point to P. Then the location of 183 P, the length of the perpendicular, the angle of the perpendicular, 184 and the location of the center, can be found in turn. 185 186 This needs to be better documented, with figures. There are eight 187 possible arcs, since there are three binary possibilities: (1) tool 188 inside or outside arc, (2) clockwise or counterclockwise (3) two 189 positions for each arc (of the given radius) tangent to the tool 190 outline and through the end point. All eight are calculated below, 191 since theta, radius2, and turn may each have two values. 192 193 To see two positions for each arc, imagine the arc is a hoop, the 194 tool is a cylindrical pin, and the arc may rotate around the end point. 195 The rotation covers all possible positions of the arc. It is easy to 196 see the hoop is constrained by the pin at two different angles, whether 197 the pin is inside or outside the hoop. 198 199 */ 200 201 int Interp::arc_data_comp_r(int move, //!< either G_2 (cw arc) or G_3 (ccw arc) 202 int plane, 203 int side, //!< either RIGHT or LEFT 204 double tool_radius, //!< radius of the tool 205 double current_x, //!< first coordinate of current point 206 double current_y, //!< second coordinate of current point 207 double end_x, //!< first coordinate of arc end point 208 double end_y, //!< second coordinate of arc end point 209 double big_radius, //!< radius of arc 210 int p_number, 211 double *center_x, //!< pointer to first coordinate of center of arc 212 double *center_y, //!< pointer to second coordinate of center of arc 213 int *turn, //!< pointer to number of full or partial circles CCW 214 double tolerance) //!< tolerance of differing radii 215 { 216 double abs_radius; // absolute value of big_radius 217 218 abs_radius = fabs(big_radius); 219 CHKS(((abs_radius <= tool_radius) && (((side == LEFT) && (move == G_3)) || 220 ((side == RIGHT) && (move == G_2)))), 221 NCE_TOOL_RADIUS_NOT_LESS_THAN_ARC_RADIUS_WITH_COMP); 222 223 return arc_data_r(move, plane, current_x, current_y, end_x, end_y, big_radius, p_number, 224 center_x, center_y, turn, tolerance); 225 226 } 227 228 /****************************************************************************/ 229 230 /*! arc_data_ijk 231 232 Returned Value: int 233 If any of the following errors occur, this returns the error code shown. 234 Otherwise, it returns INTERP_OK. 235 1. The two calculable values of the radius differ by more than 236 tolerance: NCE_RADIUS_TO_END_OF_ARC_DIFFERS_FROM_RADIUS_TO_START 237 2. The move code is not G_2 or G_3: NCE_BUG_CODE_NOT_G2_OR_G3 238 3. Either of the two calculable values of the radius is zero: 239 NCE_ZERO_RADIUS_ARC 240 241 Side effects: 242 This finds and sets the values of center_x, center_y, and turn. 243 244 Called by: 245 convert_arc2 246 convert_arc_comp2 247 248 This finds the center coordinates and number of full or partial turns 249 counterclockwise of a helical or circular arc in ijk-format. This 250 function is used by convert_arc2 for all three planes, so "x" and 251 "y" really mean "first_coordinate" and "second_coordinate" wherever 252 they are used here as suffixes of variable names. The i and j prefixes 253 are handled similarly. 254 255 */ 256 257 int Interp::arc_data_ijk(int move, //!< either G_2 (cw arc) or G_3 (ccw arc) 258 int plane, 259 double current_x, //!< first coordinate of current point 260 double current_y, //!< second coordinate of current point 261 double end_x, //!< first coordinate of arc end point 262 double end_y, //!< second coordinate of arc end point 263 int ij_absolute, //!<how to interpret i/j numbers 264 double i_number, //!<first coordinate of center (abs or incr) 265 double j_number, //!<second coordinate of center (abs or incr) 266 int p_number, 267 double *center_x, //!< pointer to first coordinate of center of arc 268 double *center_y, //!< pointer to second coordinate of center of arc 269 int *turn, //!< pointer to no. of full or partial circles CCW 270 double radius_tolerance, //!<minimum radius tolerance 271 double spiral_abs_tolerance, //!<tolerance of start and end radius difference 272 double spiral_rel_tolerance) 273 { 274 double radius; /* radius to current point */ 275 double radius2; /* radius to end point */ 276 char a = arc_axis1(plane), b = arc_axis2(plane); 277 278 if ( ij_absolute ) { 279 *center_x = (i_number); 280 *center_y = (j_number); 281 } else { 282 *center_x = (current_x + i_number); 283 *center_y = (current_y + j_number); 284 } 285 radius = hypot((*center_x - current_x), (*center_y - current_y)); 286 radius2 = hypot((*center_x - end_x), (*center_y - end_y)); 287 CHKS(((radius < radius_tolerance) || (radius2 < radius_tolerance)),_("Zero-radius arc: " 288 "start=(%c%.4f,%c%.4f) center=(%c%.4f,%c%.4f) end=(%c%.4f,%c%.4f) r1=%.4f r2=%.4f"), 289 a, current_x, b, current_y, 290 a, *center_x, b, *center_y, 291 a, end_x, b, end_y, radius, radius2); 292 double abs_err = fabs(radius - radius2); 293 double rel_err = abs_err / std::max(radius, radius2); 294 CHKS((abs_err > spiral_abs_tolerance * 100.0) || 295 (rel_err > spiral_rel_tolerance && abs_err > spiral_abs_tolerance), 296 _("Radius to end of arc differs from radius to start: " 297 "start=(%c%.4f,%c%.4f) center=(%c%.4f,%c%.4f) end=(%c%.4f,%c%.4f) " 298 "r1=%.4f r2=%.4f abs_err=%.4g rel_err=%.4f%%"), 299 a, current_x, b, current_y, 300 a, *center_x, b, *center_y, 301 a, end_x, b, end_y, radius, radius2, 302 abs_err, rel_err*100); 303 304 if (move == G_2) 305 *turn = -1 * p_number; 306 else if (move == G_3) 307 *turn = 1 * p_number; 308 else 309 ERS(NCE_BUG_CODE_NOT_G2_OR_G3); 310 return INTERP_OK; 311 } 312 313 /****************************************************************************/ 314 315 /*! arc_data_r 316 317 Returned Value: int 318 If any of the following errors occur, this returns the error shown. 319 Otherwise, it returns INTERP_OK. 320 1. The radius is too small to reach the end point: 321 NCE_ARC_RADIUS_TOO_SMALL_TO_REACH_END_POINT 322 2. The current point is the same as the end point of the arc 323 (so that it is not possible to locate the center of the circle): 324 NCE_CURRENT_POINT_SAME_AS_END_POINT_OF_ARC 325 326 Side effects: 327 This finds and sets the values of center_x, center_y, and turn. 328 329 Called by: 330 convert_arc2 331 convert_arc_comp2 332 333 This finds the center coordinates and number of full or partial turns 334 counterclockwise of a helical or circular arc in the r format. This 335 function is used by convert_arc2 for all three planes, so "x" and 336 "y" really mean "first_coordinate" and "second_coordinate" wherever 337 they are used here as suffixes of variable names. 338 339 If the value of the radius argument is negative, that means [NCMS, 340 page 21] that an arc larger than a semicircle is to be made. 341 Otherwise, an arc of a semicircle or less is made. 342 343 The algorithm used here is based on finding the midpoint M of the line 344 L between the current point and the end point of the arc. The center 345 of the arc lies on a line through M perpendicular to L. 346 347 */ 348 349 int Interp::arc_data_r(int move, //!< either G_2 (cw arc) or G_3 (ccw arc) 350 int plane, 351 double current_x, //!< first coordinate of current point 352 double current_y, //!< second coordinate of current point 353 double end_x, //!< first coordinate of arc end point 354 double end_y, //!< second coordinate of arc end point 355 double radius, //!< radius of arc 356 int p_number, 357 double *center_x, //!< pointer to first coordinate of center of arc 358 double *center_y, //!< pointer to second coordinate of center of arc 359 int *turn, //!< pointer to number of full or partial circles CCW 360 double tolerance) //!< tolerance of differing radii 361 { 362 double abs_radius; /* absolute value of given radius */ 363 double half_length; /* distance from M to end point */ 364 double mid_x; /* first coordinate of M */ 365 double mid_y; /* second coordinate of M */ 366 double offset; /* distance from M to center */ 367 double theta; /* angle of line from M to center */ 368 double turn2; /* absolute value of half of turn */ 369 370 CHKS(((end_x == current_x) && (end_y == current_y)), 371 NCE_CURRENT_POINT_SAME_AS_END_POINT_OF_ARC); 372 abs_radius = fabs(radius); 373 mid_x = (end_x + current_x) / 2.0; 374 mid_y = (end_y + current_y) / 2.0; 375 half_length = hypot((mid_x - end_x), (mid_y - end_y)); 376 CHKS(((half_length - abs_radius) > tolerance), 377 NCE_ARC_RADIUS_TOO_SMALL_TO_REACH_END_POINT); 378 if ((half_length / abs_radius) > (1 - TINY)) 379 half_length = abs_radius; /* allow a small error for semicircle */ 380 /* check needed before calling asin */ 381 if (((move == G_2) && (radius > 0)) || ((move == G_3) && (radius < 0))) 382 theta = atan2((end_y - current_y), (end_x - current_x)) - M_PI_2l; 383 else 384 theta = atan2((end_y - current_y), (end_x - current_x)) + M_PI_2l; 385 386 turn2 = asin(half_length / abs_radius); 387 offset = abs_radius * cos(turn2); 388 *center_x = mid_x + (offset * cos(theta)); 389 *center_y = mid_y + (offset * sin(theta)); 390 *turn = (move == G_2) ? -1 * p_number : 1 * p_number; 391 392 return INTERP_OK; 393 }