blendmath.c
1 /******************************************************************** 2 * Description: blendmath.c 3 * Circular arc blend math functions 4 * 5 * Author: Robert W. Ellenberg 6 * License: GPL Version 2 7 * System: Linux 8 * 9 * Copyright (c) 2014 All rights reserved. 10 * 11 * Last change: 12 ********************************************************************/ 13 14 #include "posemath.h" 15 #include "tc_types.h" 16 #include "tc.h" 17 #include "tp_types.h" 18 #include "rtapi_math.h" 19 #include "spherical_arc.h" 20 #include "blendmath.h" 21 #include "tp_debug.h" 22 23 /** @section utilityfuncs Utility functions */ 24 25 /** 26 * Find the maximum angle allowed between "tangent" segments. 27 * @param v speed of motion in worst case (i.e. at max feed). 28 * @param acc magnitude of acceleration allowed during "kink". 29 * 30 * Since we are discretized by a timestep, the maximum allowable 31 * "kink" in a trajectory is bounded by normal acceleration. A small 32 * kink will effectively be one step along the tightest radius arc 33 * possible at a given speed. 34 */ 35 double findMaxTangentAngle(double v_plan, double acc_limit, double cycle_time) 36 { 37 //Find acc hiccup we're allowed to get 38 //TODO somewhat redundant with findKinkAccel, should refactor 39 double acc_margin = BLEND_ACC_RATIO_NORMAL * BLEND_KINK_FACTOR * acc_limit; 40 double dx = v_plan / cycle_time; 41 if (dx > 0.0) { 42 return (acc_margin / dx); 43 } else { 44 tp_debug_print(" Velocity or period is negative!\n"); 45 //Should not happen... 46 return TP_ANGLE_EPSILON; 47 } 48 } 49 50 51 /** 52 * Find the acceleration required to create a specific change in path 53 * direction, assuming constant speed. 54 * This determines how much of a "spike" in acceleration will occur due to a 55 * slight mismatch between tangent directions at the start / end of a segment. 56 */ 57 double findKinkAccel(double kink_angle, double v_plan, double cycle_time) 58 { 59 double dx = v_plan / cycle_time; 60 if (dx > 0.0) { 61 return (dx * kink_angle); 62 } else { 63 rtapi_print_msg(RTAPI_MSG_ERR, "dx < 0 in KinkAccel\n"); 64 return 0; 65 } 66 } 67 68 69 /** 70 * Sign function that returns a valid numerical result for sign(0), rather than NaN. 71 */ 72 double fsign(double f) 73 { 74 if (f>0) { 75 return 1.0; 76 } else if (f < 0) { 77 return -1.0; 78 } else { 79 //Technically this should be NAN but that's a useless result for tp purposes 80 return 0; 81 } 82 } 83 84 /** negate a value (or not) based on a bool parameter */ 85 static inline double negate(double f, int neg) 86 { 87 return (neg) ? -f : f; 88 } 89 90 /** Clip the input at the specified minimum (in place). */ 91 int clip_min(double * const x, double min) { 92 if ( *x < min ) { 93 *x = min; 94 return 1; 95 } 96 return 0; 97 } 98 99 100 /** Clip the input at the specified maximum (in place). */ 101 int clip_max(double * const x, double max) { 102 if ( *x > max ) { 103 *x = max; 104 return 1; 105 } 106 return 0; 107 } 108 109 /** 110 * Saturate a value x to be within +/- max. 111 */ 112 double saturate(double x, double max) { 113 if ( x > max ) { 114 return max; 115 } 116 else if ( x < (-max) ) { 117 return -max; 118 } 119 else { 120 return x; 121 } 122 } 123 124 /** 125 * Saturate a value x to be within max and min. 126 */ 127 double bisaturate(double x, double max, double min) { 128 if ( x > max ) { 129 return max; 130 } 131 else if ( x < min ) { 132 return min; 133 } 134 else { 135 return x; 136 } 137 } 138 139 140 /** 141 * Apply bounds to a value x. 142 */ 143 inline double bound(double x, double max, double min) { 144 if ( x > max ) { 145 return max; 146 } 147 else if ( x < (min) ) { 148 return min; 149 } 150 else { 151 return x; 152 } 153 } 154 155 156 /** In-place saturation function */ 157 int sat_inplace(double * const x, double max) { 158 if ( *x > max ) { 159 *x = max; 160 return 1; 161 } 162 else if ( *x < -max ) { 163 *x = -max; 164 return -1; 165 } 166 return 0; 167 } 168 169 #if 0 170 static int pmCirclePrint(PmCircle const * const circ) { 171 tp_debug_print(" center = %f %f %f\n", 172 circ->center.x, 173 circ->center.y, 174 circ->center.z); 175 tp_debug_print(" radius = %f\n", circ->radius); 176 tp_debug_print(" spiral = %f\n", circ->spiral); 177 tp_debug_print(" angle = %f\n", circ->angle); 178 //TODO add other debug data here as needed 179 return TP_ERR_OK; 180 } 181 #endif 182 183 /** 184 * @section geomfuncs Geometry check functions 185 */ 186 187 /** 188 * Calculate the best-fit circle to the spiral segment. 189 * @param circ spiral to be approximated 190 * @param base_pt the point about which the circle is fit 191 * @param u_tan tangent unit vector at the base point of the approximation 192 * @param[out] center_out displaced center for circular approximation 193 * @param[out] radius_out adjusted radius 194 * 195 * The adjusted center for the circle fit is found by displacing the center 196 * along the spiral tangent by the spiral coefficient. The adjusted radius is 197 * the distance between the base point and this new center. 198 * 199 */ 200 static inline int findSpiralApproximation(PmCircle const * const circ, 201 PmCartesian const * const base_pt, 202 PmCartesian const * const u_tan, 203 PmCartesian * const center_out, 204 double * const radius_out) 205 { 206 double dr = circ->spiral / circ->angle; 207 208 /*tp_debug_print("In findSpiralApproximation\n");*/ 209 /*tp_debug_print(" dr = %f\n",dr);*/ 210 /*tp_debug_print(" utan = %f %f %f\n",*/ 211 /*u_tan->x,*/ 212 /*u_tan->y,*/ 213 /*u_tan->z);*/ 214 pmCartScalMult(u_tan, dr, center_out); 215 /*tp_debug_print(" circcenter = %f %f %f\n",*/ 216 /*circ->center.x,*/ 217 /*circ->center.y,*/ 218 /*circ->center.z);*/ 219 pmCartCartAddEq(center_out, &circ->center); 220 221 PmCartesian r_adjust; 222 pmCartCartSub(base_pt, center_out, &r_adjust); 223 pmCartMag(&r_adjust, radius_out); 224 225 tp_debug_print(" adjusted center = %f %f %f\n", 226 center_out->x, 227 center_out->y, 228 center_out->z); 229 230 tp_debug_print(" adjusted radius = %f\n", *radius_out); 231 232 return TP_ERR_OK; 233 } 234 235 236 /** 237 * Calculate the angle to trim from a circle based on the blend geometry. 238 * 239 * @param P intersection point 240 * @param arc_center calculated center of blend arc 241 * @param center actual center of circle (not spiral approximated center) 242 * @return trim angle 243 */ 244 static inline double findTrimAngle(PmCartesian const * const P, 245 PmCartesian const * const arc_center, 246 PmCartesian const * const center) 247 { 248 //Define vectors relative to circle center 249 PmCartesian u_P; 250 pmCartCartSub(P, center, &u_P); 251 pmCartUnitEq(&u_P); 252 253 PmCartesian u_arccenter; 254 pmCartCartSub(arc_center, center, &u_arccenter); 255 pmCartUnitEq(&u_arccenter); 256 257 double dot; 258 pmCartCartDot(&u_arccenter, &u_P, &dot); 259 double dphi = acos(saturate(dot,1.0)); 260 tp_debug_print(" dphi = %g\n",dphi); 261 return dphi; 262 } 263 264 265 /** 266 * Verify that a blend arc is tangent to a circular arc. 267 */ 268 int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const arc, BlendGeom3 const * const geom, BlendParameters const * const param, double cycle_time, int at_end) 269 { 270 // Debug Information to diagnose tangent issues 271 PmCartesian u_circ, u_arc; 272 arcTangent(arc, &u_arc, at_end); 273 274 if (at_end) { 275 pmCircleTangentVector(circ, 0, &u_circ); 276 } else { 277 pmCircleTangentVector(circ, circ->angle, &u_circ); 278 } 279 280 pmCartUnitEq(&u_arc); 281 282 // Find angle between tangent unit vectors 283 double dot; 284 pmCartCartDot(&u_circ, &u_arc, &dot); 285 double blend_angle = acos(saturate(dot,1.0)); 286 287 // Check against the maximum allowed tangent angle for the given velocity and acceleration 288 double angle_max = findMaxTangentAngle(param->v_plan, param->a_max, cycle_time); 289 290 tp_debug_print("tangent angle = %f, max = %f\n", 291 blend_angle, 292 angle_max); 293 294 tp_debug_print("circ_tan = [%g %g %g]\n", 295 u_circ.x, 296 u_circ.y, 297 u_circ.z); 298 tp_debug_print("arc_tan = [%g %g %g]\n", 299 u_arc.x, 300 u_arc.y, 301 u_arc.z); 302 303 PmCartesian diff; 304 pmCartCartSub(&u_arc,&u_circ,&diff); 305 tp_debug_print("diff = [%g %g %g]\n", 306 diff.x, 307 diff.y, 308 diff.z); 309 310 if (blend_angle > angle_max) { 311 tp_debug_print("angle too large\n"); 312 return TP_ERR_FAIL; 313 } 314 315 return TP_ERR_OK; 316 } 317 318 319 /** 320 * Checks if two UNIT vectors are parallel to the given angle tolerance (in radians). 321 * @warning tol depends on the small angle approximation and will not be 322 * accurate for angles larger than about 10 deg. This function is meant for 323 * small tolerances! 324 */ 325 int pmCartCartParallel(PmCartesian const * const u1, 326 PmCartesian const * const u2, 327 double tol) 328 { 329 double d_diff; 330 { 331 PmCartesian u_diff; 332 pmCartCartSub(u1, u2, &u_diff); 333 pmCartMagSq(&u_diff, &d_diff); 334 } 335 336 tp_debug_json_start(pmCartCartParallel); 337 tp_debug_json_double(d_diff); 338 tp_debug_json_end(); 339 340 return d_diff < tol; 341 } 342 343 /** 344 * Checks if two UNIT vectors are anti-parallel to the given angle tolerance (in radians). 345 * @warning tol depends on the small angle approximation and will not be 346 * accurate for angles larger than about 10 deg. This function is meant for 347 * small tolerances! 348 */ 349 int pmCartCartAntiParallel(PmCartesian const * const u1, 350 PmCartesian const * const u2, 351 double tol) 352 { 353 double d_sum; 354 { 355 PmCartesian u_sum; 356 pmCartCartAdd(u1, u2, &u_sum); 357 pmCartMagSq(&u_sum, &d_sum); 358 } 359 360 tp_debug_json_start(pmCartCartAntiParallel); 361 tp_debug_json_double(d_sum); 362 tp_debug_json_end(); 363 364 return d_sum < tol; 365 } 366 367 368 /** 369 * Check if two cartesian vectors are parallel or anti-parallel 370 * The input tolerance specifies what the maximum angle between the 371 * lines containing two vectors is. Note that vectors pointing in 372 * opposite directions are still considered parallel, since their 373 * containing lines are parallel. 374 * @param u1 input unit vector 1 375 * @param u2 input unit vector 2 376 * @pre BOTH u1 and u2 must be unit vectors or calculation may be skewed. 377 */ 378 int pmUnitCartsColinear(PmCartesian const * const u1, 379 PmCartesian const * const u2) 380 { 381 return pmCartCartParallel(u1, u2, TP_ANGLE_EPSILON_SQ) || pmCartCartAntiParallel(u1, u2, TP_ANGLE_EPSILON_SQ); 382 } 383 384 385 /** 386 * Somewhat redundant function to calculate the segment intersection angle. 387 * The intersection angle is half of the supplement of the "divergence" angle 388 * between unit vectors. If two unit vectors are pointing in the same 389 * direction, then the intersection angle is PI/2. This is based on the 390 * simple_tp formulation for tolerances. 391 */ 392 int findIntersectionAngle(PmCartesian const * const u1, 393 PmCartesian const * const u2, double * const theta) 394 { 395 double dot; 396 pmCartCartDot(u1, u2, &dot); 397 398 if (dot > 1.0 || dot < -1.0) { 399 tp_debug_print("dot product %.16g outside domain of acos! u1 = %.16g %.16g %.16g, u2 = %.16g %.16g %.16g\n", 400 dot, 401 u1->x, 402 u1->y, 403 u1->z, 404 u2->x, 405 u2->y, 406 u2->z); 407 sat_inplace(&dot,1.0); 408 } 409 410 *theta = acos(-dot)/2.0; 411 return TP_ERR_OK; 412 } 413 414 415 /** Calculate the minimum of the three values in a PmCartesian. */ 416 double pmCartMin(PmCartesian const * const in) 417 { 418 return fmin(fmin(in->x,in->y),in->z); 419 } 420 421 422 /** 423 * Calculate the diameter of a circle incscribed on a central cross section of a 3D 424 * rectangular prism. 425 * 426 * @param normal normal direction of plane slicing prism. 427 * @param extents distance from center to one corner of the prism. 428 * @param diameter diameter of inscribed circle on cross section. 429 * 430 */ 431 int calculateInscribedDiameter(PmCartesian const * const normal, 432 PmCartesian const * const bounds, double * const diameter) 433 { 434 if (!normal ) { 435 return TP_ERR_MISSING_INPUT; 436 } 437 438 double n_mag; 439 pmCartMagSq(normal, &n_mag); 440 double mag_err = fabs(1.0 - n_mag); 441 if (mag_err > pmSqrt(TP_POS_EPSILON)) { 442 /*rtapi_print_msg(RTAPI_MSG_ERR,"normal vector <%.12g,%.12f,%.12f> has magnitude error = %e\n",*/ 443 /*normal->x,*/ 444 /*normal->y,*/ 445 /*normal->z,*/ 446 /*mag_err);*/ 447 return TP_ERR_FAIL; 448 } 449 450 PmCartesian planar_x,planar_y,planar_z; 451 452 //Find perpendicular component of unit directions 453 // FIXME Assumes normal is unit length 454 455 /* This section projects the X / Y / Z unit vectors onto the plane 456 * containing the motions. The operation is done "backwards" here due to a 457 * quirk with posemath. 458 * 459 */ 460 pmCartScalMult(normal, -normal->x, &planar_x); 461 pmCartScalMult(normal, -normal->y, &planar_y); 462 pmCartScalMult(normal, -normal->z, &planar_z); 463 464 planar_x.x += 1.0; 465 planar_y.y += 1.0; 466 planar_z.z += 1.0; 467 468 pmCartAbs(&planar_x, &planar_x); 469 pmCartAbs(&planar_y, &planar_y); 470 pmCartAbs(&planar_z, &planar_z); 471 472 // Crude way to prevent divide-by-zero-error 473 planar_x.x = fmax(planar_x.x,TP_POS_EPSILON); 474 planar_y.y = fmax(planar_y.y,TP_POS_EPSILON); 475 planar_z.z = fmax(planar_z.z,TP_POS_EPSILON); 476 477 double x_scale, y_scale, z_scale; 478 pmCartMag(&planar_x, &x_scale); 479 pmCartMag(&planar_y, &y_scale); 480 pmCartMag(&planar_z, &z_scale); 481 482 double x_extent=0, y_extent=0, z_extent=0; 483 if (bounds->x != 0) { 484 x_extent = bounds->x / x_scale; 485 } 486 if (bounds->y != 0) { 487 y_extent = bounds->y / y_scale; 488 } 489 if (bounds->z != 0) { 490 z_extent = bounds->z / z_scale; 491 } 492 493 // Find the highest value to start from 494 *diameter = fmax(fmax(x_extent, y_extent),z_extent); 495 496 // Only for active axes, find the minimum extent 497 if (bounds->x != 0) { 498 *diameter = fmin(*diameter, x_extent); 499 } 500 if (bounds->y != 0) { 501 *diameter = fmin(*diameter, y_extent); 502 } 503 if (bounds->z != 0) { 504 *diameter = fmin(*diameter, z_extent); 505 } 506 507 return TP_ERR_OK; 508 } 509 510 511 512 int findAccelScale(PmCartesian const * const acc, 513 PmCartesian const * const bounds, 514 PmCartesian * const scale) 515 { 516 if (!acc || !bounds ) { 517 return TP_ERR_MISSING_INPUT; 518 } 519 520 if (!scale ) { 521 return TP_ERR_MISSING_OUTPUT; 522 } 523 524 // Find the scale of acceleration vs. machine accel bounds 525 if (bounds->x != 0) { 526 scale->x = fabs(acc->x / bounds->x); 527 } else { 528 scale->x = 0; 529 } 530 if (bounds->y != 0) { 531 scale->y = fabs(acc->y / bounds->y); 532 } else { 533 scale->y = 0; 534 } 535 536 if (bounds->z != 0) { 537 scale->z = fabs(acc->z / bounds->z); 538 } else { 539 scale->z = 0; 540 } 541 542 return TP_ERR_OK; 543 } 544 545 546 547 548 /** Find real roots of a quadratic equation in standard form. */ 549 int quadraticFormula(double A, double B, double C, double * const root0, 550 double * const root1) 551 { 552 double disc = pmSq(B) - 4.0 * A * C; 553 if (disc < 0) { 554 tp_debug_print("discriminant %.12g < 0, A=%.12g, B=%.12g,C=%.12g\n", disc, A, B, C); 555 return TP_ERR_FAIL; 556 } 557 double t1 = pmSqrt(disc); 558 if (root0) { 559 *root0 = ( -B + t1) / (2.0 * A); 560 } 561 if (root1) { 562 *root1 = ( -B - t1) / (2.0 * A); 563 } 564 return TP_ERR_OK; 565 } 566 567 /** 568 * @section blending blend math functions 569 */ 570 571 /** 572 * Setup common geom parameters based on trajectory segments. 573 * This function populates the geom structure and "input" fields of 574 * the blend parameter structure. It returns an error if the segments 575 * are not coplanar, or if one or both segments is not a circular arc. 576 * 577 * @param geom Stores simplified geometry used to calculate blend params. 578 * @param prev_tc first linear move to blend 579 * @param tc second linear move to blend 580 */ 581 int blendGeom3Init(BlendGeom3 * const geom, 582 TC_STRUCT const * const prev_tc, 583 TC_STRUCT const * const tc) 584 { 585 geom->v_max1 = prev_tc->maxvel; 586 geom->v_max2 = tc->maxvel; 587 588 // Get tangent unit vectors to each arc at the intersection point 589 int res_u1 = tcGetEndTangentUnitVector(prev_tc, &geom->u_tan1); 590 int res_u2 = tcGetStartTangentUnitVector(tc, &geom->u_tan2); 591 592 // Initialize u1 and u2 by assuming they match the tangent direction 593 geom->u1 = geom->u_tan1; 594 geom->u2 = geom->u_tan2; 595 596 int res_intersect = tcGetIntersectionPoint(prev_tc, tc, &geom->P); 597 598 tp_debug_print("Intersection point P = %f %f %f\n", 599 geom->P.x, 600 geom->P.y, 601 geom->P.z); 602 603 // Find angle between tangent vectors 604 int res_angle = findIntersectionAngle(&geom->u_tan1, 605 &geom->u_tan2, 606 &geom->theta_tan); 607 608 // Test for intersection angle errors 609 if(PM_PI / 2.0 - geom->theta_tan < TP_ANGLE_EPSILON) { 610 tp_debug_print("Intersection angle too close to pi/2, can't compute normal\n"); 611 return TP_ERR_TOLERANCE; 612 } 613 614 if(geom->theta_tan < TP_ANGLE_EPSILON) { 615 tp_debug_print("Intersection angle too small for arc fit\n"); 616 return TP_ERR_TOLERANCE; 617 } 618 619 blendCalculateNormals3(geom); 620 621 return res_u1 | 622 res_u2 | 623 res_intersect | 624 res_angle; 625 } 626 627 628 /** 629 * Initialize common fields in parameters structure. 630 * 631 * @param geom Stores simplified geometry used to calculate blend params. 632 * @param param Abstracted parameters for blending calculations 633 * @param acc_bound maximum X, Y, Z machine acceleration 634 * @param vel_bound maximum X, Y, Z machine velocity 635 * @param maxFeedScale maximum allowed feed override (set in INI) 636 */ 637 int blendParamKinematics(BlendGeom3 * const geom, 638 BlendParameters * const param, 639 TC_STRUCT const * const prev_tc, 640 TC_STRUCT const * const tc, 641 PmCartesian const * const acc_bound, 642 PmCartesian const * const vel_bound, 643 double maxFeedScale) 644 { 645 646 // KLUDGE: common operations, but not exactly kinematics 647 param->phi = (PM_PI - param->theta * 2.0); 648 649 double nominal_tolerance; 650 tcFindBlendTolerance(prev_tc, tc, ¶m->tolerance, &nominal_tolerance); 651 652 // Calculate max acceleration based on plane containing lines 653 int res_dia = calculateInscribedDiameter(&geom->binormal, acc_bound, ¶m->a_max); 654 655 // Store max normal acceleration 656 param->a_n_max = param->a_max * BLEND_ACC_RATIO_NORMAL; 657 tp_debug_print("a_max = %f, a_n_max = %f\n", param->a_max, 658 param->a_n_max); 659 660 // Find the nominal velocity for the blend segment with no overrides 661 double v_req_prev = tcGetMaxTargetVel(prev_tc, 1.0); 662 double v_req_this = tcGetMaxTargetVel(tc, 1.0); 663 tp_debug_print("vr_prev = %f, vr_this = %f\n", v_req_prev, v_req_this); 664 param->v_req = fmax(v_req_prev, v_req_this); 665 666 // Find the worst-case velocity we should reach for either segment 667 param->v_goal = fmax(tcGetMaxTargetVel(prev_tc, maxFeedScale), 668 tcGetMaxTargetVel(tc, maxFeedScale)); 669 670 // Calculate the maximum planar velocity 671 double v_planar_max; 672 //FIXME sloppy handling of return value 673 res_dia |= calculateInscribedDiameter(&geom->binormal, vel_bound, &v_planar_max); 674 tp_debug_print("v_planar_max = %f\n", v_planar_max); 675 676 // Clip the angle at a reasonable value (less than 90 deg), to prevent div by zero 677 double phi_effective = fmin(param->phi, PM_PI * 0.49); 678 679 // Copy over maximum velocities, clipping velocity to place altitude within base 680 double v_max1 = fmin(prev_tc->maxvel, tc->maxvel / cos(phi_effective)); 681 double v_max2 = fmin(tc->maxvel, prev_tc->maxvel / cos(phi_effective)); 682 683 tp_debug_print("v_max1 = %f, v_max2 = %f\n", v_max1, v_max2); 684 685 // Get "altitude" 686 double v_area = v_max1 * v_max2 / 2.0 * sin(param->phi); 687 tp_debug_print("phi = %f\n", param->phi); 688 tp_debug_print("v_area = %f\n", v_area); 689 690 // Get "base" of triangle 691 PmCartesian tmp1, tmp2, diff; 692 pmCartScalMult(&geom->u1, v_max1, &tmp1); 693 pmCartScalMult(&geom->u2, v_max2, &tmp2); 694 pmCartCartSub(&tmp2, &tmp1, &diff); 695 double base; 696 pmCartMag(&diff, &base); 697 tp_debug_print("v_base = %f\n", base); 698 699 double v_max_alt = 2.0 * v_area / base; 700 701 // Can't do altitude-based velocity calculation if we have arcs 702 if (prev_tc->motion_type != TC_LINEAR || tc->motion_type != TC_LINEAR) { 703 v_max_alt = 0.0; 704 } 705 706 tp_debug_print("v_max_alt = %f\n", v_max_alt); 707 double v_max = fmax(v_max_alt, v_planar_max); 708 709 tp_debug_print("v_max = %f\n", v_max); 710 param->v_goal = fmin(param->v_goal, v_max); 711 712 tp_debug_print("v_goal = %f, max scale = %f\n", param->v_goal, maxFeedScale); 713 714 return res_dia; 715 } 716 717 /** 718 * Setup blend parameters based on a line and an arc. 719 * This function populates the geom structure and "input" fields of 720 * the blend parameter structure. It returns an error if the segments 721 * are not coplanar, or if one or both segments is not a circular arc. 722 * 723 * @param geom Stores simplified geometry used to calculate blend params. 724 * @param param Abstracted parameters for blending calculations 725 * @param prev_tc first linear move to blend 726 * @param tc second linear move to blend 727 * @param acc_bound maximum X, Y, Z machine acceleration 728 * @param vel_bound maximum X, Y, Z machine velocity 729 * @param maxFeedScale maximum allowed feed override (set in INI) 730 */ 731 int blendInit3FromLineArc(BlendGeom3 * const geom, BlendParameters * const param, 732 TC_STRUCT const * const prev_tc, 733 TC_STRUCT const * const tc, 734 PmCartesian const * const acc_bound, 735 PmCartesian const * const vel_bound, 736 double maxFeedScale) 737 { 738 739 if (tc->motion_type != TC_CIRCULAR || prev_tc->motion_type != TC_LINEAR) { 740 return TP_ERR_INPUT_TYPE; 741 } 742 743 int res_init = blendGeom3Init(geom, prev_tc, tc); 744 if (res_init != TP_ERR_OK) { 745 return res_init; 746 } 747 748 //Fit spiral approximation 749 findSpiralApproximation(&tc->coords.circle.xyz, 750 &geom->P, 751 &geom->u_tan2, 752 &geom->center2, 753 &geom->radius2); 754 // Handle convexity 755 param->convex2 = arcConvexTest(&geom->center2, &geom->P, &geom->u_tan1, true); 756 tp_debug_print("circ2 convex: %d\n", 757 param->convex2); 758 759 //Identify max angle for first arc by blend limits 760 // TODO better name? 761 double blend_angle_2 = param->convex2 ? geom->theta_tan : PM_PI / 2.0; 762 763 param->phi2_max = fmin(tc->coords.circle.xyz.angle / 3.0, blend_angle_2); 764 param->theta = geom->theta_tan; 765 766 if (param->convex2) { 767 PmCartesian blend_point; 768 pmCirclePoint(&tc->coords.circle.xyz, 769 param->phi2_max / 2.0, 770 &blend_point); 771 //Create new unit vector based on secant line 772 // Direction is away from P (at start of segment) 773 pmCartCartSub(&blend_point, &geom->P, &geom->u2); 774 pmCartUnitEq(&geom->u2); 775 //Reduce theta proportionally to the angle between the secant and the normal 776 param->theta = fmin(param->theta, geom->theta_tan - param->phi2_max / 4.0); 777 } 778 779 tp_debug_print("phi2_max = %f\n", param->phi2_max); 780 blendGeom3Print(geom); 781 782 // Check that we're not below the minimum intersection angle (making too tight an arc) 783 // FIXME make this an INI setting? 784 const double theta_min = PM_PI / 6.0; 785 if (param->theta < theta_min) { 786 tp_debug_print("theta = %f < min %f, aborting arc...\n", 787 param->theta, 788 theta_min); 789 } 790 791 tp_debug_print("theta = %f\n", param->theta); 792 793 param->phi = (PM_PI - param->theta * 2.0); 794 795 param->L1 = fmin(prev_tc->target, prev_tc->nominal_length / 2.0); 796 797 if (param->convex2) { 798 //use half of the length of the chord 799 param->L2 = sin(param->phi2_max/4.0) * geom->radius2; 800 } else { 801 param->L2 = param->phi2_max * geom->radius2; 802 } 803 804 tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); 805 806 // Setup common parameters 807 int res_kin = blendParamKinematics(geom, 808 param, 809 prev_tc, 810 tc, 811 acc_bound, 812 vel_bound, 813 maxFeedScale); 814 815 return res_kin; 816 } 817 818 int blendInit3FromArcLine(BlendGeom3 * const geom, BlendParameters * const param, 819 TC_STRUCT const * const prev_tc, 820 TC_STRUCT const * const tc, 821 PmCartesian const * const acc_bound, 822 PmCartesian const * const vel_bound, 823 double maxFeedScale) 824 { 825 826 if (tc->motion_type != TC_LINEAR || prev_tc->motion_type != TC_CIRCULAR) { 827 return TP_ERR_INPUT_TYPE; 828 } 829 830 int res_init = blendGeom3Init(geom, prev_tc, tc); 831 if (res_init != TP_ERR_OK) { 832 return res_init; 833 } 834 835 findSpiralApproximation(&prev_tc->coords.circle.xyz, 836 &geom->P, 837 &geom->u_tan1, 838 &geom->center1, 839 &geom->radius1); 840 841 param->convex1 = arcConvexTest(&geom->center1, &geom->P, &geom->u_tan2, false); 842 tp_debug_print("circ1 convex: %d\n", 843 param->convex1); 844 845 //Identify max angle for first arc by blend limits 846 // TODO better name? 847 double blend_angle_1 = param->convex1 ? geom->theta_tan : PM_PI / 2.0; 848 849 param->phi1_max = fmin(prev_tc->coords.circle.xyz.angle * 2.0 / 3.0, blend_angle_1); 850 param->theta = geom->theta_tan; 851 852 // Build the correct unit vector for the linear approximation 853 if (param->convex1) { 854 PmCartesian blend_point; 855 pmCirclePoint(&prev_tc->coords.circle.xyz, 856 prev_tc->coords.circle.xyz.angle - param->phi1_max / 2.0 , 857 &blend_point); 858 //Create new unit vector based on secant line 859 // Direction is toward P (at end of segment) 860 pmCartCartSub(&geom->P, &blend_point, &geom->u1); 861 pmCartUnitEq(&geom->u1); 862 863 //Reduce theta proportionally to the angle between the secant and the normal 864 param->theta = fmin(param->theta, geom->theta_tan - param->phi1_max / 4.0); 865 } 866 867 blendGeom3Print(geom); 868 tp_debug_print("phi1_max = %f\n", param->phi1_max); 869 870 // Check that we're not below the minimum intersection angle (making too tight an arc) 871 // FIXME make this an INI setting? 872 const double theta_min = PM_PI / 6.0; 873 if (param->theta < theta_min) { 874 tp_debug_print("theta = %f < min %f, aborting arc...\n", 875 param->theta, 876 theta_min); 877 } 878 879 tp_debug_print("theta = %f\n", param->theta); 880 881 // Use end radius here 882 param->L1 = param->phi1_max * (geom->radius1); 883 param->L2 = tc->nominal_length / 2.0; 884 885 if (param->convex1) { 886 //use half of the length of the chord 887 param->L1 = sin(param->phi1_max/4.0) * geom->radius1; 888 } 889 tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); 890 891 // Setup common parameters 892 int res_kin = blendParamKinematics(geom, 893 param, 894 prev_tc, 895 tc, 896 acc_bound, 897 vel_bound, 898 maxFeedScale); 899 900 return res_kin; 901 } 902 903 904 /** 905 * Setup blend parameters based on two circular arc segments. 906 * This function populates the geom structure and "input" fields of 907 * the blend parameter structure. It returns an error if the segments 908 * are not coplanar, or if one or both segments is not a circular arc. 909 * 910 * @param geom Stores simplified geometry used to calculate blend params. 911 * @param param Abstracted parameters for blending calculations 912 * @param prev_tc first linear move to blend 913 * @param tc second linear move to blend 914 * @param acc_bound maximum X, Y, Z machine acceleration 915 * @param vel_bound maximum X, Y, Z machine velocity 916 * @param maxFeedScale maximum allowed feed override (set in INI) 917 */ 918 int blendInit3FromArcArc(BlendGeom3 * const geom, BlendParameters * const param, 919 TC_STRUCT const * const prev_tc, 920 TC_STRUCT const * const tc, 921 PmCartesian const * const acc_bound, 922 PmCartesian const * const vel_bound, 923 double maxFeedScale) 924 { 925 if (tc->motion_type != TC_CIRCULAR || prev_tc->motion_type != TC_CIRCULAR) { 926 return TP_ERR_FAIL; 927 } 928 929 int res_init = blendGeom3Init(geom, prev_tc, tc); 930 if (res_init != TP_ERR_OK) { 931 return res_init; 932 } 933 934 findSpiralApproximation(&prev_tc->coords.circle.xyz, 935 &geom->P, 936 &geom->u_tan1, 937 &geom->center1, 938 &geom->radius1); 939 940 findSpiralApproximation(&tc->coords.circle.xyz, 941 &geom->P, 942 &geom->u_tan2, 943 &geom->center2, 944 &geom->radius2); 945 946 947 //Do normal calculation here since we need this information for accel / vel limits 948 blendCalculateNormals3(geom); 949 950 // Get intersection point from circle start 951 pmCirclePoint(&tc->coords.circle.xyz, 0.0, &geom->P); 952 tp_debug_print("Intersection point P = %f %f %f\n", 953 geom->P.x, 954 geom->P.y, 955 geom->P.z); 956 957 param->convex1 = arcConvexTest(&geom->center1, &geom->P, &geom->u_tan2, false); 958 param->convex2 = arcConvexTest(&geom->center2, &geom->P, &geom->u_tan1, true); 959 tp_debug_print("circ1 convex: %d, circ2 convex: %d\n", 960 param->convex1, 961 param->convex2); 962 963 //Identify max angle for first arc by blend limits 964 // TODO better name? 965 double blend_angle_1 = param->convex1 ? geom->theta_tan : PM_PI / 2.0; 966 double blend_angle_2 = param->convex2 ? geom->theta_tan : PM_PI / 2.0; 967 968 param->phi1_max = fmin(prev_tc->coords.circle.xyz.angle * 2.0 / 3.0, blend_angle_1); 969 param->phi2_max = fmin(tc->coords.circle.xyz.angle / 3.0, blend_angle_2); 970 971 param->theta = geom->theta_tan; 972 973 // Build the correct unit vector for the linear approximation 974 if (param->convex1) { 975 PmCartesian blend_point; 976 pmCirclePoint(&prev_tc->coords.circle.xyz, 977 prev_tc->coords.circle.xyz.angle - param->phi1_max / 2.0, 978 &blend_point); 979 //Create new unit vector based on secant line 980 // Direction is toward P (at end of segment) 981 pmCartCartSub(&geom->P, &blend_point, &geom->u1); 982 pmCartUnitEq(&geom->u1); 983 984 //Reduce theta proportionally to the angle between the secant and the normal 985 param->theta = fmin(param->theta, geom->theta_tan - param->phi1_max / 4.0); 986 987 } 988 989 if (param->convex2) { 990 PmCartesian blend_point; 991 pmCirclePoint(&tc->coords.circle.xyz, 992 param->phi2_max / 2.0, 993 &blend_point); 994 //Create new unit vector based on secant line 995 // Direction is away from P (at start of segment) 996 pmCartCartSub(&blend_point, &geom->P, &geom->u2); 997 pmCartUnitEq(&geom->u2); 998 999 //Reduce theta proportionally to the angle between the secant and the normal 1000 param->theta = fmin(param->theta, geom->theta_tan - param->phi2_max / 4.0); 1001 } 1002 blendGeom3Print(geom); 1003 1004 1005 // Check that we're not below the minimum intersection angle (making too tight an arc) 1006 // FIXME make this an INI setting? 1007 const double theta_min = PM_PI / 12.0; 1008 if (param->theta < theta_min) { 1009 tp_debug_print("theta = %f < min %f, aborting arc...\n", 1010 param->theta, 1011 theta_min); 1012 return TP_ERR_FAIL; 1013 } 1014 1015 tp_debug_print("theta = %f\n", param->theta); 1016 1017 param->phi = (PM_PI - param->theta * 2.0); 1018 1019 param->L1 = param->phi1_max * geom->radius1; 1020 param->L2 = param->phi2_max * geom->radius2; 1021 1022 if (param->convex1) { 1023 //use half of the length of the chord 1024 param->L1 = sin(param->phi1_max/4.0) * geom->radius1; 1025 } 1026 if (param->convex2) { 1027 //use half of the length of the chord 1028 param->L2 = sin(param->phi2_max/4.0) * geom->radius2; 1029 } 1030 tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); 1031 tp_debug_print("phi1_max = %f\n",param->phi1_max); 1032 tp_debug_print("phi2_max = %f\n",param->phi2_max); 1033 1034 // Setup common parameters 1035 int res_kin = blendParamKinematics(geom, 1036 param, 1037 prev_tc, 1038 tc, 1039 acc_bound, 1040 vel_bound, 1041 maxFeedScale); 1042 1043 return res_kin; 1044 } 1045 1046 /** 1047 * Setup blend parameters based on two linear segments. 1048 * This function populates the geom structure and "input" fields of the blend parameter structure based. 1049 * @param geom Stores simplified geometry used to calculate blend params. 1050 * @param param Abstracted parameters for blending calculations 1051 * @param prev_tc first linear move to blend 1052 * @param tc second linear move to blend 1053 * @param acc_bound maximum X, Y, Z machine acceleration 1054 * @param vel_bound maximum X, Y, Z machine velocity 1055 * @param maxFeedScale maximum allowed feed override (set in INI) 1056 */ 1057 int blendInit3FromLineLine(BlendGeom3 * const geom, BlendParameters * const param, 1058 TC_STRUCT const * const prev_tc, 1059 TC_STRUCT const * const tc, 1060 PmCartesian const * const acc_bound, 1061 PmCartesian const * const vel_bound, 1062 double maxFeedScale) 1063 { 1064 1065 if (tc->motion_type != TC_LINEAR || prev_tc->motion_type != TC_LINEAR) { 1066 return TP_ERR_FAIL; 1067 } 1068 1069 int res_init = blendGeom3Init(geom, prev_tc, tc); 1070 if (res_init != TP_ERR_OK) { 1071 return res_init; 1072 } 1073 1074 param->theta = geom->theta_tan; 1075 1076 tp_debug_print("theta = %f\n", param->theta); 1077 1078 param->phi = (PM_PI - param->theta * 2.0); 1079 1080 blendGeom3Print(geom); 1081 1082 //Nominal length restriction prevents gobbling too much of parabolic blends 1083 param->L1 = fmin(prev_tc->target, prev_tc->nominal_length * BLEND_DIST_FRACTION); 1084 param->L2 = tc->target * BLEND_DIST_FRACTION; 1085 tp_debug_print("prev. nominal length = %f, next nominal_length = %f\n", 1086 prev_tc->nominal_length, tc->nominal_length); 1087 tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); 1088 1089 // Setup common parameters 1090 int res_kin = blendParamKinematics(geom, 1091 param, 1092 prev_tc, 1093 tc, 1094 acc_bound, 1095 vel_bound, 1096 maxFeedScale); 1097 1098 return res_kin; 1099 } 1100 1101 1102 /** 1103 * Calculate plane normal and binormal based on unit direction vectors. 1104 */ 1105 int blendCalculateNormals3(BlendGeom3 * const geom) 1106 { 1107 1108 int err_cross = pmCartCartCross(&geom->u_tan1, 1109 &geom->u_tan2, 1110 &geom->binormal); 1111 int err_unit_b = pmCartUnitEq(&geom->binormal); 1112 1113 tp_debug_print("binormal = [%f %f %f]\n", geom->binormal.x, 1114 geom->binormal.y, 1115 geom->binormal.z); 1116 1117 pmCartCartSub(&geom->u_tan2, &geom->u_tan1, &geom->normal); 1118 int err_unit_n = pmCartUnitEq(&geom->normal); 1119 1120 tp_debug_print("normal = [%f %f %f]\n", geom->normal.x, 1121 geom->normal.y, 1122 geom->normal.z); 1123 return (err_cross || err_unit_b || err_unit_n); 1124 } 1125 1126 /** 1127 * Compute blend parameters based on line data. 1128 * Blend arc parameters such as radius and velocity are calculated here. These 1129 * parameters are later used to create the actual arc geometry in other 1130 * functions. 1131 */ 1132 int blendComputeParameters(BlendParameters * const param) 1133 { 1134 1135 // Find maximum distance h from arc center to intersection point 1136 double h_tol = param->tolerance / (1.0 - sin(param->theta)); 1137 1138 // Find maximum distance along lines allowed by tolerance 1139 double d_tol = cos(param->theta) * h_tol; 1140 tp_debug_print(" d_tol = %f\n", d_tol); 1141 1142 // Find minimum distance by blend length constraints 1143 double d_lengths = fmin(param->L1, param->L2); 1144 double d_geom = fmin(d_lengths, d_tol); 1145 // Find radius from the limiting length 1146 double R_geom = tan(param->theta) * d_geom; 1147 1148 // Find maximum velocity allowed by accel and radius 1149 double v_normal = pmSqrt(param->a_n_max * R_geom); 1150 tp_debug_print("v_normal = %f\n", v_normal); 1151 1152 param->v_plan = fmin(v_normal, param->v_goal); 1153 1154 /*Get the limiting velocity of the equivalent parabolic blend. We use the 1155 * time it would take to do a "stock" parabolic blend as a metric for how 1156 * much of the segment to consume. A long segment will have a high 1157 * "triangle" velocity, so the radius will only be as large as is needed to 1158 * reach the cornering speed. A short segment will have a low triangle 1159 * velocity, much lower than the actual curvature limit, which can be used 1160 * to calculate an equivalent blend radius. 1161 * */ 1162 double a_parabolic = param->a_max * 0.5; 1163 double v_triangle = pmSqrt(2.0 * a_parabolic * d_geom); 1164 double t_blend = fmin(v_triangle, param->v_plan) / (a_parabolic); 1165 double s_blend = t_blend * param->v_plan; 1166 double R_blend = fmin(s_blend / param->phi, R_geom); //Clamp by limiting radius 1167 1168 param->R_plan = fmax(pmSq(param->v_plan) / param->a_n_max, R_blend); 1169 param->d_plan = param->R_plan / tan(param->theta); 1170 1171 tp_debug_print("v_plan = %f\n", param->v_plan); 1172 tp_debug_print("R_plan = %f\n", param->R_plan); 1173 tp_debug_print("d_plan = %f\n", param->d_plan); 1174 1175 /* "Actual" velocity means the velocity when feed override is 1.0. Recall 1176 * that v_plan may be greater than v_req by the max feed override. If our 1177 * worst-case planned velocity is higher than the requested velocity, then 1178 * clip at the requested velocity. This allows us to increase speed above 1179 * the feed override limits. 1180 */ 1181 if (param->v_plan > param->v_req) { 1182 param->v_actual = param->v_req; 1183 } else { 1184 param->v_actual = param->v_plan; 1185 } 1186 1187 // Store arc length of blend arc for future checks 1188 param->s_arc = param->R_plan * param->phi; 1189 1190 if (param->R_plan < TP_POS_EPSILON) { 1191 tp_debug_print("#Blend radius too small, aborting arc\n"); 1192 return TP_ERR_FAIL; 1193 } 1194 1195 if (param->s_arc < TP_MIN_ARC_LENGTH) { 1196 tp_debug_print("#Blend arc length too small, aborting arc\n"); 1197 return TP_ERR_FAIL; 1198 } 1199 return TP_ERR_OK; 1200 } 1201 1202 1203 /** Check if the previous line segment will be consumed based on the blend arc parameters. */ 1204 int blendCheckConsume(BlendParameters * const param, 1205 BlendPoints3 const * const points, 1206 TC_STRUCT const * const prev_tc, int gap_cycles) 1207 { 1208 //Initialize values 1209 param->consume = 0; 1210 param->line_length = 0; 1211 if (!prev_tc) { 1212 return -1; 1213 } 1214 1215 if (prev_tc->motion_type != TC_LINEAR) { 1216 return 0; 1217 } 1218 1219 //Check for segment length limits 1220 double L_prev = prev_tc->target - points->trim1; 1221 double prev_seg_time = L_prev / param->v_plan; 1222 1223 bool can_consume = tcCanConsume(prev_tc); 1224 param->consume = (prev_seg_time < gap_cycles * prev_tc->cycle_time && can_consume); 1225 if (param->consume) { 1226 tp_debug_print("consuming prev line, L_prev = %g\n", 1227 L_prev); 1228 param->line_length = L_prev; 1229 } 1230 return 0; 1231 } 1232 1233 1234 /** 1235 * Compute spherical arc points based on blend arc data. 1236 * Once blend parameters are computed, the three arc points are calculated 1237 * here. 1238 */ 1239 int blendFindPoints3(BlendPoints3 * const points, BlendGeom3 const * const geom, 1240 BlendParameters const * const param) 1241 { 1242 // Find center of blend arc along normal vector 1243 double center_dist = param->R_plan / sin(param->theta); 1244 tp_debug_print("center_dist = %f\n", center_dist); 1245 1246 pmCartScalMult(&geom->normal, center_dist, &points->arc_center); 1247 pmCartCartAddEq(&points->arc_center, &geom->P); 1248 tp_debug_print("arc_center = %f %f %f\n", 1249 points->arc_center.x, 1250 points->arc_center.y, 1251 points->arc_center.z); 1252 1253 // Start point is d_plan away from intersection P in the 1254 // negative direction of u1 1255 pmCartScalMult(&geom->u1, -param->d_plan, &points->arc_start); 1256 pmCartCartAddEq(&points->arc_start, &geom->P); 1257 tp_debug_print("arc_start = %f %f %f\n", 1258 points->arc_start.x, 1259 points->arc_start.y, 1260 points->arc_start.z); 1261 1262 // End point is d_plan away from intersection P in the 1263 // positive direction of u1 1264 pmCartScalMult(&geom->u2, param->d_plan, &points->arc_end); 1265 pmCartCartAddEq(&points->arc_end, &geom->P); 1266 tp_debug_print("arc_end = %f %f %f\n", 1267 points->arc_end.x, 1268 points->arc_end.y, 1269 points->arc_end.z); 1270 1271 //For line case, just copy over d_plan since it's the same 1272 points->trim1 = param->d_plan; 1273 points->trim2 = param->d_plan; 1274 1275 return TP_ERR_OK; 1276 } 1277 1278 1279 /** 1280 * Take results of line blend calculation and project onto circular arc and line 1281 */ 1282 int blendLineArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, 1283 BlendParameters * const param, BlendGeom3 const * const geom, 1284 PmCartLine const * const line1, PmCircle const * const circ2) 1285 { 1286 1287 // Define distances from actual center to circle centers 1288 double d2 = negate(param->R_plan, param->convex2) + geom->radius2; 1289 tp_debug_print("d2 = %f\n", d2); 1290 1291 //Get unit vector normal to line in plane, towards arc center 1292 PmCartesian n1; 1293 pmCartCartCross(&geom->binormal, &geom->u1, &n1); 1294 pmCartUnitEq(&n1); 1295 1296 tp_debug_print("n1 = %f %f %f\n", 1297 n1.x, 1298 n1.y, 1299 n1.z); 1300 1301 PmCartesian r_PC2; 1302 pmCartCartSub(&geom->center2, &geom->P, &r_PC2); 1303 1304 double c2_u,c2_n; //Components of C2-P on u1 and n1 1305 pmCartCartDot(&r_PC2, &geom->u1, &c2_u); 1306 pmCartCartDot(&r_PC2, &n1, &c2_n); 1307 1308 tp_debug_print("c2_u = %f, c2_n = %f\n", 1309 c2_u, 1310 c2_n); 1311 1312 double d_L; // badly named distance along line to intersection 1313 double A = 1; 1314 double B = 2.0 * c2_u; 1315 double C = pmSq(c2_u) - pmSq(d2) + pmSq(param->R_plan - c2_n); 1316 double root0,root1; 1317 int res_dist = quadraticFormula(A, B, C, &root0, &root1); 1318 if (res_dist) { 1319 return TP_ERR_FAIL; 1320 } 1321 1322 tp_debug_print("root0 = %f, root1 = %f\n", root0, 1323 root1); 1324 d_L = fmin(fabs(root0),fabs(root1)); 1325 if (d_L < 0) { 1326 tp_debug_print("d_L can't be < 0, aborting...\n"); 1327 return TP_ERR_FAIL; 1328 } 1329 1330 PmCartesian C_u, C_n; 1331 1332 pmCartScalMult(&geom->u1, -d_L, &C_u); 1333 pmCartScalMult(&n1, param->R_plan, &C_n); 1334 1335 PmCartesian r_PC; 1336 //Continue with correct solution, get actual center 1337 pmCartCartAdd(&C_u, &C_n, &r_PC); 1338 pmCartCartAdd(&geom->P, &r_PC, &points->arc_center); 1339 tp_debug_print("arc center = %f %f %f\n", 1340 points->arc_center.x, 1341 points->arc_center.y, 1342 points->arc_center.z); 1343 1344 //Verify tolerances 1345 double h; 1346 pmCartMag(&r_PC, &h); 1347 tp_debug_print("center_dist = %f\n", h); 1348 1349 double T_final = h - param->R_plan; 1350 tp_debug_print("T_final = %f\n",T_final); 1351 if (T_final > param->tolerance) { 1352 tp_debug_print("Projected circle T (%f) exceeds tolerance %f, aborting blend arc\n", 1353 T_final, 1354 param->tolerance); 1355 return TP_ERR_FAIL; 1356 } 1357 1358 points->trim1 = d_L; 1359 1360 points->trim2 = findTrimAngle(&geom->P, 1361 &points->arc_center, 1362 &geom->center2); 1363 1364 return TP_ERR_OK; 1365 } 1366 1367 1368 /** 1369 * Take results of line blend calculation and project onto circular arc and line 1370 */ 1371 int blendArcLinePostProcess(BlendPoints3 * const points, 1372 BlendPoints3 const * const points_in, 1373 BlendParameters * const param, 1374 BlendGeom3 const * const geom, 1375 PmCircle const * const circ1, 1376 PmCartLine const * const line2) 1377 { 1378 1379 // Define distance from actual arc center to circle center 1380 double d1 = negate(param->R_plan, param->convex1) + geom->radius1; 1381 tp_debug_print("d1 = %f\n", d1); 1382 1383 //Get unit vector normal to line in plane, towards arc center 1384 PmCartesian n2; 1385 pmCartCartCross(&geom->binormal, &geom->u2, &n2); 1386 pmCartUnitEq(&n2); 1387 1388 tp_debug_print("n2 = %f %f %f\n", 1389 n2.x, 1390 n2.y, 1391 n2.z); 1392 1393 PmCartesian r_PC1; 1394 pmCartCartSub(&geom->center1, &geom->P, &r_PC1); 1395 double c1_u, c1_n; //Components of C1-P on u2 and n2 1396 pmCartCartDot(&r_PC1, &geom->u2, &c1_u); 1397 pmCartCartDot(&r_PC1, &n2, &c1_n); 1398 1399 double d_L; // badly named distance along line to intersection 1400 double A = 1; 1401 double B = 2.0 * c1_u; 1402 double C = pmSq(c1_u) - pmSq(d1) + pmSq(param->R_plan - c1_n); 1403 double root0,root1; 1404 int res_dist = quadraticFormula(A, B, C, &root0, &root1); 1405 if (res_dist) { 1406 return TP_ERR_FAIL; 1407 } 1408 1409 tp_debug_print("root0 = %f, root1 = %f\n", root0, 1410 root1); 1411 d_L = fmin(fabs(root0),fabs(root1)); 1412 if (d_L < 0) { 1413 tp_debug_print("d_L can't be < 0, aborting...\n"); 1414 return TP_ERR_FAIL; 1415 } 1416 1417 PmCartesian C_u, C_n; 1418 1419 pmCartScalMult(&geom->u2, d_L, &C_u); 1420 pmCartScalMult(&n2, param->R_plan, &C_n); 1421 1422 PmCartesian r_PC; 1423 //Continue with correct solution, get actual center 1424 pmCartCartAdd(&C_u, &C_n, &r_PC); 1425 pmCartCartAdd(&geom->P, &r_PC, &points->arc_center); 1426 tp_debug_print("arc center = %f %f %f\n", 1427 points->arc_center.x, 1428 points->arc_center.y, 1429 points->arc_center.z); 1430 1431 //Verify tolerances 1432 double h; 1433 pmCartMag(&r_PC, &h); 1434 tp_debug_print("center_dist = %f\n", h); 1435 1436 double T_final = h - param->R_plan; 1437 if (T_final > param->tolerance) { 1438 tp_debug_print("Projected circle T (%f) exceeds tolerance %f, aborting blend arc\n", 1439 T_final, 1440 param->tolerance); 1441 return TP_ERR_FAIL; 1442 } 1443 tp_debug_print("T_final = %f\n",T_final); 1444 1445 points->trim1 = findTrimAngle(&geom->P, 1446 &points->arc_center, 1447 &geom->center1); 1448 1449 points->trim2 = d_L; 1450 1451 return TP_ERR_OK; 1452 } 1453 1454 1455 /** 1456 * "Post-process" results from linear approximation to fit the circular segments. 1457 * This step handles the projection from the linear approximation of each 1458 * circle. Given the solved radius and tolerance, this function updates the 1459 * points structure with the exact trim angles for each segment. 1460 */ 1461 int blendArcArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, 1462 BlendParameters * const param, BlendGeom3 const * const geom, 1463 PmCircle const * const circ1, PmCircle const * const circ2) 1464 { 1465 1466 // Create "shifted center" approximation of spiral circles 1467 // TODO refers to u1 instead of utan? 1468 // Define distances from actual center to adjusted circle centers 1469 double d1 = negate(param->R_plan, param->convex1) + geom->radius1; 1470 double d2 = negate(param->R_plan, param->convex2) + geom->radius2; 1471 tp_debug_print("d1 = %f, d2 = %f\n", d1, d2); 1472 1473 //Find "x" distance between C1 and C2 1474 PmCartesian r_C1C2; 1475 pmCartCartSub(&geom->center2, &geom->center1, &r_C1C2); 1476 double c2x; 1477 pmCartMag(&r_C1C2, &c2x); 1478 1479 // Compute the new center location 1480 1481 double Cx = (-pmSq(d1) + pmSq(d2)-pmSq(c2x)) / (-2.0 * c2x); 1482 double Cy = pmSqrt(pmSq(d1) - pmSq(Cx)); 1483 1484 tp_debug_print("Cx = %f, Cy = %f\n",Cx,Cy); 1485 1486 // Find the basis vector uc from center1 to center2 1487 PmCartesian uc; 1488 //TODO catch failures here 1489 int norm_err = pmCartUnit(&r_C1C2, &uc); 1490 if (norm_err) { 1491 return TP_ERR_FAIL; 1492 } 1493 1494 // Find the basis vector perpendicular to the binormal and uc 1495 PmCartesian nc; 1496 pmCartCartCross(&geom->binormal, &uc, &nc); 1497 1498 //Check if nc is in the same half-plane as the intersection normal. if not, 1499 //we need to flip it around to choose the correct solution. 1500 double dot1; 1501 pmCartCartDot(&geom->normal, &nc, &dot1); 1502 if (dot1 < 0) { 1503 pmCartNegEq(&nc); 1504 } 1505 norm_err = pmCartUnitEq(&nc); 1506 if (norm_err) { 1507 return TP_ERR_FAIL; 1508 } 1509 1510 //Find components of center position wrt circle 1 center. 1511 PmCartesian c_x, c_y; 1512 pmCartScalMult(&uc, Cx, &c_x); 1513 pmCartScalMult(&nc, Cy, &c_y); 1514 1515 //Get vector from P to first center 1516 PmCartesian r_PC1; 1517 pmCartCartSub(&geom->center1, &geom->P, &r_PC1); 1518 1519 // Get "test vectors, relative distance from solution center to P 1520 PmCartesian test1, test2; 1521 pmCartCartAdd(&r_PC1, &c_x, &test1); 1522 test2=test1; 1523 1524 //Add and subtract c_y component to get equivalent of two Y solutions 1525 pmCartCartAddEq(&test1, &c_y); 1526 pmCartCartSubEq(&test2, &c_y); 1527 1528 double mag1,mag2; 1529 pmCartMag(&test1, &mag1); 1530 pmCartMag(&test2, &mag2); 1531 1532 if (mag2 < mag1) 1533 { 1534 //negative solution is closer 1535 pmCartNegEq(&c_y); 1536 } 1537 1538 //Continue with correct solution, get actual center 1539 PmCartesian r_C1C; 1540 pmCartCartAdd(&c_x, &c_y, &r_C1C); 1541 pmCartCartAdd(&geom->center1, &r_C1C, &points->arc_center); 1542 tp_debug_print("arc center = %f %f %f\n", 1543 points->arc_center.x, 1544 points->arc_center.y, 1545 points->arc_center.z); 1546 1547 //Find components of center position wrt circle 2 center. 1548 PmCartesian r_C2C; 1549 pmCartCartSub(&points->arc_center, &geom->center2, &r_C2C); 1550 1551 PmCartesian r_PC; 1552 pmCartCartSub(&points->arc_center, &geom->P, &r_PC); 1553 1554 //Verify tolerances 1555 double h; 1556 pmCartMag(&r_PC, &h); 1557 tp_debug_print("center_dist = %f\n", h); 1558 1559 double T_final = h - param->R_plan; 1560 if (T_final > param->tolerance) { 1561 tp_debug_print("Projected circle T (%f) exceeds tolerance %f, aborting blend arc\n", 1562 T_final, 1563 param->tolerance); 1564 return TP_ERR_FAIL; 1565 } 1566 tp_debug_print("T_final = %f\n",T_final); 1567 1568 points->trim1 = findTrimAngle(&geom->P, 1569 &points->arc_center, 1570 &geom->center1); 1571 points->trim2 = findTrimAngle(&geom->P, 1572 &points->arc_center, 1573 &geom->center2); 1574 1575 tp_debug_print("trim1 = %f, trim2 = %f\n", 1576 points->trim1, 1577 points->trim2); 1578 1579 return TP_ERR_OK; 1580 } 1581 1582 1583 /** 1584 * Setup the spherical arc struct based on the blend arc data. 1585 */ 1586 int arcFromBlendPoints3(SphericalArc * const arc, BlendPoints3 const * const points, 1587 BlendGeom3 const * const geom, BlendParameters const * const param) 1588 { 1589 // If we consume the previous line, the remaining line length gets added here 1590 arc->uTan = geom->u_tan1; 1591 arc->line_length = param->line_length; 1592 arc->binormal = geom->binormal; 1593 1594 // Create the arc from the processed points 1595 return arcInitFromPoints(arc, &points->arc_start, 1596 &points->arc_end, &points->arc_center); 1597 } 1598 1599 int blendGeom3Print(BlendGeom3 const * const geom) 1600 { 1601 tp_debug_print("u1 = %f %f %f\n", 1602 geom->u1.x, 1603 geom->u1.y, 1604 geom->u1.z); 1605 1606 tp_debug_print("u2 = %f %f %f\n", 1607 geom->u2.x, 1608 geom->u2.y, 1609 geom->u2.z); 1610 return 0; 1611 } 1612 1613 int blendPoints3Print(BlendPoints3 const * const points) 1614 { 1615 tp_debug_print("arc_start = %f %f %f\n", 1616 points->arc_start.x, 1617 points->arc_start.y, 1618 points->arc_start.z); 1619 1620 tp_debug_print("arc_center = %f %f %f\n", 1621 points->arc_center.x, 1622 points->arc_center.y, 1623 points->arc_center.z); 1624 1625 tp_debug_print("arc_end = %f %f %f\n", 1626 points->arc_end.x, 1627 points->arc_end.y, 1628 points->arc_end.z); 1629 1630 return 0; 1631 1632 } 1633 1634 double pmCartAbsMax(PmCartesian const * const v) 1635 { 1636 return fmax(fmax(fabs(v->x),fabs(v->y)),fabs(v->z)); 1637 } 1638 1639 1640 PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle, 1641 double v_max, 1642 double a_max) 1643 { 1644 double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max; 1645 1646 double eff_radius = pmCircleEffectiveMinRadius(circle); 1647 // Find the acceleration necessary to reach the maximum velocity 1648 double a_n_vmax = pmSq(v_max) / fmax(eff_radius, DOUBLE_FUZZ); 1649 // Find the maximum velocity that still obeys our desired tangential / total acceleration ratio 1650 double v_max_cutoff = pmSqrt(a_n_max_cutoff * eff_radius); 1651 1652 double v_max_actual = v_max; 1653 double acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL; 1654 1655 if (a_n_vmax > a_n_max_cutoff) { 1656 v_max_actual = v_max_cutoff; 1657 } else { 1658 acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max)); 1659 } 1660 1661 tp_debug_json_start(pmCircleActualMaxVel); 1662 tp_debug_json_double(eff_radius); 1663 tp_debug_json_double(v_max); 1664 tp_debug_json_double(v_max_cutoff); 1665 tp_debug_json_double(a_n_max_cutoff); 1666 tp_debug_json_double(a_n_vmax); 1667 tp_debug_json_double(acc_ratio_tan); 1668 tp_debug_json_end(); 1669 1670 PmCircleLimits limits = { 1671 v_max_actual, 1672 acc_ratio_tan 1673 }; 1674 1675 return limits; 1676 } 1677 1678 1679 /** @section spiralfuncs Functions to approximate spiral arc length */ 1680 1681 /** 1682 * Intermediate function to find the angle for a parameter from 0..1 along the 1683 * spiral arc. 1684 */ 1685 static int pmCircleAngleFromParam(PmCircle const * const circle, 1686 SpiralArcLengthFit const * const fit, 1687 double t, 1688 double * const angle) 1689 { 1690 if (fit->spiral_in) { 1691 t = 1.0 - t; 1692 } 1693 //TODO error or cleanup input to prevent param outside 0..1 1694 double s_in = t * fit->total_planar_length; 1695 1696 // Quadratic formula to invert arc length -> angle 1697 1698 double A = fit->b0; 1699 double B = fit->b1; 1700 double C = -s_in; 1701 1702 double disc = pmSq(B) - 4.0 * A * C ; 1703 if (disc < 0) { 1704 rtapi_print_msg(RTAPI_MSG_ERR, "discriminant %f is negative in angle calculation\n",disc); 1705 return TP_ERR_FAIL; 1706 } 1707 1708 /* 1709 * Stability of inverting the arc-length relationship. 1710 * Since the b1 coefficient is analogous to arc radius, we can be 1711 * reasonably assured that it will be large enough not to cause numerical 1712 * errors. If this is not the case, then the arc itself is degenerate (very 1713 * small radius), and this condition should be caught well before here. 1714 * 1715 * Since an arc with a very small spiral coefficient will have a small b0 1716 * coefficient in the fit, we use the Citardauq Formula to ensure that the 1717 * positive root does not lose precision due to subtracting near-similar values. 1718 * 1719 * For more information, see: 1720 * http://people.csail.mit.edu/bkph/articles/Quadratics.pdf 1721 */ 1722 1723 double angle_out = (2.0 * C) / ( -B - pmSqrt(disc)); 1724 1725 if (fit->spiral_in) { 1726 // Spiral fit assumes that we're spiraling out, so 1727 // parameterize from opposite end 1728 angle_out = circle->angle - angle_out; 1729 } 1730 1731 *angle = angle_out; 1732 return TP_ERR_OK; 1733 } 1734 1735 1736 static void printSpiralArcLengthFit(SpiralArcLengthFit const * const fit) 1737 { 1738 tp_debug_print("Spiral fit: b0 = %.12f, b1 = %.12f, length = %.12f, spiral_in = %d\n", 1739 fit->b0, 1740 fit->b1, 1741 fit->total_planar_length, 1742 fit->spiral_in); 1743 } 1744 1745 /** 1746 * Approximate the arc length function of a general spiral. 1747 * 1748 * The closed-form arc length of a general archimedean spiral is rather 1749 * computationally messy to work with. 1750 * See http://mathworld.wolfram.com/ArchimedesSpiral.html for the actual form. 1751 * 1752 * The simplification here is made possible by a few assumptions: 1753 * 1) That the spiral starts with a nonzero radius 1754 * 2) The spiral coefficient (i.e. change in radius / angle) is not too large 1755 * 3) The spiral coefficient has some minimum magnitude ("perfect" circles are handled as a special case) 1756 * 1757 * The 2nd-order fit below works by matching slope at the start and end of the 1758 * arc length vs. angle curve. This completely specifies the 2nd order fit. 1759 * Also, this fit predicts a total arc length >= the true arc length, which 1760 * means the true speed along the curve will be the same or slower than the 1761 * nominal speed. 1762 */ 1763 int findSpiralArcLengthFit(PmCircle const * const circle, 1764 SpiralArcLengthFit * const fit) 1765 { 1766 // Additional data for arc length approximation 1767 double spiral_coef = circle->spiral / circle->angle; 1768 double min_radius = circle->radius; 1769 1770 if (fsign(circle->spiral) < 0.0) { 1771 // Treat as positive spiral, parameterized in opposite 1772 // direction 1773 spiral_coef*=-1.0; 1774 // Treat final radius as starting radius for fit, so we add the 1775 // negative spiral term to get the minimum radius 1776 // 1777 min_radius+=circle->spiral; 1778 fit->spiral_in = true; 1779 } else { 1780 fit->spiral_in = false; 1781 } 1782 tp_debug_print("radius = %.12f, angle = %.12f\n", min_radius, circle->angle); 1783 tp_debug_print("spiral_coef = %.12f\n", spiral_coef); 1784 1785 1786 //Compute the slope of the arc length vs. angle curve at the start and end of the segment 1787 double slope_start = pmSqrt(pmSq(min_radius) + pmSq(spiral_coef)); 1788 double slope_end = pmSqrt(pmSq(min_radius + spiral_coef * circle->angle) + pmSq(spiral_coef)); 1789 1790 fit->b0 = (slope_end - slope_start) / (2.0 * circle->angle); 1791 fit->b1 = slope_start; 1792 1793 fit->total_planar_length = fit->b0 * pmSq(circle->angle) + fit->b1 * circle->angle; 1794 printSpiralArcLengthFit(fit); 1795 1796 // Check against start and end angle 1797 double angle_end_chk = 0.0; 1798 int res_angle = pmCircleAngleFromParam(circle, fit, 1.0, &angle_end_chk); 1799 if (res_angle != TP_ERR_OK) { 1800 //TODO better error message 1801 rtapi_print_msg(RTAPI_MSG_ERR, 1802 "Spiral fit failed\n"); 1803 return TP_ERR_FAIL; 1804 } 1805 1806 // Check fit against angle 1807 double fit_err = angle_end_chk - circle->angle; 1808 if (fabs(fit_err) > TP_ANGLE_EPSILON) { 1809 rtapi_print_msg(RTAPI_MSG_ERR, 1810 "Spiral fit angle difference is %e, maximum allowed is %e\n", 1811 fit_err, 1812 TP_ANGLE_EPSILON); 1813 return TP_ERR_FAIL; 1814 } 1815 1816 return TP_ERR_OK; 1817 } 1818 1819 1820 /** 1821 * Compute the angle around a circular segment from the total progress along 1822 * the curve. 1823 */ 1824 int pmCircleAngleFromProgress(PmCircle const * const circle, 1825 SpiralArcLengthFit const * const fit, 1826 double progress, 1827 double * const angle) 1828 { 1829 double h2; 1830 pmCartMagSq(&circle->rHelix, &h2); 1831 double s_end = pmSqrt(pmSq(fit->total_planar_length) + h2); 1832 // Parameterize by total progress along helix 1833 double t = progress / s_end; 1834 return pmCircleAngleFromParam(circle, fit, t, angle); 1835 } 1836 1837 1838 /** 1839 * Find the effective minimum radius for acceleration calculations. 1840 * The radius of curvature of a spiral is larger than the circle of the same 1841 * radius. 1842 */ 1843 double pmCircleEffectiveMinRadius(PmCircle const * circle) 1844 { 1845 double dr = circle->spiral / circle->angle; 1846 double h2; 1847 pmCartMagSq(&circle->rHelix, &h2); 1848 1849 // Exact representation of spiral arc length flattened into 1850 double n_inner = pmSq(dr) + pmSq(circle->radius); 1851 double den = n_inner+pmSq(dr); 1852 double num = pmSqrt(n_inner * n_inner * n_inner); 1853 double r_spiral = num / den; 1854 1855 // Curvature of helix, assuming that helical motion is independent of plane motion 1856 double effective_radius = h2 / r_spiral + r_spiral; 1857 1858 return effective_radius; 1859 } 1860