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