interp_g7x.cc
1 #include <list> 2 #include <tuple> 3 #include <vector> 4 #include <fstream> 5 #include <iostream> 6 #include <deque> 7 #include <memory> 8 #include <complex> 9 10 11 template <class T> 12 std::string to_string(T &d) 13 { 14 std::ostringstream out; 15 out << d; 16 return out.str(); 17 } 18 19 //////////////////////////////////////////////////////////////////////////////// 20 class motion_base { 21 public: 22 virtual void straight_move(std::complex<double> end)=0; 23 virtual void straight_rapid(std::complex<double> end)=0; 24 virtual void circular_move(int ccw,std::complex<double> center, 25 std::complex<double> end)=0; 26 }; 27 28 class motion_null:public motion_base { 29 public: 30 void straight_move(std::complex<double> end) {} 31 void straight_rapid(std::complex<double> end) {} 32 void circular_move(int ccw,std::complex<double> center, 33 std::complex<double> end) {} 34 }; 35 36 //////////////////////////////////////////////////////////////////////////////// 37 class round_segment; 38 class straight_segment; 39 40 static constexpr double tolerance=1e-6; 41 42 class segment { 43 protected: 44 std::complex<double> start, end; 45 double finish; 46 public: 47 segment(double sz,double sx,double ez,double ex):start(sz,sx),end(ez,ex),finish(0) {} 48 segment(std::complex<double> s, std::complex<double> e):start(s),end(e) {} 49 typedef std::deque<double> intersections_t; 50 virtual void intersection_z(double x, intersections_t &is)=0; 51 virtual bool climb(std::complex<double>&, motion_base*)=0; 52 virtual bool dive(std::complex<double>&,double, motion_base*,bool)=0; 53 virtual void climb_only(std::complex<double>&, motion_base*)=0; 54 virtual void draw(motion_base*)=0; 55 virtual void offset(double)=0; 56 virtual void intersect(segment*)=0; 57 virtual void intersect_end(round_segment *p)=0; 58 virtual void intersect_end(straight_segment *p)=0; 59 virtual std::unique_ptr<segment> dup(void)=0; 60 virtual double radius(void)=0; 61 virtual bool monotonic(void) { return real(end-start)<=1e-3; } 62 virtual void do_finish(segment *prev, segment *next) {} 63 std::complex<double> &sp(void) { return start; } 64 std::complex<double> &ep(void) { return end; } 65 66 virtual void flip_imag(void) { start=conj(start); end=conj(end); } 67 virtual void flip_real(void) { start=-conj(start); end=-conj(end); } 68 virtual void rotate(void) { 69 start=-start*std::complex<double>(0,1); 70 end=-end*std::complex<double>(0,1); 71 } 72 virtual void move(std::complex<double> d) { start+=d; end+=d; } 73 friend class round_segment; 74 75 protected: 76 std::complex<double> corner_finish(segment *prev, segment *next) { 77 auto p=prev->dup(); 78 auto n=next->dup(); 79 80 p->offset(finish); 81 n->offset(finish); 82 if(real(p->end-n->start)>1e-2) { 83 finish=-finish; 84 p=prev->dup(); 85 n=next->dup(); 86 p->offset(finish); 87 n->offset(finish); 88 } 89 if(real(p->end-n->start)>1e-2) 90 throw(std::string("Corner finish failed at ") + to_string(prev->end)); 91 p->intersect(n.get()); 92 std::complex<double> center=(p->end+n->start)/2.0; 93 p->offset(-finish); 94 n->offset(-finish); 95 start=prev->end=p->end; 96 end=next->start=n->start; 97 return center; 98 } 99 }; 100 101 class straight_segment:public segment { 102 public: 103 straight_segment(double sx, double sz,double ex, double ez): 104 segment(sz,sx,ez,ex) {} 105 straight_segment(std::complex<double> s,std::complex<double> e): 106 segment(s,e) {} 107 void intersection_z(double x, intersections_t &is); 108 bool climb(std::complex<double>&,motion_base*); 109 bool dive(std::complex<double>&,double, motion_base*,bool); 110 void climb_only(std::complex<double>&,motion_base*); 111 void draw(motion_base *out) { out->straight_move(end); } 112 void offset(double distance) { 113 std::complex<double> d=std::complex<double>(0,1)*distance*(start-end)/std::complex<double>(abs(start-end),0); 114 start+=d; 115 end+=d; 116 } 117 void intersect(segment *p); 118 void intersect_end(round_segment *p); 119 void intersect_end(straight_segment *p); 120 std::unique_ptr<segment> dup(void) { 121 return std::unique_ptr<segment>(new straight_segment(*this)); 122 } 123 double radius(void) { return abs(start-end); } 124 }; 125 126 void straight_segment::intersection_z(double x, intersections_t &is) 127 { 128 if(std::min(start.imag(),end.imag())>x+tolerance 129 || std::max(start.imag(),end.imag())<x-tolerance 130 ) 131 return; 132 if(std::abs(imag(start-end))<tolerance) { 133 is.push_back(start.real()); 134 is.push_back(end.real()); 135 } else 136 is.push_back((x-start.imag()) 137 /(end.imag()-start.imag())*(end.real()-start.real())+start.real()); 138 } 139 140 bool straight_segment::climb(std::complex<double> &location, 141 motion_base *output 142 ) { 143 if(end.imag()+tolerance<start.imag()) 144 return 1; // not climbing 145 if(abs(location-start)>tolerance) 146 throw("How did we get here?"); 147 output->straight_move(end); 148 location=end; 149 return 0; 150 } 151 152 void straight_segment::climb_only(std::complex<double> &location, 153 motion_base *output 154 ) { 155 if(end.imag()<start.imag()) 156 return; // not climbing 157 intersections_t is; 158 intersection_z(location.imag(),is); 159 if(!is.size()) 160 return; 161 162 location.real(is.front()); 163 output->straight_move(location); 164 output->straight_move(end); 165 location=end; 166 return; 167 } 168 169 bool straight_segment::dive(std::complex<double> &location, 170 double x, 171 motion_base *output, bool fast) 172 { 173 if(start.imag()<=end.imag()+tolerance) // Not a diving segment 174 return 1; 175 if(x<end.imag()) { 176 if(fast) 177 output->straight_rapid(end); 178 else 179 output->straight_move(end); 180 location=end; 181 return 0; 182 } 183 intersections_t is; 184 intersection_z(x,is); 185 if(!is.size()) 186 throw("x too large in straight dive"); 187 std::complex<double> ep(is.front(),x); 188 if(fast) 189 output->straight_rapid(ep); 190 else 191 output->straight_move(ep); 192 location=ep; 193 return 0; 194 } 195 196 class round_segment:public segment { 197 protected: 198 int ccw; 199 std::complex<double> center; 200 public: 201 round_segment(int c, double sx, double sz, 202 double cx, double cz, double ex, double ez): 203 segment(sz,sx,ez,ex), ccw(c), center(cz,cx) 204 { 205 } 206 round_segment(int cc, std::complex<double> s, 207 std::complex<double> c, std::complex<double> e): 208 segment(s,e), ccw(cc), center(c) 209 { 210 } 211 void intersection_z(double x,intersections_t &is); 212 bool climb(std::complex<double>&,motion_base*); 213 bool dive(std::complex<double>&,double,motion_base*,bool); 214 void climb_only(std::complex<double>&,motion_base*); 215 void draw(motion_base *out) { out->circular_move(ccw,center,end);} 216 void offset(double distance) { 217 double factor=(abs(start-center)+(ccw? 1:-1)*distance) 218 /abs(start-center); 219 start=factor*(start-center)+center; 220 end=factor*(end-center)+center; 221 } 222 void intersect(segment *p); 223 void intersect_end(round_segment *p); 224 void intersect_end(straight_segment *p); 225 std::complex<double> cp(void) { return center; } 226 std::unique_ptr<segment> dup(void) { 227 return std::unique_ptr<segment>(new round_segment(*this)); 228 } 229 double radius(void) { return std::min(abs(start-end),abs(start-center)); } 230 void flip_imag(void) { ccw=!ccw; start=conj(start); end=conj(end); 231 center=conj(center); } 232 void flip_real(void) { ccw=!ccw; start=-conj(start); 233 end=-conj(end); center=-conj(center); } 234 virtual void rotate(void) { 235 start=-start*std::complex<double>(0,1); 236 end=-end*std::complex<double>(0,1); 237 center=-center*std::complex<double>(0,1); 238 } 239 virtual bool monotonic(void) { 240 if(finish!=0) 241 return true; 242 double entry=imag(start-center); 243 double exit=imag(end-center); 244 double dz=real(end-start); 245 if(ccw) 246 return entry>=-1e-3 && exit>=-1e-3 && dz<=-1e-3; 247 else 248 return entry<=1e-3 && exit<=1e-3 && dz<=-1e-3; 249 } 250 virtual void move(std::complex<double> d) { start+=d; center+=d; end+=d; } 251 private: 252 bool on_segment(std::complex<double> p); 253 friend class straight_segment; 254 }; 255 256 257 inline bool round_segment::on_segment(std::complex<double> p) 258 { 259 if(ccw) 260 return imag(conj(start-center)*(p-center))>=-tolerance 261 && imag(conj(end-center)*(p-center))<=tolerance; 262 else 263 return imag(conj(start-center)*(p-center))<=tolerance 264 && imag(conj(end-center)*(p-center))>=-tolerance; 265 } 266 267 void round_segment::intersection_z(double x, intersections_t &is) 268 { 269 std::complex<double> r=start-center; 270 double s=-(x-abs(r)-center.imag())*(x+abs(r)-center.imag()); 271 if(s<-tolerance) 272 return; 273 if(s<0) 274 s=0; 275 s=sqrt(s); 276 std::complex<double> p1(real(center+s),x); 277 if(on_segment(p1)) 278 is.push_back(real(p1)); 279 std::complex<double> p2(real(center-s),x); 280 if(on_segment(p2)) 281 is.push_back(real(p2)); 282 } 283 284 bool round_segment::climb(std::complex<double> &location, 285 motion_base *output 286 ) { 287 if(!ccw) { // G2 288 if(location.real()>center.real()) 289 return 1; 290 if(abs(location-end)>1e-3) 291 output->circular_move(ccw,center,end); 292 location=end; 293 return 0; 294 } else { 295 if(location.real()<center.real()) 296 return 1; 297 std::complex<double> ep=end; 298 if(end.real()<center.real()) { 299 ep.real(center.real()); 300 ep.imag(center.imag()+abs(start-center)); 301 } 302 if(abs(location-ep)>1e-3) 303 output->circular_move(ccw,center,ep); 304 location=ep; 305 return 1; 306 } 307 } 308 309 bool round_segment::dive(std::complex<double> &location, 310 double x, motion_base *output, bool 311 ) { 312 if(abs(location-end)<tolerance || real(location-end)<=tolerance) { 313 location=end; 314 return 0; 315 } 316 intersections_t is; 317 intersection_z(x,is); 318 if(!is.size()) 319 intersection_z(x-tolerance,is); 320 321 if(ccw) { // G3 322 if(location.real()-tolerance>center.real()) 323 return 1; 324 if(!is.size() || is.back()>real(center)) { 325 output->circular_move(ccw,center,end); 326 location=end; 327 return 0; 328 } else { 329 std::complex<double> ep(is.back(),x); 330 output->circular_move(ccw,center,ep); 331 location=ep; 332 return 0; 333 } 334 } else { 335 if(location.real()<=center.real()) 336 return 1; // we're already climbing 337 if(!is.size()) { 338 // Curve not hit, move to the end 339 output->circular_move(ccw,center,end); 340 location=end; 341 return 0; 342 } else if(is.front()<real(center)) { 343 // also is.size()==1 344 // also abs(location-start)<tolerance 345 // curve hit at the back side only, let pocket decrement x 346 return 0; 347 } else { 348 // Arc cut twice, move to the front intersection 349 std::complex<double> ep(is.front(),x); 350 output->circular_move(ccw,center,ep); 351 location=ep; 352 return 0; 353 } 354 } 355 } 356 357 void round_segment::climb_only(std::complex<double> &location, 358 motion_base *output 359 ) { 360 intersections_t is; 361 intersection_z(location.imag(),is); 362 if(!is.size()) 363 return; 364 if(!ccw) { // G2 365 if(is.back()>center.real()) 366 return; 367 location.real(is.back()); 368 output->straight_move(location); 369 if(abs(location-end)>1e-3) 370 output->circular_move(ccw,center,end); 371 location=end; 372 return; 373 } else { 374 if(is.front()<center.real()) 375 return; 376 location.real(is.front()); 377 output->straight_move(location); 378 std::complex<double> ep=end; 379 if(end.real()<center.real()) { 380 ep.real(center.real()); 381 ep.imag(center.imag()+abs(start-center)); 382 } 383 if(abs(location-ep)>1e-3) 384 output->circular_move(ccw,center,ep); 385 location=ep; 386 return; 387 } 388 } 389 390 391 //////////////////////////////////////////////////////////////////////////////// 392 void straight_segment::intersect(segment *p) 393 { 394 p->intersect_end(this); 395 } 396 397 void round_segment::intersect(segment *p) 398 { 399 p->intersect_end(this); 400 } 401 402 void straight_segment::intersect_end(straight_segment *p) 403 { 404 // correct end of p and start of this 405 auto rot=conj(start-end)/abs(start-end); 406 auto ps=(p->start-end)*rot; 407 auto pe=(p->end-end)*rot; 408 if(imag(ps-pe)==0) { 409 throw("Cannot intersect parallel lines"); 410 } 411 auto f=imag(ps)/imag(ps-pe); 412 auto is=(ps+f*(pe-ps))/rot+end; 413 start=p->end=is; 414 } 415 416 void straight_segment::intersect_end(round_segment *p) 417 { 418 // correct end of p and start of this 419 // (arc followed by a straight 420 if(abs(start-p->end)<tolerance) 421 return; 422 423 auto rot=conj(start-end)/abs(start-end); 424 auto pe=(p->end-end)*rot; 425 auto pc=(p->center-end)*rot; 426 427 double b=norm(pc-pe)-imag(pc)*imag(pc); 428 if(b<0) { 429 b=0; 430 } else 431 b=sqrt(b); 432 433 auto s1=(real(pc)+b)/rot+end; 434 auto s2=(real(pc)-b)/rot+end; 435 436 if(abs(start-s1)<abs(start-s2)) 437 start=p->end=s1; 438 else 439 start=p->end=s2; 440 } 441 442 void round_segment::intersect_end(straight_segment *p) 443 { 444 // correct end of p and start of this 445 // (straight followed by an arc) 446 if(abs(start-p->end)<tolerance) 447 return; 448 449 auto rot=conj(p->start-p->end)/abs(p->start-p->end); 450 auto pe=(end-p->end)*rot; 451 auto pc=(center-p->end)*rot; 452 453 double b=norm(pc-pe)-imag(pc)*imag(pc); 454 if(b<0) { 455 b=0; 456 } else 457 b=sqrt(b); 458 459 auto s1=(real(pc)+b)/rot+p->end; 460 auto s2=(real(pc)-b)/rot+p->end; 461 462 if(abs(start-s1)<abs(start-s2)) 463 start=p->end=s1; 464 else 465 start=p->end=s2; 466 } 467 468 void round_segment::intersect_end(round_segment *p) 469 { 470 // correct end of p and start of this 471 auto a=abs(start-center); 472 auto b=abs(p->start-p->center); 473 auto c=abs(center-p->center); 474 auto cosB=(c*c+a*a-b*b)/2.0/a/c; 475 double cosB2=cosB*cosB; 476 if(cosB2>1) 477 cosB2=1; 478 std::complex<double> rot(cosB,sqrt(1-cosB2)); 479 auto is=rot*(p->center-center)/c*a+center; 480 p->end=start=is; 481 } 482 483 //////////////////////////////////////////////////////////////////////////////// 484 // Corner finishes 485 486 class fillet_segment:public round_segment { 487 public: 488 fillet_segment(double d,std::complex<double> l):round_segment(0,l,l,l) { 489 finish=d; 490 } 491 492 void do_finish(segment *prev, segment *next) { 493 center=corner_finish(prev,next); 494 ccw=imag(start-center)>0 || imag(end-center)>0; 495 finish=0; 496 } 497 }; 498 499 class chamfer_segment:public straight_segment { 500 public: 501 chamfer_segment(double d,std::complex<double> l):straight_segment(l,l) { 502 finish=d; 503 } 504 505 void do_finish(segment *prev, segment *next) { 506 corner_finish(prev,next); 507 } 508 }; 509 510 511 //////////////////////////////////////////////////////////////////////////////// 512 513 template <int swap> 514 class swapped_motion:public motion_base { 515 motion_base *orig; 516 public: 517 swapped_motion(motion_base *motion):orig(motion) { 518 } 519 virtual void straight_move(std::complex<double> end) { 520 switch(swap) { 521 case 0: orig->straight_move(end); break; 522 case 1: orig->straight_move(-conj(end)); break; 523 case 2: orig->straight_move(conj(end)); break; 524 case 3: orig->straight_move(-end); break; 525 case 4: orig->straight_move(std::complex<double>(0,1)*end); break; 526 case 5: orig->straight_move(conj(std::complex<double>(0,1)*end)); break; 527 case 6: orig->straight_move(-conj(std::complex<double>(0,1)*end)); break; 528 case 7: orig->straight_move(-std::complex<double>(0,1)*end); break; 529 } 530 } 531 virtual void straight_rapid(std::complex<double> end) { 532 switch(swap) { 533 case 0: orig->straight_rapid(end); break; 534 case 1: orig->straight_rapid(-conj(end)); break; 535 case 2: orig->straight_rapid(conj(end)); break; 536 case 3: orig->straight_rapid(-end); break; 537 case 4: orig->straight_rapid(std::complex<double>(0,1)*end); break; 538 case 5: orig->straight_rapid(conj(std::complex<double>(0,1)*end)); break; 539 case 6: orig->straight_rapid(-conj(std::complex<double>(0,1)*end)); break; 540 case 7: orig->straight_rapid(-std::complex<double>(0,1)*end); break; 541 } 542 } 543 virtual void circular_move(int ccw,std::complex<double> center, 544 std::complex<double> end) 545 { 546 switch(swap) { 547 case 0: orig->circular_move(ccw,center,end); break; 548 case 1: orig->circular_move(!ccw,-conj(center),-conj(end)); break; 549 case 2: orig->circular_move(!ccw,conj(center),conj(end)); break; 550 case 3: orig->circular_move(ccw,-center,-end); break; 551 case 4: orig->circular_move(ccw,std::complex<double>(0,1)*center,std::complex<double>(0,1)*end); break; 552 case 5: orig->circular_move(!ccw,conj(std::complex<double>(0,1)*center),conj(std::complex<double>(0,1)*end)); break; 553 case 6: orig->circular_move(!ccw,-conj(std::complex<double>(0,1)*center),-conj(std::complex<double>(0,1)*end)); break; 554 case 7: orig->circular_move(ccw,-std::complex<double>(0,1)*center,-std::complex<double>(0,1)*end); break; 555 } 556 } 557 }; 558 559 //////////////////////////////////////////////////////////////////////////////// 560 class g7x:public std::list<std::unique_ptr<segment>> { 561 double delta; 562 std::complex<double> escape; 563 int flip_state; 564 std::deque<std::complex<double>> pocket_starts; 565 private: 566 void pocket(int cycle, std::complex<double> location, iterator p, 567 motion_base *out); 568 void add_distance(double distance); 569 570 /* Rotate profile by 90 degrees */ 571 void rotate(void) { 572 for(auto p=begin(); p!=end(); p++) 573 (*p)->rotate(); 574 flip_state^=4; 575 } 576 577 /* Change the direction of the profile to have Z and X decreasing */ 578 void swap(void) { 579 double dir_x=imag(front()->ep()-back()->ep()); 580 double dir_z=real(front()->ep()-back()->ep()); 581 if(dir_x>0) { 582 for(auto p=begin(); p!=end(); p++) 583 (*p)->flip_imag(); 584 flip_state^=2; 585 } 586 if(dir_z<0) { 587 for(auto p=begin(); p!=end(); p++) 588 (*p)->flip_real(); 589 flip_state^=1; 590 } 591 } 592 593 std::unique_ptr<motion_base> motion(motion_base *out) { 594 switch(flip_state) { 595 case 0: return std::unique_ptr<swapped_motion<0>>(new swapped_motion<0>(out)); 596 case 1: return std::unique_ptr<swapped_motion<1>>(new swapped_motion<1>(out)); 597 case 2: return std::unique_ptr<swapped_motion<2>>(new swapped_motion<2>(out)); 598 case 3: return std::unique_ptr<swapped_motion<3>>(new swapped_motion<3>(out)); 599 case 4: return std::unique_ptr<swapped_motion<4>>(new swapped_motion<4>(out)); 600 case 5: return std::unique_ptr<swapped_motion<5>>(new swapped_motion<5>(out)); 601 case 6: return std::unique_ptr<swapped_motion<6>>(new swapped_motion<6>(out)); 602 case 7: return std::unique_ptr<swapped_motion<7>>(new swapped_motion<7>(out)); 603 } 604 throw("This can't happen"); 605 } 606 607 void monotonic(void) { 608 if(real(front()->ep()-front()->sp())>0) { 609 front()->sp().real(real(front()->ep())); 610 } 611 for(auto p=begin(); p!=end(); p++) { 612 if(!(*p)->monotonic()) 613 throw("Not monotonic"); 614 } 615 } 616 617 void do_finish(void) { 618 for(auto h=++begin(); h!=--end(); ) { 619 auto p(h); --p; 620 (*h)->do_finish((*p).get(),(*(++h)).get()); 621 } 622 for(auto p=begin(); p!=end(); p++) 623 if((*p)->radius()<1e-3) 624 erase(p--); 625 } 626 627 public: 628 g7x(void) : delta{0.5}, escape{0.3,0.3}, flip_state{} {} 629 g7x(g7x const &other) { 630 delta=other.delta; 631 escape=other.escape; 632 flip_state=other.flip_state; 633 for(auto p=other.begin(); p!=other.end(); p++) 634 emplace_back((*p)->dup()); 635 } 636 637 /* 638 x,z from where the distance is computed, also a rapid to this 639 location between each pass. 640 d Starting distance 641 e Ending distance 642 p Number of passes to go from d to e 643 */ 644 void do_g70(motion_base *out, double x, double z, double d, double e, 645 double p 646 ) { 647 front()->sp()=std::complex<double>(z,x); 648 649 g7x path(*this); 650 path.pop_front(); 651 path.swap(); 652 for(auto p=path.begin(); p!=path.end(); p++) { 653 if(!(*p)->monotonic()) { 654 path.rotate(); 655 path.swap(); 656 break; 657 } 658 } 659 path.monotonic(); 660 if(path.flip_state&4) 661 rotate(); 662 swap(); 663 do_finish(); 664 monotonic(); 665 auto swapped_out=motion(out); 666 667 for(int pass=p; pass>0; pass--) { 668 double distance=(pass-1)*(d-e)/p+e; 669 g7x path(*this); 670 path.add_distance(distance); 671 672 swapped_out->straight_rapid(path.front()->sp()); 673 swapped_out->straight_rapid(path.front()->ep()); 674 for(auto p=++path.begin(); p!=--path.end(); p++) 675 (*p)->draw(swapped_out.get()); 676 path.back()->draw(swapped_out.get()); 677 } 678 } 679 680 /* 681 cycle 0 Complete cut including pockets. 682 1 Do not cut any pockets. 683 2 Only cut after first pocket (pick up where 1 stopped) 684 x,z From where the cutting is done. 685 d Final distance to profile 686 i Increment of cutting (depth of cut) 687 r Distance to retract 688 */ 689 void do_g71(motion_base *out, int cycle, double x, double z, 690 double u, double w, 691 double d, double i, double r, bool do_rotate=false 692 ) { 693 front()->sp()=std::complex<double>(z,x); 694 if(do_rotate) 695 rotate(); 696 swap(); 697 do_finish(); 698 monotonic(); 699 add_distance(d); 700 std::complex<double> displacement(w,u); 701 for(auto p=begin(); p!=end(); p++) 702 (*p)->move(displacement); 703 auto swapped_out=motion(out); 704 705 delta=std::max(i,2*tolerance); 706 escape=r*std::complex<double>{1,1}; 707 708 swapped_out->straight_rapid(front()->sp()); 709 if(imag(back()->ep())<imag(front()->sp())) { 710 auto ep=back()->ep(); 711 ep.imag(imag(front()->sp())); 712 emplace_back(std::unique_ptr<straight_segment>(new straight_segment( 713 back()->ep(),ep 714 ))); 715 } 716 pocket_starts.push_back(front()->sp()); 717 pocket(cycle,front()->sp(), begin(), swapped_out.get()); 718 } 719 720 void do_g72(motion_base *out, int cycle, double x, double z, 721 double u, double w, 722 double d, double i, double r 723 ) { 724 do_g71(out, cycle, x, z, u, w, d, i, r, true); 725 } 726 }; 727 728 729 void g7x::pocket(int cycle, std::complex<double> location, iterator p, 730 motion_base *out 731 ) { 732 double x=imag(pocket_starts.back()); 733 734 if(cycle==2) { 735 // This skips the initial roughing pass 736 for(; p!=end(); p++) { 737 if((*p)->dive(location,-1e9,out,p==begin())) 738 break; 739 } 740 cycle=3; 741 } 742 743 while(p!=end()) { 744 while(x-tolerance>imag(location)) 745 x-=delta; 746 if((*p)->dive(location,x,out,p==begin())) { 747 if(cycle==1) { 748 /* After the initial roughing pass, move along the final 749 contour and we're done 750 */ 751 for(; p!=end(); p++) 752 (*p)->climb_only(location,out); 753 } else { 754 /* Move along the final contour until a pocket is found, 755 then start cutting that pocket 756 */ 757 for(; p!=end(); p++) { 758 if((*p)->climb(location,out)) { 759 if(cycle==3) { 760 if(imag(location)>imag(pocket_starts.back()) 761 && pocket_starts.size()>1 762 ) { 763 pocket_starts.pop_back(); 764 } 765 if(pocket_starts.size()==1) { 766 pocket_starts.push_back(location); 767 } 768 } 769 pocket(cycle, location,p,out); 770 return; 771 } 772 } 773 } 774 return; 775 } 776 if(std::abs(imag(location)-x)>tolerance) { 777 /* Our x coordinate is beyond the current segment, move onto 778 the next 779 */ 780 p++; 781 continue; 782 } 783 784 for(auto ip=p; ip!=end(); ip++) { 785 segment::intersections_t is; 786 (*ip)->intersection_z(location.imag(),is); 787 if(!is.size()) 788 continue; 789 790 /* We can hit the diving curve, if its a circle */ 791 double destination_z=ip!=p? is.front():is.back(); 792 double distance=std::abs(destination_z-real(location)); 793 if(destination_z-tolerance>real(location)) 794 continue; // Hitting the diving curve at the starting point only 795 if(distance<tolerance) { 796 if(p==ip) 797 // Hitting the diving curve at the starting point. 798 continue; 799 else if(abs(location-(*ip)->sp())<tolerance) { 800 // Another curve, at the entry point, move to that curve 801 p=ip; 802 break; 803 } else { 804 // Just a zero length segment, consider it done 805 x-=delta; 806 break; 807 } 808 } 809 810 std::complex<double> dest=location; 811 dest.real(destination_z); 812 out->straight_move(dest); 813 // If the travelled distance is too small for the entire escape 814 double escape_scale=std::min(1.0,distance/2/real(escape)); 815 dest+=escape_scale*escape; 816 out->straight_rapid(dest); 817 dest=location-conj(escape_scale*escape); 818 out->straight_rapid(dest); 819 if(p==begin()) 820 out->straight_rapid(location); 821 else 822 out->straight_move(location); 823 x-=delta; 824 break; 825 } 826 } 827 } 828 829 void g7x::add_distance(double distance) { 830 auto v1=front()->ep()-front()->sp(); 831 auto v2=back()->sp()-front()->sp(); 832 auto angle=v1/v2; 833 if(imag(angle)<0) 834 distance=-distance; 835 auto of(std::move(front())); 836 pop_front(); 837 double current_distance=0; 838 while(current_distance!=distance) { 839 double max_distance=1e9; 840 for(auto p=begin(); p!=end(); p++) 841 max_distance=std::min(max_distance,(*p)->radius()/2); 842 max_distance=std::min(max_distance,std::abs(distance-current_distance)); 843 if(distance<0) 844 max_distance=-max_distance; 845 846 for(auto p=begin(); p!=end(); p++) { 847 (*p)->offset(max_distance); 848 if((*p)->radius()<1e-3) 849 erase(p--); 850 } 851 852 for(auto p=begin(); p!=--end(); p++) { 853 auto n=p; ++n; 854 if(real((*p)->ep()-(*n)->sp())>1e-2) { 855 // insert connecting arc 856 auto s((*p)->dup()); 857 auto e((*n)->dup()); 858 s->offset(-max_distance); 859 e->offset(-max_distance); 860 auto center=(s->ep()+e->sp())/2.0; 861 emplace_back(std::unique_ptr<round_segment>(new round_segment( 862 distance>0,(*p)->ep(),center,(*n)->sp()))); 863 p++; 864 } 865 } 866 867 for(auto p=begin(); p!=--end(); p++) { 868 if(!(*p)->monotonic()) { 869 std::cout << "Oops " << (*p)->sp()-(*p)->ep() << std::endl; 870 auto pp=p; --pp; 871 if((*p)->radius()<(*pp)->radius()) 872 erase(p); 873 else 874 erase(pp); 875 p=begin(); 876 } 877 auto n=p; ++n; 878 if((*p)->radius()<1e-3) { 879 erase(p); 880 p=begin(); 881 } else 882 (*p)->intersect(n->get()); 883 } 884 current_distance+=max_distance; 885 } 886 for(auto p=begin(); p!=--end(); p++) { 887 auto n=p; ++n; 888 auto mid=((*p)->ep()+(*n)->sp())/2.0; 889 (*p)->ep()=(*n)->sp()=mid; 890 } 891 892 of->ep()=front()->sp(); 893 push_front(std::move(of)); 894 } 895 896 #ifndef IGNORE_LINUXCNC 897 //////////////////////////////////////////////////////////////////////////////// 898 #include <unistd.h> 899 #include <stdio.h> 900 #include <stdlib.h> 901 #include <math.h> 902 #include <string.h> 903 #include <ctype.h> 904 #include <sys/types.h> 905 #include <sys/stat.h> 906 #include <string> 907 #include "rtapi_math.h" 908 #include "rs274ngc.hh" 909 #include "rs274ngc_return.hh" 910 #include "rs274ngc_interp.hh" 911 #include "interp_internal.hh" 912 #include "interp_queue.hh" 913 #include "interp_parameter_def.hh" 914 915 #include "units.h" 916 #include <iostream> 917 918 class motion_machine:public motion_base { 919 Interp *interp; 920 setup_pointer settings; 921 block_pointer block; 922 public: 923 motion_machine(Interp *i, setup_pointer s, block_pointer b): 924 interp(i), settings(s), block(b) { } 925 926 void straight_move(std::complex<double> end) { 927 block->x_flag=1; 928 block->x_number=imag(end); 929 block->z_flag=1; 930 block->z_number=real(end); 931 int r=interp->convert_straight(G_1, block, settings); 932 if(r!=INTERP_OK) 933 throw(r); 934 } 935 936 void straight_rapid(std::complex<double> end) { 937 block->x_flag=1; 938 block->x_number=imag(end); 939 block->z_flag=1; 940 block->z_number=real(end); 941 int r=interp->convert_straight(G_0, block, settings); 942 if(r!=INTERP_OK) 943 throw(r); 944 } 945 void circular_move(int ccw,std::complex<double> center, 946 std::complex<double> end 947 ) { 948 block->x_flag=1; 949 block->x_number=imag(end); 950 block->z_flag=1; 951 block->z_number=real(end); 952 block->i_flag=1; 953 block->i_number=imag(center); 954 block->k_flag=1; 955 block->k_number=real(center); 956 957 if(!equal(settings->current_z,block->z_number) 958 || !equal(settings->current_x,block->x_number) 959 ) { 960 int r=interp->convert_arc(ccw? G_3:G_2, block, settings); 961 if(r!=INTERP_OK) 962 throw(r); 963 } 964 } 965 }; 966 967 class switch_settings { 968 Interp *interp; 969 setup_pointer settings; 970 DISTANCE_MODE saved_distance_mode, saved_ijk_distance_mode; 971 read_function_pointer read_a, read_c, read_u, read_w; 972 public: 973 switch_settings(Interp *i,setup_pointer s):interp(i), settings(s) 974 { 975 saved_distance_mode=settings->distance_mode; 976 settings->distance_mode=MODE_ABSOLUTE; 977 saved_ijk_distance_mode=settings->ijk_distance_mode; 978 settings->ijk_distance_mode=MODE_ABSOLUTE; 979 read_a=interp->_readers[(int)'a']; 980 read_c=interp->_readers[(int)'c']; 981 read_u=interp->_readers[(int)'u']; 982 read_w=interp->_readers[(int)'w']; 983 interp->_readers[(int)'a']=interp->default_readers[(int)'a']; 984 interp->_readers[(int)'c']=interp->default_readers[(int)'c']; 985 interp->_readers[(int)'u']=interp->default_readers[(int)'u']; 986 interp->_readers[(int)'w']=interp->default_readers[(int)'w']; 987 } 988 ~switch_settings(void) { 989 settings->distance_mode=saved_distance_mode; 990 settings->ijk_distance_mode=saved_ijk_distance_mode; 991 interp->_readers[(int)'a']=read_a; 992 interp->_readers[(int)'c']=read_c; 993 interp->_readers[(int)'u']=read_u; 994 interp->_readers[(int)'w']=read_w; 995 } 996 DISTANCE_MODE ijk_distance_mode(void) { return saved_ijk_distance_mode; } 997 DISTANCE_MODE distance_mode(void) { return saved_distance_mode; } 998 }; 999 1000 int Interp::convert_g7x(int mode, 1001 block_pointer block, //!< pointer to a block of RS274 instructions 1002 setup_pointer settings) //!< pointer to machine settings 1003 { 1004 1005 if(!block->q_flag) 1006 ERS("G7x.x requires a Q word"); 1007 1008 int cycle=block->g_modes[1]; 1009 int subcycle=cycle%10; 1010 cycle/=10; 1011 1012 if(settings->cutter_comp_side && cycle!=70) 1013 ERS("G%d.%d cannot be used with cutter compensation enabled", 1014 cycle, subcycle); 1015 if(settings->plane!=CANON_PLANE_XZ) 1016 ERS("G%d.%d can only be used in XZ plane (G18)", 1017 cycle, subcycle); 1018 1019 switch_settings old(this,settings); 1020 1021 auto original_block=*block; 1022 1023 double x=settings->current_x; 1024 double z=settings->current_z; 1025 if(old.distance_mode()==MODE_INCREMENTAL) { 1026 if(block->x_flag) 1027 x+=block->x_number; 1028 if(block->z_flag) 1029 z+=block->z_number; 1030 } else { 1031 if(block->x_flag) 1032 x=block->x_number; 1033 if(block->z_flag) 1034 z=block->z_number; 1035 } 1036 original_block.x_number=x; 1037 original_block.z_number=z; 1038 1039 g7x path; 1040 std::complex<double> start(z,x); 1041 1042 auto exit_call_level=settings->call_level; 1043 CHP(read((std::string("O")+std::to_string(block->q_number)+" CALL").c_str())); 1044 for(;;) { 1045 if(block->o_name!=0) 1046 CHP(convert_control_functions(block, settings)); 1047 if(settings->call_level==exit_call_level) 1048 break; 1049 1050 for(int n=0; n<settings->parameter_occurrence; n++) 1051 settings->parameters[settings->parameter_numbers[n]]= 1052 settings->parameter_values[n]; 1053 1054 for(int n=0; n<settings->named_parameter_occurrence; n++) 1055 CHP(store_named_param(&_setup, settings->named_parameters[n], 1056 settings->named_parameter_values[n] 1057 )); 1058 settings->named_parameter_occurrence = 0; 1059 1060 std::complex<double> end(start); 1061 std::complex<double> center(0,0); 1062 1063 if(old.distance_mode()==MODE_INCREMENTAL) 1064 end=0; 1065 1066 if(block->u_flag) { 1067 if(old.distance_mode()==MODE_INCREMENTAL) 1068 ERS("G7x error: Cannot use U in incremental mode (G91)"); 1069 if(block->x_flag) 1070 ERS("G7x error: Cannot use U and X in the same block"); 1071 auto u=block->u_number; 1072 if(settings->lathe_diameter_mode) 1073 u/=2; 1074 end.imag(end.imag()+u); 1075 } else if(block->x_flag) 1076 end.imag(block->x_number); 1077 1078 if(block->w_flag) { 1079 if(old.distance_mode()==MODE_INCREMENTAL) 1080 ERS("G7x error: Cannot use W in incremental mode (G91)"); 1081 if(block->z_flag) 1082 ERS("G7x error: Cannot use W and Z in the same block"); 1083 end.real(end.real()+block->w_number); 1084 } else if(block->z_flag) 1085 end.real(block->z_number); 1086 1087 if(block->i_flag) center.imag(block->i_number); 1088 if(block->k_flag) center.real(block->k_number); 1089 1090 if(old.distance_mode()==MODE_INCREMENTAL) 1091 end+=start; 1092 1093 if(block->g_modes[1]!=-1) 1094 settings->motion_mode=block->g_modes[1]; 1095 if(start!=end) { 1096 switch(settings->motion_mode) { 1097 case 0: 1098 case 10: 1099 path.emplace_back(std::unique_ptr<straight_segment>(new straight_segment( 1100 start, end 1101 ))); 1102 break; 1103 case 20: 1104 case 30: 1105 if(block->r_flag) { 1106 if(block->i_flag || block->k_flag) 1107 ERS("G7X error: both R and I or K flag used for arc"); 1108 double r=block->r_number; 1109 center=(start+end)/2.0; 1110 auto d=std::complex<double>(0,1)*sqrt((r*r-norm(end-start)/4)/norm(end-start)) 1111 *(end-start); 1112 if(settings->motion_mode==30) 1113 center+=d; 1114 else 1115 center-=d; 1116 } else { 1117 if(!block->i_flag && !block->k_flag) 1118 ERS("G7X error: either I or K must be present for arc"); 1119 if(old.ijk_distance_mode()==MODE_INCREMENTAL) 1120 center+=start; 1121 } 1122 path.emplace_back(std::unique_ptr<round_segment>(new round_segment( 1123 settings->motion_mode==30, start, center, end 1124 ))); 1125 break; 1126 } 1127 if(settings->motion_mode==0 || settings->motion_mode==10 1128 || settings->motion_mode==20 || settings->motion_mode==30 1129 ) { 1130 if(block->a_flag && block->c_flag) 1131 ERS("G7X error: Both A and C parameters on a corner"); 1132 if(block->a_flag) 1133 path.emplace_back(std::unique_ptr<fillet_segment>(new fillet_segment( 1134 block->a_number, end 1135 ))); 1136 if(block->c_flag) 1137 path.emplace_back(std::unique_ptr<chamfer_segment>(new chamfer_segment( 1138 block->c_number, end 1139 ))); 1140 } 1141 1142 settings->current_x=imag(end); 1143 settings->current_z=real(end); 1144 start=end; 1145 } 1146 CHP(read()); 1147 } 1148 1149 double d=0, e=0, i=1, p=1, r=0.5, u=0, w=0; 1150 if(original_block.d_flag) d=original_block.d_number_float; 1151 if(original_block.e_flag) e=original_block.e_number; 1152 if(original_block.i_flag) i=original_block.i_number; 1153 if(original_block.p_flag) p=original_block.p_number; 1154 if(original_block.r_flag) r=original_block.r_number; 1155 if(original_block.u_flag) u=original_block.u_number; 1156 if(original_block.w_flag) w=original_block.w_number; 1157 if(original_block.x_flag) x=original_block.x_number; 1158 if(original_block.z_flag) z=original_block.z_number; 1159 1160 if(i<=0) 1161 ERS("G7X error: I must be greater than zero."); 1162 1163 motion_machine motion(this, settings, block); 1164 try { 1165 switch(cycle) { 1166 case 70: path.do_g70(&motion,x,z,d,e,p); break; 1167 case 71: path.do_g71(&motion,subcycle,x,z,u,w,d,i,r); break; 1168 case 72: path.do_g72(&motion,subcycle,x,z,u,w,d,i,r); break; 1169 } 1170 } catch(std::string &s) { 1171 ERS("G7X error: %s", s.c_str()); 1172 return INTERP_ERROR; 1173 } catch(int i) { 1174 return i; 1175 } 1176 1177 return INTERP_OK; 1178 } 1179 #endif