fillet

最新推荐文章于 2023-11-29 18:45:11 发布

Simon-son

最新推荐文章于 2023-11-29 18:45:11 发布

阅读量198

点赞数

本文链接：https://blog.csdn.net/u014751788/article/details/106929534

版权

```C++
int SketchObject::fillet(int GeoId1, int GeoId2,
                         const Base::Vector3d& refPnt1, const Base::Vector3d& refPnt2,
                         double radius, bool trim)
{
    if (GeoId1 < 0 || GeoId1 > getHighestCurveIndex() ||
        GeoId2 < 0 || GeoId2 > getHighestCurveIndex())
        return -1;

const Part::Geometry *geo1 = getGeometry(GeoId1);
    const Part::Geometry *geo2 = getGeometry(GeoId2);
    if (geo1->getTypeId() == Part::GeomLineSegment::getClassTypeId() &&
        geo2->getTypeId() == Part::GeomLineSegment::getClassTypeId() ) {
        const Part::GeomLineSegment *lineSeg1 = static_cast<const Part::GeomLineSegment*>(geo1);
        const Part::GeomLineSegment *lineSeg2 = static_cast<const Part::GeomLineSegment*>(geo2);

Base::Vector3d filletCenter;
        if (!Part::findFilletCenter(lineSeg1, lineSeg2, radius, refPnt1, refPnt2, filletCenter))
            return -1;
        Base::Vector3d dir1 = lineSeg1->getEndPoint() - lineSeg1->getStartPoint();
        Base::Vector3d dir2 = lineSeg2->getEndPoint() - lineSeg2->getStartPoint();

// the intersection point will and two distances will be necessary later for trimming the lines
        Base::Vector3d intersection, dist1, dist2;

// create arc from known parameters and lines
        int filletId;
        Part::GeomArcOfCircle *arc = Part::createFilletGeometry(lineSeg1, lineSeg2, filletCenter, radius);
        if (arc) {
            // calculate intersection and distances before we invalidate lineSeg1 and lineSeg2
            if (!find2DLinesIntersection(lineSeg1, lineSeg2, intersection)) {
                delete arc;
                return -1;
            }
            dist1.ProjectToLine(arc->getStartPoint(/*emulateCCW=*/true)-intersection, dir1);
            dist2.ProjectToLine(arc->getStartPoint(/*emulateCCW=*/true)-intersection, dir2);
            Part::Geometry *newgeo = arc;
            filletId = addGeometry(newgeo);
            if (filletId < 0) {
                delete arc;
                return -1;
            }
        }
        else
            return -1;

if (trim) {
            PointPos PosId1 = (filletCenter-intersection)*dir1 > 0 ? start : end;
            PointPos PosId2 = (filletCenter-intersection)*dir2 > 0 ? start : end;

delConstraintOnPoint(GeoId1, PosId1, false);
            delConstraintOnPoint(GeoId2, PosId2, false);
            Sketcher::Constraint *tangent1 = new Sketcher::Constraint();
            Sketcher::Constraint *tangent2 = new Sketcher::Constraint();

tangent1->Type = Sketcher::Tangent;
            tangent1->First = GeoId1;
            tangent1->FirstPos = PosId1;
            tangent1->Second = filletId;

tangent2->Type = Sketcher::Tangent;
            tangent2->First = GeoId2;
            tangent2->FirstPos = PosId2;
            tangent2->Second = filletId;

if (dist1.Length() < dist2.Length()) {
                tangent1->SecondPos = start;
                tangent2->SecondPos = end;
                movePoint(GeoId1, PosId1, arc->getStartPoint(/*emulateCCW=*/true),false,true);
                movePoint(GeoId2, PosId2, arc->getEndPoint(/*emulateCCW=*/true),false,true);
            }
            else {
                tangent1->SecondPos = end;
                tangent2->SecondPos = start;
                movePoint(GeoId1, PosId1, arc->getEndPoint(/*emulateCCW=*/true),false,true);
                movePoint(GeoId2, PosId2, arc->getStartPoint(/*emulateCCW=*/true),false,true);
            }

addConstraint(tangent1);
            addConstraint(tangent2);
            delete tangent1;
            delete tangent2;
        }
        delete arc;
        
        if(noRecomputes) // if we do not have a recompute after the geometry creation, the sketch must be solved to update the DoF of the solver
            solve();
        
        return 0;
    }
    else if(geo1->isDerivedFrom(Part::GeomBoundedCurve::getClassTypeId())  &&
            geo2->isDerivedFrom(Part::GeomBoundedCurve::getClassTypeId())) {
        
        auto distancetorefpoints = [](Base::Vector3d ip1, Base::Vector3d ip2, Base::Vector3d ref1, Base::Vector3d ref2) {
                    return (ip1 - ref1).Length() + (ip2 - ref2).Length();
                };
                
        auto selectintersection = [&distancetorefpoints](std::vector<std::pair<Base::Vector3d, Base::Vector3d>> & points,
                                     std::pair<Base::Vector3d, Base::Vector3d>& interpoints,
                                     const Base::Vector3d& refPnt1, const Base::Vector3d& refPnt2) {
        
            if(points.size() == 0) {
                return -1;
            }
            else {
                
                double dist = distancetorefpoints(points[0].first, points[0].second, refPnt1, refPnt2);
                int i = 0, si = 0;
                
                for( auto ipoints : points)
                {
                    double d = distancetorefpoints(ipoints.first, ipoints.second, refPnt1, refPnt2);
                    
                    if (d<dist) {
                        si = i;
                        dist = d;
                    }
                        
                    i++;
                }
                
                interpoints = points[si];
                
                return 0;
            }
        };
                
        // NOTE: While it is not a requirement that the endpoints of the corner to trim are coincident
        //       for GeomTrimmedCurves, it is for GeomBoundedCurves. The reason is that there is no basiscurve
        //       that can be extended to find an intersection.
        //
        //       However, GeomTrimmedCurves sometimes run into problems when trying to calculate the intersection
        //       of basis curves, for example in the case of hyperbola sometimes the cosh goes out of range while
        //       calculating this intersection of basis curves.
        //
        //        Consequently:
        //        i. for GeomBoundedCurves, other than GeomTrimmedCurves, a coincident endpoint is mandatory.
        //        ii. for GeomTrimmedCurves, if there is a coincident endpoint, it is used for the fillet,
        //        iii. for GeomTrimmedCurves, if there is not a coincident endpoint, an intersection of basis curves
        //             is attempted.
        
        const Part::GeomCurve *curve1 = static_cast<const Part::GeomCurve*>(geo1);
        const Part::GeomCurve *curve2 = static_cast<const Part::GeomCurve*>(geo2);
        
        double refparam1;
        double refparam2;
        
        try {
            if(!curve1->closestParameter(refPnt1,refparam1))
                return -1;
        }
        catch (Base::CADKernelError e) {
            e.ReportException();
            THROWM(Base::CADKernelError, "Unable to determine the parameter of the first selected curve at the reference point.")
        }
        
        try {
             if(!curve2->closestParameter(refPnt2,refparam2))
                return -1;
        }
        catch (Base::CADKernelError e) {
            e.ReportException();            
            THROWM(Base::CADKernelError, "Unable to determine the parameter of the second selected curve at the reference point.")
        }
        
        //Base::Console().Log("Ref param: (%f);(%f)",refparam1,refparam2);
        
        std::pair<Base::Vector3d, Base::Vector3d> interpoints;
        std::vector<std::pair<Base::Vector3d, Base::Vector3d>> points;
        
        
        // look for coincident constraints between curves, take the coincident closest to the refpoints
        Sketcher::PointPos curve1PosId = Sketcher::none;
        Sketcher::PointPos curve2PosId = Sketcher::none;
        
        double dist=INFINITY;
        
        const std::vector<Constraint *> &constraints = this->Constraints.getValues();

for (std::vector<Constraint *>::const_iterator it=constraints.begin(); it != constraints.end(); ++it) {
            if ((*it)->Type == Sketcher::Coincident) {
                if ((*it)->First == GeoId1 && (*it)->Second == GeoId2) {
                    Base::Vector3d tmpp1 = getPoint((*it)->First,(*it)->FirstPos);
                    Base::Vector3d tmpp2 = getPoint((*it)->Second,(*it)->SecondPos);
                    double tmpdist = distancetorefpoints(tmpp1, 
                                                         tmpp2,
                                                         refPnt1,
                                                         refPnt2);
                    if(tmpdist < dist) {
                        curve1PosId = (*it)->FirstPos;
                        curve2PosId = (*it)->SecondPos;
                        dist = tmpdist;
                        interpoints = std::make_pair(tmpp1,tmpp2);
                    }
                }
                else if ((*it)->First == GeoId2 && (*it)->Second == GeoId1) {
                    Base::Vector3d tmpp2 = getPoint((*it)->First,(*it)->FirstPos);
                    Base::Vector3d tmpp1 = getPoint((*it)->Second,(*it)->SecondPos);
                    double tmpdist = distancetorefpoints(tmpp1, 
                                                         tmpp2,
                                                         refPnt1,
                                                         refPnt2);
                    if(tmpdist < dist) {                    
                        curve2PosId = (*it)->FirstPos;
                        curve1PosId = (*it)->SecondPos;
                        dist = tmpdist;
                        interpoints = std::make_pair(tmpp1,tmpp2);
                    }
                }
            }
        }
        
        if( curve1PosId == Sketcher::none ) { 
            // no coincident was found, try basis curve intersection if GeomTrimmedCurve
            if( geo1->isDerivedFrom(Part::GeomTrimmedCurve::getClassTypeId()) &&
                geo2->isDerivedFrom(Part::GeomTrimmedCurve::getClassTypeId())) { 
        
            
                const Part::GeomTrimmedCurve *tcurve1 = static_cast<const Part::GeomTrimmedCurve*>(geo1);
                const Part::GeomTrimmedCurve *tcurve2 = static_cast<const Part::GeomTrimmedCurve*>(geo2);
                
                try {
                    if(!tcurve1->intersectBasisCurves(tcurve2,points))
                        return -1;
                }
                catch (Base::CADKernelError e) {
                    e.ReportException();
                    THROWMT(Base::CADKernelError,QT_TRANSLATE_NOOP("Exceptions", "Unable to guess intersection of curves. Try adding a coincident constraint between the vertices of the curves you are intending to fillet."))
                }

int res = selectintersection(points,interpoints,refPnt1, refPnt2);
            
                if(res != 0)
                    return res;
                   
            }
            else
                return -1; // not a GeomTrimmedCurve and no coincident point.
        }

// Now that we know where the curves intersect, get the parameters in the curves of those points
        double intparam1;
        double intparam2;

try {
            if(!curve1->closestParameter(interpoints.first,intparam1))
                return -1;
        }
        catch (Base::CADKernelError e) {
            e.ReportException();
            THROWM(Base::CADKernelError,"Unable to determine the parameter of the first selected curve at the intersection of the curves.")
        }
        
        try {
            if(!curve2->closestParameter(interpoints.second,intparam2))
                return -1;
        }
        catch (Base::CADKernelError e) {
            e.ReportException();
            THROWM(Base::CADKernelError,"Unable to determine the parameter of the second selected curve at the intersection of the curves.")
        }
        
        // get a fillet radius if zero was given
        Base::Vector3d ref21 = refPnt2 - refPnt1;
                
        if(radius == .0f) {
            // guess a radius
            radius = ref21.Length();
        }
        
        // get the starting parameters of each curve
        double spc1 = curve1->getFirstParameter();
        double spc2 = curve2->getFirstParameter(); 
        
        //Base::Console().Log("Start param: (%f);(%f)\n",spc1,spc2);
        
        //Base::Vector3d c1pf = curve1->pointAtParameter(spc1);
        //Base::Vector3d c2pf = curve2->pointAtParameter(spc2);
        
        //Base::Console().Log("start point curves: (%f,%f,%f);(%f,%f,%f)\n",c1pf.x,c1pf.y,c1pf.z,c2pf.x,c2pf.y,c2pf.z);
        
        // We create Offset curves at the suggested radius, the direction of offset is estimated from the tangency vector
        Base::Vector3d tdir1 = curve1->firstDerivativeAtParameter(refparam1);
        Base::Vector3d tdir2 = curve2->firstDerivativeAtParameter(refparam2);

//Base::Console().Log("tangent vectors: (%f,%f,%f);(%f,%f,%f)\n",tdir1.x,tdir1.y,tdir1.z,tdir2.x,tdir2.y,tdir2.z);
        
        //Base::Console().Log("inter-ref vector: (%f,%f,%f)\n",ref21.x,ref21.y,ref21.z);
        
        Base::Vector3d vn(0,0,1);
        
        double sdir1 = tdir1.Cross(ref21).Dot(vn);
        double sdir2 = tdir2.Cross(-ref21).Dot(vn);
        
        //Base::Console().Log("sign of offset: (%f,%f)\n",sdir1,sdir2);
        //Base::Console().Log("radius: %f\n",radius);
        
        Part::GeomOffsetCurve * ocurve1 = new Part::GeomOffsetCurve(Handle(Geom_Curve)::DownCast(curve1->handle()), (sdir1<0)?radius:-radius, vn);
        
        Part::GeomOffsetCurve * ocurve2 = new Part::GeomOffsetCurve(Handle(Geom_Curve)::DownCast(curve2->handle()), (sdir2<0)?radius:-radius, vn);
 
        //Base::Vector3d oc1pf = ocurve1->pointAtParameter(ocurve1->getFirstParameter());
        //Base::Vector3d oc2pf = ocurve2->pointAtParameter(ocurve2->getFirstParameter());
        
        //Base::Console().Log("start point offset curves: (%f,%f,%f);(%f,%f,%f)\n",oc1pf.x,oc1pf.y,oc1pf.z,oc2pf.x,oc2pf.y,oc2pf.z);        
        
        /*auto printoffsetcurve = [](Part::GeomOffsetCurve *c) {
        
            for(double param = c->getFirstParameter(); param < c->getLastParameter(); param = param + (c->getLastParameter()-c->getFirstParameter())/10)
                Base::Console().Log("\n%f: (%f,%f,0)\n", param, c->pointAtParameter(param).x,c->pointAtParameter(param).y);
        
        };
        
        printoffsetcurve(ocurve1);
        printoffsetcurve(ocurve2);*/
            
        // Next we calculate the intersection of offset curves to get the center of the fillet
        std::pair<Base::Vector3d, Base::Vector3d> filletcenterpoint;
        std::vector<std::pair<Base::Vector3d, Base::Vector3d>> offsetintersectionpoints;

try {
            if(!ocurve1->intersect(ocurve2,offsetintersectionpoints))
                return -1;
        }
        catch (Base::CADKernelError e) {
            e.ReportException();          
            THROWM(Base::CADKernelError,"Unable to find intersection between offset curves.")
        }
        
        int res = selectintersection(offsetintersectionpoints,filletcenterpoint,refPnt1, refPnt2);
        
        if(res != 0)
            return res;
        
        double refoparam1;
        double refoparam2;
        
        try {
            if(!curve1->closestParameter(filletcenterpoint.first,refoparam1))
                return -1;
        }     
        catch (Base::CADKernelError e) {
            e.ReportException();
            THROWM(Base::CADKernelError,"Unable to determine the starting point of the arc.")
        }
        
        try {
            if(!curve2->closestParameter(filletcenterpoint.second,refoparam2))
                return -1;
        }
        catch (Base::CADKernelError e) {
            e.ReportException();
            THROWM(Base::CADKernelError,"Unable to determine the end point of the arc.")
        }
        
        // Next we calculate the closest points to the fillet center, so the points where tangency is to be applied
        Base::Vector3d refp1 = curve1->pointAtParameter(refoparam1);
        Base::Vector3d refp2 = curve2->pointAtParameter(refoparam2);
        
        //Base::Console().Log("refpoints: (%f,%f,%f);(%f,%f,%f)",refp1.x,refp1.y,refp1.z,refp2.x,refp2.y,refp2.z);
        
        
        // Now we create arc for the fillet
        double startAngle, endAngle, range;
       
        Base::Vector3d radDir1 = refp1 - filletcenterpoint.first;
        Base::Vector3d radDir2 = refp2 - filletcenterpoint.first;
        
        startAngle = atan2(radDir1.y, radDir1.x);
        
        range = atan2(-radDir1.y*radDir2.x+radDir1.x*radDir2.y,
                    radDir1.x*radDir2.x+radDir1.y*radDir2.y);
        
        endAngle = startAngle + range;

if (endAngle < startAngle)
            std::swap(startAngle, endAngle);

if (endAngle > 2*M_PI )
            endAngle -= 2*M_PI;

if (startAngle < 0 )
            endAngle += 2*M_PI;

// Create Arc Segment
        Part::GeomArcOfCircle *arc = new Part::GeomArcOfCircle();
        arc->setRadius(radDir1.Length());
        arc->setCenter(filletcenterpoint.first);
        arc->setRange(startAngle, endAngle, /*emulateCCWXY=*/true);
        
        // add arc to sketch geometry
        int filletId;
        if (arc) {
            Part::Geometry *newgeo = arc;
            filletId = addGeometry(newgeo);
            if (filletId < 0) {
                delete arc;
                return -1;
            }
        }
        else
            return -1;

if (trim) {
            
            auto selectend = [](double intparam, double refparam, double startparam) {
                if( (intparam>refparam && startparam >= refparam) ||
                    (intparam<refparam && startparam <= refparam) ) {
                        return start;
                }
                else {
                        return end;
                }
            };
            
            // Two cases: 
            // a) there as a coincidence constraint
            // b) we used the basis curve intersection
            
            
            if( curve1PosId == Sketcher::none ) {
                curve1PosId = selectend(intparam1,refoparam1,spc1);
                curve2PosId = selectend(intparam2,refoparam2,spc2);
            }

delConstraintOnPoint(GeoId1, curve1PosId, false);
            delConstraintOnPoint(GeoId2, curve2PosId, false);
            
            
            Sketcher::Constraint *tangent1 = new Sketcher::Constraint();
            Sketcher::Constraint *tangent2 = new Sketcher::Constraint();

tangent1->Type = Sketcher::Tangent;
            tangent1->First = GeoId1;
            tangent1->FirstPos = curve1PosId;
            tangent1->Second = filletId;

tangent2->Type = Sketcher::Tangent;
            tangent2->First = GeoId2;
            tangent2->FirstPos = curve2PosId;
            tangent2->Second = filletId;

double dist1 = (refp1 - arc->getStartPoint(true)).Length();
            double dist2 = (refp1 - arc->getEndPoint(true)).Length();
            
            //Base::Console().Log("dists_refpoint_to_arc_sp_ep: (%f);(%f)",dist1,dist2);
            
            if (dist1 < dist2) {
                tangent1->SecondPos = start;
                tangent2->SecondPos = end;
                movePoint(GeoId1, curve1PosId, arc->getStartPoint(true),false,true);
                movePoint(GeoId2, curve2PosId, arc->getEndPoint(true),false,true);
            }
            else {
                tangent1->SecondPos = end;
                tangent2->SecondPos = start;
                movePoint(GeoId1, curve1PosId, arc->getEndPoint(true),false,true);
                movePoint(GeoId2, curve2PosId, arc->getStartPoint(true),false,true);
            }
            
            addConstraint(tangent1);
            addConstraint(tangent2);
            delete tangent1;
            delete tangent2;
        }
        delete arc;
        delete ocurve1;
        delete ocurve2;
        
        if(noRecomputes) // if we do not have a recompute after the geometry creation, the sketch must be solved to update the DoF of the solver
            solve();
        
        return 0;
    }
    return -1;
}
```