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
19 // File : GEOMImpl_Fillet1d.cxx
22 #include "GEOMImpl_Fillet1d.hxx"
24 #include <BRep_Tool.hxx>
25 #include <BRepAdaptor_Curve.hxx>
26 #include <BRepBuilderAPI_MakeEdge.hxx>
29 #include <gp_Circ.hxx>
30 #include <Geom2d_Line.hxx>
31 #include <Geom2dAPI_ProjectPointOnCurve.hxx>
32 #include <Geom2dAPI_InterCurveCurve.hxx>
33 #include <GeomAPI_ProjectPointOnCurve.hxx>
34 #include <GeomProjLib.hxx>
35 #include <Geom_Circle.hxx>
36 #include <Precision.hxx>
37 #include <TColStd_ListIteratorOfListOfReal.hxx>
40 * class GEOMImpl_Fillet1d
43 //=======================================================================
44 //function : Constructor
46 //=======================================================================
47 GEOMImpl_Fillet1d::GEOMImpl_Fillet1d(const TopoDS_Edge& theEdge1,
48 const TopoDS_Edge& theEdge2,
49 const gp_Pln& thePlane)
50 : myEdgesExchnged( Standard_False )
52 myPlane = new Geom_Plane(thePlane);
54 BRepAdaptor_Curve aBAC1(theEdge1);
55 BRepAdaptor_Curve aBAC2(theEdge2);
56 if (aBAC1.GetType() < aBAC2.GetType())
57 { // first curve must be more complicated
60 myEdgesExchnged = Standard_True;
68 Handle(Geom_Curve) aCurve1 = BRep_Tool::Curve(myEdge1, myStart1, myEnd1);
69 Handle(Geom_Curve) aCurve2 = BRep_Tool::Curve(myEdge2, myStart2, myEnd2);
71 myCurve1 = GeomProjLib::Curve2d(aCurve1, myStart1, myEnd1, myPlane);
72 myCurve2 = GeomProjLib::Curve2d(aCurve2, myStart2, myEnd2, myPlane);
74 while (myCurve1->IsPeriodic() && myStart1 >= myEnd1)
75 myEnd1 += myCurve1->Period();
76 while (myCurve2->IsPeriodic() && myStart2 >= myEnd2)
77 myEnd2 += myCurve2->Period();
79 if (aBAC1.GetType() == aBAC2.GetType())
81 if (myEnd2 - myStart2 < myEnd1 - myStart1)
82 { // first curve must be parametrically shorter
83 TopoDS_Edge anEdge = myEdge1;
86 Handle(Geom2d_Curve) aCurve = myCurve1;
89 Standard_Real a = myStart1;
95 myEdgesExchnged = Standard_True;
100 //=======================================================================
101 //function : isRadiusIntersected
102 //purpose : local function
103 //=======================================================================
104 static Standard_Boolean isRadiusIntersected(const Handle(Geom2d_Curve)& theCurve,
105 const gp_Pnt2d theStart,
106 const gp_Pnt2d theEnd,
107 const Standard_Boolean theStartConnected)
109 const Standard_Real aTol = Precision::Confusion();
110 const Standard_Real anAngTol = Precision::Angular();
111 Geom2dAPI_InterCurveCurve anInter(theCurve, new Geom2d_Line(theStart,
112 gp_Dir2d(gp_Vec2d(theStart, theEnd))), aTol);
115 for(a = anInter.NbPoints(); a > 0; a--)
117 aPoint = anInter.Point(a);
118 if ( aPoint.Distance(theStart) < aTol && !theStartConnected )
119 return Standard_True;
120 if (aPoint.Distance(theEnd) < aTol * 200)
121 return Standard_True;
122 if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol))
123 return Standard_True;
125 Handle(Geom2d_Curve) aCurve;
126 for(a = anInter.NbSegments(); a > 0; a--)
128 anInter.Segment(a, aCurve);
129 aPoint = aCurve->Value(aCurve->FirstParameter());
130 if (aPoint.Distance(theStart) < aTol)
131 if (!theStartConnected)
132 return Standard_True;
133 if (aPoint.Distance(theEnd) < aTol)
134 return Standard_True;
135 if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol))
136 return Standard_True;
137 aPoint = aCurve->Value(aCurve->LastParameter());
138 if (aPoint.Distance(theStart) < aTol)
139 if (!theStartConnected)
140 return Standard_True;
141 if (aPoint.Distance(theEnd) < aTol)
142 return Standard_True;
143 if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol))
144 return Standard_True;
146 return Standard_False;
150 //=======================================================================
151 //function : fillPoint
153 //=======================================================================
154 void GEOMImpl_Fillet1d::fillPoint(GEOMImpl_Fillet1dPoint* thePoint)
158 const Standard_Real aTol = Precision::Confusion();
159 myCurve1->D1(thePoint->GetParam(), aPoint, aVec);
160 if (aVec.SquareMagnitude() < aTol)
163 gp_Vec2d aPerp(((myStartSide)?-1:1) * aVec.Y(), ((myStartSide)?1:-1) * aVec.X());
165 aPerp.Multiply(myRadius);
166 gp_Pnt2d aCenter = aPoint.Translated(aPerp);
167 thePoint->SetCenter(aCenter);
169 // on the intersection point
170 Standard_Boolean aValid = Standard_True;
171 Geom2dAPI_ProjectPointOnCurve aProjInt(aPoint, myCurve2);
172 if (aProjInt.NbPoints() && aPoint.Distance(aProjInt.NearestPoint()) < aTol)
173 aValid = Standard_False;
175 aValid = !isRadiusIntersected(myCurve2, aPoint, aCenter, Standard_True);
177 Geom2dAPI_ProjectPointOnCurve aProj(aCenter, myCurve2);
178 Standard_Integer a, aNB = aProj.NbPoints();
179 for(a = aNB; a > 0; a--)
181 if (aPoint.Distance(aProj.Point(a)) < aTol)
184 Standard_Boolean aValid2 = aValid;
186 aValid2 = !isRadiusIntersected(myCurve1, aCenter, aProj.Point(a), Standard_False);
188 // checking the right parameter
189 Standard_Real aParam = aProj.Parameter(a);
190 while(myCurve2->IsPeriodic() && aParam < myStart2)
191 aParam += myCurve2->Period();
193 thePoint->AddValue(aProj.Distance(a) * aProj.Distance(a) - myRadius * myRadius,
194 (aParam >= myStart2 && aParam <= myEnd2 && aValid2));
195 if (fabs(fabs(aProj.Distance(a)) - myRadius) < aTol)
196 thePoint->SetParam2(aParam);
200 //=======================================================================
201 //function : fillDiff
203 //=======================================================================
204 void GEOMImpl_Fillet1d::fillDiff(GEOMImpl_Fillet1dPoint* thePoint, Standard_Real theDiffStep, Standard_Boolean theFront)
206 GEOMImpl_Fillet1dPoint* aDiff =
207 new GEOMImpl_Fillet1dPoint(thePoint->GetParam() + (theFront?(theDiffStep):(-theDiffStep)));
209 if (!thePoint->ComputeDifference(aDiff))
211 aDiff->SetParam(thePoint->GetParam() + (theFront?(-theDiffStep):(theDiffStep)));
213 thePoint->ComputeDifference(aDiff);
218 //=======================================================================
221 //=======================================================================
222 Standard_Boolean GEOMImpl_Fillet1d::Perform(const Standard_Real theRadius)
224 myDegreeOfRecursion = 0;
225 myResultParams.Clear();
226 myResultOrientation.Clear();
228 Standard_Real aNBSteps = 100;
229 Geom2dAdaptor_Curve aGAC(myCurve1);
230 switch (aGAC.GetType())
238 case GeomAbs_Ellipse:
241 case GeomAbs_BezierCurve:
242 case GeomAbs_BSplineCurve:
243 aNBSteps = 2 + aGAC.Degree() * aGAC.NbPoles();
245 default: // unknown: maximum
249 myRadius = theRadius;
250 Standard_Real aParam, aStep, aDStep;
251 aStep = (myEnd1 - myStart1) / aNBSteps;
252 aDStep = aStep/1000.;
254 Standard_Integer aCycle;
255 for(aCycle = 2, myStartSide = Standard_False; aCycle; myStartSide = !myStartSide, aCycle--)
257 GEOMImpl_Fillet1dPoint *aLeft = NULL, *aRight = NULL;
259 for(aParam = myStart1 + aStep; aParam < myEnd1 || fabs(myEnd1 - aParam) < Precision::Confusion(); aParam += aStep)
263 aLeft = new GEOMImpl_Fillet1dPoint(aParam - aStep);
265 fillDiff(aLeft, aDStep, Standard_True);
268 aRight = new GEOMImpl_Fillet1dPoint(aParam);
270 fillDiff(aRight, aDStep, Standard_False);
272 aLeft->FilterPoints(aRight);
273 performNewton(aLeft, aRight);
281 if (myResultParams.Extent())
282 return Standard_True;
284 return Standard_False;
287 //=======================================================================
288 //function : processPoint
290 //=======================================================================
291 Standard_Boolean GEOMImpl_Fillet1d::processPoint(GEOMImpl_Fillet1dPoint* theLeft,
292 GEOMImpl_Fillet1dPoint* theRight,
293 Standard_Real theParameter)
295 if (theParameter >= theLeft->GetParam() && theParameter < theRight->GetParam())
297 Standard_Real aDX = theRight->GetParam() - theLeft->GetParam();
298 if (theParameter - theLeft->GetParam() < aDX / 100.)
300 theParameter = theLeft->GetParam() + aDX / 100.;
302 if (theRight->GetParam() - theParameter < aDX / 100.)
304 theParameter = theRight->GetParam() - aDX / 100.;
307 // Protection on infinite loop.
308 myDegreeOfRecursion++;
309 Standard_Real diffx = 0.001 * aDX;
310 if (myDegreeOfRecursion > 1000)
313 if (myDegreeOfRecursion > 10000)
316 if (myDegreeOfRecursion > 100000)
318 return Standard_True;
323 GEOMImpl_Fillet1dPoint* aPoint1 = theLeft->Copy();
324 GEOMImpl_Fillet1dPoint* aPoint2 = new GEOMImpl_Fillet1dPoint(theParameter);
326 fillDiff(aPoint2, diffx, Standard_True);
328 aPoint1->FilterPoints(aPoint2);
329 performNewton(aPoint1, aPoint2);
330 aPoint2->FilterPoints(theRight);
331 performNewton(aPoint2, theRight);
335 return Standard_True;
338 return Standard_False;
341 //=======================================================================
342 //function : performNewton
344 //=======================================================================
345 void GEOMImpl_Fillet1d::performNewton(GEOMImpl_Fillet1dPoint* theLeft,
346 GEOMImpl_Fillet1dPoint* theRight)
349 // check the left: if this is solution store it and remove it from the list of researching points of theLeft
350 a = theLeft->HasSolution(myRadius);
353 if (theLeft->IsValid(a))
355 myResultParams.Append(theLeft->GetParam());
356 myResultOrientation.Append(myStartSide);
361 Standard_Real aDX = theRight->GetParam() - theLeft->GetParam();
362 if ( aDX < Precision::Confusion() / 1000000.)
364 a = theRight->HasSolution(myRadius);
366 if (theRight->IsValid(a))
368 myResultParams.Append(theRight->GetParam());
369 myResultOrientation.Append(myStartSide);
374 for(a = 1; a <= theLeft->GetNBValues(); a++)
376 Standard_Integer aNear = theLeft->GetNear(a);
378 Standard_Real aA = (theRight->GetDiff(aNear) - theLeft->GetDiff(a)) / aDX;
379 Standard_Real aB = theLeft->GetDiff(a) - aA * theLeft->GetParam();
380 Standard_Real aC = theLeft->GetValue(a) - theLeft->GetDiff(a) * theLeft->GetParam() +
381 aA * theLeft->GetParam() * theLeft->GetParam() / 2.0;
382 Standard_Real aDet = aB * aB - 2.0 * aA * aC;
384 if ( fabs(aDet) < gp::Resolution() )
387 if (fabs(aA) < Precision::Confusion())
389 if (fabs(aB) > 10e-20)
391 Standard_Real aX0 = - aC / aB; // use extremum
392 if (aX0 > theLeft->GetParam() && aX0 < theRight->GetParam())
393 processPoint(theLeft, theRight, aX0);
397 processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
402 if (fabs(aB) > fabs(aDet * 1000000.))
403 { // possible floating point operations accuracy errors
404 processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
411 Standard_Boolean aRes = processPoint(theLeft, theRight, (- aB + aDet) / aA);
413 aRes = processPoint(theLeft, theRight, (- aB - aDet) / aA);
415 processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
419 Standard_Real aX0 = - aB / aA; // use extremum
420 if (aX0 > theLeft->GetParam() && aX0 < theRight->GetParam())
421 processPoint(theLeft, theRight, aX0);
423 processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
430 //=======================================================================
433 //=======================================================================
434 TopoDS_Edge GEOMImpl_Fillet1d::Result(const gp_Pnt& thePoint,
435 TopoDS_Edge& theEdge1,
436 TopoDS_Edge& theEdge2)
439 gp_Pnt2d aTargetPoint2d;
440 Standard_Real aX, aY;
441 ElSLib::PlaneParameters(myPlane->Pln().Position(), thePoint, aX, aY);
442 aTargetPoint2d.SetCoord(aX, aY);
444 // choose the nearest circle
445 Standard_Real aDistance, aP;
446 GEOMImpl_Fillet1dPoint *aNearest;
448 TColStd_ListIteratorOfListOfReal anIter(myResultParams);
449 for(aNearest = NULL, a = 1; anIter.More(); anIter.Next(), a++)
451 myStartSide = (myResultOrientation.Value(a)) ? Standard_True : Standard_False;
452 GEOMImpl_Fillet1dPoint *aPoint = new GEOMImpl_Fillet1dPoint(anIter.Value());
454 if (!aPoint->HasSolution(myRadius))
456 aP = fabs(aPoint->GetCenter().Distance(aTargetPoint2d) - myRadius);
457 if (!aNearest || aP < aDistance)
471 // create circle edge
472 gp_Pnt aCenter = ElSLib::PlaneValue(aNearest->GetCenter().X(),
473 aNearest->GetCenter().Y(),
474 myPlane->Pln().Position());
475 Handle(Geom_Circle) aCircle =
476 new Geom_Circle(gp_Ax2(aCenter, myPlane->Pln().Axis().Direction()), myRadius);
477 gp_Pnt2d aPoint2d1, aPoint2d2;
478 myCurve1->D0(aNearest->GetParam(), aPoint2d1);
479 myCurve2->D0(aNearest->GetParam2(), aPoint2d2);
480 gp_Pnt aPoint1 = ElSLib::PlaneValue(aPoint2d1.X(), aPoint2d1.Y(), myPlane->Pln().Position());
481 gp_Pnt aPoint2 = ElSLib::PlaneValue(aPoint2d2.X(), aPoint2d2.Y(), myPlane->Pln().Position());
483 GeomAPI_ProjectPointOnCurve aProj(thePoint, aCircle);
484 Standard_Real aTarGetParam = aProj.LowerDistanceParameter();
485 gp_Pnt aPointOnCircle = aProj.NearestPoint();
487 // Check extrema point manually, because there is a bug in Open CASCADE
488 // in calculation of nearest point to a circle near the parameter 0.0
489 gp_Pnt p0 = ElCLib::Value(0.0, aCircle->Circ());
490 if (p0.Distance(thePoint) < aPointOnCircle.Distance(thePoint))
496 aProj.Perform(aPoint1);
497 Standard_Real aParam1 = aProj.LowerDistanceParameter();
498 aProj.Perform(aPoint2);
499 Standard_Real aParam2 = aProj.LowerDistanceParameter();
500 Standard_Boolean aIsOut = ((aParam1 < aTarGetParam && aParam2 < aTarGetParam) ||
501 (aParam1 > aTarGetParam && aParam2 > aTarGetParam));
502 if (aParam1 > aParam2)
504 BRepBuilderAPI_MakeEdge aBuilder(aCircle->Circ(),
505 aIsOut ? aParam2 : aParam1,
506 aIsOut? aParam1 : aParam2);
507 aResult = aBuilder.Edge();
510 Standard_Real aStart, anEnd;
511 Handle(Geom_Curve) aCurve = BRep_Tool::Curve(myEdge1, aStart, anEnd);
513 aCurve->D1(aNearest->GetParam(), aPoint1, aDir);
516 aCircle->D1(aParam1, aPoint1, aCircleDir);
517 if ((aCircleDir.Angle(aDir) > M_PI / 2.0) ^ aIsOut)
518 aStart = aNearest->GetParam();
520 anEnd = aNearest->GetParam();
522 if (fabs(aStart - anEnd) > Precision::Confusion())
525 BRepBuilderAPI_MakeEdge aDivider1(aCurve, aStart, anEnd);
527 theEdge2 = aDivider1.Edge();
529 theEdge1 = aDivider1.Edge();
532 aCurve = BRep_Tool::Curve(myEdge2, aStart, anEnd);
533 aCurve->D1(aNearest->GetParam2(), aPoint2, aDir);
535 aCircle->D1(aParam2, aPoint2, aCircleDir);
536 if ((aCircleDir.Angle(aDir) > M_PI / 2.0) ^ (!aIsOut))
537 aStart = aNearest->GetParam2();
539 anEnd = aNearest->GetParam2();
541 if (fabs(aStart - anEnd) > Precision::Confusion())
543 BRepBuilderAPI_MakeEdge aDivider2(aCurve, aStart, anEnd);
545 theEdge1 = aDivider2.Edge();
547 theEdge2 = aDivider2.Edge();
554 //=======================================================================
555 //function : AddValue
557 //=======================================================================
558 void GEOMImpl_Fillet1dPoint::AddValue(Standard_Real theValue, Standard_Boolean theValid)
561 for(a = 1; a <= myV.Length(); a++)
563 if (theValue < myV.Value(a))
565 myV.InsertBefore(a, theValue);
566 myValid.InsertBefore(a, (Standard_Integer)theValid);
570 myV.Append(theValue);
571 myValid.Append((Standard_Integer)theValid);
574 //=======================================================================
575 //function : ComputeDifference
577 //=======================================================================
578 Standard_Boolean GEOMImpl_Fillet1dPoint::ComputeDifference(GEOMImpl_Fillet1dPoint* thePoint)
581 Standard_Boolean aDiffsSet = (myD.Length() != 0);
582 Standard_Real aDX = thePoint->GetParam() - myParam, aDY;
583 if (thePoint->myV.Length() == myV.Length())
584 { // absolutely the same points
585 for(a = 1; a <= myV.Length(); a++)
587 aDY = thePoint->myV.Value(a) - myV.Value(a);
589 myD.SetValue(a, fabs(aDX) > gp::Resolution() ? (aDY/aDX) : 0);
591 myD.Append( fabs(aDX) > gp::Resolution() ? (aDY/aDX) : 0);
593 return Standard_True;
595 // between the diffeerent points searching for nearest analogs
597 for(a = 1; a <= myV.Length(); a++)
599 for(b = 1; b <= thePoint->myV.Length(); b++)
601 if (b == 1 || fabs(thePoint->myV.Value(b) - myV.Value(a)) < fabs(aDY))
602 aDY = thePoint->myV.Value(b) - myV.Value(a);
606 if ( fabs(aDX) > gp::Resolution() && fabs(aDY / aDX) < fabs(myD.Value(a)))
607 myD.SetValue(a, aDY / aDX);
613 myD.Append( fabs(aDX) > gp::Resolution() ? aDY/aDX : 0);
617 return Standard_False;
620 //=======================================================================
621 //function : FilterPoints
623 //=======================================================================
624 void GEOMImpl_Fillet1dPoint::FilterPoints(GEOMImpl_Fillet1dPoint* thePoint)
626 Standard_Integer a, b;
627 TColStd_SequenceOfReal aDiffs;
628 Standard_Real aY, aY2, aDX = thePoint->GetParam() - myParam;
629 for(a = 1; a <= myV.Length(); a++)
631 // searching for near point from thePoint
632 Standard_Integer aNear = 0;
633 Standard_Real aDiff = aDX * 10000.;
634 aY = myV.Value(a) + myD.Value(a) * aDX;
635 for(b = 1; b <= thePoint->myV.Length(); b++)
637 // calculate hypothesis value of the Y2 with the constant first and second derivative
638 aY2 = aY + aDX * (thePoint->myD.Value(b) - myD.Value(a)) / 2.0;
639 if (aNear == 0 || fabs(aY2 - thePoint->myV.Value(b)) < fabs(aDiff))
642 aDiff = aY2 - thePoint->myV.Value(b);
648 if (myV.Value(a) * thePoint->myV.Value(aNear) > 0)
649 {// the same sign at the same sides of the interval
650 if (myV.Value(a) * myD.Value(a) > 0)
652 if (fabs(myD.Value(a)) > Precision::Confusion())
657 if (fabs(myV.Value(a)) > fabs(thePoint->myV.Value(aNear)))
658 if (thePoint->myV.Value(aNear) * thePoint->myD.Value(aNear) < 0 &&
659 fabs(thePoint->myD.Value(aNear)) > Precision::Confusion())
669 if (myV.Value(a) * thePoint->myV.Value(aNear) > 0)
671 if ((myV.Value(a) + myD.Value(a) * aDX) * myV.Value(a) > Precision::Confusion() &&
672 (thePoint->myV.Value(aNear) + thePoint->myD.Value(aNear) * aDX) * thePoint->myV.Value(aNear) > Precision::Confusion())
681 if ( fabs(aDX) < gp::Resolution() || fabs(aDiff / aDX) > 1.e+7)
688 { // there is no near: remove it from the list
696 Standard_Boolean aFound = Standard_False;
697 for(b = 1; b <= myNear.Length(); b++)
699 if (myNear.Value(b) == aNear)
701 if (fabs(aDiffs.Value(b)) < fabs(aDiff))
702 { // return this 'near'
703 aFound = Standard_True;
711 { // remove the old 'near'
724 myNear.Append(aNear);
725 aDiffs.Append(aDiff);
731 //=======================================================================
734 //=======================================================================
735 GEOMImpl_Fillet1dPoint* GEOMImpl_Fillet1dPoint::Copy()
737 GEOMImpl_Fillet1dPoint* aCopy = new GEOMImpl_Fillet1dPoint(myParam);
739 for(a = 1; a <= myV.Length(); a++)
741 aCopy->myV.Append(myV.Value(a));
742 aCopy->myD.Append(myD.Value(a));
743 aCopy->myValid.Append(myValid.Value(a));
748 //=======================================================================
749 //function : HasSolution
751 //=======================================================================
752 Standard_Integer GEOMImpl_Fillet1dPoint::HasSolution(const Standard_Real theRadius)
755 for(a = 1; a <= myV.Length(); a++)
757 if (fabs(sqrt(fabs(fabs(myV.Value(a)) + theRadius * theRadius)) - theRadius) < Precision::Confusion() / 10.)
763 //=======================================================================
764 //function : RemoveSolution
766 //=======================================================================
767 void GEOMImpl_Fillet1dPoint::RemoveSolution(Standard_Integer theIndex)
769 myV.Remove(theIndex);
770 myD.Remove(theIndex);
771 myValid.Remove(theIndex);
772 myNear.Remove(theIndex);