708fae829569a2ba8b4e4465567a26d5cd083c87
[occt.git] / 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 }