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