5fe2c3cc449c4e3925cbc804dbf497b84a2978db
[occt.git] / src / ChFi2d / ChFi2d_FilletAlgo.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_FilletAlgo.hxx>
15
16 #include <GeomProjLib.hxx>
17 #include <BRep_Tool.hxx>
18 #include <Precision.hxx>
19 #include <ElSLib.hxx>
20 #include <ElCLib.hxx>
21
22 #include <Geom2dAPI_ProjectPointOnCurve.hxx>
23 #include <GeomAPI_ProjectPointOnCurve.hxx>
24 #include <Geom2dAPI_InterCurveCurve.hxx>
25
26 #include <TopoDS.hxx>
27 #include <TopoDS_Iterator.hxx>
28 #include <TColStd_ListIteratorOfListOfReal.hxx>
29
30 #include <gp_Circ.hxx>
31 #include <Geom_Circle.hxx>
32 #include <Geom2d_Line.hxx>
33
34 #include <BRepBuilderAPI_MakeEdge.hxx>
35 #include <BRepAdaptor_Curve.hxx>
36
37 ChFi2d_FilletAlgo::ChFi2d_FilletAlgo()
38 {
39
40 }
41
42 ChFi2d_FilletAlgo::ChFi2d_FilletAlgo(const TopoDS_Wire& theWire, const gp_Pln& thePlane) 
43 {
44   Init(theWire, thePlane);
45 }
46
47 ChFi2d_FilletAlgo::ChFi2d_FilletAlgo(const TopoDS_Edge& theEdge1, 
48                                      const TopoDS_Edge& theEdge2, 
49                                      const gp_Pln& thePlane) 
50 {
51   Init(theEdge1, theEdge2, thePlane);
52 }
53
54 void ChFi2d_FilletAlgo::Init(const TopoDS_Wire& theWire, const gp_Pln& thePlane) 
55 {
56   TopoDS_Edge theEdge1, theEdge2;
57   TopoDS_Iterator itr(theWire);
58   for (; itr.More(); itr.Next())
59   {
60     if (theEdge1.IsNull())
61       theEdge1 = TopoDS::Edge(itr.Value());
62     else if (theEdge2.IsNull())
63       theEdge2 = TopoDS::Edge(itr.Value());
64     else
65       break;
66   }
67   if (theEdge1.IsNull() || theEdge2.IsNull())
68     Standard_ConstructionError::Raise("The fillet algorithms expects a wire consisting of two edges.");
69   Init(theEdge1, theEdge2, thePlane);
70 }
71
72 void ChFi2d_FilletAlgo::Init(const TopoDS_Edge& theEdge1, 
73                              const TopoDS_Edge& theEdge2, 
74                              const gp_Pln& thePlane)
75 {
76   myPlane = new Geom_Plane(thePlane);
77
78   myEdgesExchnged = Standard_False;
79
80   BRepAdaptor_Curve aBAC1(theEdge1);
81   BRepAdaptor_Curve aBAC2(theEdge2);
82   if (aBAC1.GetType() < aBAC2.GetType()) 
83   { // first curve must be more complicated
84     myEdge1 = theEdge2;
85     myEdge2 = theEdge1;
86     myEdgesExchnged = Standard_True;
87   }      
88   else 
89   {
90     myEdge1 = theEdge1;
91     myEdge2 = theEdge2;
92   }
93
94   Handle(Geom_Curve) aCurve1 = BRep_Tool::Curve(myEdge1, myStart1, myEnd1);
95   Handle(Geom_Curve) aCurve2 = BRep_Tool::Curve(myEdge2, myStart2, myEnd2);
96
97   myCurve1 = GeomProjLib::Curve2d(aCurve1, myStart1, myEnd1, myPlane);
98   myCurve2 = GeomProjLib::Curve2d(aCurve2, myStart2, myEnd2, myPlane);
99
100   while (myCurve1->IsPeriodic() && myStart1 >= myEnd1)
101     myEnd1 += myCurve1->Period();
102   while (myCurve2->IsPeriodic() && myStart2 >= myEnd2)
103     myEnd2 += myCurve2->Period();
104  
105   if (aBAC1.GetType() == aBAC2.GetType()) 
106   {
107     if (myEnd2 - myStart2 < myEnd1 - myStart1) 
108     { // first curve must be parametrically shorter
109       TopoDS_Edge anEdge = myEdge1;
110       myEdge1 = myEdge2;
111       myEdge2 = anEdge;
112       Handle(Geom2d_Curve) aCurve = myCurve1;
113       myCurve1 = myCurve2;
114       myCurve2 = aCurve;
115       Standard_Real a = myStart1;
116       myStart1 = myStart2;
117       myStart2 = a;
118       a = myEnd1;
119       myEnd1 = myEnd2;
120       myEnd2 = a;
121       myEdgesExchnged = Standard_True;
122     }
123   }
124 }
125
126 //! This function returns true if linear segment from start point of the 
127 //! fillet arc to the end point is intersected by the first or second 
128 //! curve: in this case fillet is invalid.
129 static Standard_Boolean IsRadiusIntersected(const Handle(Geom2d_Curve)& theCurve, const Standard_Real theCurveMin, const double theCurveMax,
130                                             const gp_Pnt2d theStart, const gp_Pnt2d theEnd, const Standard_Boolean theStartConnected) 
131 {
132   //Check the given start and end if they are identical. If yes
133   //return false
134   if (theStart.SquareDistance(theEnd) < Precision::SquareConfusion())
135   {
136     return Standard_False;
137   }
138   Handle(Geom2d_Line) line = new Geom2d_Line(theStart, gp_Dir2d(gp_Vec2d(theStart, theEnd)));
139   Geom2dAPI_InterCurveCurve anInter(theCurve, line, Precision::Confusion());
140   Standard_Integer a;
141   gp_Pnt2d aPoint;
142   for(a = anInter.NbPoints(); a > 0; a--) 
143   {
144     aPoint = anInter.Point(a);
145     Geom2dAPI_ProjectPointOnCurve aProjInt(aPoint, theCurve, theCurveMin, theCurveMax);
146     if (aProjInt.NbPoints() < 1 || aProjInt.LowerDistanceParameter() > Precision::Confusion()) 
147       continue; // point is not on edge
148
149     if (aPoint.Distance(theStart) < Precision::Confusion())
150     {
151       if (!theStartConnected) 
152         return Standard_True;
153     }
154     if (aPoint.Distance(theEnd) < Precision::Confusion()) 
155       return Standard_True;
156     if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), Precision::Angular())) 
157       return Standard_True;
158   }
159   Handle(Geom2d_Curve) aCurve = theCurve;
160   for(a = anInter.NbSegments(); a > 0; a--) 
161   {
162     //anInter.Segment(a, aCurve); //not implemented (bug in OCC)
163     aPoint = aCurve->Value(aCurve->FirstParameter());
164
165     Geom2dAPI_ProjectPointOnCurve aProjInt(aPoint, theCurve, theCurveMin, theCurveMax);
166     if (aProjInt.NbPoints() && aProjInt.LowerDistanceParameter() < Precision::Confusion()) 
167     { // point is on edge
168       if (aPoint.Distance(theStart) < Precision::Confusion()) 
169         if (!theStartConnected) 
170           return Standard_True;
171       if (aPoint.Distance(theEnd) < Precision::Confusion()) 
172         return Standard_True;
173       if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), Precision::Angular())) 
174         return Standard_True;
175     }
176     aPoint = aCurve->Value(aCurve->LastParameter());
177
178     aProjInt.Init(aPoint, theCurve, theCurveMin, theCurveMax);
179     if (aProjInt.NbPoints() && aProjInt.LowerDistanceParameter() < Precision::Confusion()) 
180     { // point is on edge
181       if (aPoint.Distance(theStart) < Precision::Confusion()) 
182         if (!theStartConnected) 
183           return Standard_True;
184       if (aPoint.Distance(theEnd) < Precision::Confusion()) 
185         return Standard_True;
186       if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), Precision::Angular())) 
187         return Standard_True;
188     }
189   }
190   return Standard_False;
191 }
192
193 void ChFi2d_FilletAlgo::FillPoint(FilletPoint* thePoint, const Standard_Real theLimit) 
194 {
195   
196   // on the intersection point
197   Standard_Boolean aValid = Standard_False;
198   Standard_Real aStep = Precision::Confusion();
199   gp_Pnt2d aCenter, aPoint; // center of fillet and point on curve1
200   Standard_Real aParam = thePoint->getParam();
201   if (theLimit < aParam) aStep = -aStep;
202   for(aValid = Standard_False; !aValid; aParam += aStep) 
203   {
204     if ((aParam - aStep - theLimit) * (aParam - theLimit) <= 0) 
205       break; // limit was exceeded
206     aStep *= 2;
207     gp_Vec2d aVec;
208     myCurve1->D1(aParam, aPoint, aVec);
209     if (aVec.SquareMagnitude() < Precision::Confusion()) 
210       continue;
211
212     gp_Vec2d aPerp(((myStartSide)?-1:1) * aVec.Y(), ((myStartSide)?1:-1) * aVec.X());
213     aPerp.Normalize();
214     aPerp.Multiply(myRadius);
215     aCenter = aPoint.Translated(aPerp);
216
217
218     Geom2dAPI_ProjectPointOnCurve aProjInt(aPoint, myCurve2, myStart2, myEnd2);
219     if (aProjInt.NbPoints() == 0 || aPoint.Distance(aProjInt.NearestPoint()) > Precision::Confusion()) 
220     {
221       aValid = Standard_True;
222       break;
223     }
224   }
225   if (aValid) 
226   {
227     thePoint->setParam(aParam);
228     thePoint->setCenter(aCenter);
229     aValid = !IsRadiusIntersected(myCurve2, myStart2, myEnd2, aPoint, aCenter, Standard_True);
230   }
231   
232   Geom2dAPI_ProjectPointOnCurve aProj(aCenter, myCurve2);
233   int a, aNB = aProj.NbPoints();
234   for(a = aNB; a > 0; a--) 
235   {
236     if (aPoint.SquareDistance(aProj.Point(a)) < Precision::Confusion()) 
237       continue;
238         
239     Standard_Boolean aValid2 = aValid;
240     if (aValid2) 
241       aValid2 = !IsRadiusIntersected(myCurve1, myStart1, myEnd1, aCenter, aProj.Point(a), Standard_False);
242
243     // checking the right parameter
244     Standard_Real aParam = aProj.Parameter(a);
245     while(myCurve2->IsPeriodic() && aParam < myStart2)
246       aParam += myCurve2->Period();
247
248     const Standard_Real d = aProj.Distance(a);
249     thePoint->appendValue(d * d - myRadius * myRadius, (aParam >= myStart2 && aParam <= myEnd2 && aValid2));
250     if (Abs(d - myRadius) < Precision::Confusion())
251       thePoint->setParam2(aParam);
252   }
253 }
254
255 void ChFi2d_FilletAlgo::FillDiff(FilletPoint* thePoint, Standard_Real theDiffStep, Standard_Boolean theFront) 
256 {
257   Standard_Real aDelta = theFront?(theDiffStep):(-theDiffStep);
258   FilletPoint* aDiff = new FilletPoint(thePoint->getParam() + aDelta);
259   FillPoint(aDiff, aDelta * 999.);
260   if (!thePoint->calculateDiff(aDiff)) 
261   {
262     aDiff->setParam(thePoint->getParam() - aDelta);
263     FillPoint(aDiff,  - aDelta * 999);
264     thePoint->calculateDiff(aDiff);
265   }
266   delete aDiff;
267 }
268
269 // returns true, if at least one result was found
270 Standard_Boolean ChFi2d_FilletAlgo::Perform(const Standard_Real theRadius) 
271 {
272   myDegreeOfRecursion = 0;
273   myResultParams.Clear();
274   myResultOrientation.Clear();
275
276   Standard_Real aNBSteps;
277   Geom2dAdaptor_Curve aGAC(myCurve1);
278   switch (aGAC.GetType()) 
279   {
280   case GeomAbs_Line:
281     aNBSteps = 2;
282     break;
283   case GeomAbs_Circle:
284     aNBSteps = 4;
285     break;
286   case GeomAbs_Ellipse:
287     aNBSteps = 5;
288     break;
289   case GeomAbs_BSplineCurve:
290     aNBSteps = 2 + aGAC.Degree() * aGAC.NbPoles();
291     break;
292   default: // unknown: maximum
293     aNBSteps = 100;
294   }
295   //cout<<"aNBSteps = "<<aNBSteps<<endl;
296
297   myRadius = theRadius;
298   Standard_Real aParam, aStep, aDStep;
299   aStep = (myEnd1 - myStart1) / aNBSteps;
300   aDStep = 1.e-4 * aStep;
301
302   int aCycle;
303   for(aCycle = 2, myStartSide = Standard_False; aCycle; myStartSide = !myStartSide, aCycle--) 
304   {
305     FilletPoint *aLeft = NULL, *aRight;
306     
307     for(aParam = myStart1 + aStep; aParam < myEnd1 || Abs(myEnd1 - aParam) < Precision::Confusion(); aParam += aStep) 
308     {
309       if (!aLeft) 
310       {
311         aLeft = new FilletPoint(aParam - aStep);
312         FillPoint(aLeft, aParam);
313         FillDiff(aLeft, aDStep, Standard_True);
314       }
315       
316       aRight = new FilletPoint(aParam);
317       FillPoint(aRight, aParam - aStep);
318       FillDiff(aRight, aDStep, Standard_False);
319       
320       aLeft->FilterPoints(aRight);
321       PerformNewton(aLeft, aRight);
322       
323       delete aLeft;
324       aLeft = aRight;
325     }//for
326     delete aLeft;
327   }//for
328
329   return !myResultParams.IsEmpty();
330 }
331
332 Standard_Boolean ChFi2d_FilletAlgo::ProcessPoint(FilletPoint* theLeft, FilletPoint* theRight, Standard_Real theParameter) 
333 {
334   if (theParameter >= theLeft->getParam() && theParameter < theRight->getParam()) 
335   {
336     Standard_Real aDX = (theRight->getParam() - theLeft->getParam());
337     if (theParameter - theLeft->getParam() < aDX/100.0) 
338     {
339       theParameter = theLeft->getParam() + aDX/100.0;
340     }
341     if (theRight->getParam() - theParameter < aDX/100.0) 
342     {
343       theParameter = theRight->getParam() - aDX/100.0;
344     }
345
346     // Protection on infinite loops.
347     myDegreeOfRecursion++;
348     Standard_Real diffx = 0.001 * aDX;
349     if (myDegreeOfRecursion > 100)
350     {
351       diffx *= 10.0;
352       if (myDegreeOfRecursion > 1000)
353       {
354         diffx *= 10.0;
355         if (myDegreeOfRecursion > 3000)
356         {
357           return Standard_True;
358         }
359       }
360     }
361
362     FilletPoint* aPoint1 = theLeft->Copy();
363     FilletPoint* aPoint2 = new FilletPoint(theParameter);
364     FillPoint(aPoint2, aPoint1->getParam());
365     FillDiff(aPoint2, diffx, Standard_True);
366     
367     aPoint1->FilterPoints(aPoint2);
368     PerformNewton(aPoint1, aPoint2);
369     aPoint2->FilterPoints(theRight);
370     PerformNewton(aPoint2, theRight);
371
372     delete aPoint1;
373     delete aPoint2;
374     return Standard_True;
375   }
376
377   return Standard_False;
378 }
379
380 void ChFi2d_FilletAlgo::PerformNewton(FilletPoint* theLeft, FilletPoint* theRight) 
381 {
382   int a;
383   // check the left: if this is solution store it and remove it from the list of researching points of theLeft
384   a = theLeft->hasSolution(myRadius);
385   if (a) 
386   {
387     if (theLeft->isValid(a)) 
388     {
389       myResultParams.Append(theLeft->getParam());
390       myResultOrientation.Append(myStartSide);
391     }
392     return;
393   }
394
395   Standard_Real aDX = theRight->getParam() - theLeft->getParam();
396   if (aDX < 1.e-6 * Precision::Confusion())
397   {
398     a = theRight->hasSolution(myRadius);
399     if (a && theRight->isValid(a)) 
400     {
401       myResultParams.Append(theRight->getParam());
402       myResultOrientation.Append(myStartSide);
403     }
404     return;
405   }
406   for(a = 1; a <= theLeft->getNBValues(); a++) 
407   {
408     int aNear = theLeft->getNear(a);
409         
410     Standard_Real aA = (theRight->getDiff(aNear) - theLeft->getDiff(a)) / aDX;
411     Standard_Real aB = theLeft->getDiff(a) - aA * theLeft->getParam();
412     Standard_Real aC = theLeft->getValue(a) - theLeft->getDiff(a) * theLeft->getParam() + aA * theLeft->getParam() * theLeft->getParam() / 2.0;
413     Standard_Real aDet = aB * aB - 2.0 * aA * aC;
414     
415     if (Abs(aA) < Precision::Confusion()) 
416     { // linear case
417       //cout<<"###"<<endl;
418       if (Abs(aB) > 10e-20) 
419       {
420         Standard_Real aX0 = - aC / aB; // use extremum
421         if (aX0 > theLeft->getParam() && aX0 < theRight->getParam())
422           ProcessPoint(theLeft, theRight, aX0);
423       }
424       else 
425       {
426         ProcessPoint(theLeft, theRight, theLeft->getParam() + aDX / 2.0); // linear division otherwise
427       }
428     } 
429     else
430     {
431       if (Abs(aB) > Abs(aDet * 1000000.)) 
432       { // possible floating point operations accurancy errors
433         //cout<<"*";
434         ProcessPoint(theLeft, theRight, theLeft->getParam() + aDX / 2.0); // linear division otherwise
435       } 
436       else
437       {
438         if (aDet > 0) 
439         { // two solutions
440           aDet = sqrt(aDet);
441           Standard_Boolean aRes = ProcessPoint(theLeft, theRight, (- aB + aDet) / aA);
442           if (!aRes) 
443             aRes = ProcessPoint(theLeft, theRight, (- aB - aDet) / aA);
444           if (!aRes) 
445             ProcessPoint(theLeft, theRight, theLeft->getParam() + aDX / 2.0); // linear division otherwise
446         } 
447         else 
448         {
449           //cout<<"%%%"<<endl;
450           Standard_Real aX0 = - aB / aA; // use extremum
451           if (aX0 > theLeft->getParam() && aX0 < theRight->getParam())
452             ProcessPoint(theLeft, theRight, aX0);
453           else 
454             ProcessPoint(theLeft, theRight, theLeft->getParam() + aDX / 2.0); // linear division otherwise
455         }
456       }
457     }
458   }//for
459 }
460
461 // returns number of possible solutions.
462 int ChFi2d_FilletAlgo::NbResults(const gp_Pnt& thePoint)
463 {
464   Standard_Real aX, aY;
465   gp_Pnt2d aTargetPoint2d;
466   ElSLib::PlaneParameters(myPlane->Pln().Position(), thePoint, aX, aY);
467   aTargetPoint2d.SetCoord(aX, aY);
468    
469   //iterate through all possible solutions.
470   int i = 1, nb = 0;
471   TColStd_ListIteratorOfListOfReal anIter(myResultParams);
472   for(; anIter.More(); anIter.Next(), i++) 
473   {
474     myStartSide = (myResultOrientation.Value(i)) ? Standard_True : Standard_False;
475     FilletPoint *aPoint = new FilletPoint(anIter.Value());
476     FillPoint(aPoint, anIter.Value() + 1.);
477     if (aPoint->hasSolution(myRadius)) 
478       nb++;
479     delete aPoint;
480   }//for
481
482   return nb;
483 }
484
485 // returns result (fillet edge, modified edge1, modified edge2), neares to the given point <thePoint>
486 TopoDS_Edge ChFi2d_FilletAlgo::Result(const gp_Pnt& thePoint, TopoDS_Edge& theEdge1, TopoDS_Edge& theEdge2, const int iSolution) 
487 {
488   TopoDS_Edge aResult;
489   gp_Pnt2d aTargetPoint2d;
490   Standard_Real aX, aY;
491   ElSLib::PlaneParameters(myPlane->Pln().Position(), thePoint, aX, aY);
492   aTargetPoint2d.SetCoord(aX, aY);
493    
494   // choose the nearest circle
495   Standard_Real aDistance = 0.0, aP;
496   FilletPoint *aNearest;
497   int a, iSol = 1;
498   TColStd_ListIteratorOfListOfReal anIter(myResultParams);
499   for(aNearest = NULL, a = 1; anIter.More(); anIter.Next(), a++) 
500   {
501     myStartSide = (myResultOrientation.Value(a))?Standard_True:Standard_False;
502     FilletPoint *aPoint = new FilletPoint(anIter.Value());
503     FillPoint(aPoint, anIter.Value() + 1.);
504     if (!aPoint->hasSolution(myRadius))
505     {
506       delete aPoint;
507       continue;
508     }
509     aP = DBL_MAX;
510     if (iSolution == -1)
511     {
512       aP = Abs(aPoint->getCenter().Distance(aTargetPoint2d) - myRadius);
513     }
514     else if (iSolution == iSol)
515     {
516       aP = 0.0;
517     }
518     if (!aNearest || aP < aDistance) 
519     {
520       aNearest = aPoint;
521       aDistance = aP;
522     } 
523     else 
524     {
525       delete aPoint;
526     }
527     if (iSolution == iSol)
528       break;
529     iSol++;
530   }//for
531    
532   if (!aNearest) 
533     return aResult;
534    
535   // create circle edge
536   gp_Pnt aCenter = ElSLib::PlaneValue(aNearest->getCenter().X(), aNearest->getCenter().Y(), myPlane->Pln().Position());
537   Handle(Geom_Circle) aCircle = new Geom_Circle(gp_Ax2(aCenter, myPlane->Pln().Axis().Direction()), myRadius);
538   gp_Pnt2d aPoint2d1, aPoint2d2;
539   myCurve1->D0(aNearest->getParam(), aPoint2d1);
540   myCurve2->D0(aNearest->getParam2(), aPoint2d2);
541   gp_Pnt aPoint1 = ElSLib::PlaneValue(aPoint2d1.X(), aPoint2d1.Y(), myPlane->Pln().Position());
542   gp_Pnt aPoint2 = ElSLib::PlaneValue(aPoint2d2.X(), aPoint2d2.Y(), myPlane->Pln().Position());
543
544   GeomAPI_ProjectPointOnCurve aProj(thePoint, aCircle);
545   Standard_Real aTargetParam = aProj.LowerDistanceParameter();
546   gp_Pnt aPointOnCircle = aProj.NearestPoint();
547
548   // There is a bug in Open CASCADE in calculation of nearest point to a circle near the parameter 0.0
549   // Therefore I check this extrema point manually:
550   gp_Pnt p0 = ElCLib::Value(0.0, aCircle->Circ());
551   if (p0.Distance(thePoint) < aPointOnCircle.Distance(thePoint))
552   {
553     aTargetParam = 0.0;
554     aPointOnCircle = p0;
555   }
556
557   aProj.Perform(aPoint1);
558   Standard_Real aParam1 = aProj.LowerDistanceParameter();
559   aProj.Perform(aPoint2);
560   Standard_Real aParam2 = aProj.LowerDistanceParameter();
561   Standard_Boolean aIsOut = ((aParam1 < aTargetParam && aParam2 < aTargetParam) || (aParam1 > aTargetParam && aParam2 > aTargetParam));
562   if (aParam1 > aParam2) 
563     aIsOut = !aIsOut;
564   BRepBuilderAPI_MakeEdge aBuilder(aCircle->Circ(), aIsOut ? aParam2 : aParam1, aIsOut? aParam1 : aParam2);
565   aResult = aBuilder.Edge();
566
567   // divide edges
568   Standard_Real aStart, anEnd;
569   Handle(Geom_Curve) aCurve = BRep_Tool::Curve(myEdge1, aStart, anEnd);
570   gp_Vec aDir;
571   aCurve->D1(aNearest->getParam(), aPoint1, aDir);
572
573   gp_Vec aCircleDir;
574   aCircle->D1(aParam1, aPoint1, aCircleDir);
575     
576   if ((aCircleDir.Angle(aDir) > M_PI / 2.0) ^ aIsOut)
577     aStart = aNearest->getParam();
578   else
579     anEnd = aNearest->getParam();
580
581   //Check the case when start and end are identical. This happens
582   //when the edge decreases to size 0. Old ww5 allows such
583   //cases. So we are again bug compatible
584   if (fabs(aStart - anEnd) < Precision::Confusion())
585     anEnd = aStart + Precision::Confusion();
586   //Divide edge
587   BRepBuilderAPI_MakeEdge aDivider1(aCurve, aStart, anEnd);
588   if (myEdgesExchnged) 
589     theEdge2 = aDivider1.Edge();
590   else 
591     theEdge1 = aDivider1.Edge();
592
593   aCurve = BRep_Tool::Curve(myEdge2, aStart, anEnd);
594   aCurve->D1(aNearest->getParam2(), aPoint2, aDir);
595    
596   aCircle->D1(aParam2, aPoint2, aCircleDir);
597
598   if ((aCircleDir.Angle(aDir) > M_PI / 2.0) ^ (!aIsOut))
599     aStart = aNearest->getParam2();
600   else
601     anEnd = aNearest->getParam2();
602
603   //Check the case when start and end are identical. This happens
604   //when the edge decreases to size 0. Old ww5 allows such
605   //cases. So we are again bug compatible
606   if (fabs(aStart - anEnd) < Precision::Confusion())
607     anEnd = aStart + Precision::Confusion();
608   BRepBuilderAPI_MakeEdge aDivider2(aCurve, aStart, anEnd);
609   if (myEdgesExchnged) 
610     theEdge1 = aDivider2.Edge();
611   else 
612     theEdge2 = aDivider2.Edge();
613
614   delete aNearest;
615   return aResult;
616 }
617
618 void FilletPoint::appendValue(Standard_Real theValue, Standard_Boolean theValid) 
619 {
620   Standard_Integer a;
621   for(a = 1; a <= myV.Length(); a++) 
622   {
623     if (theValue < myV.Value(a)) 
624     {
625       myV.InsertBefore(a, theValue);
626       myValid.InsertBefore(a, (int)theValid);
627       return;
628     }
629   }
630   myV.Append(theValue);
631   myValid.Append((int)theValid);
632 }
633
634 Standard_Boolean FilletPoint::calculateDiff(FilletPoint* thePoint) 
635 {
636   Standard_Integer a;
637   Standard_Boolean aDiffsSet = (myD.Length() != 0);
638   Standard_Real aDX = thePoint->getParam() - myParam, aDY = 0.0;
639   if (thePoint->myV.Length() == myV.Length()) 
640   { // absolutely the same points
641     for(a = 1; a <= myV.Length(); a++) 
642     {
643       aDY = thePoint->myV.Value(a) - myV.Value(a);
644       if (aDiffsSet) 
645         myD.SetValue(a, aDY / aDX);
646       else 
647         myD.Append(aDY / aDX);
648     }
649     return Standard_True;
650   }
651   // between the diffeerent points searching for nearest analogs
652   Standard_Integer b;
653   for(a = 1; a <= myV.Length(); a++) 
654   {
655     for(b = 1; b <= thePoint->myV.Length(); b++) 
656     {
657       if (b == 1 || Abs(thePoint->myV.Value(b) - myV.Value(a)) < Abs(aDY))
658         aDY = thePoint->myV.Value(b) - myV.Value(a);
659     }
660     if (aDiffsSet) 
661     {
662       if (Abs(aDY / aDX) < Abs(myD.Value(a)))
663         myD.SetValue(a, aDY / aDX);
664     } 
665     else 
666     {
667       myD.Append(aDY / aDX);
668     }
669   }//for
670     
671   return Standard_False;
672 }
673
674 void FilletPoint::FilterPoints(FilletPoint* thePoint) 
675 {
676   Standard_Integer a, b;
677   TColStd_SequenceOfReal aDiffs;
678   Standard_Real aY, aY2, aDX = thePoint->getParam() - myParam;
679   for(a = 1; a <= myV.Length(); a++) 
680   {
681     // searching for near point from thePoint
682     Standard_Integer aNear = 0;
683     Standard_Real aDiff = aDX * 10000.;
684     aY = myV.Value(a) + myD.Value(a) * aDX;
685     for(b = 1; b <= thePoint->myV.Length(); b++) 
686     {
687       // calculate hypothesis value of the Y2 with the constant first and second derivative
688       aY2 = aY + aDX * (thePoint->myD.Value(b) - myD.Value(a)) / 2.0;
689       if (aNear == 0 || Abs(aY2 - thePoint->myV.Value(b)) < Abs(aDiff)) 
690       {
691         aNear = b;
692         aDiff = aY2 - thePoint->myV.Value(b);
693       }
694     }//for b...
695
696     if (aNear) 
697     {
698       if (myV.Value(a) * thePoint->myV.Value(aNear) > 0) 
699       {// the same sign at the same sides of the interval
700         if (myV.Value(a) * myD.Value(a) > 0) 
701         {
702           if (Abs(myD.Value(a)) > Precision::Confusion()) 
703             aNear = 0;
704         } 
705         else 
706         {
707           if (Abs(myV.Value(a)) > Abs(thePoint->myV.Value(aNear)))
708             if (thePoint->myV.Value(aNear) * thePoint->myD.Value(aNear) < 0 && Abs(thePoint->myD.Value(aNear)) > Precision::Confusion())
709             {
710               aNear = 0;
711             }
712         }
713       }
714     }//if aNear
715
716     if (aNear) 
717     {
718       if (myV.Value(a) * thePoint->myV.Value(aNear) > 0) 
719       {
720         if ((myV.Value(a) + myD.Value(a) * aDX) * myV.Value(a) > Precision::Confusion() &&
721             (thePoint->myV.Value(aNear) + thePoint->myD.Value(aNear) * aDX) * thePoint->myV.Value(aNear) > Precision::Confusion())
722         {
723           aNear = 0;
724         }
725       }
726     }//if aNear
727     
728     if (aNear)
729     {
730       if (Abs(aDiff / aDX) > 1.e+7) 
731       {
732         aNear = 0;
733       }
734     }
735
736     if (aNear == 0) 
737     {   // there is no near: remove it from the list
738       myV.Remove(a);
739       myD.Remove(a);
740       myValid.Remove(a);
741       a--;
742     } 
743     else 
744     {
745       Standard_Boolean aFound = Standard_False;
746       for(b = 1; b <= myNear.Length(); b++) 
747       {
748         if (myNear.Value(b) == aNear) 
749         {
750           if (Abs(aDiffs.Value(b)) < Abs(aDiff)) 
751           { // return this 'near'
752             aFound = Standard_True;
753             myV.Remove(a);
754             myD.Remove(a);
755             myValid.Remove(a);
756             a--;
757             break;
758           } 
759           else 
760           { // remove the old 'near'
761             myV.Remove(b);
762             myD.Remove(b);
763             myValid.Remove(b);
764             myNear.Remove(b);
765             aDiffs.Remove(b);
766             a--;
767             break;
768           }
769         }
770       }//for b...
771       if (!aFound) 
772       {
773         myNear.Append(aNear);
774         aDiffs.Append(aDiff);
775       }
776     }//else
777   }//for a...
778 }
779
780 FilletPoint* FilletPoint::Copy() 
781 {
782   FilletPoint* aCopy = new FilletPoint(myParam);
783   Standard_Integer a;
784   for(a = 1; a <= myV.Length(); a++) 
785   {
786     aCopy->myV.Append(myV.Value(a));
787     aCopy->myD.Append(myD.Value(a));
788     aCopy->myValid.Append(myValid.Value(a));
789   }
790   return aCopy;
791 }
792
793 int FilletPoint::hasSolution(const Standard_Real theRadius) 
794 {
795   Standard_Integer a;
796   for(a = 1; a <= myV.Length(); a++) 
797   {
798     if (Abs(sqrt(Abs(Abs(myV.Value(a)) + theRadius * theRadius)) - theRadius) < Precision::Confusion()) 
799       return a;
800   }
801   return 0;
802 }
803
804 void FilletPoint::remove(int theIndex) 
805 {
806   myV.Remove(theIndex);
807   myD.Remove(theIndex);
808   myValid.Remove(theIndex);
809   myNear.Remove(theIndex);
810 }