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