700100d39512472a37dffb735bf60e7c04dd8755
[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       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 }