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       shrinke1.Nullify();
 402       if (segment1)
 403       {
 404         BRepBuilderAPI_MakeEdge mkSegment1;
 405         if (e1.Orientation() == TopAbs_FORWARD)
 406           mkSegment1.Init(AC1.Curve().Curve(), AC1.FirstParameter(), AC1.LastParameter() - start);
 407         else
 408           mkSegment1.Init(AC1.Curve().Curve(), AC1.FirstParameter() + start, AC1.LastParameter());
 409         if (mkSegment1.IsDone())
 410           shrinke1 = mkSegment1.Edge();
 411       }
 412       else
 413       {
 414         BRepBuilderAPI_MakeEdge mkCirc1;
 415         if (e1.Orientation() == TopAbs_FORWARD)
 416           mkCirc1.Init(AC1.Curve().Curve(), AC1.FirstParameter(), AC1.LastParameter() - start);
 417         else
 418           mkCirc1.Init(AC1.Curve().Curve(), AC1.FirstParameter() + start, AC1.LastParameter());
 419         if (mkCirc1.IsDone())
 420           shrinke1 = mkCirc1.Edge();
 421       }
 422
 423       // Right neighbour.
 424       shrinke2.Nullify();
 425       if (segment2)
 426       {
 427         BRepBuilderAPI_MakeEdge mkSegment2;
 428         if (e2.Orientation() == TopAbs_FORWARD)
 429           mkSegment2.Init(AC2.Curve().Curve(), AC2.FirstParameter() + end, AC2.LastParameter());
 430         else
 431           mkSegment2.Init(AC2.Curve().Curve(), AC2.FirstParameter(), AC2.LastParameter() - end);
 432         if (mkSegment2.IsDone())
 433           shrinke2 = mkSegment2.Edge();
 434       }
 435       else
 436       {
 437         BRepBuilderAPI_MakeEdge mkCirc2;
 438         if (e2.Orientation() == TopAbs_FORWARD)
 439           mkCirc2.Init(AC2.Curve().Curve(), AC2.FirstParameter() + end, AC2.LastParameter());
 440         else
 441           mkCirc2.Init(AC2.Curve().Curve(), AC2.FirstParameter(), AC2.LastParameter() - end);
 442         if (mkCirc2.IsDone())
 443           shrinke2 = mkCirc2.Edge();
 444       }
 445
 446       bRet = !shrinke1.IsNull() && !shrinke2.IsNull();
 447     }// fillet edge is done
 448   }// shrinking is good
 449
 450   return bRet;
 451 }
 452
 453 // Retrieves a result (fillet and shrinked neighbours).
 454 const TopoDS_Edge& ChFi2d_AnaFilletAlgo::Result(TopoDS_Edge& theE1, TopoDS_Edge& theE2)
 455 {
 456   theE1 = shrinke1;
 457   theE2 = shrinke2;
 458   return fillet;
 459 }
 460
 461 // WW5 method to compute fillet.
 462 // It returns a constructed fillet definition:
 463 //     center point (xc, yc)
 464 //     point on the 1st segment (xstart, ystart)
 465 //     point on the 2nd segment (xend, yend)
 466 //     is the arc of fillet clockwise (cw = true) or counterclockwise (cw = false).
 467 Standard_Boolean ChFi2d_AnaFilletAlgo::SegmentFilletSegment(const Standard_Real radius,
 468                                                             Standard_Real& xc, Standard_Real& yc,
 469                                                             Standard_Boolean& cw,
 470                                                             Standard_Real& start, Standard_Real& end)
 471 {
 472   // Make normalized vectors at p12.
 473   gp_Pnt2d p11(x11, y11);
 474   gp_Pnt2d p12(x12, y12);
 475   gp_Pnt2d p22(x22, y22);
 476
 477   // Check length of segments.
 478   if (IsEqual(p12, p11) || IsEqual(p12, p22))
 479   {
 480     return Standard_False;
 481   }
 482
 483   // Make vectors.
 484   gp_Vec2d v1(p12, p11);
 485   gp_Vec2d v2(p12, p22);
 486   v1.Normalize();
 487   v2.Normalize();
 488
 489   // Make bisectrissa.
 490   gp_Vec2d bisec = 0.5 * (v1 + v2);
 491
 492   // Check bisectrissa.
 493   if (bisec.SquareMagnitude() < Precision::SquareConfusion())
 494     return Standard_False;
 495
 496   // Normalize the bisectrissa.
 497   bisec.Normalize();
 498
 499   // Angle at bisectrissa.
 500   Standard_Real beta = v1.Angle(bisec);
 501
 502   // Length along the bisectrissa till the center of fillet.
 503   Standard_Real L = radius / sin(fabs(beta));
 504
 505   // Center point of fillet.
 506   gp_Pnt2d pc = p12.Translated(L * bisec);
 507   pc.Coord(xc, yc);
 508
 509   // Shrinking length along segments.
 510   start = sqrt(L * L - radius * radius);
 511   end = start;
 512
 513   // Orientation of fillet.
 514   cw = beta > 0.0;
 515   return Standard_True;
 516 }
 517
 518 // A function constructs a fillet between a segment and an arc.
 519 Standard_Boolean ChFi2d_AnaFilletAlgo::SegmentFilletArc(const Standard_Real radius,
 520                                                         Standard_Real& xc, Standard_Real& yc,
 521                                                         Standard_Boolean& cw,
 522                                                         Standard_Real& start, Standard_Real& end,
 523                                                         Standard_Real& xend, Standard_Real& yend)
 524 {
 525   // Make a line parallel to the segment at the side of center point of fillet.
 526   // This side may be defined through making a bisectrissa for vectors at p12 (or p21).
 527
 528   // Make 2D points.
 529   gp_Pnt2d p12(x12, y12);
 530   gp_Pnt2d p11(x11, y11);
 531   gp_Pnt2d pc2(xc2, yc2);
 532
 533   // Check length of segment.
 534   if (p11.SquareDistance(p12) < gp::Resolution())
 535     return Standard_False;
 536
 537   // Make 2D vectors.
 538   gp_Vec2d v1(p12, p11);
 539   gp_Vec2d v2(p12, pc2);
 540
 541   // Rotate the arc vector to become tangential at p21.
 542   if (cw2)
 543     v2.Rotate(+M_PI_2);
 544   else
 545     v2.Rotate(-M_PI_2);
 546
 547   // If vectors coincide (segment and arc are tangent),
 548   // the algorithm doesn't work...
 549   Standard_Real angle = v1.Angle(v2);
 550   if (fabs(angle) < Precision::Angular())
 551     return Standard_False;
 552
 553   // Make a bissectrisa of vectors at p12.
 554   v2.Normalize();
 555   v1.Normalize();
 556   gp_Vec2d bisec = 0.5 * (v1 + v2);
 557
 558   // If segment and arc look in opposite direction,
 559   // no fillet is possible.
 560   if (bisec.SquareMagnitude() < gp::Resolution())
 561     return Standard_False;
 562
 563   // Define an appropriate point to choose center of fillet.
 564   bisec.Normalize();
 565   gp_Pnt2d nearp = p12.Translated(radius * bisec);
 566   gp_Lin2d nearl(p12, bisec);
 567
 568   // Make a line parallel to segment and
 569   // passing near the "near" point.
 570   gp_Vec2d d1(v1);
 571   gp_Lin2d line(p11, -d1);
 572   d1.Rotate(M_PI_2);
 573   line.Translate(radius * d1);
 574   if (line.Distance(nearp) > radius)
 575     line.Translate(-2.0 * radius * d1);
 576
 577   // Make a circle of radius of the arc +/- fillet radius.
 578   gp_Ax2d axes(pc2, gp::DX2d());
 579   gp_Circ2d circ(axes, radius2 + radius);
 580   if (radius2 > radius && circ.Distance(nearp) > radius)
 581     circ.SetRadius(radius2 - radius);
 582
 583   // Calculate intersection of the line and the circle.
 584   IntAna2d_AnaIntersection intersector(line, circ);
 585   if (!intersector.IsDone() || !intersector.NbPoints())
 586     return Standard_False;
 587
 588   // Find center point of fillet.
 589   Standard_Integer i;
 590   Standard_Real minDist = DBL_MAX;
 591   for (i = 1; i <= intersector.NbPoints(); ++i)
 592   {
 593     const IntAna2d_IntPoint& intp = intersector.Point(i);
 594     const gp_Pnt2d& p = intp.Value();
 595
 596     Standard_Real d = nearl.Distance(p);
 597     if (d < minDist)
 598     {
 599       minDist = d;
 600       p.Coord(xc, yc);
 601     }
 602   }
 603
 604   // Shrink of segment.
 605   gp_Pnt2d pc(xc, yc);
 606   Standard_Real L2 = pc.SquareDistance(p12);
 607   const Standard_Real Rf2 = radius * radius;
 608   start = sqrt(L2 - Rf2);
 609
 610   // Shrink of arc.
 611   gp_Vec2d pcc(pc2, pc);
 612   end = fabs(gp_Vec2d(pc2, p12).Angle(pcc));
 613
 614   // Duplicate the information on shrink the arc:
 615   // calculate a point on the arc coinciding with the end of fillet.
 616   line.SetLocation(pc2);
 617   line.SetDirection(pcc);
 618   circ.SetLocation(pc2);
 619   circ.SetRadius(radius2);
 620   intersector.Perform(line, circ);
 621   if (!intersector.IsDone() || !intersector.NbPoints())
 622     return Standard_False;
 623
 624   xend = DBL_MAX;
 625   yend = DBL_MAX;
 626   for (i = 1; i <= intersector.NbPoints(); ++i)
 627   {
 628     const IntAna2d_IntPoint& intp = intersector.Point(i);
 629     const gp_Pnt2d& p = intp.Value();
 630
 631     const Standard_Real d2 = p.SquareDistance(pc);
 632     if (fabs(d2 - Rf2) < Precision::Confusion())
 633     {
 634       p.Coord(xend, yend);
 635       break;
 636     }
 637   }
 638
 639   // Orientation of the fillet.
 640   angle = v1.Angle(v2);
 641   cw = angle > 0.0;
 642   return Standard_True;
 643 }
 644
 645 // A function constructs a fillet between an arc and a segment.
 646 Standard_Boolean ChFi2d_AnaFilletAlgo::ArcFilletSegment(const Standard_Real radius,
 647                                                         Standard_Real& xc, Standard_Real& yc,
 648                                                         Standard_Boolean& cw,
 649                                                         Standard_Real& start, Standard_Real& end,
 650                                                         Standard_Real& xstart, Standard_Real& ystart)
 651 {
 652   // Make a line parallel to the segment at the side of center point of fillet.
 653   // This side may be defined through making a bisectrissa for vectors at p12 (or p21).
 654
 655   // Make 2D points.
 656   gp_Pnt2d p12(x12, y12);
 657   gp_Pnt2d p22(x22, y22);
 658   gp_Pnt2d pc1(xc1, yc1);
 659
 660   // Check length of segment.
 661   if (p12.SquareDistance(p22) < gp::Resolution())
 662     return Standard_False;
 663
 664   // Make 2D vectors.
 665   gp_Vec2d v1(p12, pc1);
 666   gp_Vec2d v2(p12, p22);
 667
 668   // Rotate the arc vector to become tangential at p21.
 669   if (cw1)
 670     v1.Rotate(-M_PI_2);
 671   else
 672     v1.Rotate(+M_PI_2);
 673
 674   // If vectors coincide (segment and arc are tangent),
 675   // the algorithm doesn't work...
 676   Standard_Real angle = v1.Angle(v2);
 677   if (fabs(angle) < Precision::Angular())
 678     return Standard_False;
 679
 680   // Make a bisectrissa of vectors at p12.
 681   v1.Normalize();
 682   v2.Normalize();
 683   gp_Vec2d bisec = 0.5 * (v1 + v2);
 684
 685   // If segment and arc look in opposite direction,
 686   // no fillet is possible.
 687   if (bisec.SquareMagnitude() < gp::Resolution())
 688     return Standard_False;
 689
 690   // Define an appropriate point to choose center of fillet.
 691   bisec.Normalize();
 692   gp_Pnt2d nearPoint = p12.Translated(radius * bisec);
 693   gp_Lin2d nearLine(p12, bisec);
 694
 695   // Make a line parallel to segment and
 696   // passing near the "near" point.
 697   gp_Vec2d d2(v2);
 698   gp_Lin2d line(p22, -d2);
 699   d2.Rotate(M_PI_2);
 700   line.Translate(radius * d2);
 701   if (line.Distance(nearPoint) > radius)
 702     line.Translate(-2.0 * radius * d2);
 703
 704   // Make a circle of radius of the arc +/- fillet radius.
 705   gp_Ax2d axes(pc1, gp::DX2d());
 706   gp_Circ2d circ(axes, radius1 + radius);
 707   if (radius1 > radius && circ.Distance(nearPoint) > radius)
 708     circ.SetRadius(radius1 - radius);
 709
 710   // Calculate intersection of the line and the big circle.
 711   IntAna2d_AnaIntersection intersector(line, circ);
 712   if (!intersector.IsDone() || !intersector.NbPoints())
 713     return Standard_False;
 714
 715   // Find center point of fillet.
 716   Standard_Integer i;
 717   Standard_Real minDist = DBL_MAX;
 718   for (i = 1; i <= intersector.NbPoints(); ++i)
 719   {
 720     const IntAna2d_IntPoint& intp = intersector.Point(i);
 721     const gp_Pnt2d& p = intp.Value();
 722
 723     Standard_Real d = nearLine.Distance(p);
 724     if (d < minDist)
 725     {
 726       minDist = d;
 727       p.Coord(xc, yc);
 728     }
 729   }
 730
 731   // Shrink of segment.
 732   gp_Pnt2d pc(xc, yc);
 733   Standard_Real L2 = pc.SquareDistance(p12);
 734   const Standard_Real Rf2 = radius * radius;
 735   end = sqrt(L2 - Rf2);
 736
 737   // Shrink of arc.
 738   gp_Vec2d pcc(pc1, pc);
 739   start = fabs(gp_Vec2d(pc1, p12).Angle(pcc));
 740
 741   // Duplicate the information on shrink the arc:
 742   // calculate a point on the arc coinciding with the start of fillet.
 743   line.SetLocation(pc1);
 744   line.SetDirection(pcc);
 745   circ.SetLocation(pc1);
 746   circ.SetRadius(radius1);
 747   intersector.Perform(line, circ);
 748   if (!intersector.IsDone() || !intersector.NbPoints())
 749     return Standard_False;
 750
 751   xstart = DBL_MAX;
 752   ystart = DBL_MAX;
 753   for (i = 1; i <= intersector.NbPoints(); ++i)
 754   {
 755     const IntAna2d_IntPoint& intp = intersector.Point(i);
 756     const gp_Pnt2d& p = intp.Value();
 757
 758     const Standard_Real d2 = p.SquareDistance(pc);
 759     if (fabs(d2 - Rf2) < Precision::SquareConfusion())
 760     {
 761       p.Coord(xstart, ystart);
 762       break;
 763     }
 764   }
 765
 766   // Orientation of the fillet.
 767   angle = v2.Angle(v1);
 768   cw = angle < 0.0;
 769   return Standard_True;
 770 }
 771
 772 // WW5 method to compute fillet: arc - arc.
 773 // It returns a constructed fillet definition:
 774 //     center point (xc, yc)
 775 //     shrinking parameter of the 1st circle (start)
 776 //     shrinking parameter of the 2nd circle (end)
 777 //     if the arc of fillet clockwise (cw = true) or counterclockwise (cw = false).
 778 Standard_Boolean ChFi2d_AnaFilletAlgo::ArcFilletArc(const Standard_Real radius,
 779                                                     Standard_Real& xc, Standard_Real& yc,
 780                                                     Standard_Boolean& cw,
 781                                                     Standard_Real& start, Standard_Real& end)
 782 {
 783   // Make points.
 784   const gp_Pnt2d pc1(xc1, yc1);
 785   const gp_Pnt2d pc2(xc2, yc2);
 786   const gp_Pnt2d p12(x12, y12);
 787
 788   // Make vectors at p12.
 789   gp_Vec2d v1(pc1, p12);
 790   gp_Vec2d v2(pc2, p12);
 791
 792   // Rotate the vectors so that they are tangent to circles at p12.
 793   if (cw1)
 794     v1.Rotate(+M_PI_2);
 795   else
 796     v1.Rotate(-M_PI_2);
 797   if (cw2)
 798     v2.Rotate(-M_PI_2);
 799   else
 800     v2.Rotate(+M_PI_2);
 801
 802   // Make a "check" point for choosing an offset circle.
 803   v1.Normalize();
 804   v2.Normalize();
 805   gp_Vec2d bisec = 0.5 * (v1 + v2);
 806   if (bisec.SquareMagnitude() < gp::Resolution())
 807     return Standard_False;
 808
 809   const gp_Pnt2d checkp = p12.Translated(radius * bisec);
 810   const gp_Lin2d checkl(p12, bisec);
 811
 812   // Make two circles of radius r1 +/- r and r2 +/- r
 813   // with center point equal to pc1 and pc2.
 814   // Arc 1.
 815   gp_Ax2d axes(pc1, gp::DX2d());
 816   gp_Circ2d c1(axes, radius1 + radius);
 817   if (radius1 > radius && c1.Distance(checkp) > radius)
 818     c1.SetRadius(radius1 - radius);
 819   // Arc 2.
 820   axes.SetLocation(pc2);
 821   gp_Circ2d c2(axes, radius2 + radius);
 822   if (radius2 > radius && c2.Distance(checkp) > radius)
 823     c2.SetRadius(radius2 - radius);
 824
 825   // Calculate an intersection point of these two circles
 826   // and choose the one closer to the "check" point.
 827   IntAna2d_AnaIntersection intersector(c1, c2);
 828   if (!intersector.IsDone() || !intersector.NbPoints())
 829     return Standard_False;
 830
 831   // Find center point of fillet.
 832   gp_Pnt2d pc;
 833   Standard_Real minDist = DBL_MAX;
 834   for (int i = 1; i <= intersector.NbPoints(); ++i)
 835   {
 836     const IntAna2d_IntPoint& intp = intersector.Point(i);
 837     const gp_Pnt2d& p = intp.Value();
 838
 839     Standard_Real d = checkp.SquareDistance(p);
 840     if (d < minDist)
 841     {
 842       minDist = d;
 843       pc = p;
 844     }
 845   }
 846   pc.Coord(xc, yc);
 847
 848   // Orientation of fillet.
 849   Standard_Real angle = v1.Angle(v2);
 850   if (fabs(angle) < Precision::Angular())
 851   {
 852     angle = gp_Vec2d(pc, pc1).Angle(gp_Vec2d(pc, pc2));
 853     cw = angle < 0.0;
 854   }
 855   else
 856   {
 857     cw = angle > 0.0;
 858   }
 859
 860   // Shrinking of circles.
 861   start = fabs(gp_Vec2d(pc1, p12).Angle(gp_Vec2d(pc1, pc)));
 862   end = fabs(gp_Vec2d(pc2, p12).Angle(gp_Vec2d(pc2, pc)));
 863   return Standard_True;
 864 }
 865
 866 // Cuts intersecting edges of a contour.
 867 Standard_Boolean ChFi2d_AnaFilletAlgo::Cut(const gp_Pln& thePlane, TopoDS_Edge& theE1, TopoDS_Edge& theE2)
 868 {
 869   gp_Pnt p;
 870   Standard_Boolean found(Standard_False);
 871   Standard_Real param1 = 0.0, param2 = 0.0;
 872   Standard_Real f1, l1, f2, l2;
 873   Handle(Geom_Curve) c1 = BRep_Tool::Curve(theE1, f1, l1);
 874   Handle(Geom_Curve) c2 = BRep_Tool::Curve(theE2, f2, l2);
 875   GeomAPI_ExtremaCurveCurve extrema(c1, c2, f1, l1, f2, l2);
 876   if (extrema.NbExtrema())
 877   {
 878     Standard_Integer i, nb = extrema.NbExtrema();
 879     for (i = 1; i <= nb; ++i)
 880     {
 881       const Standard_Real d = extrema.Distance(i);
 882       if (d < Precision::Confusion())
 883       {
 884         extrema.Parameters(i, param1, param2);
 885         if (fabs(l1 - param1) > Precision::Confusion() &&
 886             fabs(f2 - param2) > Precision::Confusion())
 887         {
 888           found = Standard_True;
 889           extrema.Points(i, p, p);
 890           break;
 891         }
 892       }
 893     }
 894   }
 895
 896   if (found)
 897   {
 898     BRepBuilderAPI_MakeEdge mkEdge1(c1, f1, param1);
 899     if (mkEdge1.IsDone())
 900     {
 901       theE1 = mkEdge1.Edge();
 902
 903       BRepBuilderAPI_MakeEdge mkEdge2(c2, param2, l2);
 904       if (mkEdge2.IsDone())
 905       {
 906         theE2 = mkEdge2.Edge();
 907
 908         gp_Pnt2d p2d = ProjLib::Project(thePlane, p);
 909         p2d.Coord(x12, y12);
 910         x21 = x12;
 911         y21 = y12;
 912         return Standard_True;
 913       }
 914     }
 915   }
 916   return Standard_False;
 917 }