From 70eb14ac9c20fa351f3555e9441e0ef9595dbfbc Mon Sep 17 00:00:00 2001 From: Florian Foinant-Willig Date: Thu, 23 Mar 2023 20:53:20 +0100 Subject: [PATCH] Sketcher : Circle to Line Distance Constraint --- src/Mod/Sketcher/App/Sketch.cpp | 24 +++-- src/Mod/Sketcher/App/planegcs/Constraints.cpp | 90 ++++++++++++++++++- src/Mod/Sketcher/App/planegcs/Constraints.h | 21 ++++- src/Mod/Sketcher/App/planegcs/GCS.cpp | 8 ++ src/Mod/Sketcher/App/planegcs/GCS.h | 2 + src/Mod/Sketcher/App/planegcs/Geo.cpp | 6 ++ src/Mod/Sketcher/App/planegcs/Geo.h | 6 +- src/Mod/Sketcher/Gui/CommandConstraints.cpp | 43 ++++++++- .../Gui/EditModeConstraintCoinManager.cpp | 40 +++++++-- 9 files changed, 217 insertions(+), 23 deletions(-) diff --git a/src/Mod/Sketcher/App/Sketch.cpp b/src/Mod/Sketcher/App/Sketch.cpp index 370865f94b..ee16c80229 100644 --- a/src/Mod/Sketcher/App/Sketch.cpp +++ b/src/Mod/Sketcher/App/Sketch.cpp @@ -2753,19 +2753,29 @@ int Sketch::addDistanceConstraint(int geoId1, PointPos pos1, int geoId2, PointPo return -1; } -// circle-circle offset distance constraint +// circle-(circle or line) distance constraint int Sketch::addDistanceConstraint(int geoId1, int geoId2, double * value, bool driving) { - if ((Geoms[geoId1].type == Circle) && (Geoms[geoId2].type == Circle)) { - GCS::Circle &c1 = Circles[Geoms[geoId1].index]; - GCS::Circle &c2 = Circles[Geoms[geoId2].index]; - int tag = ++ConstraintsCounter; - GCSsys.addConstraintC2CDistance(c1, c2, value, tag, driving); - return ConstraintsCounter; + if (Geoms[geoId1].type == Circle) { + if (Geoms[geoId2].type == Circle) { + GCS::Circle &c1 = Circles[Geoms[geoId1].index]; + GCS::Circle &c2 = Circles[Geoms[geoId2].index]; + int tag = ++ConstraintsCounter; + GCSsys.addConstraintC2CDistance(c1, c2, value, tag, driving); + return ConstraintsCounter; + } else if (Geoms[geoId2].type == Line) { + GCS::Circle &c = Circles[Geoms[geoId1].index]; + GCS::Line &l = Lines[Geoms[geoId2].index]; + int tag = ++ConstraintsCounter; + GCSsys.addConstraintC2LDistance(c, l, value, tag, driving); + return ConstraintsCounter; + } } return -1; } + + int Sketch::addRadiusConstraint(int geoId, double * value, bool driving) { geoId = checkGeoId(geoId); diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index fd535db294..5777f911bf 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -840,7 +840,7 @@ double ConstraintP2LDistance::error() double dist = *distance(); double dx = x2 - x1; double dy = y2 - y1; - double d = sqrt(dx * dx + dy * dy); + double d = sqrt(dx * dx + dy * dy); // line length double area = std::abs(-x0 * dy + y0 * dx + x1 * y2 - x2 * y1);// = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area) @@ -2847,4 +2847,92 @@ double ConstraintC2CDistance::grad(double *param) return deriv * scale; } +// -------------------------------------------------------- +// ConstraintC2LDistance +ConstraintC2LDistance::ConstraintC2LDistance(Circle &c, Line &l, double *d) +{ + this->d = d; + pvec.push_back(d); + + this->circle = c; + this->circle.PushOwnParams(pvec); + + this->line = l; + this->line.PushOwnParams(pvec); + + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +ConstraintType ConstraintC2LDistance::getTypeId() +{ + return C2LDistance; +} + +void ConstraintC2LDistance::rescale(double coef) +{ + scale = coef; +} + +void ConstraintC2LDistance::ReconstructGeomPointers() +{ + int i = 0; + i++;// skip the first parameter as there is the inline function distance for it + circle.ReconstructOnNewPvec(pvec, i); + line.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +void ConstraintC2LDistance::errorgrad(double *err, double *grad, double *param) +{ + if (pvecChangedFlag) ReconstructGeomPointers(); + + DeriVector2 ct (circle.center, param); + DeriVector2 p1 (line.p1, param); + DeriVector2 p2 (line.p2, param); + DeriVector2 v_line = p2.subtr(p1); + DeriVector2 v_p1ct = ct.subtr(p1); + + //center to line distance (=h) and its derivative (=dh) + double darea = 0.0; + double area = v_line.crossProdNorm(v_p1ct, darea); //parallelogram oriented area + + double dlength; + double length = v_line.length(dlength); + + double h = std::abs(area) / length; + double dh = (std::copysign(darea, area) - h * dlength) / length; + // + + if (err) { + *err = *distance() + *circle.rad - h; + } + else if (grad) { + if ( param == distance() || param == circle.rad) { + *grad = 1.0; + } else { + *grad = dh; + } + } +} + +double ConstraintC2LDistance::error() +{ + double err; + errorgrad(&err,nullptr,nullptr); + return scale * err; +} + +double ConstraintC2LDistance::grad(double *param) +{ + if (findParamInPvec(param) == -1) + return 0.0; + + double deriv; + errorgrad(nullptr, &deriv, param); + + return deriv * scale; +} + } //namespace GCS diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h index 6f2144e166..3c5d4b3343 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -73,7 +73,8 @@ namespace GCS WeightedLinearCombination = 27, SlopeAtBSplineKnot = 28, PointOnBSpline = 29, - C2CDistance = 30 + C2CDistance = 30, + C2LDistance = 31 }; enum InternalAlignmentType { @@ -763,6 +764,24 @@ namespace GCS double grad(double *) override; }; + // C2LDistance + class ConstraintC2LDistance : public Constraint + { + private: + Circle circle; + Line line; + double *d; + inline double* distance() { return pvec[0]; } + void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of c, l + void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. + public: + ConstraintC2LDistance(Circle &c, Line &l, double *d); + ConstraintType getTypeId() override; + void rescale(double coef=1.) override; + double error() override; + double grad(double *) override; + }; + } //namespace GCS #endif // PLANEGCS_CONSTRAINTS_H diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index 6a44355ff9..26f695af59 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -839,6 +839,14 @@ int System::addConstraintC2CDistance(Circle &c1, Circle &c2, double *dist, int t return addConstraint(constr); } +int System::addConstraintC2LDistance(Circle &c, Line &l, double *dist, int tagId, bool driving) +{ + Constraint *constr = new ConstraintC2LDistance(c, l, dist); + constr->setTag(tagId); + constr->setDriving(driving); + return addConstraint(constr); +} + // derived constraints int System::addConstraintP2PCoincident(Point &p1, Point &p2, int tagId, bool driving) diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index b572244a4b..a4cf60b230 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/src/Mod/Sketcher/App/planegcs/GCS.h @@ -334,6 +334,8 @@ namespace GCS int addConstraintC2CDistance(Circle& c1, Circle& c2, double* dist, int tagId, bool driving = true); + int addConstraintC2LDistance(Circle &c, Line &l, double *dist, int tagId, + bool driving = true); // internal alignment constraints int addConstraintInternalAlignmentPoint2Ellipse(Ellipse& e, Point& p1, diff --git a/src/Mod/Sketcher/App/planegcs/Geo.cpp b/src/Mod/Sketcher/App/planegcs/Geo.cpp index 4a184fe7cc..ff2ddb1d7b 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.cpp +++ b/src/Mod/Sketcher/App/planegcs/Geo.cpp @@ -91,6 +91,12 @@ DeriVector2 DeriVector2::divD(double val, double dval) const x / val, y / val, dx / val - x * dval / (val * val), dy / val - y * dval / (val * val)); } +double DeriVector2::crossProdNorm(const DeriVector2 &v2, double &dprd) const +{ + dprd = dx*v2.y + x*v2.dy - dy*v2.x - y*v2.dx; + return x*v2.y - y*v2.x; +} + DeriVector2 Curve::Value(double /*u*/, double /*du*/, const double* /*derivparam*/) const { assert(false /*Value() is not implemented*/); diff --git a/src/Mod/Sketcher/App/planegcs/Geo.h b/src/Mod/Sketcher/App/planegcs/Geo.h index 6251831351..374d9ca1b5 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.h +++ b/src/Mod/Sketcher/App/planegcs/Geo.h @@ -94,13 +94,16 @@ namespace GCS double length(double& dlength) const;// returns length and writes length deriv into the dlength argument. - // unlike other vectors in FreeCAD, this normalization creates a new vector instead of // modifying existing one. DeriVector2 getNormalized() const;// returns zero vector if the original is zero. double scalarProd(const DeriVector2& v2, double* dprd = nullptr) const;// calculates scalar product of two vectors and returns the result. The derivative // of the result is written into argument dprd. + double crossProdNorm(const DeriVector2& v2, double& dprd) + const;// calculates the norm of the cross product of the two vectors. + // DeriVector2 are considered as 3d vectors with null z. The derivative + // of the result is written into argument dprd. DeriVector2 sum(const DeriVector2& v2) const {// adds two vectors and returns result return DeriVector2(x + v2.x, y + v2.y, dx + v2.dx, dy + v2.dy); @@ -132,7 +135,6 @@ namespace GCS return DeriVector2( x * m1 + v2.x * m2, y * m1 + v2.y * m2, dx * m1 + v2.dx * m2, dy * m1 + v2.dy * m2); } - }; /////////////////////////////////////// diff --git a/src/Mod/Sketcher/Gui/CommandConstraints.cpp b/src/Mod/Sketcher/Gui/CommandConstraints.cpp index 58cb9c2eb1..30bbc45a40 100644 --- a/src/Mod/Sketcher/Gui/CommandConstraints.cpp +++ b/src/Mod/Sketcher/Gui/CommandConstraints.cpp @@ -2290,11 +2290,11 @@ void CmdSketcherConstrainDistance::activated(int iMsg) return; } } - else if (isEdge(GeoId1,PosId1) && isEdge(GeoId2,PosId2)) { // circle to circle distance + else if (isEdge(GeoId1,PosId1) && isEdge(GeoId2,PosId2)) { const Part::Geometry *geom1 = Obj->getGeometry(GeoId1); const Part::Geometry *geom2 = Obj->getGeometry(GeoId2); if (geom1->getTypeId() == Part::GeomCircle::getClassTypeId() - && geom2->getTypeId() == Part::GeomCircle::getClassTypeId() ) { + && geom2->getTypeId() == Part::GeomCircle::getClassTypeId() ) { // circle to circle distance auto circleSeg1 = static_cast(geom1); double radius1 = circleSeg1->getRadius(); Base::Vector3d center1 = circleSeg1->getCenter(); @@ -2336,6 +2336,43 @@ void CmdSketcherConstrainDistance::activated(int iMsg) else finishDatumConstraint (this, Obj, true); + return; + } else if ((geom1->getTypeId() == Part::GeomCircle::getClassTypeId() + && geom2->getTypeId() == Part::GeomLineSegment::getClassTypeId()) + || (geom1->getTypeId() == Part::GeomLineSegment::getClassTypeId() + && geom2->getTypeId() == Part::GeomCircle::getClassTypeId()) ) { // circle to line distance + + if (geom1->getTypeId() == Part::GeomLineSegment::getClassTypeId()){ + std::swap(geom1, geom2); //Assume circle is first + std::swap(GeoId1, GeoId2); + } + + auto circleSeg = static_cast(geom1); + double radius = circleSeg->getRadius(); + Base::Vector3d center = circleSeg->getCenter(); + + auto lineSeg= static_cast(geom2); + Base::Vector3d pnt1 = lineSeg->getStartPoint(); + Base::Vector3d pnt2 = lineSeg->getEndPoint(); + Base::Vector3d d = pnt2 - pnt1; + double ActDist = std::abs(-center.x*d.y+center.y*d.x+pnt1.x*pnt2.y-pnt2.x*pnt1.y) / d.Length() - radius; + + openCommand(QT_TRANSLATE_NOOP("Command", "Add circle to line distance constraint")); + Gui::cmdAppObjectArgs(selection[0].getObject(), + "addConstraint(Sketcher.Constraint('Distance',%d,%d,%f)) ", + GeoId1,GeoId2,ActDist); + + if (arebothpointsorsegmentsfixed || constraintCreationMode==Reference) { // it is a constraint on a external line, make it non-driving + const std::vector &ConStr = Obj->Constraints.getValues(); + + Gui::cmdAppObjectArgs(selection[0].getObject(), + "setDriving(%i,%s)", + ConStr.size()-1,"False"); + finishDatumConstraint (this, Obj, false); + } + else + finishDatumConstraint (this, Obj, true); + return; } } @@ -2467,7 +2504,7 @@ void CmdSketcherConstrainDistance::applyConstraint(std::vector &selSe finishDatumConstraint (this, Obj, true); } else if (geom->getTypeId() == Part::GeomCircle::getClassTypeId()) { - // allow this selection but do nothing as it needs 2 circles + // allow this selection but do nothing as it needs 2 circles or 1 circle and 1 line } else { Gui::TranslatedNotification(Obj, diff --git a/src/Mod/Sketcher/Gui/EditModeConstraintCoinManager.cpp b/src/Mod/Sketcher/Gui/EditModeConstraintCoinManager.cpp index 0f30acd5e7..e9af7e5546 100644 --- a/src/Mod/Sketcher/Gui/EditModeConstraintCoinManager.cpp +++ b/src/Mod/Sketcher/Gui/EditModeConstraintCoinManager.cpp @@ -660,17 +660,39 @@ Restart: pnt1 = geolistfacade.getPoint(Constr->First, Constr->FirstPos); const Part::Geometry *geo = geolistfacade.getGeometryFromGeoId(Constr->Second); - if (geo->getTypeId() == Part::GeomLineSegment::getClassTypeId()) { // point to line distance - const Part::GeomLineSegment *lineSeg = static_cast(geo); - Base::Vector3d l2p1 = lineSeg->getStartPoint(); - Base::Vector3d l2p2 = lineSeg->getEndPoint(); - // calculate the projection of p1 onto line2 - pnt2.ProjectToLine(pnt1-l2p1, l2p2-l2p1); - pnt2 += pnt1; + if (geo->getTypeId() == Part::GeomLineSegment::getClassTypeId()) { + if (Constr->SecondPos != Sketcher::PointPos::none) { // point to line distance + const Part::GeomLineSegment *lineSeg = static_cast(geo); + Base::Vector3d l2p1 = lineSeg->getStartPoint(); + Base::Vector3d l2p2 = lineSeg->getEndPoint(); + // calculate the projection of p1 onto line2 + pnt2.ProjectToLine(pnt1-l2p1, l2p2-l2p1); + pnt2 += pnt1; + } else { + const Part::Geometry *geo1 = geolistfacade.getGeometryFromGeoId(Constr->First); + if (geo1->getTypeId() == Part::GeomCircle::getClassTypeId()) { // circle to line distance + const Part::GeomLineSegment *lineSeg = static_cast(geo); + const Part::GeomCircle *circleSeg = static_cast(geo1); + Base::Vector3d ct = circleSeg->getCenter(); + Base::Vector3d l2p1 = lineSeg->getStartPoint(); + Base::Vector3d l2p2 = lineSeg->getEndPoint(); + double radius = circleSeg->getRadius(); + pnt2.ProjectToLine(ct-l2p1, l2p2-l2p1); //project on the line translated to origin + Base::Vector3d dir = pnt2; + dir.Normalize(); + pnt1 = ct + dir * radius; + pnt2 += ct; + Base::Vector3d d = l2p2 - l2p1; + double ActDist = std::abs(-ct.x*d.y+ct.y*d.x+l2p1.x*l2p2.y-l2p2.x*l2p1.y) / d.Length() - radius; + if (ActDist < 0) { + std::swap(pnt1, pnt2); + } + } + } - } else if (geo->getTypeId() == Part::GeomCircle::getClassTypeId()) { // circle to circle distance + } else if (geo->getTypeId() == Part::GeomCircle::getClassTypeId()) { const Part::Geometry *geo1 = geolistfacade.getGeometryFromGeoId(Constr->First); - if (geo1->getTypeId() == Part::GeomCircle::getClassTypeId()) { + if (geo1->getTypeId() == Part::GeomCircle::getClassTypeId()) { // circle to circle distance const Part::GeomCircle *circleSeg1 = static_cast(geo1); auto circleSeg2 = static_cast(geo); GetCirclesMinimalDistance(circleSeg1, circleSeg2, pnt1, pnt2);