--- /dev/null
+// Copyright (c) 1999-2012 OPEN CASCADE SAS
+//
+// The content of this file is subject to the Open CASCADE Technology Public
+// License Version 6.5 (the "License"). You may not use the content of this file
+// except in compliance with the License. Please obtain a copy of the License
+// at http://www.opencascade.org and read it completely before using this file.
+//
+// The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
+// main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
+//
+// The Original Code and all software distributed under the License is
+// distributed on an "AS IS" basis, without warranty of any kind, and the
+// Initial Developer hereby disclaims all such warranties, including without
+// limitation, any warranties of merchantability, fitness for a particular
+// purpose or non-infringement. Please see the License for the specific terms
+// and conditions governing the rights and limitations under the License.
+
+#include <ChFi2d_AnaFilletAlgo.hxx>
+
+#include <gp_Ax3.hxx>
+#include <gp_Circ.hxx>
+#include <gp_Lin2d.hxx>
+#include <gp_Circ2d.hxx>
+
+#include <Standard_TypeMismatch.hxx>
+
+#include <BRepBuilderAPI_MakeEdge.hxx>
+#include <BRepBuilderAPI_MakeWire.hxx>
+#include <BRepBuilderAPI_MakeFace.hxx>
+
+#include <GeomAPI_ExtremaCurveCurve.hxx>
+#include <IntAna2d_AnaIntersection.hxx>
+#include <ShapeAnalysis_Wire.hxx>
+#include <Geom_Circle.hxx>
+
+#include <BRepAdaptor_Curve.hxx>
+#include <BRep_Tool.hxx>
+
+#include <TopoDS.hxx>
+#include <TopoDS_Iterator.hxx>
+
+#include <ProjLib.hxx>
+#include <TopExp.hxx>
+#include <ElSLib.hxx>
+
+// Compute the flag: CW || CCW
+static Standard_Boolean isCW(const BRepAdaptor_Curve& AC)
+{
+ const Standard_Real f = AC.FirstParameter();
+ const Standard_Real l = AC.LastParameter();
+ Handle(Geom_Circle) circle = Handle(Geom_Circle)::DownCast(AC.Curve().Curve());
+ gp_Pnt start = AC.Value(f);
+ gp_Pnt end = AC.Value(l);
+ gp_Pnt center = AC.Circle().Location();
+ gp_Ax3 plane = AC.Circle().Position();
+
+ // Get point on circle at half angle
+ gp_Pnt m;
+ circle->D0(0.5 * (f + l), m);
+
+ // Compare angles between vectors to middle point and to the end point.
+ gp_Vec startv(center, start), endv(center, end), middlev(center, m);
+ double middlea = startv.AngleWithRef(middlev, plane.Direction());
+ while(middlea < 0.0)
+ middlea += 2.0 * M_PI;
+ double enda = startv.AngleWithRef(endv, plane.Direction());
+ while(enda < 0.0)
+ enda += 2.0 * M_PI;
+
+ Standard_Boolean is_cw = middlea > enda ? Standard_True : Standard_False;
+ return is_cw;
+}
+
+// Equality of points computed through square distance between the points.
+static Standard_Boolean IsEqual(const gp_Pnt& p1, const gp_Pnt& p2)
+{
+ return p1.SquareDistance(p2) < Precision::SquareConfusion();
+}
+static Standard_Boolean IsEqual(const gp_Pnt2d& p1, const gp_Pnt2d& p2)
+{
+ return p1.SquareDistance(p2) < Precision::SquareConfusion();
+}
+
+// An empty constructor.
+// Use the method Init() to initialize the class.
+ChFi2d_AnaFilletAlgo::ChFi2d_AnaFilletAlgo()
+{
+
+}
+
+// An constructor.
+// It expects two edges having a common point of type:
+// - segment
+// - arc of circle.
+ChFi2d_AnaFilletAlgo::ChFi2d_AnaFilletAlgo(const TopoDS_Wire& theWire,
+ const gp_Pln& thePlane)
+{
+ Init(theWire, thePlane);
+}
+
+// A constructor.
+// It expects two edges having a common point of type:
+// - segment
+// - arc of circle.
+ChFi2d_AnaFilletAlgo::ChFi2d_AnaFilletAlgo(const TopoDS_Edge& theEdge1,
+ const TopoDS_Edge& theEdge2,
+ const gp_Pln& thePlane)
+{
+ // Make a wire consisting of two edges.
+ Init(theEdge1, theEdge2, thePlane);
+}
+
+// Initializes the class by a wire consisting of two edges.
+void ChFi2d_AnaFilletAlgo::Init(const TopoDS_Wire& theWire, const gp_Pln& thePlane)
+{
+ plane = thePlane;
+ TopoDS_Iterator itr(theWire);
+ for (; itr.More(); itr.Next())
+ {
+ if (e1.IsNull())
+ e1 = TopoDS::Edge(itr.Value());
+ else if (e2.IsNull())
+ e2 = TopoDS::Edge(itr.Value());
+ }
+ if (e1.IsNull() || e2.IsNull())
+ Standard_TypeMismatch::Raise("The algorithm expects a wire consisting of two linear or circular edges.");
+
+ // Left neighbour.
+ BRepAdaptor_Curve AC1(e1);
+ if (AC1.GetType() != GeomAbs_Line && AC1.GetType() != GeomAbs_Circle)
+ Standard_TypeMismatch::Raise("A segment or an arc of circle is expected.");
+
+ TopoDS_Vertex v1, v2;
+ TopExp::Vertices(e1, v1, v2, Standard_True);
+ if (v1.IsNull() || v2.IsNull())
+ Standard_Failure::Raise("An infinite edge.");
+
+ gp_Pnt P1 = BRep_Tool::Pnt(v1);
+ gp_Pnt P2 = BRep_Tool::Pnt(v2);
+ gp_Pnt2d p1 = ProjLib::Project(thePlane, P1);
+ gp_Pnt2d p2 = ProjLib::Project(thePlane, P2);
+ p1.Coord(x11, y11);
+ p2.Coord(x12, y12);
+
+ segment1 = true;
+ if (AC1.GetType() == GeomAbs_Circle)
+ {
+ segment1 = false;
+ gp_Circ c = AC1.Circle();
+
+ gp_Pnt2d loc = ProjLib::Project(thePlane, c.Location());
+ loc.Coord(xc1, yc1);
+
+ radius1 = c.Radius();
+ cw1 = isCW(AC1);
+ }
+
+ // Right neighbour.
+ BRepAdaptor_Curve AC2(e2);
+ if (AC2.GetType() != GeomAbs_Line && AC2.GetType() != GeomAbs_Circle)
+ Standard_TypeMismatch::Raise("A segment or an arc of circle is expected.");
+
+ TopExp::Vertices(e2, v1, v2, Standard_True);
+ if (v1.IsNull() || v2.IsNull())
+ Standard_Failure::Raise("An infinite edge.");
+
+ P1 = BRep_Tool::Pnt(v1);
+ P2 = BRep_Tool::Pnt(v2);
+ p1 = ProjLib::Project(thePlane, P1);
+ p2 = ProjLib::Project(thePlane, P2);
+ p1.Coord(x21, y21);
+ p2.Coord(x22, y22);
+
+ segment2 = true;
+ if (AC2.GetType() == GeomAbs_Circle)
+ {
+ segment2 = false;
+ gp_Circ c = AC2.Circle();
+
+ gp_Pnt2d loc = ProjLib::Project(thePlane, c.Location());
+ loc.Coord(xc2, yc2);
+
+ radius2 = c.Radius();
+ cw2 = isCW(AC2);
+ }
+}
+
+// Initializes the class by two edges.
+void ChFi2d_AnaFilletAlgo::Init(const TopoDS_Edge& theEdge1, const TopoDS_Edge& theEdge2,
+ const gp_Pln& thePlane)
+{
+ // Make a wire consisting of two edges.
+
+ // Get common point.
+ TopoDS_Vertex v11, v12, v21, v22;
+ TopExp::Vertices(theEdge1, v11, v12, Standard_True);
+ TopExp::Vertices(theEdge2, v21, v22, Standard_True);
+ if (v11.IsNull() || v12.IsNull() || v21.IsNull() || v22.IsNull())
+ Standard_Failure::Raise("An infinite edge.");
+
+ gp_Pnt p11 = BRep_Tool::Pnt(v11);
+ gp_Pnt p12 = BRep_Tool::Pnt(v12);
+ gp_Pnt p21 = BRep_Tool::Pnt(v21);
+ gp_Pnt p22 = BRep_Tool::Pnt(v22);
+
+ gp_Pnt pcommon;
+ if (IsEqual(p11, p21) || IsEqual(p11, p22))
+ {
+ pcommon = p11;
+ }
+ else if (IsEqual(p12, p21) || IsEqual(p12, p22))
+ {
+ pcommon = p12;
+ }
+ else
+ Standard_Failure::Raise("The edges have no common point.");
+
+ // Reverse the edges in case of need (to construct a wire).
+ Standard_Boolean is1stReversed(Standard_False), is2ndReversed(Standard_False);
+ if (IsEqual(pcommon, p11))
+ is1stReversed = Standard_True;
+ else if (IsEqual(pcommon, p22))
+ is2ndReversed = Standard_True;
+
+ // Make a wire.
+ BRepBuilderAPI_MakeWire mkWire;
+ if (is1stReversed)
+ mkWire.Add(TopoDS::Edge(theEdge1.Reversed()));
+ else
+ mkWire.Add(theEdge1);
+ if (is2ndReversed)
+ mkWire.Add(TopoDS::Edge(theEdge2.Reversed()));
+ else
+ mkWire.Add(theEdge2);
+ if (!mkWire.IsDone())
+ Standard_Failure::Raise("Can't make a wire.");
+
+ const TopoDS_Wire& W = mkWire.Wire();
+ Init(W, thePlane);
+}
+
+// Calculates a fillet.
+Standard_Boolean ChFi2d_AnaFilletAlgo::Perform(const Standard_Real radius)
+{
+ Standard_Boolean bRet(false);
+ if (e1.IsNull() || e2.IsNull() ||
+ radius < Precision::Confusion())
+ {
+ return bRet;
+ }
+
+ // Fillet definition.
+ Standard_Real xc = 0.0, yc = 0.0;
+ Standard_Real start = 0.0, end = 0.0; // parameters on neighbours
+ Standard_Real xstart = DBL_MAX, ystart = DBL_MAX; // point on left neighbour
+ Standard_Real xend = DBL_MAX, yend = DBL_MAX; // point on right neighbour
+ Standard_Boolean cw = Standard_False;
+
+ // Analytical algorithm works for non-intersecting arcs only.
+ // Check arcs on self-intersection.
+ Standard_Boolean isCut(Standard_False);
+ if (!segment1 || !segment2)
+ {
+ BRepBuilderAPI_MakeWire mkWire(e1, e2);
+ if (mkWire.IsDone())
+ {
+ const TopoDS_Wire& W = mkWire.Wire();
+ BRepBuilderAPI_MakeFace mkFace(plane);
+ if (mkFace.IsDone())
+ {
+ const TopoDS_Face& F = mkFace.Face();
+ ShapeAnalysis_Wire analyzer(W, F, Precision::Confusion());
+ if (analyzer.CheckSelfIntersection() == Standard_True)
+ {
+ // Cut the edges at the point of intersection.
+ isCut = Standard_True;
+ if (!Cut(plane, e1, e2))
+ {
+ return Standard_False;
+ }
+ }
+ }
+ }
+ }// a case of segment - segment
+
+ // Choose the case.
+ BRepAdaptor_Curve AC1(e1), AC2(e2);
+ if (segment1 && segment2)
+ {
+ bRet = SegmentFilletSegment(radius, xc, yc, cw, start, end);
+ }
+ else if (segment1 && !segment2)
+ {
+ bRet = SegmentFilletArc(radius, xc, yc, cw, start, end, xend, yend);
+ }
+ else if (!segment1 && segment2)
+ {
+ bRet = ArcFilletSegment(radius, xc, yc, cw, start, end, xstart, ystart);
+ }
+ else if (!segment1 && !segment2)
+ {
+ bRet = ArcFilletArc(radius, xc, yc, cw, start, end);
+ }
+
+ if (!bRet)
+ return Standard_False;
+
+ // Invert the fillet for left-handed plane.
+ if (plane.Position().Direct() == Standard_False)
+ cw = !cw;
+
+ // Construct a fillet.
+ // Make circle.
+ gp_Pnt center = ElSLib::Value(xc, yc, plane);
+ const gp_Dir& normal = plane.Position().Direction();
+ gp_Circ circ(gp_Ax2(center, cw ? -normal : normal), radius);
+
+ // Fillet may only shrink a neighbour edge, it can't prolongate it.
+ const Standard_Real delta1 = AC1.LastParameter() - AC1.FirstParameter();
+ const Standard_Real delta2 = AC2.LastParameter() - AC2.FirstParameter();
+ if (!isCut && (start > delta1 || end > delta2))
+ {
+ // Check a case when a neighbour edge almost disappears:
+ // try to reduce the fillet radius for a little (1.e-5 mm).
+ const Standard_Real little = 100.0 * Precision::Confusion();
+ const Standard_Real d1 = fabs(start - delta1);
+ const Standard_Real d2 = fabs(end - delta2);
+ if (d1 < little || d2 < little)
+ {
+ if (segment1 && segment2)
+ {
+ bRet = SegmentFilletSegment(radius - little, xc, yc, cw, start, end);
+ }
+ else if (segment1 && !segment2)
+ {
+ bRet = SegmentFilletArc(radius - little, xc, yc, cw, start, end, xend, yend);
+ }
+ else if (!segment1 && segment2)
+ {
+ bRet = ArcFilletSegment(radius - little, xc, yc, cw, start, end, xstart, ystart);
+ }
+ else if (!segment1 && !segment2)
+ {
+ bRet = ArcFilletArc(radius - little, xc, yc, cw, start, end);
+ }
+ if (bRet)
+ {
+ // Invert the fillet for left-handed planes.
+ if (plane.Position().Direct() == Standard_False)
+ cw = !cw;
+
+ // Make the circle again.
+ center = ElSLib::Value(xc, yc, plane);
+ circ.SetLocation(center);
+ circ.SetRadius(radius - little);
+ }
+ else
+ {
+ return Standard_False;
+ }
+ }
+ else
+ {
+ return Standard_False;
+ }
+ }
+ if (bRet)
+ {
+ // start: (xstart, ystart) - pstart.
+ gp_Pnt pstart;
+ if (xstart != DBL_MAX)
+ {
+ pstart = ElSLib::Value(xstart, ystart, plane);
+ }
+ else
+ {
+ if (e1.Orientation() == TopAbs_FORWARD)
+ pstart = AC1.Value(AC1.LastParameter() - start);
+ else
+ pstart = AC1.Value(AC1.FirstParameter() + start);
+ }
+ // end: (xend, yend) -> pend.
+ gp_Pnt pend;
+ if (xend != DBL_MAX)
+ {
+ pend = ElSLib::Value(xend, yend, plane);
+ }
+ else
+ {
+ if (e2.Orientation() == TopAbs_FORWARD)
+ pend = AC2.Value(AC2.FirstParameter() + end);
+ else
+ pend = AC2.Value(AC2.LastParameter() - end);
+ }
+
+ // Make arc.
+ BRepBuilderAPI_MakeEdge mkEdge(circ, pstart, pend);
+ bRet = mkEdge.IsDone();
+ if (bRet)
+ {
+ fillet = mkEdge.Edge();
+
+ // Limit the neighbours.
+ // Left neighbour.
+ shrinke1.Nullify();
+ if (segment1)
+ {
+ BRepBuilderAPI_MakeEdge mkSegment1;
+ if (e1.Orientation() == TopAbs_FORWARD)
+ mkSegment1.Init(AC1.Curve().Curve(), AC1.FirstParameter(), AC1.LastParameter() - start);
+ else
+ mkSegment1.Init(AC1.Curve().Curve(), AC1.FirstParameter() + start, AC1.LastParameter());
+ if (mkSegment1.IsDone())
+ shrinke1 = mkSegment1.Edge();
+ }
+ else
+ {
+ BRepBuilderAPI_MakeEdge mkCirc1;
+ if (e1.Orientation() == TopAbs_FORWARD)
+ mkCirc1.Init(AC1.Curve().Curve(), AC1.FirstParameter(), AC1.LastParameter() - start);
+ else
+ mkCirc1.Init(AC1.Curve().Curve(), AC1.FirstParameter() + start, AC1.LastParameter());
+ if (mkCirc1.IsDone())
+ shrinke1 = mkCirc1.Edge();
+ }
+
+ // Right neighbour.
+ shrinke2.Nullify();
+ if (segment2)
+ {
+ BRepBuilderAPI_MakeEdge mkSegment2;
+ if (e2.Orientation() == TopAbs_FORWARD)
+ mkSegment2.Init(AC2.Curve().Curve(), AC2.FirstParameter() + end, AC2.LastParameter());
+ else
+ mkSegment2.Init(AC2.Curve().Curve(), AC2.FirstParameter(), AC2.LastParameter() - end);
+ if (mkSegment2.IsDone())
+ shrinke2 = mkSegment2.Edge();
+ }
+ else
+ {
+ BRepBuilderAPI_MakeEdge mkCirc2;
+ if (e2.Orientation() == TopAbs_FORWARD)
+ mkCirc2.Init(AC2.Curve().Curve(), AC2.FirstParameter() + end, AC2.LastParameter());
+ else
+ mkCirc2.Init(AC2.Curve().Curve(), AC2.FirstParameter(), AC2.LastParameter() - end);
+ if (mkCirc2.IsDone())
+ shrinke2 = mkCirc2.Edge();
+ }
+
+ bRet = !shrinke1.IsNull() && !shrinke2.IsNull();
+ }// fillet edge is done
+ }// shrinking is good
+
+ return bRet;
+}
+
+// Retrieves a result (fillet and shrinked neighbours).
+const TopoDS_Edge& ChFi2d_AnaFilletAlgo::Result(TopoDS_Edge& e1, TopoDS_Edge& e2)
+{
+ e1 = shrinke1;
+ e2 = shrinke2;
+ return fillet;
+}
+
+// WW5 method to compute fillet.
+// It returns a constructed fillet definition:
+// center point (xc, yc)
+// point on the 1st segment (xstart, ystart)
+// point on the 2nd segment (xend, yend)
+// is the arc of fillet clockwise (cw = true) or counterclockwise (cw = false).
+Standard_Boolean ChFi2d_AnaFilletAlgo::SegmentFilletSegment(const Standard_Real radius,
+ Standard_Real& xc, Standard_Real& yc,
+ Standard_Boolean& cw,
+ Standard_Real& start, Standard_Real& end)
+{
+ // Make normalized vectors at p12.
+ gp_Pnt2d p11(x11, y11);
+ gp_Pnt2d p12(x12, y12);
+ gp_Pnt2d p22(x22, y22);
+
+ // Check length of segments.
+ if (IsEqual(p12, p11) || IsEqual(p12, p22))
+ {
+ return Standard_False;
+ }
+
+ // Make vectors.
+ gp_Vec2d v1(p12, p11);
+ gp_Vec2d v2(p12, p22);
+ v1.Normalize();
+ v2.Normalize();
+
+ // Make bisectrissa.
+ gp_Vec2d bisec = 0.5 * (v1 + v2);
+
+ // Check bisectrissa.
+ if (bisec.SquareMagnitude() < Precision::SquareConfusion())
+ return Standard_False;
+
+ // Normalize the bisectrissa.
+ bisec.Normalize();
+
+ // Angle at bisectrissa.
+ Standard_Real beta = v1.Angle(bisec);
+
+ // Length along the bisectrissa till the center of fillet.
+ Standard_Real L = radius / sin(fabs(beta));
+
+ // Center point of fillet.
+ gp_Pnt2d pc = p12.Translated(L * bisec);
+ pc.Coord(xc, yc);
+
+ // Shrinking length along segments.
+ start = sqrt(L * L - radius * radius);
+ end = start;
+
+ // Orientation of fillet.
+ cw = beta > 0.0;
+ return Standard_True;
+}
+
+// A function constructs a fillet between a segment and an arc.
+Standard_Boolean ChFi2d_AnaFilletAlgo::SegmentFilletArc(const Standard_Real radius,
+ Standard_Real& xc, Standard_Real& yc,
+ Standard_Boolean& cw,
+ Standard_Real& start, Standard_Real& end,
+ Standard_Real& xend, Standard_Real& yend)
+{
+ // Make a line parallel to the segment at the side of center point of fillet.
+ // This side may be defined through making a bisectrissa for vectors at p12 (or p21).
+
+ // Make 2D points.
+ gp_Pnt2d p12(x12, y12);
+ gp_Pnt2d p11(x11, y11);
+ gp_Pnt2d pc2(xc2, yc2);
+
+ // Check length of segment.
+ if (p11.SquareDistance(p12) < gp::Resolution())
+ return Standard_False;
+
+ // Make 2D vectors.
+ gp_Vec2d v1(p12, p11);
+ gp_Vec2d v2(p12, pc2);
+
+ // Rotate the arc vector to become tangential at p21.
+ if (cw2)
+ v2.Rotate(+M_PI_2);
+ else
+ v2.Rotate(-M_PI_2);
+
+ // If vectors coincide (segment and arc are tangent),
+ // the algorithm doesn't work...
+ Standard_Real angle = v1.Angle(v2);
+ if (fabs(angle) < Precision::Angular())
+ return Standard_False;
+
+ // Make a bissectrisa of vectors at p12.
+ v2.Normalize();
+ v1.Normalize();
+ gp_Vec2d bisec = 0.5 * (v1 + v2);
+
+ // If segment and arc look in opposite direction,
+ // no fillet is possible.
+ if (bisec.SquareMagnitude() < gp::Resolution())
+ return Standard_False;
+
+ // Define an appropriate point to choose center of fillet.
+ bisec.Normalize();
+ gp_Pnt2d nearp = p12.Translated(radius * bisec);
+ gp_Lin2d nearl(p12, bisec);
+
+ // Make a line parallel to segment and
+ // passing near the "near" point.
+ gp_Vec2d d1(v1);
+ gp_Lin2d line(p11, -d1);
+ d1.Rotate(M_PI_2);
+ line.Translate(radius * d1);
+ if (line.Distance(nearp) > radius)
+ line.Translate(-2.0 * radius * d1);
+
+ // Make a circle of radius of the arc +/- fillet radius.
+ gp_Ax2d axes(pc2, gp::DX2d());
+ gp_Circ2d circ(axes, radius2 + radius);
+ if (radius2 > radius && circ.Distance(nearp) > radius)
+ circ.SetRadius(radius2 - radius);
+
+ // Calculate intersection of the line and the circle.
+ IntAna2d_AnaIntersection intersector(line, circ);
+ if (!intersector.IsDone() || !intersector.NbPoints())
+ return Standard_False;
+
+ // Find center point of fillet.
+ Standard_Integer i;
+ Standard_Real minDist = DBL_MAX;
+ for (i = 1; i <= intersector.NbPoints(); ++i)
+ {
+ const IntAna2d_IntPoint& intp = intersector.Point(i);
+ const gp_Pnt2d& p = intp.Value();
+
+ Standard_Real d = nearl.Distance(p);
+ if (d < minDist)
+ {
+ minDist = d;
+ p.Coord(xc, yc);
+ }
+ }
+
+ // Shrink of segment.
+ gp_Pnt2d pc(xc, yc);
+ Standard_Real L2 = pc.SquareDistance(p12);
+ const Standard_Real Rf2 = radius * radius;
+ start = sqrt(L2 - Rf2);
+
+ // Shrink of arc.
+ gp_Vec2d pcc(pc2, pc);
+ end = fabs(gp_Vec2d(pc2, p12).Angle(pcc));
+
+ // Duplicate the information on shrink the arc:
+ // calculate a point on the arc coinciding with the end of fillet.
+ line.SetLocation(pc2);
+ line.SetDirection(pcc);
+ circ.SetLocation(pc2);
+ circ.SetRadius(radius2);
+ intersector.Perform(line, circ);
+ if (!intersector.IsDone() || !intersector.NbPoints())
+ return Standard_False;
+
+ xend = DBL_MAX;
+ yend = DBL_MAX;
+ for (i = 1; i <= intersector.NbPoints(); ++i)
+ {
+ const IntAna2d_IntPoint& intp = intersector.Point(i);
+ const gp_Pnt2d& p = intp.Value();
+
+ const Standard_Real d2 = p.SquareDistance(pc);
+ if (fabs(d2 - Rf2) < Precision::Confusion())
+ {
+ p.Coord(xend, yend);
+ break;
+ }
+ }
+
+ // Orientation of the fillet.
+ angle = v1.Angle(v2);
+ cw = angle > 0.0;
+ return Standard_True;
+}
+
+// A function constructs a fillet between an arc and a segment.
+Standard_Boolean ChFi2d_AnaFilletAlgo::ArcFilletSegment(const Standard_Real radius,
+ Standard_Real& xc, Standard_Real& yc,
+ Standard_Boolean& cw,
+ Standard_Real& start, Standard_Real& end,
+ Standard_Real& xstart, Standard_Real& ystart)
+{
+ // Make a line parallel to the segment at the side of center point of fillet.
+ // This side may be defined through making a bisectrissa for vectors at p12 (or p21).
+
+ // Make 2D points.
+ gp_Pnt2d p12(x12, y12);
+ gp_Pnt2d p22(x22, y22);
+ gp_Pnt2d pc1(xc1, yc1);
+
+ // Check length of segment.
+ if (p12.SquareDistance(p22) < gp::Resolution())
+ return Standard_False;
+
+ // Make 2D vectors.
+ gp_Vec2d v1(p12, pc1);
+ gp_Vec2d v2(p12, p22);
+
+ // Rotate the arc vector to become tangential at p21.
+ if (cw1)
+ v1.Rotate(-M_PI_2);
+ else
+ v1.Rotate(+M_PI_2);
+
+ // If vectors coincide (segment and arc are tangent),
+ // the algorithm doesn't work...
+ Standard_Real angle = v1.Angle(v2);
+ if (fabs(angle) < Precision::Angular())
+ return Standard_False;
+
+ // Make a bisectrissa of vectors at p12.
+ v1.Normalize();
+ v2.Normalize();
+ gp_Vec2d bisec = 0.5 * (v1 + v2);
+
+ // If segment and arc look in opposite direction,
+ // no fillet is possible.
+ if (bisec.SquareMagnitude() < gp::Resolution())
+ return Standard_False;
+
+ // Define an appropriate point to choose center of fillet.
+ bisec.Normalize();
+ gp_Pnt2d nearPoint = p12.Translated(radius * bisec);
+ gp_Lin2d nearLine(p12, bisec);
+
+ // Make a line parallel to segment and
+ // passing near the "near" point.
+ gp_Vec2d d2(v2);
+ gp_Lin2d line(p22, -d2);
+ d2.Rotate(M_PI_2);
+ line.Translate(radius * d2);
+ if (line.Distance(nearPoint) > radius)
+ line.Translate(-2.0 * radius * d2);
+
+ // Make a circle of radius of the arc +/- fillet radius.
+ gp_Ax2d axes(pc1, gp::DX2d());
+ gp_Circ2d circ(axes, radius1 + radius);
+ if (radius1 > radius && circ.Distance(nearPoint) > radius)
+ circ.SetRadius(radius1 - radius);
+
+ // Calculate intersection of the line and the big circle.
+ IntAna2d_AnaIntersection intersector(line, circ);
+ if (!intersector.IsDone() || !intersector.NbPoints())
+ return Standard_False;
+
+ // Find center point of fillet.
+ Standard_Integer i;
+ Standard_Real minDist = DBL_MAX;
+ for (i = 1; i <= intersector.NbPoints(); ++i)
+ {
+ const IntAna2d_IntPoint& intp = intersector.Point(i);
+ const gp_Pnt2d& p = intp.Value();
+
+ Standard_Real d = nearLine.Distance(p);
+ if (d < minDist)
+ {
+ minDist = d;
+ p.Coord(xc, yc);
+ }
+ }
+
+ // Shrink of segment.
+ gp_Pnt2d pc(xc, yc);
+ Standard_Real L2 = pc.SquareDistance(p12);
+ const Standard_Real Rf2 = radius * radius;
+ end = sqrt(L2 - Rf2);
+
+ // Shrink of arc.
+ gp_Vec2d pcc(pc1, pc);
+ start = fabs(gp_Vec2d(pc1, p12).Angle(pcc));
+
+ // Duplicate the information on shrink the arc:
+ // calculate a point on the arc coinciding with the start of fillet.
+ line.SetLocation(pc1);
+ line.SetDirection(pcc);
+ circ.SetLocation(pc1);
+ circ.SetRadius(radius1);
+ intersector.Perform(line, circ);
+ if (!intersector.IsDone() || !intersector.NbPoints())
+ return Standard_False;
+
+ xstart = DBL_MAX;
+ ystart = DBL_MAX;
+ for (i = 1; i <= intersector.NbPoints(); ++i)
+ {
+ const IntAna2d_IntPoint& intp = intersector.Point(i);
+ const gp_Pnt2d& p = intp.Value();
+
+ const Standard_Real d2 = p.SquareDistance(pc);
+ if (fabs(d2 - Rf2) < Precision::SquareConfusion())
+ {
+ p.Coord(xstart, ystart);
+ break;
+ }
+ }
+
+ // Orientation of the fillet.
+ angle = v2.Angle(v1);
+ cw = angle < 0.0;
+ return Standard_True;
+}
+
+// WW5 method to compute fillet: arc - arc.
+// It returns a constructed fillet definition:
+// center point (xc, yc)
+// shrinking parameter of the 1st circle (start)
+// shrinking parameter of the 2nd circle (end)
+// if the arc of fillet clockwise (cw = true) or counterclockwise (cw = false).
+Standard_Boolean ChFi2d_AnaFilletAlgo::ArcFilletArc(const Standard_Real radius,
+ Standard_Real& xc, Standard_Real& yc,
+ Standard_Boolean& cw,
+ Standard_Real& start, Standard_Real& end)
+{
+ // Make points.
+ const gp_Pnt2d pc1(xc1, yc1);
+ const gp_Pnt2d pc2(xc2, yc2);
+ const gp_Pnt2d p12(x12, y12);
+
+ // Make vectors at p12.
+ gp_Vec2d v1(pc1, p12);
+ gp_Vec2d v2(pc2, p12);
+
+ // Rotate the vectors so that they are tangent to circles at p12.
+ if (cw1)
+ v1.Rotate(+M_PI_2);
+ else
+ v1.Rotate(-M_PI_2);
+ if (cw2)
+ v2.Rotate(-M_PI_2);
+ else
+ v2.Rotate(+M_PI_2);
+
+ // Make a "check" point for choosing an offset circle.
+ v1.Normalize();
+ v2.Normalize();
+ gp_Vec2d bisec = 0.5 * (v1 + v2);
+ if (bisec.SquareMagnitude() < gp::Resolution())
+ return Standard_False;
+
+ const gp_Pnt2d checkp = p12.Translated(radius * bisec);
+ const gp_Lin2d checkl(p12, bisec);
+
+ // Make two circles of radius r1 +/- r and r2 +/- r
+ // with center point equal to pc1 and pc2.
+ // Arc 1.
+ gp_Ax2d axes(pc1, gp::DX2d());
+ gp_Circ2d c1(axes, radius1 + radius);
+ if (radius1 > radius && c1.Distance(checkp) > radius)
+ c1.SetRadius(radius1 - radius);
+ // Arc 2.
+ axes.SetLocation(pc2);
+ gp_Circ2d c2(axes, radius2 + radius);
+ if (radius2 > radius && c2.Distance(checkp) > radius)
+ c2.SetRadius(radius2 - radius);
+
+ // Calculate an intersection point of these two circles
+ // and choose the one closer to the "check" point.
+ IntAna2d_AnaIntersection intersector(c1, c2);
+ if (!intersector.IsDone() || !intersector.NbPoints())
+ return Standard_False;
+
+ // Find center point of fillet.
+ gp_Pnt2d pc;
+ Standard_Real minDist = DBL_MAX;
+ for (int i = 1; i <= intersector.NbPoints(); ++i)
+ {
+ const IntAna2d_IntPoint& intp = intersector.Point(i);
+ const gp_Pnt2d& p = intp.Value();
+
+ Standard_Real d = checkp.SquareDistance(p);
+ if (d < minDist)
+ {
+ minDist = d;
+ pc = p;
+ }
+ }
+ pc.Coord(xc, yc);
+
+ // Orientation of fillet.
+ Standard_Real angle = v1.Angle(v2);
+ if (fabs(angle) < Precision::Angular())
+ {
+ angle = gp_Vec2d(pc, pc1).Angle(gp_Vec2d(pc, pc2));
+ cw = angle < 0.0;
+ }
+ else
+ {
+ cw = angle > 0.0;
+ }
+
+ // Shrinking of circles.
+ start = fabs(gp_Vec2d(pc1, p12).Angle(gp_Vec2d(pc1, pc)));
+ end = fabs(gp_Vec2d(pc2, p12).Angle(gp_Vec2d(pc2, pc)));
+ return Standard_True;
+}
+
+// Cuts intersecting edges of a contour.
+Standard_Boolean ChFi2d_AnaFilletAlgo::Cut(const gp_Pln& plane, TopoDS_Edge& e1, TopoDS_Edge& e2)
+{
+ gp_Pnt p;
+ Standard_Boolean found(Standard_False);
+ Standard_Real param1 = 0.0, param2 = 0.0;
+ Standard_Real f1, l1, f2, l2;
+ Handle(Geom_Curve) c1 = BRep_Tool::Curve(e1, f1, l1);
+ Handle(Geom_Curve) c2 = BRep_Tool::Curve(e2, f2, l2);
+ GeomAPI_ExtremaCurveCurve extrema(c1, c2, f1, l1, f2, l2);
+ if (extrema.NbExtrema())
+ {
+ Standard_Integer i, nb = extrema.NbExtrema();
+ for (i = 1; i <= nb; ++i)
+ {
+ const Standard_Real d = extrema.Distance(i);
+ if (d < Precision::Confusion())
+ {
+ extrema.Parameters(i, param1, param2);
+ if (fabs(l1 - param1) > Precision::Confusion() &&
+ fabs(f2 - param2) > Precision::Confusion())
+ {
+ found = Standard_True;
+ extrema.Points(i, p, p);
+ break;
+ }
+ }
+ }
+ }
+
+ if (found)
+ {
+ BRepBuilderAPI_MakeEdge mkEdge1(c1, f1, param1);
+ if (mkEdge1.IsDone())
+ {
+ e1 = mkEdge1.Edge();
+
+ BRepBuilderAPI_MakeEdge mkEdge2(c2, param2, l2);
+ if (mkEdge2.IsDone())
+ {
+ e2 = mkEdge2.Edge();
+
+ gp_Pnt2d p2d = ProjLib::Project(plane, p);
+ p2d.Coord(x12, y12);
+ x21 = x12;
+ y21 = y12;
+ return Standard_True;
+ }
+ }
+ }
+ return Standard_False;
+}