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