interp_find.cc
1 /******************************************************************** 2 * Description: interp_find.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 "rtapi_math.h" 26 #include "rs274ngc.hh" 27 #include "interp_return.hh" 28 #include "interp_internal.hh" 29 #include "rs274ngc_interp.hh" 30 #include "units.h" 31 32 /****************************************************************************/ 33 34 /*! find_arc_length 35 36 Returned Value: double (length of path between start and end points) 37 38 Side effects: none 39 40 Called by: 41 inverse_time_rate_arc 42 inverse_time_rate_arc2 43 inverse_time_rate_as 44 45 This calculates the length of the path that will be made relative to 46 the XYZ axes for a motion in which the X,Y,Z, motion is a circular or 47 helical arc with its axis parallel to the Z-axis. If tool length 48 compensation is on, this is the path of the tool tip; if off, the 49 length of the path of the spindle tip. Any rotary axis motion is 50 ignored. 51 52 If the arc is helical, it is coincident with the hypotenuse of a right 53 triangle wrapped around a cylinder. If the triangle is unwrapped, its 54 base is [the radius of the cylinder times the number of radians in the 55 helix] and its height is [z2 - z1], and the path length can be found 56 by the Pythagorean theorem. 57 58 This is written as though it is only for arcs whose axis is parallel to 59 the Z-axis, but it will serve also for arcs whose axis is parallel 60 to the X-axis or Y-axis, with suitable permutation of the arguments. 61 62 This works correctly when turn is zero (find_turn returns 0 in that 63 case). 64 65 */ 66 67 double Interp::find_arc_length(double x1, //!< X-coordinate of start point 68 double y1, //!< Y-coordinate of start point 69 double z1, //!< Z-coordinate of start point 70 double center_x, //!< X-coordinate of arc center 71 double center_y, //!< Y-coordinate of arc center 72 int turn, //!< no. of full or partial circles CCW 73 double x2, //!< X-coordinate of end point 74 double y2, //!< Y-coordinate of end point 75 double z2) //!< Z-coordinate of end point 76 { 77 double radius; 78 double theta; /* amount of turn of arc in radians */ 79 80 radius = hypot((center_x - x1), (center_y - y1)); 81 theta = find_turn(x1, y1, center_x, center_y, turn, x2, y2); 82 if (z2 == z1) 83 return (radius * fabs(theta)); 84 else 85 return hypot((radius * theta), (z2 - z1)); 86 } 87 88 89 /* Find the real destination, given the axis's current position, the 90 commanded destination, and the direction to turn (which comes from 91 the sign of the commanded value in the gcode). Modulo 360 positions 92 of the axis are considered equivalent and we just need to find the 93 nearest one. */ 94 95 int Interp::unwrap_rotary(double *r, double sign_of, double commanded, double current, char axis) { 96 double result; 97 int neg = copysign(1.0, sign_of) < 0.0; 98 CHKS((sign_of <= -360.0 || sign_of >= 360.0), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), sign_of, axis); 99 100 double d = floor(current/360.0); 101 result = fabs(commanded) + (d*360.0); 102 if(!neg && result < current) result += 360.0; 103 if(neg && result > current) result -= 360.0; 104 *r = result; 105 106 return INTERP_OK; 107 } 108 109 110 /****************************************************************************/ 111 112 /*! find_ends 113 114 Returned Value: int (INTERP_OK) 115 116 Side effects: 117 The values of px, py, pz, aa_p, bb_p, and cc_p are set 118 119 Called by: 120 convert_arc 121 convert_home 122 convert_probe 123 convert_straight 124 125 This finds the coordinates of a point, "end", in the currently 126 active coordinate system, and sets the values of the pointers to the 127 coordinates (which are the arguments to the function). 128 129 In all cases, if no value for the coodinate is given in the block, the 130 current value for the coordinate is used. When cutter radius 131 compensation is on, this function is called before compensation 132 calculations are performed, so the current value of the programmed 133 point is used, not the current value of the actual current_point. 134 135 There are three cases for when the coordinate is included in the block: 136 137 1. G_53 is active. This means to interpret the coordinates as machine 138 coordinates. That is accomplished by adding the two offsets to the 139 coordinate given in the block. 140 141 2. Absolute coordinate mode is in effect. The coordinate in the block 142 is used. 143 144 3. Incremental coordinate mode is in effect. The coordinate in the 145 block plus either (i) the programmed current position - when cutter 146 radius compensation is in progress, or (2) the actual current position. 147 148 */ 149 150 151 int Interp::find_ends(block_pointer block, //!< pointer to a block of RS274/NGC instructions 152 setup_pointer s, //!< pointer to machine settings 153 double *px, //!< pointer to end_x 154 double *py, //!< pointer to end_y 155 double *pz, //!< pointer to end_z 156 double *AA_p, //!< pointer to end_a 157 double *BB_p, //!< pointer to end_b 158 double *CC_p, //!< pointer to end_c 159 double *u_p, double *v_p, double *w_p) 160 { 161 int middle; 162 int comp; 163 164 middle = !s->cutter_comp_firstmove; 165 comp = (s->cutter_comp_side); 166 167 if (block->g_modes[GM_MODAL_0] == G_53) { /* distance mode is absolute in this case */ 168 #ifdef DEBUG_EMC 169 COMMENT("interpreter: offsets temporarily suspended"); 170 #endif 171 CHKS((block->radius_flag || block->theta_flag), _("Cannot use polar coordinates with G53")); 172 173 double cx = s->current_x; 174 double cy = s->current_y; 175 rotate(&cx, &cy, s->rotation_xy); 176 177 if(block->x_flag) { 178 *px = block->x_number - s->origin_offset_x - s->axis_offset_x - s->tool_offset.tran.x; 179 } else { 180 *px = cx; 181 } 182 183 if(block->y_flag) { 184 *py = block->y_number - s->origin_offset_y - s->axis_offset_y - s->tool_offset.tran.y; 185 } else { 186 *py = cy; 187 } 188 189 rotate(px, py, -s->rotation_xy); 190 191 if(block->z_flag) { 192 *pz = block->z_number - s->origin_offset_z - s->axis_offset_z - s->tool_offset.tran.z; 193 } else { 194 *pz = s->current_z; 195 } 196 197 if(block->a_flag) { 198 if(s->a_axis_wrapped) { 199 CHP(unwrap_rotary(AA_p, block->a_number, 200 block->a_number - s->AA_origin_offset - s->AA_axis_offset - s->tool_offset.a, 201 s->AA_current, 'A')); 202 } else { 203 *AA_p = block->a_number - s->AA_origin_offset - s->AA_axis_offset; 204 } 205 } else { 206 *AA_p = s->AA_current; 207 } 208 209 if(block->b_flag) { 210 if(s->b_axis_wrapped) { 211 CHP(unwrap_rotary(BB_p, block->b_number, 212 block->b_number - s->BB_origin_offset - s->BB_axis_offset - s->tool_offset.b, 213 s->BB_current, 'B')); 214 } else { 215 *BB_p = block->b_number - s->BB_origin_offset - s->BB_axis_offset; 216 } 217 } else { 218 *BB_p = s->BB_current; 219 } 220 221 if(block->c_flag) { 222 if(s->c_axis_wrapped) { 223 CHP(unwrap_rotary(CC_p, block->c_number, 224 block->c_number - s->CC_origin_offset - s->CC_axis_offset - s->tool_offset.c, 225 s->CC_current, 'C')); 226 } else { 227 *CC_p = block->c_number - s->CC_origin_offset - s->CC_axis_offset; 228 } 229 } else { 230 *CC_p = s->CC_current; 231 } 232 233 if(block->u_flag) { 234 *u_p = block->u_number - s->u_origin_offset - s->u_axis_offset - s->tool_offset.u; 235 } else { 236 *u_p = s->u_current; 237 } 238 239 if(block->v_flag) { 240 *v_p = block->v_number - s->v_origin_offset - s->v_axis_offset - s->tool_offset.v; 241 } else { 242 *v_p = s->v_current; 243 } 244 245 if(block->w_flag) { 246 *w_p = block->w_number - s->w_origin_offset - s->w_axis_offset - s->tool_offset.w; 247 } else { 248 *w_p = s->w_current; 249 } 250 } else if (s->distance_mode == MODE_ABSOLUTE) { 251 252 if(block->x_flag) { 253 *px = block->x_number; 254 } else { 255 // both cutter comp planes affect X ... 256 *px = (comp && middle) ? s->program_x : s->current_x; 257 } 258 259 if(block->y_flag) { 260 *py = block->y_number; 261 } else { 262 // but only XY affects Y ... 263 *py = (comp && middle && s->plane == CANON_PLANE_XY) ? s->program_y : s->current_y; 264 } 265 266 if(block->radius_flag && block->theta_flag) { 267 CHKS((block->x_flag || block->y_flag), _("Cannot specify X or Y words with polar coordinate")); 268 *px = block->radius * cos(D2R(block->theta)); 269 *py = block->radius * sin(D2R(block->theta)); 270 } else if(block->radius_flag) { 271 double theta; 272 CHKS((block->x_flag || block->y_flag), _("Cannot specify X or Y words with polar coordinate")); 273 CHKS((*py == 0 && *px == 0), _("Must specify angle in polar coordinate if at the origin")); 274 theta = atan2(*py, *px); 275 *px = block->radius * cos(theta); 276 *py = block->radius * sin(theta); 277 } else if(block->theta_flag) { 278 double radius; 279 CHKS((block->x_flag || block->y_flag), _("Cannot specify X or Y words with polar coordinate")); 280 radius = hypot(*py, *px); 281 *px = radius * cos(D2R(block->theta)); 282 *py = radius * sin(D2R(block->theta)); 283 } 284 285 if(block->z_flag) { 286 *pz = block->z_number; 287 } else { 288 // and only XZ affects Z. 289 *pz = (comp && middle && s->plane == CANON_PLANE_XZ) ? s->program_z : s->current_z; 290 } 291 292 if(block->a_flag) { 293 if(s->a_axis_wrapped) { 294 CHP(unwrap_rotary(AA_p, block->a_number, block->a_number, s->AA_current, 'A')); 295 } else { 296 *AA_p = block->a_number; 297 } 298 } else { 299 *AA_p = s->AA_current; 300 } 301 302 if(block->b_flag) { 303 if(s->b_axis_wrapped) { 304 CHP(unwrap_rotary(BB_p, block->b_number, block->b_number, s->BB_current, 'B')); 305 } else { 306 *BB_p = block->b_number; 307 } 308 } else { 309 *BB_p = s->BB_current; 310 } 311 312 if(block->c_flag) { 313 if(s->c_axis_wrapped) { 314 CHP(unwrap_rotary(CC_p, block->c_number, block->c_number, s->CC_current, 'C')); 315 } else { 316 *CC_p = block->c_number; 317 } 318 } else { 319 *CC_p = s->CC_current; 320 } 321 322 *u_p = (block->u_flag) ? block->u_number : s->u_current; 323 *v_p = (block->v_flag) ? block->v_number : s->v_current; 324 *w_p = (block->w_flag) ? block->w_number : s->w_current; 325 326 } else { /* mode is MODE_INCREMENTAL */ 327 328 // both cutter comp planes affect X ... 329 *px = (comp && middle) ? s->program_x: s->current_x; 330 if(block->x_flag) *px += block->x_number; 331 332 // but only XY affects Y ... 333 *py = (comp && middle && s->plane == CANON_PLANE_XY) ? s->program_y: s->current_y; 334 if(block->y_flag) *py += block->y_number; 335 336 if(block->radius_flag) { 337 double radius, theta; 338 CHKS((block->x_flag || block->y_flag), _("Cannot specify X or Y words with polar coordinate")); 339 CHKS((*py == 0 && *px == 0), _("Incremental motion with polar coordinates is indeterminate when at the origin")); 340 theta = atan2(*py, *px); 341 radius = hypot(*py, *px) + block->radius; 342 *px = radius * cos(theta); 343 *py = radius * sin(theta); 344 } 345 346 if(block->theta_flag) { 347 double radius, theta; 348 CHKS((block->x_flag || block->y_flag), _("Cannot specify X or Y words with polar coordinate")); 349 CHKS((*py == 0 && *px == 0), _("G91 motion with polar coordinates is indeterminate when at the origin")); 350 theta = atan2(*py, *px) + D2R(block->theta); 351 radius = hypot(*py, *px); 352 *px = radius * cos(theta); 353 *py = radius * sin(theta); 354 } 355 356 // and only XZ affects Z. 357 *pz = (comp && middle && s->plane == CANON_PLANE_XZ) ? s->program_z: s->current_z; 358 if(block->z_flag) *pz += block->z_number; 359 360 *AA_p = s->AA_current; 361 if(block->a_flag) *AA_p += block->a_number; 362 363 *BB_p = s->BB_current; 364 if(block->b_flag) *BB_p += block->b_number; 365 366 *CC_p = s->CC_current; 367 if(block->c_flag) *CC_p += block->c_number; 368 369 *u_p = s->u_current; 370 if(block->u_flag) *u_p += block->u_number; 371 372 *v_p = s->v_current; 373 if(block->v_flag) *v_p += block->v_number; 374 375 *w_p = s->w_current; 376 if(block->w_flag) *w_p += block->w_number; 377 } 378 return INTERP_OK; 379 } 380 381 /****************************************************************************/ 382 383 /*! find_relative 384 385 Returned Value: int (INTERP_OK) 386 387 Side effects: 388 The values of x2, y2, z2, aa_2, bb_2, and cc_2 are set. 389 (NOTE: aa_2 etc. are written with lower case letters in this 390 documentation because upper case would confuse the pre-preprocessor.) 391 392 Called by: 393 convert_home 394 395 This finds the coordinates in the current system, under the current 396 tool length offset, of a point (x1, y1, z1, aa_1, bb_1, cc_1) whose absolute 397 coordinates are known. 398 399 Don't confuse this with the inverse operation. 400 401 */ 402 403 int Interp::find_relative(double x1, //!< absolute x position 404 double y1, //!< absolute y position 405 double z1, //!< absolute z position 406 double AA_1, //!< absolute a position 407 double BB_1, //!< absolute b position 408 double CC_1, //!< absolute c position 409 double u_1, 410 double v_1, 411 double w_1, 412 double *x2, //!< pointer to relative x 413 double *y2, //!< pointer to relative y 414 double *z2, //!< pointer to relative z 415 double *AA_2, //!< pointer to relative a 416 double *BB_2, //!< pointer to relative b 417 double *CC_2, //!< pointer to relative c 418 double *u_2, 419 double *v_2, 420 double *w_2, 421 setup_pointer settings) //!< pointer to machine settings 422 { 423 *x2 = x1 - settings->origin_offset_x - settings->axis_offset_x - settings->tool_offset.tran.x; 424 *y2 = y1 - settings->origin_offset_y - settings->axis_offset_y - settings->tool_offset.tran.y; 425 rotate(x2, y2, -settings->rotation_xy); 426 *z2 = z1 - settings->origin_offset_z - settings->axis_offset_z - settings->tool_offset.tran.z; 427 428 if(settings->a_axis_wrapped) { 429 CHP(unwrap_rotary(AA_2, AA_1, 430 AA_1 - settings->AA_origin_offset - settings->AA_axis_offset - settings->tool_offset.a, 431 settings->AA_current, 'A')); 432 } else { 433 *AA_2 = AA_1 - settings->AA_origin_offset - settings->AA_axis_offset; 434 } 435 436 if(settings->b_axis_wrapped) { 437 CHP(unwrap_rotary(BB_2, BB_1, 438 BB_1 - settings->BB_origin_offset - settings->BB_axis_offset - settings->tool_offset.b, 439 settings->BB_current, 'B')); 440 } else { 441 *BB_2 = BB_1 - settings->BB_origin_offset - settings->BB_axis_offset; 442 } 443 444 if(settings->c_axis_wrapped) { 445 CHP(unwrap_rotary(CC_2, CC_1, 446 CC_1 - settings->CC_origin_offset - settings->CC_axis_offset - settings->tool_offset.c, 447 settings->CC_current, 'C')); 448 } else { 449 *CC_2 = CC_1 - settings->CC_origin_offset - settings->CC_axis_offset; 450 } 451 452 *u_2 = u_1 - settings->u_origin_offset - settings->u_axis_offset - settings->tool_offset.u; 453 *v_2 = v_1 - settings->v_origin_offset - settings->v_axis_offset - settings->tool_offset.v; 454 *w_2 = w_1 - settings->w_origin_offset - settings->w_axis_offset - settings->tool_offset.w; 455 return INTERP_OK; 456 } 457 458 // find what the current coordinates would be if we were in a different system 459 460 int Interp::find_current_in_system(setup_pointer s, int system, 461 double *x, double *y, double *z, 462 double *a, double *b, double *c, 463 double *u, double *v, double *w) { 464 double *p = s->parameters; 465 466 *x = s->current_x; 467 *y = s->current_y; 468 *z = s->current_z; 469 *a = s->AA_current; 470 *b = s->BB_current; 471 *c = s->CC_current; 472 *u = s->u_current; 473 *v = s->v_current; 474 *w = s->w_current; 475 476 *x += s->axis_offset_x; 477 *y += s->axis_offset_y; 478 *z += s->axis_offset_z; 479 *a += s->AA_axis_offset; 480 *b += s->BB_axis_offset; 481 *c += s->CC_axis_offset; 482 *u += s->u_axis_offset; 483 *v += s->v_axis_offset; 484 *w += s->w_axis_offset; 485 486 rotate(x, y, s->rotation_xy); 487 488 *x += s->origin_offset_x; 489 *y += s->origin_offset_y; 490 *z += s->origin_offset_z; 491 *a += s->AA_origin_offset; 492 *b += s->BB_origin_offset; 493 *c += s->CC_origin_offset; 494 *u += s->u_origin_offset; 495 *v += s->v_origin_offset; 496 *w += s->w_origin_offset; 497 498 *x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]); 499 *y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]); 500 *z -= USER_TO_PROGRAM_LEN(p[5203 + system * 20]); 501 *a -= USER_TO_PROGRAM_ANG(p[5204 + system * 20]); 502 *b -= USER_TO_PROGRAM_ANG(p[5205 + system * 20]); 503 *c -= USER_TO_PROGRAM_ANG(p[5206 + system * 20]); 504 *u -= USER_TO_PROGRAM_LEN(p[5207 + system * 20]); 505 *v -= USER_TO_PROGRAM_LEN(p[5208 + system * 20]); 506 *w -= USER_TO_PROGRAM_LEN(p[5209 + system * 20]); 507 508 rotate(x, y, -p[5210 + system * 20]); 509 510 if (p[5210]) { 511 *x -= USER_TO_PROGRAM_LEN(p[5211]); 512 *y -= USER_TO_PROGRAM_LEN(p[5212]); 513 *z -= USER_TO_PROGRAM_LEN(p[5213]); 514 *a -= USER_TO_PROGRAM_ANG(p[5214]); 515 *b -= USER_TO_PROGRAM_ANG(p[5215]); 516 *c -= USER_TO_PROGRAM_ANG(p[5216]); 517 *u -= USER_TO_PROGRAM_LEN(p[5217]); 518 *v -= USER_TO_PROGRAM_LEN(p[5218]); 519 *w -= USER_TO_PROGRAM_LEN(p[5219]); 520 } 521 522 return INTERP_OK; 523 } 524 525 526 // find what the current coordinates would be if we were in a different system, 527 // if TLO were unapplied 528 529 int Interp::find_current_in_system_without_tlo(setup_pointer s, int system, 530 double *x, double *y, double *z, 531 double *a, double *b, double *c, 532 double *u, double *v, double *w) { 533 double *p = s->parameters; 534 535 *x = s->current_x; 536 *y = s->current_y; 537 *z = s->current_z; 538 *a = s->AA_current; 539 *b = s->BB_current; 540 *c = s->CC_current; 541 *u = s->u_current; 542 *v = s->v_current; 543 *w = s->w_current; 544 545 *x += s->axis_offset_x; 546 *y += s->axis_offset_y; 547 *z += s->axis_offset_z; 548 *a += s->AA_axis_offset; 549 *b += s->BB_axis_offset; 550 *c += s->CC_axis_offset; 551 *u += s->u_axis_offset; 552 *v += s->v_axis_offset; 553 *w += s->w_axis_offset; 554 555 rotate(x, y, s->rotation_xy); 556 557 *x += s->origin_offset_x; 558 *y += s->origin_offset_y; 559 *z += s->origin_offset_z; 560 *a += s->AA_origin_offset; 561 *b += s->BB_origin_offset; 562 *c += s->CC_origin_offset; 563 *u += s->u_origin_offset; 564 *v += s->v_origin_offset; 565 *w += s->w_origin_offset; 566 567 *x += s->tool_offset.tran.x; 568 *y += s->tool_offset.tran.y; 569 *z += s->tool_offset.tran.z; 570 *a += s->tool_offset.a; 571 *b += s->tool_offset.b; 572 *c += s->tool_offset.c; 573 *u += s->tool_offset.u; 574 *v += s->tool_offset.v; 575 *w += s->tool_offset.w; 576 577 *x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]); 578 *y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]); 579 *z -= USER_TO_PROGRAM_LEN(p[5203 + system * 20]); 580 *a -= USER_TO_PROGRAM_ANG(p[5204 + system * 20]); 581 *b -= USER_TO_PROGRAM_ANG(p[5205 + system * 20]); 582 *c -= USER_TO_PROGRAM_ANG(p[5206 + system * 20]); 583 *u -= USER_TO_PROGRAM_LEN(p[5207 + system * 20]); 584 *v -= USER_TO_PROGRAM_LEN(p[5208 + system * 20]); 585 *w -= USER_TO_PROGRAM_LEN(p[5209 + system * 20]); 586 587 rotate(x, y, -p[5210 + system * 20]); 588 589 if (p[5210]) { 590 *x -= USER_TO_PROGRAM_LEN(p[5211]); 591 *y -= USER_TO_PROGRAM_LEN(p[5212]); 592 *z -= USER_TO_PROGRAM_LEN(p[5213]); 593 *a -= USER_TO_PROGRAM_ANG(p[5214]); 594 *b -= USER_TO_PROGRAM_ANG(p[5215]); 595 *c -= USER_TO_PROGRAM_ANG(p[5216]); 596 *u -= USER_TO_PROGRAM_LEN(p[5217]); 597 *v -= USER_TO_PROGRAM_LEN(p[5218]); 598 *w -= USER_TO_PROGRAM_LEN(p[5219]); 599 } 600 601 return INTERP_OK; 602 } 603 604 /****************************************************************************/ 605 606 /*! find_straight_length 607 608 Returned Value: double (length of path between start and end points) 609 610 Side effects: none 611 612 Called by: 613 inverse_time_rate_straight 614 inverse_time_rate_as 615 616 This calculates a number to use in feed rate calculations when inverse 617 time feed mode is used, for a motion in which X,Y,Z,A,B, and C each change 618 linearly or not at all from their initial value to their end value. 619 620 This is used when the feed_reference mode is CANON_XYZ, which is 621 always in rs274NGC. 622 623 If any of the X, Y, or Z axes move or the A-axis, B-axis, and C-axis 624 do not move, this is the length of the path relative to the XYZ axes 625 from the first point to the second, and any rotary axis motion is 626 ignored. The length is the simple Euclidean distance. 627 628 The formula for the Euclidean distance "length" of a move involving 629 only the A, B and C axes is based on a conversation with Jim Frohardt at 630 Boeing, who says that the Fanuc controller on their 5-axis machine 631 interprets the feed rate this way. Note that if only one rotary axis 632 moves, this formula returns the absolute value of that axis move, 633 which is what is desired. 634 635 */ 636 637 double Interp::find_straight_length(double x2, //!< X-coordinate of end point 638 double y2, //!< Y-coordinate of end point 639 double z2, //!< Z-coordinate of end point 640 double AA_2, //!< A-coordinate of end point 641 double BB_2, //!< B-coordinate of end point 642 double CC_2, //!< C-coordinate of end point 643 double u_2, 644 double v_2, 645 double w_2, 646 double x1, //!< X-coordinate of start point 647 double y1, //!< Y-coordinate of start point 648 double z1, //!< Z-coordinate of start point 649 double AA_1, //!< A-coordinate of start point 650 double BB_1, //!< B-coordinate of start point 651 double CC_1, //!< C-coordinate of start point 652 double u_1, 653 double v_1, 654 double w_1 655 ) 656 { 657 #define tiny 1e-7 658 if ( (fabs(x1-x2) > tiny) || (fabs(y1-y2) > tiny) || (fabs(z1-z2) > tiny) ) 659 return sqrt(pow((x2 - x1), 2) + pow((y2 - y1), 2) + pow((z2 - z1), 2)); 660 else if ( (fabs(u_1-u_2) > tiny) || (fabs(v_1-v_2) > tiny) || (fabs(w_1-w_2) > tiny) ) 661 return sqrt(pow((u_2 - u_1), 2) + pow((v_2 - v_1), 2) + pow((w_2 - w_1), 2)); 662 else 663 return sqrt(pow((AA_2 - AA_1), 2) + pow((BB_2 - BB_1), 2) + pow((CC_2 - CC_1), 2)); 664 } 665 666 /****************************************************************************/ 667 668 /*! find_turn 669 670 Returned Value: double (angle in radians between two radii of a circle) 671 672 Side effects: none 673 674 Called by: find_arc_length 675 676 All angles are in radians. 677 678 */ 679 680 double Interp::find_turn(double x1, //!< X-coordinate of start point 681 double y1, //!< Y-coordinate of start point 682 double center_x, //!< X-coordinate of arc center 683 double center_y, //!< Y-coordinate of arc center 684 int turn, //!< no. of full or partial circles CCW 685 double x2, //!< X-coordinate of end point 686 double y2) //!< Y-coordinate of end point 687 { 688 double alpha; /* angle of first radius */ 689 double beta; /* angle of second radius */ 690 double theta; /* amount of turn of arc CCW - negative if CW */ 691 692 if (turn == 0) 693 return 0.0; 694 alpha = atan2((y1 - center_y), (x1 - center_x)); 695 beta = atan2((y2 - center_y), (x2 - center_x)); 696 if (turn > 0) { 697 if (beta <= alpha) 698 beta = (beta + (2 * M_PIl)); 699 theta = ((beta - alpha) + ((turn - 1) * (2 * M_PIl))); 700 } else { /* turn < 0 */ 701 702 if (alpha <= beta) 703 alpha = (alpha + (2 * M_PIl)); 704 theta = ((beta - alpha) + ((turn + 1) * (2 * M_PIl))); 705 } 706 return (theta); 707 } 708 709 int Interp::find_tool_pocket(setup_pointer settings, int toolno, int *pocket) 710 { 711 if(!settings->random_toolchanger && toolno == 0) { 712 *pocket = 0; 713 return INTERP_OK; 714 } 715 *pocket = -1; 716 for(int i=0; i<CANON_POCKETS_MAX; i++) { 717 if(settings->tool_table[i].toolno == toolno) 718 *pocket = i; 719 } 720 721 CHKS((*pocket == -1), (_("Requested tool %d not found in the tool table")), toolno); 722 return INTERP_OK; 723 }