// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
//
// See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
-//
-\r
-// File : GEOMImpl_Fillet1d.cxx\r
-// Module : GEOMImpl\r
-//\r
-#include "GEOMImpl_Fillet1d.hxx"\r
-\r
-#include <BRep_Tool.hxx>\r
-#include <BRepAdaptor_Curve.hxx>\r
-#include <BRepBuilderAPI_MakeEdge.hxx>\r
-#include <ElCLib.hxx>\r
-#include <ElSLib.hxx>\r
-#include <gp_Circ.hxx>\r
-#include <Geom2d_Line.hxx>\r
-#include <Geom2dAPI_ProjectPointOnCurve.hxx>\r
-#include <Geom2dAPI_InterCurveCurve.hxx>\r
-#include <GeomAPI_ProjectPointOnCurve.hxx>\r
-#include <GeomProjLib.hxx>\r
-#include <Geom_Circle.hxx>\r
-#include <Precision.hxx>\r
-#include <TColStd_ListIteratorOfListOfReal.hxx>\r
-\r
-/**\r
- * class GEOMImpl_Fillet1d\r
- */\r
-\r
-\r
-//=======================================================================\r
-//function : Constructor\r
-//purpose : \r
-//=======================================================================\r
-GEOMImpl_Fillet1d::GEOMImpl_Fillet1d(const TopoDS_Edge& theEdge1,\r
- const TopoDS_Edge& theEdge2,\r
- const gp_Pln& thePlane)\r
-: myEdgesExchnged( Standard_False )\r
-{\r
- myPlane = new Geom_Plane(thePlane);\r
-\r
- BRepAdaptor_Curve aBAC1(theEdge1);\r
- BRepAdaptor_Curve aBAC2(theEdge2);\r
- if (aBAC1.GetType() < aBAC2.GetType()) \r
- { // first curve must be more complicated\r
- myEdge1 = theEdge2;\r
- myEdge2 = theEdge1;\r
- myEdgesExchnged = Standard_True;\r
- } \r
- else\r
- {\r
- myEdge1 = theEdge1;\r
- myEdge2 = theEdge2;\r
- }\r
-\r
- Handle(Geom_Curve) aCurve1 = BRep_Tool::Curve(myEdge1, myStart1, myEnd1);\r
- Handle(Geom_Curve) aCurve2 = BRep_Tool::Curve(myEdge2, myStart2, myEnd2);\r
-\r
- myCurve1 = GeomProjLib::Curve2d(aCurve1, myStart1, myEnd1, myPlane);\r
- myCurve2 = GeomProjLib::Curve2d(aCurve2, myStart2, myEnd2, myPlane);\r
-\r
- while (myCurve1->IsPeriodic() && myStart1 >= myEnd1)\r
- myEnd1 += myCurve1->Period();\r
- while (myCurve2->IsPeriodic() && myStart2 >= myEnd2)\r
- myEnd2 += myCurve2->Period();\r
- \r
- if (aBAC1.GetType() == aBAC2.GetType()) \r
- {\r
- if (myEnd2 - myStart2 < myEnd1 - myStart1) \r
- { // first curve must be parametrically shorter\r
- TopoDS_Edge anEdge = myEdge1;\r
- myEdge1 = myEdge2;\r
- myEdge2 = anEdge;\r
- Handle(Geom2d_Curve) aCurve = myCurve1;\r
- myCurve1 = myCurve2;\r
- myCurve2 = aCurve;\r
- Standard_Real a = myStart1;\r
- myStart1 = myStart2;\r
- myStart2 = a;\r
- a = myEnd1;\r
- myEnd1 = myEnd2;\r
- myEnd2 = a;\r
- myEdgesExchnged = Standard_True;\r
- }\r
- }\r
-}\r
-\r
-//=======================================================================\r
-//function : isRadiusIntersected\r
-//purpose : local function\r
-//=======================================================================\r
-static Standard_Boolean isRadiusIntersected(const Handle(Geom2d_Curve)& theCurve,\r
- const gp_Pnt2d theStart,\r
- const gp_Pnt2d theEnd,\r
- const Standard_Boolean theStartConnected) \r
-{\r
- const Standard_Real aTol = Precision::Confusion();\r
- const Standard_Real anAngTol = Precision::Angular();\r
- Geom2dAPI_InterCurveCurve anInter(theCurve, new Geom2d_Line(theStart,\r
- gp_Dir2d(gp_Vec2d(theStart, theEnd))), aTol);\r
- Standard_Integer a;\r
- gp_Pnt2d aPoint;\r
- for(a = anInter.NbPoints(); a > 0; a--) \r
- {\r
- aPoint = anInter.Point(a);\r
- if ( aPoint.Distance(theStart) < aTol && !theStartConnected )\r
- return Standard_True;\r
- if (aPoint.Distance(theEnd) < aTol * 200)\r
- return Standard_True;\r
- if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol)) \r
- return Standard_True;\r
- }\r
- Handle(Geom2d_Curve) aCurve;\r
- for(a = anInter.NbSegments(); a > 0; a--) \r
- {\r
- anInter.Segment(a, aCurve);\r
- aPoint = aCurve->Value(aCurve->FirstParameter());\r
- if (aPoint.Distance(theStart) < aTol) \r
- if (!theStartConnected) \r
- return Standard_True;\r
- if (aPoint.Distance(theEnd) < aTol) \r
- return Standard_True;\r
- if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol)) \r
- return Standard_True;\r
- aPoint = aCurve->Value(aCurve->LastParameter());\r
- if (aPoint.Distance(theStart) < aTol) \r
- if (!theStartConnected) \r
- return Standard_True;\r
- if (aPoint.Distance(theEnd) < aTol) \r
- return Standard_True;\r
- if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol)) \r
- return Standard_True;\r
- }\r
- return Standard_False;\r
-}\r
-\r
-\r
-//=======================================================================\r
-//function : fillPoint\r
-//purpose : \r
-//=======================================================================\r
-void GEOMImpl_Fillet1d::fillPoint(GEOMImpl_Fillet1dPoint* thePoint) \r
-{\r
- gp_Pnt2d aPoint;\r
- gp_Vec2d aVec;\r
- const Standard_Real aTol = Precision::Confusion();\r
- myCurve1->D1(thePoint->GetParam(), aPoint, aVec);\r
- if (aVec.SquareMagnitude() < aTol) \r
- return;\r
- \r
- gp_Vec2d aPerp(((myStartSide)?-1:1) * aVec.Y(), ((myStartSide)?1:-1) * aVec.X());\r
- aPerp.Normalize();\r
- aPerp.Multiply(myRadius);\r
- gp_Pnt2d aCenter = aPoint.Translated(aPerp);\r
- thePoint->SetCenter(aCenter);\r
-\r
- // on the intersection point\r
- Standard_Boolean aValid = Standard_True;\r
- Geom2dAPI_ProjectPointOnCurve aProjInt(aPoint, myCurve2);\r
- if (aProjInt.NbPoints() && aPoint.Distance(aProjInt.NearestPoint()) < aTol) \r
- aValid = Standard_False;\r
- else \r
- aValid = !isRadiusIntersected(myCurve2, aPoint, aCenter, Standard_True);\r
- \r
- Geom2dAPI_ProjectPointOnCurve aProj(aCenter, myCurve2);\r
- Standard_Integer a, aNB = aProj.NbPoints();\r
- for(a = aNB; a > 0; a--) \r
- {\r
- if (aPoint.Distance(aProj.Point(a)) < aTol) \r
- continue;\r
- \r
- Standard_Boolean aValid2 = aValid;\r
- if (aValid2) \r
- aValid2 = !isRadiusIntersected(myCurve1, aCenter, aProj.Point(a), Standard_False);\r
-\r
- // checking the right parameter\r
- Standard_Real aParam = aProj.Parameter(a);\r
- while(myCurve2->IsPeriodic() && aParam < myStart2)\r
- aParam += myCurve2->Period();\r
-\r
- thePoint->AddValue(aProj.Distance(a) * aProj.Distance(a) - myRadius * myRadius,\r
- (aParam >= myStart2 && aParam <= myEnd2 && aValid2));\r
- if (fabs(fabs(aProj.Distance(a)) - myRadius) < aTol)\r
- thePoint->SetParam2(aParam);\r
- }\r
-}\r
-\r
-//=======================================================================\r
-//function : fillDiff\r
-//purpose : \r
-//=======================================================================\r
-void GEOMImpl_Fillet1d::fillDiff(GEOMImpl_Fillet1dPoint* thePoint, Standard_Real theDiffStep, Standard_Boolean theFront) \r
-{\r
- GEOMImpl_Fillet1dPoint* aDiff =\r
- new GEOMImpl_Fillet1dPoint(thePoint->GetParam() + (theFront?(theDiffStep):(-theDiffStep)));\r
- fillPoint(aDiff);\r
- if (!thePoint->ComputeDifference(aDiff))\r
- {\r
- aDiff->SetParam(thePoint->GetParam() + (theFront?(-theDiffStep):(theDiffStep)));\r
- fillPoint(aDiff);\r
- thePoint->ComputeDifference(aDiff);\r
- }\r
- delete aDiff;\r
-}\r
-\r
-//=======================================================================\r
-//function : Perform\r
-//purpose : \r
-//=======================================================================\r
-Standard_Boolean GEOMImpl_Fillet1d::Perform(const Standard_Real theRadius) \r
-{\r
- myDegreeOfRecursion = 0;\r
- myResultParams.Clear();\r
- myResultOrientation.Clear();\r
-\r
- Standard_Real aNBSteps = 100;\r
- Geom2dAdaptor_Curve aGAC(myCurve1);\r
- switch (aGAC.GetType()) \r
- {\r
- case GeomAbs_Line:\r
- aNBSteps = 2;\r
- break;\r
- case GeomAbs_Circle:\r
- aNBSteps = 4;\r
- break;\r
- case GeomAbs_Ellipse:\r
- aNBSteps = 5;\r
- break;\r
- case GeomAbs_BezierCurve:\r
- case GeomAbs_BSplineCurve:\r
- aNBSteps = 2 + aGAC.Degree() * aGAC.NbPoles();\r
- break;\r
- default: // unknown: maximum\r
- aNBSteps = 100;\r
- }\r
-\r
- myRadius = theRadius;\r
- Standard_Real aParam, aStep, aDStep;\r
- aStep = (myEnd1 - myStart1) / aNBSteps;\r
- aDStep = aStep/1000.;\r
-\r
- Standard_Integer aCycle;\r
- for(aCycle = 2, myStartSide = Standard_False; aCycle; myStartSide = !myStartSide, aCycle--) \r
- {\r
- GEOMImpl_Fillet1dPoint *aLeft = NULL, *aRight = NULL;\r
- \r
- for(aParam = myStart1 + aStep; aParam < myEnd1 || fabs(myEnd1 - aParam) < Precision::Confusion(); aParam += aStep) \r
- {\r
- if (!aLeft) \r
- {\r
- aLeft = new GEOMImpl_Fillet1dPoint(aParam - aStep);\r
- fillPoint(aLeft);\r
- fillDiff(aLeft, aDStep, Standard_True);\r
- }\r
- \r
- aRight = new GEOMImpl_Fillet1dPoint(aParam);\r
- fillPoint(aRight);\r
- fillDiff(aRight, aDStep, Standard_False);\r
- \r
- aLeft->FilterPoints(aRight);\r
- performNewton(aLeft, aRight);\r
- \r
- delete aLeft;\r
- aLeft = aRight;\r
- }\r
- delete aLeft;\r
- }\r
-\r
- if (myResultParams.Extent()) \r
- return Standard_True;\r
- \r
- return Standard_False;\r
-}\r
-\r
-//=======================================================================\r
-//function : processPoint\r
-//purpose : \r
-//=======================================================================\r
-Standard_Boolean GEOMImpl_Fillet1d::processPoint(GEOMImpl_Fillet1dPoint* theLeft,\r
- GEOMImpl_Fillet1dPoint* theRight,\r
- Standard_Real theParameter) \r
-{\r
- if (theParameter >= theLeft->GetParam() && theParameter < theRight->GetParam()) \r
- {\r
- Standard_Real aDX = theRight->GetParam() - theLeft->GetParam();\r
- if (theParameter - theLeft->GetParam() < aDX / 100.) \r
- {\r
- theParameter = theLeft->GetParam() + aDX / 100.;\r
- }\r
- if (theRight->GetParam() - theParameter < aDX / 100.)\r
- {\r
- theParameter = theRight->GetParam() - aDX / 100.;\r
- }\r
-\r
- // Protection on infinite loop.\r
- myDegreeOfRecursion++;\r
- Standard_Real diffx = 0.001 * aDX;\r
- if (myDegreeOfRecursion > 1000)\r
- {\r
- diffx *= 10.0;\r
- if (myDegreeOfRecursion > 10000)\r
- {\r
- diffx *= 10.0;\r
- if (myDegreeOfRecursion > 100000)\r
- {\r
- return Standard_True;\r
- }\r
- }\r
- }\r
-\r
- GEOMImpl_Fillet1dPoint* aPoint1 = theLeft->Copy();\r
- GEOMImpl_Fillet1dPoint* aPoint2 = new GEOMImpl_Fillet1dPoint(theParameter);\r
- fillPoint(aPoint2);\r
- fillDiff(aPoint2, diffx, Standard_True);\r
- \r
- aPoint1->FilterPoints(aPoint2);\r
- performNewton(aPoint1, aPoint2);\r
- aPoint2->FilterPoints(theRight);\r
- performNewton(aPoint2, theRight);\r
-\r
- delete aPoint1;\r
- delete aPoint2;\r
- return Standard_True;\r
- }\r
-\r
- return Standard_False;\r
-}\r
-\r
-//=======================================================================\r
-//function : performNewton\r
-//purpose : \r
-//=======================================================================\r
-void GEOMImpl_Fillet1d::performNewton(GEOMImpl_Fillet1dPoint* theLeft,\r
- GEOMImpl_Fillet1dPoint* theRight)\r
-{\r
- Standard_Integer a;\r
- // check the left: if this is solution store it and remove it from the list of researching points of theLeft\r
- a = theLeft->HasSolution(myRadius);\r
- if (a) \r
- {\r
- if (theLeft->IsValid(a)) \r
- {\r
- myResultParams.Append(theLeft->GetParam());\r
- myResultOrientation.Append(myStartSide);\r
- }\r
- return;\r
- }\r
-\r
- Standard_Real aDX = theRight->GetParam() - theLeft->GetParam();\r
- if ( aDX < Precision::Confusion() / 1000000.) \r
- {\r
- a = theRight->HasSolution(myRadius);\r
- if (a)\r
- if (theRight->IsValid(a)) \r
- {\r
- myResultParams.Append(theRight->GetParam());\r
- myResultOrientation.Append(myStartSide);\r
- }\r
- return;\r
- }\r
-\r
- for(a = 1; a <= theLeft->GetNBValues(); a++) \r
- {\r
- Standard_Integer aNear = theLeft->GetNear(a);\r
- \r
- Standard_Real aA = (theRight->GetDiff(aNear) - theLeft->GetDiff(a)) / aDX;\r
- Standard_Real aB = theLeft->GetDiff(a) - aA * theLeft->GetParam();\r
- Standard_Real aC = theLeft->GetValue(a) - theLeft->GetDiff(a) * theLeft->GetParam() + \r
- aA * theLeft->GetParam() * theLeft->GetParam() / 2.0;\r
- Standard_Real aDet = aB * aB - 2.0 * aA * aC;\r
-\r
- if ( fabs(aDet) < gp::Resolution() )\r
- continue;\r
- \r
- if (fabs(aA) < Precision::Confusion()) \r
- { // linear case\r
- if (fabs(aB) > 10e-20) \r
- {\r
- Standard_Real aX0 = - aC / aB; // use extremum\r
- if (aX0 > theLeft->GetParam() && aX0 < theRight->GetParam())\r
- processPoint(theLeft, theRight, aX0);\r
- }\r
- else \r
- {\r
- processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise\r
- }\r
- } \r
- else\r
- {\r
- if (fabs(aB) > fabs(aDet * 1000000.)) \r
- { // possible floating point operations accurancy errors\r
- processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise\r
- } \r
- else\r
- {\r
- if (aDet > 0) \r
- { // two solutions\r
- aDet = sqrt(aDet);\r
- Standard_Boolean aRes = processPoint(theLeft, theRight, (- aB + aDet) / aA);\r
- if (!aRes) \r
- aRes = processPoint(theLeft, theRight, (- aB - aDet) / aA);\r
- if (!aRes) \r
- processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise\r
- } \r
- else \r
- {\r
- Standard_Real aX0 = - aB / aA; // use extremum\r
- if (aX0 > theLeft->GetParam() && aX0 < theRight->GetParam())\r
- processPoint(theLeft, theRight, aX0);\r
- else \r
- processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise\r
- }\r
- }\r
- }\r
- }\r
-}\r
-\r
-//=======================================================================\r
-//function : Result\r
-//purpose : \r
-//=======================================================================\r
-TopoDS_Edge GEOMImpl_Fillet1d::Result(const gp_Pnt& thePoint,\r
- TopoDS_Edge& theEdge1,\r
- TopoDS_Edge& theEdge2) \r
-{\r
- TopoDS_Edge aResult;\r
- gp_Pnt2d aTargetPoint2d;\r
- Standard_Real aX, aY;\r
- ElSLib::PlaneParameters(myPlane->Pln().Position(), thePoint, aX, aY);\r
- aTargetPoint2d.SetCoord(aX, aY);\r
- \r
- // choose the nearest circle\r
- Standard_Real aDistance, aP;\r
- GEOMImpl_Fillet1dPoint *aNearest;\r
- Standard_Integer a;\r
- TColStd_ListIteratorOfListOfReal anIter(myResultParams);\r
- for(aNearest = NULL, a = 1; anIter.More(); anIter.Next(), a++) \r
- {\r
- myStartSide = (myResultOrientation.Value(a)) ? Standard_True : Standard_False;\r
- GEOMImpl_Fillet1dPoint *aPoint = new GEOMImpl_Fillet1dPoint(anIter.Value());\r
- fillPoint(aPoint);\r
- if (!aPoint->HasSolution(myRadius)) \r
- continue;\r
- aP = fabs(aPoint->GetCenter().Distance(aTargetPoint2d) - myRadius);\r
- if (!aNearest || aP < aDistance) \r
- {\r
- aNearest = aPoint;\r
- aDistance = aP;\r
- } \r
- else \r
- {\r
- delete aPoint;\r
- }\r
- }\r
- \r
- if (!aNearest) \r
- return aResult;\r
- \r
- // create circle edge\r
- gp_Pnt aCenter = ElSLib::PlaneValue(aNearest->GetCenter().X(),\r
- aNearest->GetCenter().Y(),\r
- myPlane->Pln().Position());\r
- Handle(Geom_Circle) aCircle =\r
- new Geom_Circle(gp_Ax2(aCenter, myPlane->Pln().Axis().Direction()), myRadius);\r
- gp_Pnt2d aPoint2d1, aPoint2d2;\r
- myCurve1->D0(aNearest->GetParam(), aPoint2d1);\r
- myCurve2->D0(aNearest->GetParam2(), aPoint2d2);\r
- gp_Pnt aPoint1 = ElSLib::PlaneValue(aPoint2d1.X(), aPoint2d1.Y(), myPlane->Pln().Position());\r
- gp_Pnt aPoint2 = ElSLib::PlaneValue(aPoint2d2.X(), aPoint2d2.Y(), myPlane->Pln().Position());\r
-\r
- GeomAPI_ProjectPointOnCurve aProj(thePoint, aCircle);\r
- Standard_Real aTarGetParam = aProj.LowerDistanceParameter();\r
- gp_Pnt aPointOnCircle = aProj.NearestPoint();\r
-\r
- // Check extrema point manually, because there is a bug in Open CASCADE\r
- // in calculation of nearest point to a circle near the parameter 0.0\r
- gp_Pnt p0 = ElCLib::Value(0.0, aCircle->Circ());\r
- if (p0.Distance(thePoint) < aPointOnCircle.Distance(thePoint))\r
- {\r
- aTarGetParam = 0.0;\r
- aPointOnCircle = p0;\r
- }\r
-\r
- aProj.Perform(aPoint1);\r
- Standard_Real aParam1 = aProj.LowerDistanceParameter();\r
- aProj.Perform(aPoint2);\r
- Standard_Real aParam2 = aProj.LowerDistanceParameter();\r
- Standard_Boolean aIsOut = ((aParam1 < aTarGetParam && aParam2 < aTarGetParam) || \r
- (aParam1 > aTarGetParam && aParam2 > aTarGetParam));\r
- if (aParam1 > aParam2) \r
- aIsOut = !aIsOut;\r
- BRepBuilderAPI_MakeEdge aBuilder(aCircle->Circ(),\r
- aIsOut ? aParam2 : aParam1,\r
- aIsOut? aParam1 : aParam2);\r
- aResult = aBuilder.Edge();\r
-\r
- // divide edges\r
- Standard_Real aStart, anEnd;\r
- Handle(Geom_Curve) aCurve = BRep_Tool::Curve(myEdge1, aStart, anEnd);\r
- gp_Vec aDir;\r
- aCurve->D1(aNearest->GetParam(), aPoint1, aDir);\r
-\r
- gp_Vec aCircleDir;\r
- aCircle->D1(aParam1, aPoint1, aCircleDir);\r
- if ((aCircleDir.Angle(aDir) > PI / 2.0) ^ aIsOut)\r
- aStart = aNearest->GetParam();\r
- else\r
- anEnd = aNearest->GetParam();\r
-\r
- if (fabs(aStart - anEnd) > Precision::Confusion())\r
- {\r
- //Divide edge\r
- BRepBuilderAPI_MakeEdge aDivider1(aCurve, aStart, anEnd);\r
- if (myEdgesExchnged) \r
- theEdge2 = aDivider1.Edge();\r
- else \r
- theEdge1 = aDivider1.Edge();\r
- }\r
-\r
- aCurve = BRep_Tool::Curve(myEdge2, aStart, anEnd);\r
- aCurve->D1(aNearest->GetParam2(), aPoint2, aDir);\r
- \r
- aCircle->D1(aParam2, aPoint2, aCircleDir);\r
- if ((aCircleDir.Angle(aDir) > PI / 2.0) ^ (!aIsOut))\r
- aStart = aNearest->GetParam2();\r
- else\r
- anEnd = aNearest->GetParam2();\r
-\r
- if (fabs(aStart - anEnd) > Precision::Confusion())\r
- {\r
- BRepBuilderAPI_MakeEdge aDivider2(aCurve, aStart, anEnd);\r
- if (myEdgesExchnged) \r
- theEdge1 = aDivider2.Edge();\r
- else \r
- theEdge2 = aDivider2.Edge();\r
- }\r
-\r
- delete aNearest;\r
- return aResult;\r
-}\r
-\r
-//=======================================================================\r
-//function : AddValue\r
-//purpose : \r
-//=======================================================================\r
-void GEOMImpl_Fillet1dPoint::AddValue(Standard_Real theValue, Standard_Boolean theValid) \r
-{\r
- Standard_Integer a;\r
- for(a = 1; a <= myV.Length(); a++) \r
- {\r
- if (theValue < myV.Value(a)) \r
- {\r
- myV.InsertBefore(a, theValue);\r
- myValid.InsertBefore(a, (Standard_Integer)theValid);\r
- return;\r
- }\r
- }\r
- myV.Append(theValue);\r
- myValid.Append((Standard_Integer)theValid);\r
-}\r
-\r
-//=======================================================================\r
-//function : ComputeDifference\r
-//purpose : \r
-//=======================================================================\r
-Standard_Boolean GEOMImpl_Fillet1dPoint::ComputeDifference(GEOMImpl_Fillet1dPoint* thePoint) \r
-{\r
- Standard_Integer a;\r
- Standard_Boolean aDiffsSet = (myD.Length() != 0);\r
- Standard_Real aDX = thePoint->GetParam() - myParam, aDY;\r
- if (thePoint->myV.Length() == myV.Length()) \r
- { // absolutely the same points\r
- for(a = 1; a <= myV.Length(); a++) \r
- {\r
- aDY = thePoint->myV.Value(a) - myV.Value(a);\r
- if ( aDiffsSet ) \r
- myD.SetValue(a, fabs(aDX) > gp::Resolution() ? (aDY/aDX) : 0);\r
- else\r
- myD.Append( fabs(aDX) > gp::Resolution() ? (aDY/aDX) : 0);\r
- }\r
- return Standard_True;\r
- }\r
- // between the diffeerent points searching for nearest analogs\r
- Standard_Integer b;\r
- for(a = 1; a <= myV.Length(); a++) \r
- {\r
- for(b = 1; b <= thePoint->myV.Length(); b++) \r
- {\r
- if (b == 1 || fabs(thePoint->myV.Value(b) - myV.Value(a)) < fabs(aDY))\r
- aDY = thePoint->myV.Value(b) - myV.Value(a);\r
- }\r
- if (aDiffsSet) \r
- {\r
- if ( fabs(aDX) > gp::Resolution() && fabs(aDY / aDX) < fabs(myD.Value(a)))\r
- myD.SetValue(a, aDY / aDX);\r
- else\r
- myD.SetValue(a, 0);\r
- } \r
- else \r
- {\r
- myD.Append( fabs(aDX) > gp::Resolution() ? aDY/aDX : 0);\r
- }\r
- }\r
- \r
- return Standard_False;\r
-}\r
-\r
-//=======================================================================\r
-//function : FilterPoints\r
-//purpose : \r
-//=======================================================================\r
-void GEOMImpl_Fillet1dPoint::FilterPoints(GEOMImpl_Fillet1dPoint* thePoint) \r
-{\r
- Standard_Integer a, b;\r
- TColStd_SequenceOfReal aDiffs;\r
- Standard_Real aY, aY2, aDX = thePoint->GetParam() - myParam;\r
- for(a = 1; a <= myV.Length(); a++) \r
- {\r
- // searching for near point from thePoint\r
- Standard_Integer aNear = 0;\r
- Standard_Real aDiff = aDX * 10000.;\r
- aY = myV.Value(a) + myD.Value(a) * aDX;\r
- for(b = 1; b <= thePoint->myV.Length(); b++) \r
- {\r
- // calculate hypothesis value of the Y2 with the constant first and second derivative\r
- aY2 = aY + aDX * (thePoint->myD.Value(b) - myD.Value(a)) / 2.0;\r
- if (aNear == 0 || fabs(aY2 - thePoint->myV.Value(b)) < fabs(aDiff)) \r
- {\r
- aNear = b;\r
- aDiff = aY2 - thePoint->myV.Value(b);\r
- }\r
- }//for b...\r
-\r
- if (aNear) \r
- {\r
- if (myV.Value(a) * thePoint->myV.Value(aNear) > 0) \r
- {// the same sign at the same sides of the interval\r
- if (myV.Value(a) * myD.Value(a) > 0) \r
- {\r
- if (fabs(myD.Value(a)) > Precision::Confusion()) \r
- aNear = 0;\r
- } \r
- else \r
- {\r
- if (fabs(myV.Value(a)) > fabs(thePoint->myV.Value(aNear)))\r
- if (thePoint->myV.Value(aNear) * thePoint->myD.Value(aNear) < 0 &&\r
- fabs(thePoint->myD.Value(aNear)) > Precision::Confusion())\r
- {\r
- aNear = 0;\r
- }\r
- }\r
- }\r
- }\r
-\r
- if (aNear) \r
- {\r
- if (myV.Value(a) * thePoint->myV.Value(aNear) > 0) \r
- {\r
- if ((myV.Value(a) + myD.Value(a) * aDX) * myV.Value(a) > Precision::Confusion() &&\r
- (thePoint->myV.Value(aNear) + thePoint->myD.Value(aNear) * aDX) * thePoint->myV.Value(aNear) > Precision::Confusion())\r
- {\r
- aNear = 0;\r
- }\r
- }\r
- }\r
- \r
- if (aNear)\r
- {\r
- if ( fabs(aDX) < gp::Resolution() || fabs(aDiff / aDX) > 1.e+7) \r
- {\r
- aNear = 0;\r
- }\r
- }\r
-\r
- if (aNear == 0) \r
- { // there is no near: remove it from the list\r
- myV.Remove(a);\r
- myD.Remove(a);\r
- myValid.Remove(a);\r
- a--;\r
- } \r
- else \r
- {\r
- Standard_Boolean aFound = Standard_False;\r
- for(b = 1; b <= myNear.Length(); b++) \r
- {\r
- if (myNear.Value(b) == aNear) \r
- {\r
- if (fabs(aDiffs.Value(b)) < fabs(aDiff)) \r
- { // return this 'near'\r
- aFound = Standard_True;\r
- myV.Remove(a);\r
- myD.Remove(a);\r
- myValid.Remove(a);\r
- a--;\r
- break;\r
- } \r
- else \r
- { // remove the old 'near'\r
- myV.Remove(b);\r
- myD.Remove(b);\r
- myValid.Remove(b);\r
- myNear.Remove(b);\r
- aDiffs.Remove(b);\r
- a--;\r
- break;\r
- }\r
- }\r
- }//for b...\r
- if (!aFound) \r
- {\r
- myNear.Append(aNear);\r
- aDiffs.Append(aDiff);\r
- }\r
- }\r
- }//for a...\r
-}\r
-\r
-//=======================================================================\r
-//function : Copy\r
-//purpose : \r
-//=======================================================================\r
-GEOMImpl_Fillet1dPoint* GEOMImpl_Fillet1dPoint::Copy() \r
-{\r
- GEOMImpl_Fillet1dPoint* aCopy = new GEOMImpl_Fillet1dPoint(myParam);\r
- Standard_Integer a;\r
- for(a = 1; a <= myV.Length(); a++) \r
- {\r
- aCopy->myV.Append(myV.Value(a));\r
- aCopy->myD.Append(myD.Value(a));\r
- aCopy->myValid.Append(myValid.Value(a));\r
- }\r
- return aCopy;\r
-}\r
-\r
-//=======================================================================\r
-//function : HasSolution\r
-//purpose : \r
-//=======================================================================\r
-Standard_Integer GEOMImpl_Fillet1dPoint::HasSolution(const Standard_Real theRadius) \r
-{\r
- Standard_Integer a;\r
- for(a = 1; a <= myV.Length(); a++) \r
- {\r
- if (fabs(sqrt(fabs(fabs(myV.Value(a)) + theRadius * theRadius)) - theRadius) < Precision::Confusion() / 10.) \r
- return a;\r
- }\r
- return 0;\r
-}\r
-\r
-//=======================================================================\r
-//function : RemoveSolution\r
-//purpose : \r
-//=======================================================================\r
-void GEOMImpl_Fillet1dPoint::RemoveSolution(Standard_Integer theIndex)\r
-{\r
- myV.Remove(theIndex);\r
- myD.Remove(theIndex);\r
- myValid.Remove(theIndex);\r
- myNear.Remove(theIndex);\r
-}\r
+
+// File : GEOMImpl_Fillet1d.cxx
+// Module : GEOMImpl
+
+#include "GEOMImpl_Fillet1d.hxx"
+
+#include <BRep_Tool.hxx>
+#include <BRepAdaptor_Curve.hxx>
+#include <BRepBuilderAPI_MakeEdge.hxx>
+#include <ElCLib.hxx>
+#include <ElSLib.hxx>
+#include <gp_Circ.hxx>
+#include <Geom2d_Line.hxx>
+#include <Geom2dAPI_ProjectPointOnCurve.hxx>
+#include <Geom2dAPI_InterCurveCurve.hxx>
+#include <GeomAPI_ProjectPointOnCurve.hxx>
+#include <GeomProjLib.hxx>
+#include <Geom_Circle.hxx>
+#include <Precision.hxx>
+#include <TColStd_ListIteratorOfListOfReal.hxx>
+
+/**
+ * class GEOMImpl_Fillet1d
+ */
+
+//=======================================================================
+//function : Constructor
+//purpose :
+//=======================================================================
+GEOMImpl_Fillet1d::GEOMImpl_Fillet1d(const TopoDS_Edge& theEdge1,
+ const TopoDS_Edge& theEdge2,
+ const gp_Pln& thePlane)
+: myEdgesExchnged( Standard_False )
+{
+ myPlane = new Geom_Plane(thePlane);
+
+ BRepAdaptor_Curve aBAC1(theEdge1);
+ BRepAdaptor_Curve aBAC2(theEdge2);
+ if (aBAC1.GetType() < aBAC2.GetType())
+ { // first curve must be more complicated
+ myEdge1 = theEdge2;
+ myEdge2 = theEdge1;
+ myEdgesExchnged = Standard_True;
+ }
+ else
+ {
+ myEdge1 = theEdge1;
+ myEdge2 = theEdge2;
+ }
+
+ Handle(Geom_Curve) aCurve1 = BRep_Tool::Curve(myEdge1, myStart1, myEnd1);
+ Handle(Geom_Curve) aCurve2 = BRep_Tool::Curve(myEdge2, myStart2, myEnd2);
+
+ myCurve1 = GeomProjLib::Curve2d(aCurve1, myStart1, myEnd1, myPlane);
+ myCurve2 = GeomProjLib::Curve2d(aCurve2, myStart2, myEnd2, myPlane);
+
+ while (myCurve1->IsPeriodic() && myStart1 >= myEnd1)
+ myEnd1 += myCurve1->Period();
+ while (myCurve2->IsPeriodic() && myStart2 >= myEnd2)
+ myEnd2 += myCurve2->Period();
+
+ if (aBAC1.GetType() == aBAC2.GetType())
+ {
+ if (myEnd2 - myStart2 < myEnd1 - myStart1)
+ { // first curve must be parametrically shorter
+ TopoDS_Edge anEdge = myEdge1;
+ myEdge1 = myEdge2;
+ myEdge2 = anEdge;
+ Handle(Geom2d_Curve) aCurve = myCurve1;
+ myCurve1 = myCurve2;
+ myCurve2 = aCurve;
+ Standard_Real a = myStart1;
+ myStart1 = myStart2;
+ myStart2 = a;
+ a = myEnd1;
+ myEnd1 = myEnd2;
+ myEnd2 = a;
+ myEdgesExchnged = Standard_True;
+ }
+ }
+}
+
+//=======================================================================
+//function : isRadiusIntersected
+//purpose : local function
+//=======================================================================
+static Standard_Boolean isRadiusIntersected(const Handle(Geom2d_Curve)& theCurve,
+ const gp_Pnt2d theStart,
+ const gp_Pnt2d theEnd,
+ const Standard_Boolean theStartConnected)
+{
+ const Standard_Real aTol = Precision::Confusion();
+ const Standard_Real anAngTol = Precision::Angular();
+ Geom2dAPI_InterCurveCurve anInter(theCurve, new Geom2d_Line(theStart,
+ gp_Dir2d(gp_Vec2d(theStart, theEnd))), aTol);
+ Standard_Integer a;
+ gp_Pnt2d aPoint;
+ for(a = anInter.NbPoints(); a > 0; a--)
+ {
+ aPoint = anInter.Point(a);
+ if ( aPoint.Distance(theStart) < aTol && !theStartConnected )
+ return Standard_True;
+ if (aPoint.Distance(theEnd) < aTol * 200)
+ return Standard_True;
+ if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol))
+ return Standard_True;
+ }
+ Handle(Geom2d_Curve) aCurve;
+ for(a = anInter.NbSegments(); a > 0; a--)
+ {
+ anInter.Segment(a, aCurve);
+ aPoint = aCurve->Value(aCurve->FirstParameter());
+ if (aPoint.Distance(theStart) < aTol)
+ if (!theStartConnected)
+ return Standard_True;
+ if (aPoint.Distance(theEnd) < aTol)
+ return Standard_True;
+ if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol))
+ return Standard_True;
+ aPoint = aCurve->Value(aCurve->LastParameter());
+ if (aPoint.Distance(theStart) < aTol)
+ if (!theStartConnected)
+ return Standard_True;
+ if (aPoint.Distance(theEnd) < aTol)
+ return Standard_True;
+ if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), anAngTol))
+ return Standard_True;
+ }
+ return Standard_False;
+}
+
+
+//=======================================================================
+//function : fillPoint
+//purpose :
+//=======================================================================
+void GEOMImpl_Fillet1d::fillPoint(GEOMImpl_Fillet1dPoint* thePoint)
+{
+ gp_Pnt2d aPoint;
+ gp_Vec2d aVec;
+ const Standard_Real aTol = Precision::Confusion();
+ myCurve1->D1(thePoint->GetParam(), aPoint, aVec);
+ if (aVec.SquareMagnitude() < aTol)
+ return;
+
+ gp_Vec2d aPerp(((myStartSide)?-1:1) * aVec.Y(), ((myStartSide)?1:-1) * aVec.X());
+ aPerp.Normalize();
+ aPerp.Multiply(myRadius);
+ gp_Pnt2d aCenter = aPoint.Translated(aPerp);
+ thePoint->SetCenter(aCenter);
+
+ // on the intersection point
+ Standard_Boolean aValid = Standard_True;
+ Geom2dAPI_ProjectPointOnCurve aProjInt(aPoint, myCurve2);
+ if (aProjInt.NbPoints() && aPoint.Distance(aProjInt.NearestPoint()) < aTol)
+ aValid = Standard_False;
+ else
+ aValid = !isRadiusIntersected(myCurve2, aPoint, aCenter, Standard_True);
+
+ Geom2dAPI_ProjectPointOnCurve aProj(aCenter, myCurve2);
+ Standard_Integer a, aNB = aProj.NbPoints();
+ for(a = aNB; a > 0; a--)
+ {
+ if (aPoint.Distance(aProj.Point(a)) < aTol)
+ continue;
+
+ Standard_Boolean aValid2 = aValid;
+ if (aValid2)
+ aValid2 = !isRadiusIntersected(myCurve1, aCenter, aProj.Point(a), Standard_False);
+
+ // checking the right parameter
+ Standard_Real aParam = aProj.Parameter(a);
+ while(myCurve2->IsPeriodic() && aParam < myStart2)
+ aParam += myCurve2->Period();
+
+ thePoint->AddValue(aProj.Distance(a) * aProj.Distance(a) - myRadius * myRadius,
+ (aParam >= myStart2 && aParam <= myEnd2 && aValid2));
+ if (fabs(fabs(aProj.Distance(a)) - myRadius) < aTol)
+ thePoint->SetParam2(aParam);
+ }
+}
+
+//=======================================================================
+//function : fillDiff
+//purpose :
+//=======================================================================
+void GEOMImpl_Fillet1d::fillDiff(GEOMImpl_Fillet1dPoint* thePoint, Standard_Real theDiffStep, Standard_Boolean theFront)
+{
+ GEOMImpl_Fillet1dPoint* aDiff =
+ new GEOMImpl_Fillet1dPoint(thePoint->GetParam() + (theFront?(theDiffStep):(-theDiffStep)));
+ fillPoint(aDiff);
+ if (!thePoint->ComputeDifference(aDiff))
+ {
+ aDiff->SetParam(thePoint->GetParam() + (theFront?(-theDiffStep):(theDiffStep)));
+ fillPoint(aDiff);
+ thePoint->ComputeDifference(aDiff);
+ }
+ delete aDiff;
+}
+
+//=======================================================================
+//function : Perform
+//purpose :
+//=======================================================================
+Standard_Boolean GEOMImpl_Fillet1d::Perform(const Standard_Real theRadius)
+{
+ myDegreeOfRecursion = 0;
+ myResultParams.Clear();
+ myResultOrientation.Clear();
+
+ Standard_Real aNBSteps = 100;
+ Geom2dAdaptor_Curve aGAC(myCurve1);
+ switch (aGAC.GetType())
+ {
+ case GeomAbs_Line:
+ aNBSteps = 2;
+ break;
+ case GeomAbs_Circle:
+ aNBSteps = 4;
+ break;
+ case GeomAbs_Ellipse:
+ aNBSteps = 5;
+ break;
+ case GeomAbs_BezierCurve:
+ case GeomAbs_BSplineCurve:
+ aNBSteps = 2 + aGAC.Degree() * aGAC.NbPoles();
+ break;
+ default: // unknown: maximum
+ aNBSteps = 100;
+ }
+
+ myRadius = theRadius;
+ Standard_Real aParam, aStep, aDStep;
+ aStep = (myEnd1 - myStart1) / aNBSteps;
+ aDStep = aStep/1000.;
+
+ Standard_Integer aCycle;
+ for(aCycle = 2, myStartSide = Standard_False; aCycle; myStartSide = !myStartSide, aCycle--)
+ {
+ GEOMImpl_Fillet1dPoint *aLeft = NULL, *aRight = NULL;
+
+ for(aParam = myStart1 + aStep; aParam < myEnd1 || fabs(myEnd1 - aParam) < Precision::Confusion(); aParam += aStep)
+ {
+ if (!aLeft)
+ {
+ aLeft = new GEOMImpl_Fillet1dPoint(aParam - aStep);
+ fillPoint(aLeft);
+ fillDiff(aLeft, aDStep, Standard_True);
+ }
+
+ aRight = new GEOMImpl_Fillet1dPoint(aParam);
+ fillPoint(aRight);
+ fillDiff(aRight, aDStep, Standard_False);
+
+ aLeft->FilterPoints(aRight);
+ performNewton(aLeft, aRight);
+
+ delete aLeft;
+ aLeft = aRight;
+ }
+ delete aLeft;
+ }
+
+ if (myResultParams.Extent())
+ return Standard_True;
+
+ return Standard_False;
+}
+
+//=======================================================================
+//function : processPoint
+//purpose :
+//=======================================================================
+Standard_Boolean GEOMImpl_Fillet1d::processPoint(GEOMImpl_Fillet1dPoint* theLeft,
+ GEOMImpl_Fillet1dPoint* theRight,
+ Standard_Real theParameter)
+{
+ if (theParameter >= theLeft->GetParam() && theParameter < theRight->GetParam())
+ {
+ Standard_Real aDX = theRight->GetParam() - theLeft->GetParam();
+ if (theParameter - theLeft->GetParam() < aDX / 100.)
+ {
+ theParameter = theLeft->GetParam() + aDX / 100.;
+ }
+ if (theRight->GetParam() - theParameter < aDX / 100.)
+ {
+ theParameter = theRight->GetParam() - aDX / 100.;
+ }
+
+ // Protection on infinite loop.
+ myDegreeOfRecursion++;
+ Standard_Real diffx = 0.001 * aDX;
+ if (myDegreeOfRecursion > 1000)
+ {
+ diffx *= 10.0;
+ if (myDegreeOfRecursion > 10000)
+ {
+ diffx *= 10.0;
+ if (myDegreeOfRecursion > 100000)
+ {
+ return Standard_True;
+ }
+ }
+ }
+
+ GEOMImpl_Fillet1dPoint* aPoint1 = theLeft->Copy();
+ GEOMImpl_Fillet1dPoint* aPoint2 = new GEOMImpl_Fillet1dPoint(theParameter);
+ fillPoint(aPoint2);
+ fillDiff(aPoint2, diffx, Standard_True);
+
+ aPoint1->FilterPoints(aPoint2);
+ performNewton(aPoint1, aPoint2);
+ aPoint2->FilterPoints(theRight);
+ performNewton(aPoint2, theRight);
+
+ delete aPoint1;
+ delete aPoint2;
+ return Standard_True;
+ }
+
+ return Standard_False;
+}
+
+//=======================================================================
+//function : performNewton
+//purpose :
+//=======================================================================
+void GEOMImpl_Fillet1d::performNewton(GEOMImpl_Fillet1dPoint* theLeft,
+ GEOMImpl_Fillet1dPoint* theRight)
+{
+ Standard_Integer a;
+ // check the left: if this is solution store it and remove it from the list of researching points of theLeft
+ a = theLeft->HasSolution(myRadius);
+ if (a)
+ {
+ if (theLeft->IsValid(a))
+ {
+ myResultParams.Append(theLeft->GetParam());
+ myResultOrientation.Append(myStartSide);
+ }
+ return;
+ }
+
+ Standard_Real aDX = theRight->GetParam() - theLeft->GetParam();
+ if ( aDX < Precision::Confusion() / 1000000.)
+ {
+ a = theRight->HasSolution(myRadius);
+ if (a)
+ if (theRight->IsValid(a))
+ {
+ myResultParams.Append(theRight->GetParam());
+ myResultOrientation.Append(myStartSide);
+ }
+ return;
+ }
+
+ for(a = 1; a <= theLeft->GetNBValues(); a++)
+ {
+ Standard_Integer aNear = theLeft->GetNear(a);
+
+ Standard_Real aA = (theRight->GetDiff(aNear) - theLeft->GetDiff(a)) / aDX;
+ Standard_Real aB = theLeft->GetDiff(a) - aA * theLeft->GetParam();
+ Standard_Real aC = theLeft->GetValue(a) - theLeft->GetDiff(a) * theLeft->GetParam() +
+ aA * theLeft->GetParam() * theLeft->GetParam() / 2.0;
+ Standard_Real aDet = aB * aB - 2.0 * aA * aC;
+
+ if ( fabs(aDet) < gp::Resolution() )
+ continue;
+
+ if (fabs(aA) < Precision::Confusion())
+ { // linear case
+ if (fabs(aB) > 10e-20)
+ {
+ Standard_Real aX0 = - aC / aB; // use extremum
+ if (aX0 > theLeft->GetParam() && aX0 < theRight->GetParam())
+ processPoint(theLeft, theRight, aX0);
+ }
+ else
+ {
+ processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
+ }
+ }
+ else
+ {
+ if (fabs(aB) > fabs(aDet * 1000000.))
+ { // possible floating point operations accurancy errors
+ processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
+ }
+ else
+ {
+ if (aDet > 0)
+ { // two solutions
+ aDet = sqrt(aDet);
+ Standard_Boolean aRes = processPoint(theLeft, theRight, (- aB + aDet) / aA);
+ if (!aRes)
+ aRes = processPoint(theLeft, theRight, (- aB - aDet) / aA);
+ if (!aRes)
+ processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
+ }
+ else
+ {
+ Standard_Real aX0 = - aB / aA; // use extremum
+ if (aX0 > theLeft->GetParam() && aX0 < theRight->GetParam())
+ processPoint(theLeft, theRight, aX0);
+ else
+ processPoint(theLeft, theRight, theLeft->GetParam() + aDX / 2.0); // linear division otherwise
+ }
+ }
+ }
+ }
+}
+
+//=======================================================================
+//function : Result
+//purpose :
+//=======================================================================
+TopoDS_Edge GEOMImpl_Fillet1d::Result(const gp_Pnt& thePoint,
+ TopoDS_Edge& theEdge1,
+ TopoDS_Edge& theEdge2)
+{
+ TopoDS_Edge aResult;
+ gp_Pnt2d aTargetPoint2d;
+ Standard_Real aX, aY;
+ ElSLib::PlaneParameters(myPlane->Pln().Position(), thePoint, aX, aY);
+ aTargetPoint2d.SetCoord(aX, aY);
+
+ // choose the nearest circle
+ Standard_Real aDistance, aP;
+ GEOMImpl_Fillet1dPoint *aNearest;
+ Standard_Integer a;
+ TColStd_ListIteratorOfListOfReal anIter(myResultParams);
+ for(aNearest = NULL, a = 1; anIter.More(); anIter.Next(), a++)
+ {
+ myStartSide = (myResultOrientation.Value(a)) ? Standard_True : Standard_False;
+ GEOMImpl_Fillet1dPoint *aPoint = new GEOMImpl_Fillet1dPoint(anIter.Value());
+ fillPoint(aPoint);
+ if (!aPoint->HasSolution(myRadius))
+ continue;
+ aP = fabs(aPoint->GetCenter().Distance(aTargetPoint2d) - myRadius);
+ if (!aNearest || aP < aDistance)
+ {
+ aNearest = aPoint;
+ aDistance = aP;
+ }
+ else
+ {
+ delete aPoint;
+ }
+ }
+
+ if (!aNearest)
+ return aResult;
+
+ // create circle edge
+ gp_Pnt aCenter = ElSLib::PlaneValue(aNearest->GetCenter().X(),
+ aNearest->GetCenter().Y(),
+ myPlane->Pln().Position());
+ Handle(Geom_Circle) aCircle =
+ new Geom_Circle(gp_Ax2(aCenter, myPlane->Pln().Axis().Direction()), myRadius);
+ gp_Pnt2d aPoint2d1, aPoint2d2;
+ myCurve1->D0(aNearest->GetParam(), aPoint2d1);
+ myCurve2->D0(aNearest->GetParam2(), aPoint2d2);
+ gp_Pnt aPoint1 = ElSLib::PlaneValue(aPoint2d1.X(), aPoint2d1.Y(), myPlane->Pln().Position());
+ gp_Pnt aPoint2 = ElSLib::PlaneValue(aPoint2d2.X(), aPoint2d2.Y(), myPlane->Pln().Position());
+
+ GeomAPI_ProjectPointOnCurve aProj(thePoint, aCircle);
+ Standard_Real aTarGetParam = aProj.LowerDistanceParameter();
+ gp_Pnt aPointOnCircle = aProj.NearestPoint();
+
+ // Check extrema point manually, because there is a bug in Open CASCADE
+ // in calculation of nearest point to a circle near the parameter 0.0
+ gp_Pnt p0 = ElCLib::Value(0.0, aCircle->Circ());
+ if (p0.Distance(thePoint) < aPointOnCircle.Distance(thePoint))
+ {
+ aTarGetParam = 0.0;
+ aPointOnCircle = p0;
+ }
+
+ aProj.Perform(aPoint1);
+ Standard_Real aParam1 = aProj.LowerDistanceParameter();
+ aProj.Perform(aPoint2);
+ Standard_Real aParam2 = aProj.LowerDistanceParameter();
+ Standard_Boolean aIsOut = ((aParam1 < aTarGetParam && aParam2 < aTarGetParam) ||
+ (aParam1 > aTarGetParam && aParam2 > aTarGetParam));
+ if (aParam1 > aParam2)
+ aIsOut = !aIsOut;
+ BRepBuilderAPI_MakeEdge aBuilder(aCircle->Circ(),
+ aIsOut ? aParam2 : aParam1,
+ aIsOut? aParam1 : aParam2);
+ aResult = aBuilder.Edge();
+
+ // divide edges
+ Standard_Real aStart, anEnd;
+ Handle(Geom_Curve) aCurve = BRep_Tool::Curve(myEdge1, aStart, anEnd);
+ gp_Vec aDir;
+ aCurve->D1(aNearest->GetParam(), aPoint1, aDir);
+
+ gp_Vec aCircleDir;
+ aCircle->D1(aParam1, aPoint1, aCircleDir);
+ if ((aCircleDir.Angle(aDir) > M_PI / 2.0) ^ aIsOut)
+ aStart = aNearest->GetParam();
+ else
+ anEnd = aNearest->GetParam();
+
+ if (fabs(aStart - anEnd) > Precision::Confusion())
+ {
+ //Divide edge
+ BRepBuilderAPI_MakeEdge aDivider1(aCurve, aStart, anEnd);
+ if (myEdgesExchnged)
+ theEdge2 = aDivider1.Edge();
+ else
+ theEdge1 = aDivider1.Edge();
+ }
+
+ aCurve = BRep_Tool::Curve(myEdge2, aStart, anEnd);
+ aCurve->D1(aNearest->GetParam2(), aPoint2, aDir);
+
+ aCircle->D1(aParam2, aPoint2, aCircleDir);
+ if ((aCircleDir.Angle(aDir) > M_PI / 2.0) ^ (!aIsOut))
+ aStart = aNearest->GetParam2();
+ else
+ anEnd = aNearest->GetParam2();
+
+ if (fabs(aStart - anEnd) > Precision::Confusion())
+ {
+ BRepBuilderAPI_MakeEdge aDivider2(aCurve, aStart, anEnd);
+ if (myEdgesExchnged)
+ theEdge1 = aDivider2.Edge();
+ else
+ theEdge2 = aDivider2.Edge();
+ }
+
+ delete aNearest;
+ return aResult;
+}
+
+//=======================================================================
+//function : AddValue
+//purpose :
+//=======================================================================
+void GEOMImpl_Fillet1dPoint::AddValue(Standard_Real theValue, Standard_Boolean theValid)
+{
+ Standard_Integer a;
+ for(a = 1; a <= myV.Length(); a++)
+ {
+ if (theValue < myV.Value(a))
+ {
+ myV.InsertBefore(a, theValue);
+ myValid.InsertBefore(a, (Standard_Integer)theValid);
+ return;
+ }
+ }
+ myV.Append(theValue);
+ myValid.Append((Standard_Integer)theValid);
+}
+
+//=======================================================================
+//function : ComputeDifference
+//purpose :
+//=======================================================================
+Standard_Boolean GEOMImpl_Fillet1dPoint::ComputeDifference(GEOMImpl_Fillet1dPoint* thePoint)
+{
+ Standard_Integer a;
+ Standard_Boolean aDiffsSet = (myD.Length() != 0);
+ Standard_Real aDX = thePoint->GetParam() - myParam, aDY;
+ if (thePoint->myV.Length() == myV.Length())
+ { // absolutely the same points
+ for(a = 1; a <= myV.Length(); a++)
+ {
+ aDY = thePoint->myV.Value(a) - myV.Value(a);
+ if ( aDiffsSet )
+ myD.SetValue(a, fabs(aDX) > gp::Resolution() ? (aDY/aDX) : 0);
+ else
+ myD.Append( fabs(aDX) > gp::Resolution() ? (aDY/aDX) : 0);
+ }
+ return Standard_True;
+ }
+ // between the diffeerent points searching for nearest analogs
+ Standard_Integer b;
+ for(a = 1; a <= myV.Length(); a++)
+ {
+ for(b = 1; b <= thePoint->myV.Length(); b++)
+ {
+ if (b == 1 || fabs(thePoint->myV.Value(b) - myV.Value(a)) < fabs(aDY))
+ aDY = thePoint->myV.Value(b) - myV.Value(a);
+ }
+ if (aDiffsSet)
+ {
+ if ( fabs(aDX) > gp::Resolution() && fabs(aDY / aDX) < fabs(myD.Value(a)))
+ myD.SetValue(a, aDY / aDX);
+ else
+ myD.SetValue(a, 0);
+ }
+ else
+ {
+ myD.Append( fabs(aDX) > gp::Resolution() ? aDY/aDX : 0);
+ }
+ }
+
+ return Standard_False;
+}
+
+//=======================================================================
+//function : FilterPoints
+//purpose :
+//=======================================================================
+void GEOMImpl_Fillet1dPoint::FilterPoints(GEOMImpl_Fillet1dPoint* thePoint)
+{
+ Standard_Integer a, b;
+ TColStd_SequenceOfReal aDiffs;
+ Standard_Real aY, aY2, aDX = thePoint->GetParam() - myParam;
+ for(a = 1; a <= myV.Length(); a++)
+ {
+ // searching for near point from thePoint
+ Standard_Integer aNear = 0;
+ Standard_Real aDiff = aDX * 10000.;
+ aY = myV.Value(a) + myD.Value(a) * aDX;
+ for(b = 1; b <= thePoint->myV.Length(); b++)
+ {
+ // calculate hypothesis value of the Y2 with the constant first and second derivative
+ aY2 = aY + aDX * (thePoint->myD.Value(b) - myD.Value(a)) / 2.0;
+ if (aNear == 0 || fabs(aY2 - thePoint->myV.Value(b)) < fabs(aDiff))
+ {
+ aNear = b;
+ aDiff = aY2 - thePoint->myV.Value(b);
+ }
+ }//for b...
+
+ if (aNear)
+ {
+ if (myV.Value(a) * thePoint->myV.Value(aNear) > 0)
+ {// the same sign at the same sides of the interval
+ if (myV.Value(a) * myD.Value(a) > 0)
+ {
+ if (fabs(myD.Value(a)) > Precision::Confusion())
+ aNear = 0;
+ }
+ else
+ {
+ if (fabs(myV.Value(a)) > fabs(thePoint->myV.Value(aNear)))
+ if (thePoint->myV.Value(aNear) * thePoint->myD.Value(aNear) < 0 &&
+ fabs(thePoint->myD.Value(aNear)) > Precision::Confusion())
+ {
+ aNear = 0;
+ }
+ }
+ }
+ }
+
+ if (aNear)
+ {
+ if (myV.Value(a) * thePoint->myV.Value(aNear) > 0)
+ {
+ if ((myV.Value(a) + myD.Value(a) * aDX) * myV.Value(a) > Precision::Confusion() &&
+ (thePoint->myV.Value(aNear) + thePoint->myD.Value(aNear) * aDX) * thePoint->myV.Value(aNear) > Precision::Confusion())
+ {
+ aNear = 0;
+ }
+ }
+ }
+
+ if (aNear)
+ {
+ if ( fabs(aDX) < gp::Resolution() || fabs(aDiff / aDX) > 1.e+7)
+ {
+ aNear = 0;
+ }
+ }
+
+ if (aNear == 0)
+ { // there is no near: remove it from the list
+ myV.Remove(a);
+ myD.Remove(a);
+ myValid.Remove(a);
+ a--;
+ }
+ else
+ {
+ Standard_Boolean aFound = Standard_False;
+ for(b = 1; b <= myNear.Length(); b++)
+ {
+ if (myNear.Value(b) == aNear)
+ {
+ if (fabs(aDiffs.Value(b)) < fabs(aDiff))
+ { // return this 'near'
+ aFound = Standard_True;
+ myV.Remove(a);
+ myD.Remove(a);
+ myValid.Remove(a);
+ a--;
+ break;
+ }
+ else
+ { // remove the old 'near'
+ myV.Remove(b);
+ myD.Remove(b);
+ myValid.Remove(b);
+ myNear.Remove(b);
+ aDiffs.Remove(b);
+ a--;
+ break;
+ }
+ }
+ }//for b...
+ if (!aFound)
+ {
+ myNear.Append(aNear);
+ aDiffs.Append(aDiff);
+ }
+ }
+ }//for a...
+}
+
+//=======================================================================
+//function : Copy
+//purpose :
+//=======================================================================
+GEOMImpl_Fillet1dPoint* GEOMImpl_Fillet1dPoint::Copy()
+{
+ GEOMImpl_Fillet1dPoint* aCopy = new GEOMImpl_Fillet1dPoint(myParam);
+ Standard_Integer a;
+ for(a = 1; a <= myV.Length(); a++)
+ {
+ aCopy->myV.Append(myV.Value(a));
+ aCopy->myD.Append(myD.Value(a));
+ aCopy->myValid.Append(myValid.Value(a));
+ }
+ return aCopy;
+}
+
+//=======================================================================
+//function : HasSolution
+//purpose :
+//=======================================================================
+Standard_Integer GEOMImpl_Fillet1dPoint::HasSolution(const Standard_Real theRadius)
+{
+ Standard_Integer a;
+ for(a = 1; a <= myV.Length(); a++)
+ {
+ if (fabs(sqrt(fabs(fabs(myV.Value(a)) + theRadius * theRadius)) - theRadius) < Precision::Confusion() / 10.)
+ return a;
+ }
+ return 0;
+}
+
+//=======================================================================
+//function : RemoveSolution
+//purpose :
+//=======================================================================
+void GEOMImpl_Fillet1dPoint::RemoveSolution(Standard_Integer theIndex)
+{
+ myV.Remove(theIndex);
+ myD.Remove(theIndex);
+ myValid.Remove(theIndex);
+ myNear.Remove(theIndex);
+}
