1 // Copyright (C) 2007-2012 CEA/DEN, EDF R&D, OPEN CASCADE
3 // This library is free software; you can redistribute it and/or
4 // modify it under the terms of the GNU Lesser General Public
5 // License as published by the Free Software Foundation; either
6 // version 2.1 of the License.
8 // This library is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 // Lesser General Public License for more details.
13 // You should have received a copy of the GNU Lesser General Public
14 // License along with this library; if not, write to the Free Software
15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
20 // File : GEOMImpl_Fillet1d.cxx
23 #include "GEOMImpl_Fillet1d.hxx"
25 #include <BRep_Tool.hxx>
26 #include <BRepAdaptor_Curve.hxx>
27 #include <BRepBuilderAPI_MakeEdge.hxx>
30 #include <gp_Circ.hxx>
31 #include <Geom2d_Line.hxx>
32 #include <Geom2dAPI_ProjectPointOnCurve.hxx>
33 #include <Geom2dAPI_InterCurveCurve.hxx>
34 #include <GeomAPI_ProjectPointOnCurve.hxx>
35 #include <GeomProjLib.hxx>
36 #include <Geom_Circle.hxx>
37 #include <Precision.hxx>
38 #include <TColStd_ListIteratorOfListOfReal.hxx>
41 * class GEOMImpl_Fillet1d
44 //=======================================================================
45 //function : Constructor
47 //=======================================================================
48 GEOMImpl_Fillet1d::GEOMImpl_Fillet1d(const TopoDS_Edge& theEdge1,
49 const TopoDS_Edge& theEdge2,
50 const gp_Pln& thePlane)
51 : myEdgesExchnged( Standard_False )
53 myPlane = new Geom_Plane(thePlane);
55 BRepAdaptor_Curve aBAC1(theEdge1);
56 BRepAdaptor_Curve aBAC2(theEdge2);
57 if (aBAC1.GetType() < aBAC2.GetType())
58 { // first curve must be more complicated
61 myEdgesExchnged = Standard_True;
69 Handle(Geom_Curve) aCurve1 = BRep_Tool::Curve(myEdge1, myStart1, myEnd1);
70 Handle(Geom_Curve) aCurve2 = BRep_Tool::Curve(myEdge2, myStart2, myEnd2);
72 myCurve1 = GeomProjLib::Curve2d(aCurve1, myStart1, myEnd1, myPlane);
73 myCurve2 = GeomProjLib::Curve2d(aCurve2, myStart2, myEnd2, myPlane);
75 while (myCurve1->IsPeriodic() && myStart1 >= myEnd1)
76 myEnd1 += myCurve1->Period();
77 while (myCurve2->IsPeriodic() && myStart2 >= myEnd2)
78 myEnd2 += myCurve2->Period();
80 if (aBAC1.GetType() == aBAC2.GetType())
82 if (myEnd2 - myStart2 < myEnd1 - myStart1)
83 { // first curve must be parametrically shorter
84 TopoDS_Edge anEdge = myEdge1;
87 Handle(Geom2d_Curve) aCurve = myCurve1;
90 Standard_Real a = myStart1;
96 myEdgesExchnged = Standard_True;
101 //=======================================================================
102 //function : isRadiusIntersected
103 //purpose : local function
104 //=======================================================================
105 static Standard_Boolean isRadiusIntersected(const Handle(Geom2d_Curve)& theCurve,
106 const gp_Pnt2d theStart,
107 const gp_Pnt2d theEnd,
108 const Standard_Boolean theStartConnected)
110 const Standard_Real aTol = Precision::Confusion();
111 const Standard_Real anAngTol = Precision::Angular();
112 Geom2dAPI_InterCurveCurve anInter(theCurve, new Geom2d_Line(theStart,
113 gp_Dir2d(gp_Vec2d(theStart, theEnd))), aTol);
116 for(a = anInter.NbPoints(); a > 0; a--)
118 aPoint = anInter.Point(a);
119 if ( aPoint.Distance(theStart) < aTol && !theStartConnected )
120 return Standard_True;
121 if (aPoint.Distance(theEnd) < aTol * 200)
122 return Standard_True;
123 if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol))
124 return Standard_True;
126 Handle(Geom2d_Curve) aCurve;
127 for(a = anInter.NbSegments(); a > 0; a--)
129 anInter.Segment(a, aCurve);
130 aPoint = aCurve->Value(aCurve->FirstParameter());
131 if (aPoint.Distance(theStart) < aTol)
132 if (!theStartConnected)
133 return Standard_True;
134 if (aPoint.Distance(theEnd) < aTol)
135 return Standard_True;
136 if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol))
137 return Standard_True;
138 aPoint = aCurve->Value(aCurve->LastParameter());
139 if (aPoint.Distance(theStart) < aTol)
140 if (!theStartConnected)
141 return Standard_True;
142 if (aPoint.Distance(theEnd) < aTol)
143 return Standard_True;
144 if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol))
145 return Standard_True;
147 return Standard_False;
151 //=======================================================================
152 //function : fillPoint
154 //=======================================================================
155 void GEOMImpl_Fillet1d::fillPoint(GEOMImpl_Fillet1dPoint* thePoint)
159 const Standard_Real aTol = Precision::Confusion();
160 myCurve1->D1(thePoint->GetParam(), aPoint, aVec);
161 if (aVec.SquareMagnitude() < aTol)
164 gp_Vec2d aPerp(((myStartSide)?-1:1) * aVec.Y(), ((myStartSide)?1:-1) * aVec.X());
166 aPerp.Multiply(myRadius);
167 gp_Pnt2d aCenter = aPoint.Translated(aPerp);
168 thePoint->SetCenter(aCenter);
170 // on the intersection point
171 Standard_Boolean aValid = Standard_True;
172 Geom2dAPI_ProjectPointOnCurve aProjInt(aPoint, myCurve2);
173 if (aProjInt.NbPoints() && aPoint.Distance(aProjInt.NearestPoint()) < aTol)
174 aValid = Standard_False;
176 aValid = !isRadiusIntersected(myCurve2, aPoint, aCenter, Standard_True);
178 Geom2dAPI_ProjectPointOnCurve aProj(aCenter, myCurve2);
179 Standard_Integer a, aNB = aProj.NbPoints();
180 for(a = aNB; a > 0; a--)
182 if (aPoint.Distance(aProj.Point(a)) < aTol)
185 Standard_Boolean aValid2 = aValid;
187 aValid2 = !isRadiusIntersected(myCurve1, aCenter, aProj.Point(a), Standard_False);
189 // checking the right parameter
190 Standard_Real aParam = aProj.Parameter(a);
191 while(myCurve2->IsPeriodic() && aParam < myStart2)
192 aParam += myCurve2->Period();
194 thePoint->AddValue(aProj.Distance(a) * aProj.Distance(a) - myRadius * myRadius,
195 (aParam >= myStart2 && aParam <= myEnd2 && aValid2));
196 if (fabs(fabs(aProj.Distance(a)) - myRadius) < aTol)
197 thePoint->SetParam2(aParam);
201 //=======================================================================
202 //function : fillDiff
204 //=======================================================================
205 void GEOMImpl_Fillet1d::fillDiff(GEOMImpl_Fillet1dPoint* thePoint, Standard_Real theDiffStep, Standard_Boolean theFront)
207 GEOMImpl_Fillet1dPoint* aDiff =
208 new GEOMImpl_Fillet1dPoint(thePoint->GetParam() + (theFront?(theDiffStep):(-theDiffStep)));
210 if (!thePoint->ComputeDifference(aDiff))
212 aDiff->SetParam(thePoint->GetParam() + (theFront?(-theDiffStep):(theDiffStep)));
214 thePoint->ComputeDifference(aDiff);
219 //=======================================================================
222 //=======================================================================
223 Standard_Boolean GEOMImpl_Fillet1d::Perform(const Standard_Real theRadius)
225 myDegreeOfRecursion = 0;
226 myResultParams.Clear();
227 myResultOrientation.Clear();
229 Standard_Real aNBSteps = 100;
230 Geom2dAdaptor_Curve aGAC(myCurve1);
231 switch (aGAC.GetType())
239 case GeomAbs_Ellipse:
242 case GeomAbs_BezierCurve:
243 case GeomAbs_BSplineCurve:
244 aNBSteps = 2 + aGAC.Degree() * aGAC.NbPoles();
246 default: // unknown: maximum
250 myRadius = theRadius;
251 Standard_Real aParam, aStep, aDStep;
252 aStep = (myEnd1 - myStart1) / aNBSteps;
253 aDStep = aStep/1000.;
255 Standard_Integer aCycle;
256 for(aCycle = 2, myStartSide = Standard_False; aCycle; myStartSide = !myStartSide, aCycle--)
258 GEOMImpl_Fillet1dPoint *aLeft = NULL, *aRight = NULL;
260 for(aParam = myStart1 + aStep; aParam < myEnd1 || fabs(myEnd1 - aParam) < Precision::Confusion(); aParam += aStep)
264 aLeft = new GEOMImpl_Fillet1dPoint(aParam - aStep);
266 fillDiff(aLeft, aDStep, Standard_True);
269 aRight = new GEOMImpl_Fillet1dPoint(aParam);
271 fillDiff(aRight, aDStep, Standard_False);
273 aLeft->FilterPoints(aRight);
274 performNewton(aLeft, aRight);
282 if (myResultParams.Extent())
283 return Standard_True;
285 return Standard_False;
288 //=======================================================================
289 //function : processPoint
291 //=======================================================================
292 Standard_Boolean GEOMImpl_Fillet1d::processPoint(GEOMImpl_Fillet1dPoint* theLeft,
293 GEOMImpl_Fillet1dPoint* theRight,
294 Standard_Real theParameter)
296 if (theParameter >= theLeft->GetParam() && theParameter < theRight->GetParam())
298 Standard_Real aDX = theRight->GetParam() - theLeft->GetParam();
299 if (theParameter - theLeft->GetParam() < aDX / 100.)
301 theParameter = theLeft->GetParam() + aDX / 100.;
303 if (theRight->GetParam() - theParameter < aDX / 100.)
305 theParameter = theRight->GetParam() - aDX / 100.;
308 // Protection on infinite loop.
309 myDegreeOfRecursion++;
310 Standard_Real diffx = 0.001 * aDX;
311 if (myDegreeOfRecursion > 1000)
314 if (myDegreeOfRecursion > 10000)
317 if (myDegreeOfRecursion > 100000)
319 return Standard_True;
324 GEOMImpl_Fillet1dPoint* aPoint1 = theLeft->Copy();
325 GEOMImpl_Fillet1dPoint* aPoint2 = new GEOMImpl_Fillet1dPoint(theParameter);
327 fillDiff(aPoint2, diffx, Standard_True);
329 aPoint1->FilterPoints(aPoint2);
330 performNewton(aPoint1, aPoint2);
331 aPoint2->FilterPoints(theRight);
332 performNewton(aPoint2, theRight);
336 return Standard_True;
339 return Standard_False;
342 //=======================================================================
343 //function : performNewton
345 //=======================================================================
346 void GEOMImpl_Fillet1d::performNewton(GEOMImpl_Fillet1dPoint* theLeft,
347 GEOMImpl_Fillet1dPoint* theRight)
350 // check the left: if this is solution store it and remove it from the list of researching points of theLeft
351 a = theLeft->HasSolution(myRadius);
354 if (theLeft->IsValid(a))
356 myResultParams.Append(theLeft->GetParam());
357 myResultOrientation.Append(myStartSide);
362 Standard_Real aDX = theRight->GetParam() - theLeft->GetParam();
363 if ( aDX < Precision::Confusion() / 1000000.)
365 a = theRight->HasSolution(myRadius);
367 if (theRight->IsValid(a))
369 myResultParams.Append(theRight->GetParam());
370 myResultOrientation.Append(myStartSide);
375 for(a = 1; a <= theLeft->GetNBValues(); a++)
377 Standard_Integer aNear = theLeft->GetNear(a);
379 Standard_Real aA = (theRight->GetDiff(aNear) - theLeft->GetDiff(a)) / aDX;
380 Standard_Real aB = theLeft->GetDiff(a) - aA * theLeft->GetParam();
381 Standard_Real aC = theLeft->GetValue(a) - theLeft->GetDiff(a) * theLeft->GetParam() +
382 aA * theLeft->GetParam() * theLeft->GetParam() / 2.0;
383 Standard_Real aDet = aB * aB - 2.0 * aA * aC;
385 if ( fabs(aDet) < gp::Resolution() )
388 if (fabs(aA) < Precision::Confusion())
390 if (fabs(aB) > 10e-20)
392 Standard_Real aX0 = - aC / aB; // use extremum
393 if (aX0 > theLeft->GetParam() && aX0 < theRight->GetParam())
394 processPoint(theLeft, theRight, aX0);
398 processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
403 if (fabs(aB) > fabs(aDet * 1000000.))
404 { // possible floating point operations accurancy errors
405 processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
412 Standard_Boolean aRes = processPoint(theLeft, theRight, (- aB + aDet) / aA);
414 aRes = processPoint(theLeft, theRight, (- aB - aDet) / aA);
416 processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
420 Standard_Real aX0 = - aB / aA; // use extremum
421 if (aX0 > theLeft->GetParam() && aX0 < theRight->GetParam())
422 processPoint(theLeft, theRight, aX0);
424 processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
431 //=======================================================================
434 //=======================================================================
435 TopoDS_Edge GEOMImpl_Fillet1d::Result(const gp_Pnt& thePoint,
436 TopoDS_Edge& theEdge1,
437 TopoDS_Edge& theEdge2)
440 gp_Pnt2d aTargetPoint2d;
441 Standard_Real aX, aY;
442 ElSLib::PlaneParameters(myPlane->Pln().Position(), thePoint, aX, aY);
443 aTargetPoint2d.SetCoord(aX, aY);
445 // choose the nearest circle
446 Standard_Real aDistance, aP;
447 GEOMImpl_Fillet1dPoint *aNearest;
449 TColStd_ListIteratorOfListOfReal anIter(myResultParams);
450 for(aNearest = NULL, a = 1; anIter.More(); anIter.Next(), a++)
452 myStartSide = (myResultOrientation.Value(a)) ? Standard_True : Standard_False;
453 GEOMImpl_Fillet1dPoint *aPoint = new GEOMImpl_Fillet1dPoint(anIter.Value());
455 if (!aPoint->HasSolution(myRadius))
457 aP = fabs(aPoint->GetCenter().Distance(aTargetPoint2d) - myRadius);
458 if (!aNearest || aP < aDistance)
472 // create circle edge
473 gp_Pnt aCenter = ElSLib::PlaneValue(aNearest->GetCenter().X(),
474 aNearest->GetCenter().Y(),
475 myPlane->Pln().Position());
476 Handle(Geom_Circle) aCircle =
477 new Geom_Circle(gp_Ax2(aCenter, myPlane->Pln().Axis().Direction()), myRadius);
478 gp_Pnt2d aPoint2d1, aPoint2d2;
479 myCurve1->D0(aNearest->GetParam(), aPoint2d1);
480 myCurve2->D0(aNearest->GetParam2(), aPoint2d2);
481 gp_Pnt aPoint1 = ElSLib::PlaneValue(aPoint2d1.X(), aPoint2d1.Y(), myPlane->Pln().Position());
482 gp_Pnt aPoint2 = ElSLib::PlaneValue(aPoint2d2.X(), aPoint2d2.Y(), myPlane->Pln().Position());
484 GeomAPI_ProjectPointOnCurve aProj(thePoint, aCircle);
485 Standard_Real aTarGetParam = aProj.LowerDistanceParameter();
486 gp_Pnt aPointOnCircle = aProj.NearestPoint();
488 // Check extrema point manually, because there is a bug in Open CASCADE
489 // in calculation of nearest point to a circle near the parameter 0.0
490 gp_Pnt p0 = ElCLib::Value(0.0, aCircle->Circ());
491 if (p0.Distance(thePoint) < aPointOnCircle.Distance(thePoint))
497 aProj.Perform(aPoint1);
498 Standard_Real aParam1 = aProj.LowerDistanceParameter();
499 aProj.Perform(aPoint2);
500 Standard_Real aParam2 = aProj.LowerDistanceParameter();
501 Standard_Boolean aIsOut = ((aParam1 < aTarGetParam && aParam2 < aTarGetParam) ||
502 (aParam1 > aTarGetParam && aParam2 > aTarGetParam));
503 if (aParam1 > aParam2)
505 BRepBuilderAPI_MakeEdge aBuilder(aCircle->Circ(),
506 aIsOut ? aParam2 : aParam1,
507 aIsOut? aParam1 : aParam2);
508 aResult = aBuilder.Edge();
511 Standard_Real aStart, anEnd;
512 Handle(Geom_Curve) aCurve = BRep_Tool::Curve(myEdge1, aStart, anEnd);
514 aCurve->D1(aNearest->GetParam(), aPoint1, aDir);
517 aCircle->D1(aParam1, aPoint1, aCircleDir);
518 if ((aCircleDir.Angle(aDir) > M_PI / 2.0) ^ aIsOut)
519 aStart = aNearest->GetParam();
521 anEnd = aNearest->GetParam();
523 if (fabs(aStart - anEnd) > Precision::Confusion())
526 BRepBuilderAPI_MakeEdge aDivider1(aCurve, aStart, anEnd);
528 theEdge2 = aDivider1.Edge();
530 theEdge1 = aDivider1.Edge();
533 aCurve = BRep_Tool::Curve(myEdge2, aStart, anEnd);
534 aCurve->D1(aNearest->GetParam2(), aPoint2, aDir);
536 aCircle->D1(aParam2, aPoint2, aCircleDir);
537 if ((aCircleDir.Angle(aDir) > M_PI / 2.0) ^ (!aIsOut))
538 aStart = aNearest->GetParam2();
540 anEnd = aNearest->GetParam2();
542 if (fabs(aStart - anEnd) > Precision::Confusion())
544 BRepBuilderAPI_MakeEdge aDivider2(aCurve, aStart, anEnd);
546 theEdge1 = aDivider2.Edge();
548 theEdge2 = aDivider2.Edge();
555 //=======================================================================
556 //function : AddValue
558 //=======================================================================
559 void GEOMImpl_Fillet1dPoint::AddValue(Standard_Real theValue, Standard_Boolean theValid)
562 for(a = 1; a <= myV.Length(); a++)
564 if (theValue < myV.Value(a))
566 myV.InsertBefore(a, theValue);
567 myValid.InsertBefore(a, (Standard_Integer)theValid);
571 myV.Append(theValue);
572 myValid.Append((Standard_Integer)theValid);
575 //=======================================================================
576 //function : ComputeDifference
578 //=======================================================================
579 Standard_Boolean GEOMImpl_Fillet1dPoint::ComputeDifference(GEOMImpl_Fillet1dPoint* thePoint)
582 Standard_Boolean aDiffsSet = (myD.Length() != 0);
583 Standard_Real aDX = thePoint->GetParam() - myParam, aDY;
584 if (thePoint->myV.Length() == myV.Length())
585 { // absolutely the same points
586 for(a = 1; a <= myV.Length(); a++)
588 aDY = thePoint->myV.Value(a) - myV.Value(a);
590 myD.SetValue(a, fabs(aDX) > gp::Resolution() ? (aDY/aDX) : 0);
592 myD.Append( fabs(aDX) > gp::Resolution() ? (aDY/aDX) : 0);
594 return Standard_True;
596 // between the diffeerent points searching for nearest analogs
598 for(a = 1; a <= myV.Length(); a++)
600 for(b = 1; b <= thePoint->myV.Length(); b++)
602 if (b == 1 || fabs(thePoint->myV.Value(b) - myV.Value(a)) < fabs(aDY))
603 aDY = thePoint->myV.Value(b) - myV.Value(a);
607 if ( fabs(aDX) > gp::Resolution() && fabs(aDY / aDX) < fabs(myD.Value(a)))
608 myD.SetValue(a, aDY / aDX);
614 myD.Append( fabs(aDX) > gp::Resolution() ? aDY/aDX : 0);
618 return Standard_False;
621 //=======================================================================
622 //function : FilterPoints
624 //=======================================================================
625 void GEOMImpl_Fillet1dPoint::FilterPoints(GEOMImpl_Fillet1dPoint* thePoint)
627 Standard_Integer a, b;
628 TColStd_SequenceOfReal aDiffs;
629 Standard_Real aY, aY2, aDX = thePoint->GetParam() - myParam;
630 for(a = 1; a <= myV.Length(); a++)
632 // searching for near point from thePoint
633 Standard_Integer aNear = 0;
634 Standard_Real aDiff = aDX * 10000.;
635 aY = myV.Value(a) + myD.Value(a) * aDX;
636 for(b = 1; b <= thePoint->myV.Length(); b++)
638 // calculate hypothesis value of the Y2 with the constant first and second derivative
639 aY2 = aY + aDX * (thePoint->myD.Value(b) - myD.Value(a)) / 2.0;
640 if (aNear == 0 || fabs(aY2 - thePoint->myV.Value(b)) < fabs(aDiff))
643 aDiff = aY2 - thePoint->myV.Value(b);
649 if (myV.Value(a) * thePoint->myV.Value(aNear) > 0)
650 {// the same sign at the same sides of the interval
651 if (myV.Value(a) * myD.Value(a) > 0)
653 if (fabs(myD.Value(a)) > Precision::Confusion())
658 if (fabs(myV.Value(a)) > fabs(thePoint->myV.Value(aNear)))
659 if (thePoint->myV.Value(aNear) * thePoint->myD.Value(aNear) < 0 &&
660 fabs(thePoint->myD.Value(aNear)) > Precision::Confusion())
670 if (myV.Value(a) * thePoint->myV.Value(aNear) > 0)
672 if ((myV.Value(a) + myD.Value(a) * aDX) * myV.Value(a) > Precision::Confusion() &&
673 (thePoint->myV.Value(aNear) + thePoint->myD.Value(aNear) * aDX) * thePoint->myV.Value(aNear) > Precision::Confusion())
682 if ( fabs(aDX) < gp::Resolution() || fabs(aDiff / aDX) > 1.e+7)
689 { // there is no near: remove it from the list
697 Standard_Boolean aFound = Standard_False;
698 for(b = 1; b <= myNear.Length(); b++)
700 if (myNear.Value(b) == aNear)
702 if (fabs(aDiffs.Value(b)) < fabs(aDiff))
703 { // return this 'near'
704 aFound = Standard_True;
712 { // remove the old 'near'
725 myNear.Append(aNear);
726 aDiffs.Append(aDiff);
732 //=======================================================================
735 //=======================================================================
736 GEOMImpl_Fillet1dPoint* GEOMImpl_Fillet1dPoint::Copy()
738 GEOMImpl_Fillet1dPoint* aCopy = new GEOMImpl_Fillet1dPoint(myParam);
740 for(a = 1; a <= myV.Length(); a++)
742 aCopy->myV.Append(myV.Value(a));
743 aCopy->myD.Append(myD.Value(a));
744 aCopy->myValid.Append(myValid.Value(a));
749 //=======================================================================
750 //function : HasSolution
752 //=======================================================================
753 Standard_Integer GEOMImpl_Fillet1dPoint::HasSolution(const Standard_Real theRadius)
756 for(a = 1; a <= myV.Length(); a++)
758 if (fabs(sqrt(fabs(fabs(myV.Value(a)) + theRadius * theRadius)) - theRadius) < Precision::Confusion() / 10.)
764 //=======================================================================
765 //function : RemoveSolution
767 //=======================================================================
768 void GEOMImpl_Fillet1dPoint::RemoveSolution(Standard_Integer theIndex)
770 myV.Remove(theIndex);
771 myD.Remove(theIndex);
772 myValid.Remove(theIndex);
773 myNear.Remove(theIndex);