From 1c2e7d74b52308d24dc765881f7dabf7b9e945ef Mon Sep 17 00:00:00 2001 From: Abdullah Tahiri Date: Tue, 19 Jan 2021 19:54:50 +0100 Subject: [PATCH] GCS: New Constraint for equal size lines, avoiding to use an extra parameter ============================================================================ This specific constraint removes the free parameter of the previous implementation. This solves: https://tracker.freecadweb.org/view.php?id=4501 fixes #4501 However, this implementation of equal size produces zero gradients when coordinates of lines are aligned, e.g. vertical or horizontal. These zero gradients, which are mathematically right ruin the diagnosis, which regards corresponding elements as fully constraint (because they are locked from a solver point of view), when they are simply locked, but are movable and constrainable. For this, when the rightful gradient is small enough (<1e-10) it is substituted by a surrogate gradient of 1e-10, which solves the problem with the diagnose, which treats as zero only values under 1e-13 (pivot threshold used in QR decomposition). This special behaviour fixes the wrong detection here: https://forum.freecadweb.org/viewtopic.php?f=8&t=53466&start=40#p464168 It also fixes this one: https://forum.freecadweb.org/viewtopic.php?p=468585#p468587 --- src/Mod/Sketcher/App/planegcs/Constraints.cpp | 167 ++++++++++++++---- src/Mod/Sketcher/App/planegcs/Constraints.h | 38 ++-- 2 files changed, 160 insertions(+), 45 deletions(-) diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index 1000aca769..0829e09cde 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -533,18 +533,18 @@ void ConstraintPointOnPerpBisector::errorgrad(double *err, double *grad, double DeriVector2 p0(Point(p0x(),p0y()), param); DeriVector2 p1(Point(p1x(),p1y()), param); DeriVector2 p2(Point(p2x(),p2y()), param); - + DeriVector2 d1 = p0.subtr(p1); DeriVector2 d2 = p0.subtr(p2); DeriVector2 D = p2.subtr(p1).getNormalized(); - + double projd1, dprojd1; projd1 = d1.scalarProd(D, &dprojd1); - + double projd2, dprojd2; projd2 = d2.scalarProd(D, &dprojd2); - - if (err) + + if (err) *err = projd1+projd2; if (grad) *grad = dprojd1+dprojd2; @@ -964,15 +964,15 @@ void ConstraintPointOnEllipse::rescale(double coef) } double ConstraintPointOnEllipse::error() -{ +{ double X_0 = *p1x(); double Y_0 = *p1y(); double X_c = *cx(); - double Y_c = *cy(); + double Y_c = *cy(); double X_F1 = *f1x(); double Y_F1 = *f1y(); double b = *rmin(); - + double err=sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - 2*Y_c, 2)) - 2*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); @@ -1100,7 +1100,7 @@ double ConstraintEllipseTangentLine::error() } double ConstraintEllipseTangentLine::grad(double *param) -{ +{ //first of all, check that we need to compute anything. if ( findParamInPvec(param) == -1 ) return 0.0; @@ -1218,7 +1218,7 @@ void ConstraintInternalAlignmentPoint2Ellipse::errorgrad(double *err, double *gr } double ConstraintInternalAlignmentPoint2Ellipse::error() -{ +{ double err; errorgrad(&err,0,0); return scale * err; @@ -1226,7 +1226,7 @@ double ConstraintInternalAlignmentPoint2Ellipse::error() } double ConstraintInternalAlignmentPoint2Ellipse::grad(double *param) -{ +{ //first of all, check that we need to compute anything. if ( findParamInPvec(param) == -1 ) return 0.0; @@ -1318,7 +1318,7 @@ void ConstraintInternalAlignmentPoint2Hyperbola::errorgrad(double *err, double * case HyperbolaNegativeMajorY: poa = c.sum(emaj.multD(-a, -da)); by_y_not_by_x = AlignmentType == HyperbolaNegativeMajorY; - break; + break; case HyperbolaPositiveMinorX: case HyperbolaPositiveMinorY: { @@ -1414,7 +1414,7 @@ void ConstraintEqualMajorAxesConic::errorgrad(double *err, double *grad, double } double ConstraintEqualMajorAxesConic::error() -{ +{ double err; errorgrad(&err,0,0); return scale * err; @@ -1490,7 +1490,7 @@ void ConstraintEqualFocalDistance::errorgrad(double *err, double *grad, double * } double ConstraintEqualFocalDistance::error() -{ +{ double err; errorgrad(&err,0,0); return scale * err; @@ -1588,12 +1588,12 @@ double ConstraintCurveValue::grad(double *param) { //first of all, check that we need to compute anything. if ( findParamInPvec(param) == -1 ) return 0.0; - + double deriv; errorgrad(0, &deriv, param); - + return deriv*scale; -} +} double ConstraintCurveValue::maxStep(MAP_pD_D &/*dir*/, double lim) { @@ -1647,15 +1647,15 @@ void ConstraintPointOnHyperbola::rescale(double coef) } double ConstraintPointOnHyperbola::error() -{ +{ double X_0 = *p1x(); double Y_0 = *p1y(); double X_c = *cx(); - double Y_c = *cy(); + double Y_c = *cy(); double X_F1 = *f1x(); double Y_F1 = *f1y(); double b = *rmin(); - + // Full sage worksheet at: // http://forum.freecadweb.org/viewtopic.php?f=10&t=8038&p=110447#p110447 // @@ -1682,7 +1682,7 @@ double ConstraintPointOnHyperbola::grad(double *param) param == f1x() || param == f1y() || param == cx() || param == cy() || param == rmin()) { - + double X_0 = *p1x(); double Y_0 = *p1y(); double X_c = *cx(); @@ -1690,7 +1690,7 @@ double ConstraintPointOnHyperbola::grad(double *param) double X_F1 = *f1x(); double Y_F1 = *f1y(); double b = *rmin(); - + if (param == p1x()) deriv += -(X_0 - X_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + (X_0 + X_F1 - 2*X_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - @@ -1778,26 +1778,26 @@ void ConstraintPointOnParabola::errorgrad(double *err, double *grad, double *par DeriVector2 vertex(this->parab->vertex, param); DeriVector2 point(this->p, param); //point to be constrained to parabola - + DeriVector2 focalvect = focus.subtr(vertex); - + DeriVector2 xdir = focalvect.getNormalized(); - + DeriVector2 point_to_focus = point.subtr(focus); double focal, dfocal; - + focal = focalvect.length(dfocal); - + double pf, dpf; - + pf = point_to_focus.length(dpf); - + double proj, dproj; - + proj = point_to_focus.scalarProd(xdir, &dproj); - - if (err) + + if (err) *err = pf - 2*focal - proj; if (grad) *grad = dpf - 2*dfocal - dproj; @@ -1815,10 +1815,10 @@ double ConstraintPointOnParabola::grad(double *param) { //first of all, check that we need to compute anything. if ( findParamInPvec(param) == -1 ) return 0.0; - + double deriv; errorgrad(0, &deriv, param); - + return deriv*scale; } @@ -2026,5 +2026,104 @@ double ConstraintSnell::grad(double *param) return scale * deriv; } +// ConstraintEqualLineLength +ConstraintEqualLineLength::ConstraintEqualLineLength(Line &l1, Line &l2) +{ + this->l1 = l1; + this->l1.PushOwnParams(pvec); + + this->l2 = l2; + this->l2.PushOwnParams(pvec); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +void ConstraintEqualLineLength::ReconstructGeomPointers() +{ + int i=0; + l1.ReconstructOnNewPvec(pvec, i); + l2.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintEqualLineLength::getTypeId() +{ + return EqualLineLength; +} + +void ConstraintEqualLineLength::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintEqualLineLength::errorgrad(double *err, double *grad, double *param) +{ + if (pvecChangedFlag) ReconstructGeomPointers(); + + DeriVector2 p1 (l1.p1, param); + DeriVector2 p2 (l1.p2, param); + DeriVector2 p3 (l2.p1, param); + DeriVector2 p4 (l2.p2, param); + + DeriVector2 v1 = p1.subtr(p2); + DeriVector2 v2 = p3.subtr(p4); + + double length1, dlength1; + length1 = v1.length(dlength1); + + double length2, dlength2; + length2 = v2.length(dlength2); + + if (err) + *err = length2 - length1; + + if (grad) { + *grad = dlength2 - dlength1; + // if the one of the lines gets vertical or horizontal, the gradients will become zero. this will + // affect the diagnose function and the detection of dependent/independent parameters. + // + // So here we maintain the very small derivative of 1e-10 when the gradient is under such value, such + // that the diagnose function with pivot threshold of 1e-13 treats the value as non-zero and correctly + // detects and can tell appart when a parameter is fully constrained or just locked into a maximum/minimum + if(fabs(*grad) < 1e-10) { + double surrogate = 1e-10; + if( param == l1.p1.x ) + *grad = v1.x > 0 ? surrogate : -surrogate; + if( param == l1.p1.y ) + *grad = v1.y > 0 ? surrogate : -surrogate; + if( param == l1.p2.x ) + *grad = v1.x > 0 ? -surrogate : surrogate; + if( param == l1.p2.y ) + *grad = v1.y > 0 ? -surrogate : surrogate; + if( param == l2.p1.x ) + *grad = v2.x > 0 ? surrogate : -surrogate; + if( param == l2.p1.y ) + *grad = v2.y > 0 ? surrogate : -surrogate; + if( param == l2.p2.x ) + *grad = v2.x > 0 ? -surrogate : surrogate; + if( param == l2.p2.y ) + *grad = v2.y > 0 ? -surrogate : surrogate; + } + } +} + +double ConstraintEqualLineLength::error() +{ + double err; + errorgrad(&err,0,0); + return scale * err; +} + +double ConstraintEqualLineLength::grad(double *param) +{ + if ( findParamInPvec(param) == -1 ) return 0.0; + + double deriv; + errorgrad(0, &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 52e0854e4d..3e07d03d00 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -67,9 +67,10 @@ namespace GCS PointOnHyperbola = 21, InternalAlignmentPoint2Hyperbola = 22, PointOnParabola = 23, - EqualFocalDistance = 24 + EqualFocalDistance = 24, + EqualLineLength = 25 }; - + enum InternalAlignmentType { EllipsePositiveMajorX = 0, EllipsePositiveMajorY = 1, @@ -110,7 +111,7 @@ namespace GCS void revertParams(); void setTag(int tagId) { tag = tagId; } int getTag() { return tag; } - + void setDriving(bool isdriving) { driving = isdriving; } bool isDriving() const { return driving; } @@ -120,11 +121,11 @@ namespace GCS virtual double grad(double *); // virtual void grad(MAP_pD_D &deriv); --> TODO: vectorized grad version virtual double maxStep(MAP_pD_D &dir, double lim=1.); - // Finds first occurrence of param in pvec. This is useful to test if a constraint depends - // on the parameter (it may not actually depend on it, e.g. angle-via-point doesn't depend - // on ellipse's b (radmin), but b will be included within the constraint anyway. + // Finds first occurrence of param in pvec. This is useful to test if a constraint depends + // on the parameter (it may not actually depend on it, e.g. angle-via-point doesn't depend + // on ellipse's b (radmin), but b will be included within the constraint anyway. // Returns -1 if not found. - int findParamInPvec(double* param); + int findParamInPvec(double* param); }; // Equal @@ -413,7 +414,7 @@ namespace GCS virtual double error(); virtual double grad(double *); }; - + class ConstraintEllipseTangentLine : public Constraint { private: @@ -428,7 +429,7 @@ namespace GCS virtual double error(); virtual double grad(double *); }; - + class ConstraintInternalAlignmentPoint2Ellipse : public Constraint { public: @@ -516,7 +517,7 @@ namespace GCS virtual double grad(double *); virtual double maxStep(MAP_pD_D &dir, double lim=1.); }; - + // PointOnHyperbola class ConstraintPointOnHyperbola : public Constraint { @@ -560,7 +561,7 @@ namespace GCS virtual double error(); virtual double grad(double *); }; - + class ConstraintAngleViaPoint : public Constraint { private: @@ -616,6 +617,21 @@ namespace GCS virtual double grad(double *); }; + class ConstraintEqualLineLength : public Constraint + { + private: + Line l1; + Line l2; + void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of line1, line2 + void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. + public: + ConstraintEqualLineLength(Line &l1, Line &l2); + virtual ConstraintType getTypeId(); + virtual void rescale(double coef=1.); + virtual double error(); + virtual double grad(double *); + }; + } //namespace GCS