src/ChFi2d/ChFi2d_AnaFilletAlgo.cxx

   1 // Copyright (c) 1999-2014 OPEN CASCADE SAS
   2 //
   3 // This file is part of Open CASCADE Technology software library.
   4 //
   5 // This library is free software; you can redistribute it and/or modify it under
   6 // the terms of the GNU Lesser General Public License version 2.1 as published
   7 // by the Free Software Foundation, with special exception defined in the file
   8 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
   9 // distribution for complete text of the license and disclaimer of any warranty.
  10 //
  11 // Alternatively, this file may be used under the terms of Open CASCADE
  12 // commercial license or contractual agreement.
  13
  14 #include <ChFi2d_AnaFilletAlgo.hxx>
  15
  16 #include <gp_Ax3.hxx>
  17 #include <gp_Circ.hxx>
  18 #include <gp_Lin2d.hxx>
  19 #include <gp_Circ2d.hxx>
  20
  21 #include <Standard_TypeMismatch.hxx>
  22
  23 #include <BRepBuilderAPI_MakeEdge.hxx>
  24 #include <BRepBuilderAPI_MakeWire.hxx>
  25 #include <BRepBuilderAPI_MakeFace.hxx>
  26
  27 #include <GeomAPI_ExtremaCurveCurve.hxx>
  28 #include <IntAna2d_AnaIntersection.hxx>
  29 #include <ShapeAnalysis_Wire.hxx>
  30 #include <Geom_Circle.hxx>
  31
  32 #include <BRepAdaptor_Curve.hxx>
  33 #include <BRep_Tool.hxx>
  34
  35 #include <TopoDS.hxx>
  36 #include <TopoDS_Iterator.hxx>
  37
  38 #include <ProjLib.hxx>
  39 #include <TopExp.hxx>
  40 #include <ElSLib.hxx>
  41
  42 // Compute the flag: CW || CCW
  43 static Standard_Boolean isCW(const BRepAdaptor_Curve& AC)
  44 {
  45   const Standard_Real f = AC.FirstParameter();
  46   const Standard_Real l = AC.LastParameter();
  47   Handle(Geom_Circle) circle = Handle(Geom_Circle)::DownCast(AC.Curve().Curve());
  48   gp_Pnt start = AC.Value(f);
  49   gp_Pnt end = AC.Value(l);
  50   gp_Pnt center = AC.Circle().Location();
  51   gp_Ax3 plane = AC.Circle().Position();
  52
  53   // Get point on circle at half angle
  54   gp_Pnt m;
  55   circle->D0(0.5 * (f + l), m);
  56
  57   // Compare angles between vectors to middle point and to the end point.
  58   gp_Vec startv(center, start), endv(center, end), middlev(center, m);
  59   double middlea = startv.AngleWithRef(middlev, plane.Direction());
  60   while(middlea < 0.0)
  61     middlea += 2.0 * M_PI;
  62   double enda = startv.AngleWithRef(endv, plane.Direction());
  63   while(enda < 0.0)
  64     enda += 2.0 * M_PI;
  65
  66   Standard_Boolean is_cw = middlea > enda ? Standard_True : Standard_False;
  67   return is_cw;
  68 }
  69
  70 // Equality of points computed through square distance between the points.
  71 static Standard_Boolean IsEqual(const gp_Pnt& p1, const gp_Pnt& p2)
  72 {
  73   return p1.SquareDistance(p2) < Precision::SquareConfusion();
  74 }
  75 static Standard_Boolean IsEqual(const gp_Pnt2d& p1, const gp_Pnt2d& p2)
  76 {
  77   return p1.SquareDistance(p2) < Precision::SquareConfusion();
  78 }
  79
  80 // An empty constructor.
  81 // Use the method Init() to initialize the class.
  82 ChFi2d_AnaFilletAlgo::ChFi2d_AnaFilletAlgo()
  83 {
  84
  85 }
  86
  87 // An constructor.
  88 // It expects two edges having a common point of type:
  89 // - segment
  90 // - arc of circle.
  91 ChFi2d_AnaFilletAlgo::ChFi2d_AnaFilletAlgo(const TopoDS_Wire& theWire,
  92                                            const gp_Pln& thePlane)
  93 {
  94   Init(theWire, thePlane);
  95 }
  96
  97 // A constructor.
  98 // It expects two edges having a common point of type:
  99 // - segment
 100 // - arc of circle.
 101 ChFi2d_AnaFilletAlgo::ChFi2d_AnaFilletAlgo(const TopoDS_Edge& theEdge1,
 102                                            const TopoDS_Edge& theEdge2,
 103                                            const gp_Pln& thePlane)
 104 {
 105   // Make a wire consisting of two edges.
 106   Init(theEdge1, theEdge2, thePlane);
 107 }
 108
 109 // Initializes the class by a wire consisting of two edges.
 110 void ChFi2d_AnaFilletAlgo::Init(const TopoDS_Wire& theWire, const gp_Pln& thePlane)
 111 {
 112   plane = thePlane;
 113   TopoDS_Iterator itr(theWire);
 114   for (; itr.More(); itr.Next())
 115   {
 116     if (e1.IsNull())
 117       e1 = TopoDS::Edge(itr.Value());
 118     else if (e2.IsNull())
 119       e2 = TopoDS::Edge(itr.Value());
 120   }
 121   if (e1.IsNull() || e2.IsNull())
 122     Standard_TypeMismatch::Raise("The algorithm expects a wire consisting of two linear or circular edges.");
 123
 124   // Left neighbour.
 125   BRepAdaptor_Curve AC1(e1);
 126   if (AC1.GetType() != GeomAbs_Line && AC1.GetType() != GeomAbs_Circle)
 127     Standard_TypeMismatch::Raise("A segment or an arc of circle is expected.");
 128
 129   TopoDS_Vertex v1, v2;
 130   TopExp::Vertices(e1, v1, v2, Standard_True);
 131   if (v1.IsNull() || v2.IsNull())
 132     Standard_Failure::Raise("An infinite edge.");
 133
 134   gp_Pnt P1 = BRep_Tool::Pnt(v1);
 135   gp_Pnt P2 = BRep_Tool::Pnt(v2);
 136   gp_Pnt2d p1 = ProjLib::Project(thePlane, P1);
 137   gp_Pnt2d p2 = ProjLib::Project(thePlane, P2);
 138   p1.Coord(x11, y11);
 139   p2.Coord(x12, y12);
 140
 141   segment1 = true;
 142   if (AC1.GetType() == GeomAbs_Circle)
 143   {
 144     segment1 = false;
 145     gp_Circ c = AC1.Circle();
 146
 147     gp_Pnt2d loc = ProjLib::Project(thePlane, c.Location());
 148     loc.Coord(xc1, yc1);
 149
 150     radius1 = c.Radius();
 151     cw1 = isCW(AC1);
 152   }
 153
 154   // Right neighbour.
 155   BRepAdaptor_Curve AC2(e2);
 156   if (AC2.GetType() != GeomAbs_Line && AC2.GetType() != GeomAbs_Circle)
 157     Standard_TypeMismatch::Raise("A segment or an arc of circle is expected.");
 158
 159   TopExp::Vertices(e2, v1, v2, Standard_True);
 160   if (v1.IsNull() || v2.IsNull())
 161     Standard_Failure::Raise("An infinite edge.");
 162
 163   P1 = BRep_Tool::Pnt(v1);
 164   P2 = BRep_Tool::Pnt(v2);
 165   p1 = ProjLib::Project(thePlane, P1);
 166   p2 = ProjLib::Project(thePlane, P2);
 167   p1.Coord(x21, y21);
 168   p2.Coord(x22, y22);
 169
 170   segment2 = true;
 171   if (AC2.GetType() == GeomAbs_Circle)
 172   {
 173     segment2 = false;
 174     gp_Circ c = AC2.Circle();
 175
 176     gp_Pnt2d loc = ProjLib::Project(thePlane, c.Location());
 177     loc.Coord(xc2, yc2);
 178
 179     radius2 = c.Radius();
 180     cw2 = isCW(AC2);
 181   }
 182 }
 183
 184 // Initializes the class by two edges.
 185 void ChFi2d_AnaFilletAlgo::Init(const TopoDS_Edge& theEdge1, const TopoDS_Edge& theEdge2,
 186                                 const gp_Pln& thePlane)
 187 {
 188   // Make a wire consisting of two edges.
 189
 190   // Get common point.
 191   TopoDS_Vertex v11, v12, v21, v22;
 192   TopExp::Vertices(theEdge1, v11, v12, Standard_True);
 193   TopExp::Vertices(theEdge2, v21, v22, Standard_True);
 194   if (v11.IsNull() || v12.IsNull() || v21.IsNull() || v22.IsNull())
 195     Standard_Failure::Raise("An infinite edge.");
 196
 197   gp_Pnt p11 = BRep_Tool::Pnt(v11);
 198   gp_Pnt p12 = BRep_Tool::Pnt(v12);
 199   gp_Pnt p21 = BRep_Tool::Pnt(v21);
 200   gp_Pnt p22 = BRep_Tool::Pnt(v22);
 201
 202   gp_Pnt pcommon;
 203   if (IsEqual(p11, p21) || IsEqual(p11, p22))
 204   {
 205     pcommon = p11;
 206   }
 207   else if (IsEqual(p12, p21) || IsEqual(p12, p22))
 208   {
 209     pcommon = p12;
 210   }
 211   else
 212     Standard_Failure::Raise("The edges have no common point.");
 213
 214   // Reverse the edges in case of need (to construct a wire).
 215   Standard_Boolean is1stReversed(Standard_False), is2ndReversed(Standard_False);
 216   if (IsEqual(pcommon, p11))
 217     is1stReversed = Standard_True;
 218   else if (IsEqual(pcommon, p22))
 219     is2ndReversed = Standard_True;
 220
 221   // Make a wire.
 222   BRepBuilderAPI_MakeWire mkWire;
 223   if (is1stReversed)
 224     mkWire.Add(TopoDS::Edge(theEdge1.Reversed()));
 225   else
 226     mkWire.Add(theEdge1);
 227   if (is2ndReversed)
 228     mkWire.Add(TopoDS::Edge(theEdge2.Reversed()));
 229   else
 230     mkWire.Add(theEdge2);
 231   if (!mkWire.IsDone())
 232     Standard_Failure::Raise("Can't make a wire.");
 233
 234   const TopoDS_Wire& W = mkWire.Wire();
 235   Init(W, thePlane);
 236 }
 237
 238 // Calculates a fillet.
 239 Standard_Boolean ChFi2d_AnaFilletAlgo::Perform(const Standard_Real radius)
 240 {
 241   Standard_Boolean bRet(false);
 242   if (e1.IsNull() || e2.IsNull() ||
 243       radius < Precision::Confusion())
 244   {
 245     return bRet;
 246   }
 247
 248   // Fillet definition.
 249   Standard_Real xc = 0.0, yc = 0.0;
 250   Standard_Real start = 0.0, end = 0.0;             // parameters on neighbours
 251   Standard_Real xstart = DBL_MAX, ystart = DBL_MAX; // point on left neighbour
 252   Standard_Real xend = DBL_MAX, yend = DBL_MAX;     // point on right neighbour
 253   Standard_Boolean cw = Standard_False;
 254
 255   // Analytical algorithm works for non-intersecting arcs only.
 256   // Check arcs on self-intersection.
 257   Standard_Boolean isCut(Standard_False);
 258   if (!segment1 || !segment2)
 259   {
 260     BRepBuilderAPI_MakeWire mkWire(e1, e2);
 261     if (mkWire.IsDone())
 262     {
 263       const TopoDS_Wire& W = mkWire.Wire();
 264       BRepBuilderAPI_MakeFace mkFace(plane);
 265       if (mkFace.IsDone())
 266       {
 267         const TopoDS_Face& F = mkFace.Face();
 268         ShapeAnalysis_Wire analyzer(W, F, Precision::Confusion());
 269         if (analyzer.CheckSelfIntersection() == Standard_True)
 270         {
 271           // Cut the edges at the point of intersection.
 272           isCut = Standard_True;
 273           if (!Cut(plane, e1, e2))
 274           {
 275             return Standard_False;
 276           }
 277         }
 278       }
 279     }
 280   }// a case of segment - segment
 281
 282   // Choose the case.
 283   BRepAdaptor_Curve AC1(e1), AC2(e2);
 284   if (segment1 && segment2)
 285   {
 286     bRet = SegmentFilletSegment(radius, xc, yc, cw, start, end);
 287   }
 288   else if (segment1 && !segment2)
 289   {
 290     bRet = SegmentFilletArc(radius, xc, yc, cw, start, end, xend, yend);
 291   }
 292   else if (!segment1 && segment2)
 293   {
 294     bRet = ArcFilletSegment(radius, xc, yc, cw, start, end, xstart, ystart);
 295   }
 296   else if (!segment1 && !segment2)
 297   {
 298     bRet = ArcFilletArc(radius, xc, yc, cw, start, end);
 299   }
 300
 301   if (!bRet)
 302     return Standard_False;
 303
 304   // Invert the fillet for left-handed plane.
 305   if (plane.Position().Direct() == Standard_False)
 306     cw = !cw;
 307
 308   // Construct a fillet.
 309   // Make circle.
 310   gp_Pnt center = ElSLib::Value(xc, yc, plane);
 311   const gp_Dir& normal = plane.Position().Direction();
 312   gp_Circ circ(gp_Ax2(center, cw ? -normal : normal), radius);
 313
 314   // Fillet may only shrink a neighbour edge, it can't prolongate it.
 315   const Standard_Real delta1 = AC1.LastParameter() - AC1.FirstParameter();
 316   const Standard_Real delta2 = AC2.LastParameter() - AC2.FirstParameter();
 317   if (!isCut && (start > delta1 || end > delta2))
 318   {
 319     // Check a case when a neighbour edge almost disappears:
 320     // try to reduce the fillet radius for a little (1.e-5 mm).
 321     const Standard_Real little = 100.0 * Precision::Confusion();
 322     const Standard_Real d1 = fabs(start - delta1);
 323     const Standard_Real d2 = fabs(end   - delta2);
 324     if (d1 < little || d2 < little)
 325     {
 326       if (segment1 && segment2)
 327       {
 328         bRet = SegmentFilletSegment(radius - little, xc, yc, cw, start, end);
 329       }
 330       else if (segment1 && !segment2)
 331       {
 332         bRet = SegmentFilletArc(radius - little, xc, yc, cw, start, end, xend, yend);
 333       }
 334       else if (!segment1 && segment2)
 335       {
 336         bRet = ArcFilletSegment(radius - little, xc, yc, cw, start, end, xstart, ystart);
 337       }
 338       else if (!segment1 && !segment2)
 339       {
 340         bRet = ArcFilletArc(radius - little, xc, yc, cw, start, end);
 341       }
 342       if (bRet)
 343       {
 344         // Invert the fillet for left-handed planes.
 345         if (plane.Position().Direct() == Standard_False)
 346           cw = !cw;
 347
 348         // Make the circle again.
 349         center = ElSLib::Value(xc, yc, plane);
 350         circ.SetLocation(center);
 351         circ.SetRadius(radius - little);
 352       }
 353       else
 354       {
 355         return Standard_False;
 356       }
 357     }
 358     else
 359     {
 360       return Standard_False;
 361     }
 362   }
 363   if (bRet)
 364   {
 365     // start: (xstart, ystart) - pstart.
 366     gp_Pnt pstart;
 367     if (xstart != DBL_MAX)
 368     {
 369       pstart = ElSLib::Value(xstart, ystart, plane);
 370     }
 371     else
 372     {
 373       if (e1.Orientation() == TopAbs_FORWARD)
 374         pstart = AC1.Value(AC1.LastParameter() - start);
 375       else
 376         pstart = AC1.Value(AC1.FirstParameter() + start);
 377     }
 378     // end: (xend, yend) -> pend.
 379     gp_Pnt pend;
 380     if (xend != DBL_MAX)
 381     {
 382       pend = ElSLib::Value(xend, yend, plane);
 383     }
 384     else
 385     {
 386       if (e2.Orientation() == TopAbs_FORWARD)
 387         pend = AC2.Value(AC2.FirstParameter() + end);
 388       else
 389         pend = AC2.Value(AC2.LastParameter() - end);
 390     }
 391
 392     // Make arc.
 393     BRepBuilderAPI_MakeEdge mkEdge(circ, pstart, pend);
 394     bRet = mkEdge.IsDone();
 395     if (bRet)
 396     {
 397       fillet = mkEdge.Edge();
 398
 399       // Limit the neighbours.
 400       // Left neighbour.
 401       gp_Pnt p1, p2;
 402       shrinke1.Nullify();
 403       if (e1.Orientation() == TopAbs_FORWARD)
 404       {
 405         p1 = AC1.Value(AC1.FirstParameter());
 406         p2 = pstart;
 407       }
 408       else
 409       {
 410         p1 = pstart;
 411         p2 = AC1.Value(AC1.LastParameter());
 412       }
 413       if (segment1)
 414       {
 415         BRepBuilderAPI_MakeEdge mkSegment1;
 416         mkSegment1.Init(AC1.Curve().Curve(), p1, p2);
 417         if (mkSegment1.IsDone())
 418           shrinke1 = mkSegment1.Edge();
 419       }
 420       else
 421       {
 422         BRepBuilderAPI_MakeEdge mkCirc1;
 423         mkCirc1.Init(AC1.Curve().Curve(), p1, p2);
 424         if (mkCirc1.IsDone())
 425           shrinke1 = mkCirc1.Edge();
 426       }
 427
 428       // Right neighbour.
 429       shrinke2.Nullify();
 430       if (e1.Orientation() == TopAbs_FORWARD)
 431       {
 432         p1 = pend;
 433         p2 = AC2.Value(AC2.LastParameter());
 434       }
 435       else
 436       {
 437         p1 = AC2.Value(AC2.FirstParameter());
 438         p2 = pend;
 439       }
 440       if (segment2)
 441       {
 442         BRepBuilderAPI_MakeEdge mkSegment2;
 443         mkSegment2.Init(AC2.Curve().Curve(), p1, p2);
 444         if (mkSegment2.IsDone())
 445           shrinke2 = mkSegment2.Edge();
 446       }
 447       else
 448       {
 449         BRepBuilderAPI_MakeEdge mkCirc2;
 450         mkCirc2.Init(AC2.Curve().Curve(), p1, p2);
 451         if (mkCirc2.IsDone())
 452           shrinke2 = mkCirc2.Edge();
 453       }
 454
 455       bRet = !shrinke1.IsNull() && !shrinke2.IsNull();
 456     }// fillet edge is done
 457   }// shrinking is good
 458
 459   return bRet;
 460 }
 461
 462 // Retrieves a result (fillet and shrinked neighbours).
 463 const TopoDS_Edge& ChFi2d_AnaFilletAlgo::Result(TopoDS_Edge& theE1, TopoDS_Edge& theE2)
 464 {
 465   theE1 = shrinke1;
 466   theE2 = shrinke2;
 467   return fillet;
 468 }
 469
 470 // WW5 method to compute fillet.
 471 // It returns a constructed fillet definition:
 472 //     center point (xc, yc)
 473 //     point on the 1st segment (xstart, ystart)
 474 //     point on the 2nd segment (xend, yend)
 475 //     is the arc of fillet clockwise (cw = true) or counterclockwise (cw = false).
 476 Standard_Boolean ChFi2d_AnaFilletAlgo::SegmentFilletSegment(const Standard_Real radius,
 477                                                             Standard_Real& xc, Standard_Real& yc,
 478                                                             Standard_Boolean& cw,
 479                                                             Standard_Real& start, Standard_Real& end)
 480 {
 481   // Make normalized vectors at p12.
 482   gp_Pnt2d p11(x11, y11);
 483   gp_Pnt2d p12(x12, y12);
 484   gp_Pnt2d p22(x22, y22);
 485
 486   // Check length of segments.
 487   if (IsEqual(p12, p11) || IsEqual(p12, p22))
 488   {
 489     return Standard_False;
 490   }
 491
 492   // Make vectors.
 493   gp_Vec2d v1(p12, p11);
 494   gp_Vec2d v2(p12, p22);
 495   v1.Normalize();
 496   v2.Normalize();
 497
 498   // Make bisectrissa.
 499   gp_Vec2d bisec = 0.5 * (v1 + v2);
 500
 501   // Check bisectrissa.
 502   if (bisec.SquareMagnitude() < Precision::SquareConfusion())
 503     return Standard_False;
 504
 505   // Normalize the bisectrissa.
 506   bisec.Normalize();
 507
 508   // Angle at bisectrissa.
 509   Standard_Real beta = v1.Angle(bisec);
 510
 511   // Length along the bisectrissa till the center of fillet.
 512   Standard_Real L = radius / sin(fabs(beta));
 513
 514   // Center point of fillet.
 515   gp_Pnt2d pc = p12.Translated(L * bisec);
 516   pc.Coord(xc, yc);
 517
 518   // Shrinking length along segments.
 519   start = sqrt(L * L - radius * radius);
 520   end = start;
 521
 522   // Orientation of fillet.
 523   cw = beta > 0.0;
 524   return Standard_True;
 525 }
 526
 527 // A function constructs a fillet between a segment and an arc.
 528 Standard_Boolean ChFi2d_AnaFilletAlgo::SegmentFilletArc(const Standard_Real radius,
 529                                                         Standard_Real& xc, Standard_Real& yc,
 530                                                         Standard_Boolean& cw,
 531                                                         Standard_Real& start, Standard_Real& end,
 532                                                         Standard_Real& xend, Standard_Real& yend)
 533 {
 534   // Make a line parallel to the segment at the side of center point of fillet.
 535   // This side may be defined through making a bisectrissa for vectors at p12 (or p21).
 536
 537   // Make 2D points.
 538   gp_Pnt2d p12(x12, y12);
 539   gp_Pnt2d p11(x11, y11);
 540   gp_Pnt2d pc2(xc2, yc2);
 541
 542   // Check length of segment.
 543   if (p11.SquareDistance(p12) < gp::Resolution())
 544     return Standard_False;
 545
 546   // Make 2D vectors.
 547   gp_Vec2d v1(p12, p11);
 548   gp_Vec2d v2(p12, pc2);
 549
 550   // Rotate the arc vector to become tangential at p21.
 551   if (cw2)
 552     v2.Rotate(+M_PI_2);
 553   else
 554     v2.Rotate(-M_PI_2);
 555
 556   // If vectors coincide (segment and arc are tangent),
 557   // the algorithm doesn't work...
 558   Standard_Real angle = v1.Angle(v2);
 559   if (fabs(angle) < Precision::Angular())
 560     return Standard_False;
 561
 562   // Make a bissectrisa of vectors at p12.
 563   v2.Normalize();
 564   v1.Normalize();
 565   gp_Vec2d bisec = 0.5 * (v1 + v2);
 566
 567   // If segment and arc look in opposite direction,
 568   // no fillet is possible.
 569   if (bisec.SquareMagnitude() < gp::Resolution())
 570     return Standard_False;
 571
 572   // Define an appropriate point to choose center of fillet.
 573   bisec.Normalize();
 574   gp_Pnt2d nearp = p12.Translated(radius * bisec);
 575   gp_Lin2d nearl(p12, bisec);
 576
 577   // Make a line parallel to segment and
 578   // passing near the "near" point.
 579   gp_Vec2d d1(v1);
 580   gp_Lin2d line(p11, -d1);
 581   d1.Rotate(M_PI_2);
 582   line.Translate(radius * d1);
 583   if (line.Distance(nearp) > radius)
 584     line.Translate(-2.0 * radius * d1);
 585
 586   // Make a circle of radius of the arc +/- fillet radius.
 587   gp_Ax2d axes(pc2, gp::DX2d());
 588   gp_Circ2d circ(axes, radius2 + radius);
 589   if (radius2 > radius && circ.Distance(nearp) > radius)
 590     circ.SetRadius(radius2 - radius);
 591
 592   // Calculate intersection of the line and the circle.
 593   IntAna2d_AnaIntersection intersector(line, circ);
 594   if (!intersector.IsDone() || !intersector.NbPoints())
 595     return Standard_False;
 596
 597   // Find center point of fillet.
 598   Standard_Integer i;
 599   Standard_Real minDist = DBL_MAX;
 600   for (i = 1; i <= intersector.NbPoints(); ++i)
 601   {
 602     const IntAna2d_IntPoint& intp = intersector.Point(i);
 603     const gp_Pnt2d& p = intp.Value();
 604
 605     Standard_Real d = nearl.Distance(p);
 606     if (d < minDist)
 607     {
 608       minDist = d;
 609       p.Coord(xc, yc);
 610     }
 611   }
 612
 613   // Shrink of segment.
 614   gp_Pnt2d pc(xc, yc);
 615   Standard_Real L2 = pc.SquareDistance(p12);
 616   const Standard_Real Rf2 = radius * radius;
 617   start = sqrt(L2 - Rf2);
 618
 619   // Shrink of arc.
 620   gp_Vec2d pcc(pc2, pc);
 621   end = fabs(gp_Vec2d(pc2, p12).Angle(pcc));
 622
 623   // Duplicate the information on shrink the arc:
 624   // calculate a point on the arc coinciding with the end of fillet.
 625   line.SetLocation(pc2);
 626   line.SetDirection(pcc);
 627   circ.SetLocation(pc2);
 628   circ.SetRadius(radius2);
 629   intersector.Perform(line, circ);
 630   if (!intersector.IsDone() || !intersector.NbPoints())
 631     return Standard_False;
 632
 633   xend = DBL_MAX;
 634   yend = DBL_MAX;
 635   for (i = 1; i <= intersector.NbPoints(); ++i)
 636   {
 637     const IntAna2d_IntPoint& intp = intersector.Point(i);
 638     const gp_Pnt2d& p = intp.Value();
 639
 640     const Standard_Real d2 = p.SquareDistance(pc);
 641     if (fabs(d2 - Rf2) < Precision::Confusion())
 642     {
 643       p.Coord(xend, yend);
 644       break;
 645     }
 646   }
 647
 648   // Orientation of the fillet.
 649   angle = v1.Angle(v2);
 650   cw = angle > 0.0;
 651   return Standard_True;
 652 }
 653
 654 // A function constructs a fillet between an arc and a segment.
 655 Standard_Boolean ChFi2d_AnaFilletAlgo::ArcFilletSegment(const Standard_Real radius,
 656                                                         Standard_Real& xc, Standard_Real& yc,
 657                                                         Standard_Boolean& cw,
 658                                                         Standard_Real& start, Standard_Real& end,
 659                                                         Standard_Real& xstart, Standard_Real& ystart)
 660 {
 661   // Make a line parallel to the segment at the side of center point of fillet.
 662   // This side may be defined through making a bisectrissa for vectors at p12 (or p21).
 663
 664   // Make 2D points.
 665   gp_Pnt2d p12(x12, y12);
 666   gp_Pnt2d p22(x22, y22);
 667   gp_Pnt2d pc1(xc1, yc1);
 668
 669   // Check length of segment.
 670   if (p12.SquareDistance(p22) < gp::Resolution())
 671     return Standard_False;
 672
 673   // Make 2D vectors.
 674   gp_Vec2d v1(p12, pc1);
 675   gp_Vec2d v2(p12, p22);
 676
 677   // Rotate the arc vector to become tangential at p21.
 678   if (cw1)
 679     v1.Rotate(-M_PI_2);
 680   else
 681     v1.Rotate(+M_PI_2);
 682
 683   // If vectors coincide (segment and arc are tangent),
 684   // the algorithm doesn't work...
 685   Standard_Real angle = v1.Angle(v2);
 686   if (fabs(angle) < Precision::Angular())
 687     return Standard_False;
 688
 689   // Make a bisectrissa of vectors at p12.
 690   v1.Normalize();
 691   v2.Normalize();
 692   gp_Vec2d bisec = 0.5 * (v1 + v2);
 693
 694   // If segment and arc look in opposite direction,
 695   // no fillet is possible.
 696   if (bisec.SquareMagnitude() < gp::Resolution())
 697     return Standard_False;
 698
 699   // Define an appropriate point to choose center of fillet.
 700   bisec.Normalize();
 701   gp_Pnt2d nearPoint = p12.Translated(radius * bisec);
 702   gp_Lin2d nearLine(p12, bisec);
 703
 704   // Make a line parallel to segment and
 705   // passing near the "near" point.
 706   gp_Vec2d d2(v2);
 707   gp_Lin2d line(p22, -d2);
 708   d2.Rotate(M_PI_2);
 709   line.Translate(radius * d2);
 710   if (line.Distance(nearPoint) > radius)
 711     line.Translate(-2.0 * radius * d2);
 712
 713   // Make a circle of radius of the arc +/- fillet radius.
 714   gp_Ax2d axes(pc1, gp::DX2d());
 715   gp_Circ2d circ(axes, radius1 + radius);
 716   if (radius1 > radius && circ.Distance(nearPoint) > radius)
 717     circ.SetRadius(radius1 - radius);
 718
 719   // Calculate intersection of the line and the big circle.
 720   IntAna2d_AnaIntersection intersector(line, circ);
 721   if (!intersector.IsDone() || !intersector.NbPoints())
 722     return Standard_False;
 723
 724   // Find center point of fillet.
 725   Standard_Integer i;
 726   Standard_Real minDist = DBL_MAX;
 727   for (i = 1; i <= intersector.NbPoints(); ++i)
 728   {
 729     const IntAna2d_IntPoint& intp = intersector.Point(i);
 730     const gp_Pnt2d& p = intp.Value();
 731
 732     Standard_Real d = nearLine.Distance(p);
 733     if (d < minDist)
 734     {
 735       minDist = d;
 736       p.Coord(xc, yc);
 737     }
 738   }
 739
 740   // Shrink of segment.
 741   gp_Pnt2d pc(xc, yc);
 742   Standard_Real L2 = pc.SquareDistance(p12);
 743   const Standard_Real Rf2 = radius * radius;
 744   end = sqrt(L2 - Rf2);
 745
 746   // Shrink of arc.
 747   gp_Vec2d pcc(pc1, pc);
 748   start = fabs(gp_Vec2d(pc1, p12).Angle(pcc));
 749
 750   // Duplicate the information on shrink the arc:
 751   // calculate a point on the arc coinciding with the start of fillet.
 752   line.SetLocation(pc1);
 753   line.SetDirection(pcc);
 754   circ.SetLocation(pc1);
 755   circ.SetRadius(radius1);
 756   intersector.Perform(line, circ);
 757   if (!intersector.IsDone() || !intersector.NbPoints())
 758     return Standard_False;
 759
 760   xstart = DBL_MAX;
 761   ystart = DBL_MAX;
 762   for (i = 1; i <= intersector.NbPoints(); ++i)
 763   {
 764     const IntAna2d_IntPoint& intp = intersector.Point(i);
 765     const gp_Pnt2d& p = intp.Value();
 766
 767     const Standard_Real d2 = p.SquareDistance(pc);
 768     if (fabs(d2 - Rf2) < Precision::SquareConfusion())
 769     {
 770       p.Coord(xstart, ystart);
 771       break;
 772     }
 773   }
 774
 775   // Orientation of the fillet.
 776   angle = v2.Angle(v1);
 777   cw = angle < 0.0;
 778   return Standard_True;
 779 }
 780
 781 // WW5 method to compute fillet: arc - arc.
 782 // It returns a constructed fillet definition:
 783 //     center point (xc, yc)
 784 //     shrinking parameter of the 1st circle (start)
 785 //     shrinking parameter of the 2nd circle (end)
 786 //     if the arc of fillet clockwise (cw = true) or counterclockwise (cw = false).
 787 Standard_Boolean ChFi2d_AnaFilletAlgo::ArcFilletArc(const Standard_Real radius,
 788                                                     Standard_Real& xc, Standard_Real& yc,
 789                                                     Standard_Boolean& cw,
 790                                                     Standard_Real& start, Standard_Real& end)
 791 {
 792   // Make points.
 793   const gp_Pnt2d pc1(xc1, yc1);
 794   const gp_Pnt2d pc2(xc2, yc2);
 795   const gp_Pnt2d p12(x12, y12);
 796
 797   // Make vectors at p12.
 798   gp_Vec2d v1(pc1, p12);
 799   gp_Vec2d v2(pc2, p12);
 800
 801   // Rotate the vectors so that they are tangent to circles at p12.
 802   if (cw1)
 803     v1.Rotate(+M_PI_2);
 804   else
 805     v1.Rotate(-M_PI_2);
 806   if (cw2)
 807     v2.Rotate(-M_PI_2);
 808   else
 809     v2.Rotate(+M_PI_2);
 810
 811   // Make a "check" point for choosing an offset circle.
 812   v1.Normalize();
 813   v2.Normalize();
 814   gp_Vec2d bisec = 0.5 * (v1 + v2);
 815   if (bisec.SquareMagnitude() < gp::Resolution())
 816     return Standard_False;
 817
 818   const gp_Pnt2d checkp = p12.Translated(radius * bisec);
 819   const gp_Lin2d checkl(p12, bisec);
 820
 821   // Make two circles of radius r1 +/- r and r2 +/- r
 822   // with center point equal to pc1 and pc2.
 823   // Arc 1.
 824   gp_Ax2d axes(pc1, gp::DX2d());
 825   gp_Circ2d c1(axes, radius1 + radius);
 826   if (radius1 > radius && c1.Distance(checkp) > radius)
 827     c1.SetRadius(radius1 - radius);
 828   // Arc 2.
 829   axes.SetLocation(pc2);
 830   gp_Circ2d c2(axes, radius2 + radius);
 831   if (radius2 > radius && c2.Distance(checkp) > radius)
 832     c2.SetRadius(radius2 - radius);
 833
 834   // Calculate an intersection point of these two circles
 835   // and choose the one closer to the "check" point.
 836   IntAna2d_AnaIntersection intersector(c1, c2);
 837   if (!intersector.IsDone() || !intersector.NbPoints())
 838     return Standard_False;
 839
 840   // Find center point of fillet.
 841   gp_Pnt2d pc;
 842   Standard_Real minDist = DBL_MAX;
 843   for (int i = 1; i <= intersector.NbPoints(); ++i)
 844   {
 845     const IntAna2d_IntPoint& intp = intersector.Point(i);
 846     const gp_Pnt2d& p = intp.Value();
 847
 848     Standard_Real d = checkp.SquareDistance(p);
 849     if (d < minDist)
 850     {
 851       minDist = d;
 852       pc = p;
 853     }
 854   }
 855   pc.Coord(xc, yc);
 856
 857   // Orientation of fillet.
 858   Standard_Real angle = v1.Angle(v2);
 859   if (fabs(angle) < Precision::Angular())
 860   {
 861     angle = gp_Vec2d(pc, pc1).Angle(gp_Vec2d(pc, pc2));
 862     cw = angle < 0.0;
 863   }
 864   else
 865   {
 866     cw = angle > 0.0;
 867   }
 868
 869   // Shrinking of circles.
 870   start = fabs(gp_Vec2d(pc1, p12).Angle(gp_Vec2d(pc1, pc)));
 871   end = fabs(gp_Vec2d(pc2, p12).Angle(gp_Vec2d(pc2, pc)));
 872   return Standard_True;
 873 }
 874
 875 // Cuts intersecting edges of a contour.
 876 Standard_Boolean ChFi2d_AnaFilletAlgo::Cut(const gp_Pln& thePlane, TopoDS_Edge& theE1, TopoDS_Edge& theE2)
 877 {
 878   gp_Pnt p;
 879   Standard_Boolean found(Standard_False);
 880   Standard_Real param1 = 0.0, param2 = 0.0;
 881   Standard_Real f1, l1, f2, l2;
 882   Handle(Geom_Curve) c1 = BRep_Tool::Curve(theE1, f1, l1);
 883   Handle(Geom_Curve) c2 = BRep_Tool::Curve(theE2, f2, l2);
 884   GeomAPI_ExtremaCurveCurve extrema(c1, c2, f1, l1, f2, l2);
 885   if (extrema.NbExtrema())
 886   {
 887     Standard_Integer i, nb = extrema.NbExtrema();
 888     for (i = 1; i <= nb; ++i)
 889     {
 890       const Standard_Real d = extrema.Distance(i);
 891       if (d < Precision::Confusion())
 892       {
 893         extrema.Parameters(i, param1, param2);
 894         if (fabs(l1 - param1) > Precision::Confusion() &&
 895             fabs(f2 - param2) > Precision::Confusion())
 896         {
 897           found = Standard_True;
 898           extrema.Points(i, p, p);
 899           break;
 900         }
 901       }
 902     }
 903   }
 904
 905   if (found)
 906   {
 907     BRepBuilderAPI_MakeEdge mkEdge1(c1, f1, param1);
 908     if (mkEdge1.IsDone())
 909     {
 910       theE1 = mkEdge1.Edge();
 911
 912       BRepBuilderAPI_MakeEdge mkEdge2(c2, param2, l2);
 913       if (mkEdge2.IsDone())
 914       {
 915         theE2 = mkEdge2.Edge();
 916
 917         gp_Pnt2d p2d = ProjLib::Project(thePlane, p);
 918         p2d.Coord(x12, y12);
 919         x21 = x12;
 920         y21 = y12;
 921         return Standard_True;
 922       }
 923     }
 924   }
 925   return Standard_False;
 926 }