diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index b225aa681b..d64ee08223 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -135,7 +135,10 @@ double ConstraintEqual::grad(double* param) } return scale * deriv; } - +void ConstraintEqual::evaluate() +{ + *param2() = *param1() * ratio; +} // -------------------------------------------------------- // Weighted Linear Combination @@ -639,10 +642,13 @@ ConstraintType ConstraintDifference::getTypeId() { return Difference; } - +double ConstraintDifference::value() +{ + return *param2() - *param1(); +} double ConstraintDifference::error() { - return scale * (*param2() - *param1() - *difference()); + return scale * (value() - *difference()); } double ConstraintDifference::grad(double* param) @@ -659,6 +665,10 @@ double ConstraintDifference::grad(double* param) } return scale * deriv; } +void ConstraintDifference::evaluate() +{ + *difference() = scale * value(); +} // -------------------------------------------------------- @@ -679,13 +689,15 @@ ConstraintType ConstraintP2PDistance::getTypeId() return P2PDistance; } -double ConstraintP2PDistance::error() +double ConstraintP2PDistance::value() { double dx = (*p1x() - *p2x()); double dy = (*p1y() - *p2y()); - double d = sqrt(dx * dx + dy * dy); - double dist = *distance(); - return scale * (d - dist); + return sqrt(dx * dx + dy * dy); +} +double ConstraintP2PDistance::error() +{ + return scale * (value() - *distance()); } double ConstraintP2PDistance::grad(double* param) @@ -754,6 +766,10 @@ double ConstraintP2PDistance::maxStep(MAP_pD_D& dir, double lim) } return lim; } +void ConstraintP2PDistance::evaluate() +{ + *distance() = value(); +} // -------------------------------------------------------- @@ -834,7 +850,13 @@ double ConstraintP2PAngle::maxStep(MAP_pD_D& dir, double lim) } return lim; } +void ConstraintP2PAngle::evaluate() +{ + double dx = (*p2x() - *p1x()); + double dy = (*p2y() - *p1y()); + *angle() = atan2(dy, dx) - da; +} // -------------------------------------------------------- // P2LDistance @@ -856,16 +878,20 @@ ConstraintType ConstraintP2LDistance::getTypeId() return P2LDistance; } -double ConstraintP2LDistance::error() +double ConstraintP2LDistance::value() { double x0 = *p0x(), x1 = *p1x(), x2 = *p2x(); double y0 = *p0y(), y1 = *p1y(), y2 = *p2y(); - double dist = *distance(); double dx = x2 - x1; double dy = y2 - y1; double d = sqrt(dx * dx + dy * dy); // line length - double area = std::abs(-x0 * dy + y0 * dx + x1 * y2 - x2 * y1); - return scale * (area / d - dist); + double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1; + return std::abs(area / d); +} +double ConstraintP2LDistance::error() +{ + double dist = *distance(); + return scale * (value() - dist); } double ConstraintP2LDistance::grad(double* param) @@ -962,6 +988,10 @@ double ConstraintP2LDistance::maxStep(MAP_pD_D& dir, double lim) } return lim; } +void ConstraintP2LDistance::evaluate() +{ + *distance() = value(); +} // -------------------------------------------------------- @@ -1372,6 +1402,24 @@ double ConstraintL2LAngle::maxStep(MAP_pD_D& dir, double lim) } return lim; } +double vectorAngleHelper(double x1, double y1, double x2, double y2) +{ + double a = atan2(y1, x1); + double ca = cos(a); + double sa = sin(a); + double x = x2 * ca + y2 * sa; + double y = -x2 * sa + y2 * ca; + + return atan2(y, x); +} +void ConstraintL2LAngle::evaluate() +{ + double dx1 = (*l1p2x() - *l1p1x()); + double dy1 = (*l1p2y() - *l1p1y()); + double dx2 = (*l2p2x() - *l2p1x()); + double dy2 = (*l2p2y() - *l2p1y()); + *angle() = vectorAngleHelper(dx1, dy1, dx2, dy2); +} // -------------------------------------------------------- @@ -2495,6 +2543,13 @@ double ConstraintAngleViaTwoPoints::grad(double* param) return scale * deriv; } +void ConstraintAngleViaTwoPoints::evaluate() +{ + DeriVector2 n1 = crv1->CalculateNormal(poa1); + DeriVector2 n2 = crv2->CalculateNormal(poa2); + + *angle() = vectorAngleHelper(n1.x, n1.y, n2.x, n2.y); +} // -------------------------------------------------------- // ConstraintAngleViaPointAndParam @@ -2590,6 +2645,14 @@ double ConstraintAngleViaPointAndParam::grad(double* param) return scale * deriv; } +void ConstraintAngleViaPointAndParam::evaluate() +{ + DeriVector2 n1 = crv1->CalculateNormal(cparam()); + DeriVector2 n2 = crv2->CalculateNormal(poa); + + *angle() = vectorAngleHelper(n1.x, n1.y, n2.x, n2.y); +} + // -------------------------------------------------------- // ConstraintAngleViaPointAndTwoParams @@ -2688,6 +2751,13 @@ double ConstraintAngleViaPointAndTwoParams::grad(double* param) return scale * deriv; } +void ConstraintAngleViaPointAndTwoParams::evaluate() +{ + DeriVector2 n1 = crv1->CalculateNormal(cparam1()); + DeriVector2 n2 = crv2->CalculateNormal(cparam2()); + + *angle() = vectorAngleHelper(n1.x, n1.y, n2.x, n2.y); +} // -------------------------------------------------------- @@ -2956,6 +3026,21 @@ void ConstraintC2CDistance::errorgrad(double* err, double* grad, double* param) } } } +void ConstraintC2CDistance::evaluate() +{ + double dx = *c1.center.x - *c2.center.x; + double dy = *c1.center.y - *c2.center.y; + double cdist = std::sqrt(dx * dx + dy * dy); + + auto [smallradius, bigradius] = std::minmax(*c1.rad, *c2.rad); + + if (cdist > bigradius && cdist > smallradius) { + *distance() = cdist - bigradius - smallradius; + } + else { + *distance() = bigradius - smallradius - cdist; + } +} // -------------------------------------------------------- // ConstraintC2LDistance @@ -2986,12 +3071,8 @@ void ConstraintC2LDistance::ReconstructGeomPointers() pvecChangedFlag = false; } -void ConstraintC2LDistance::errorgrad(double* err, double* grad, double* param) +double ConstraintC2LDistance::value(double& deriValue, double* param) { - if (pvecChangedFlag) { - ReconstructGeomPointers(); - } - DeriVector2 ct(circle.center, param); DeriVector2 p1(line.p1, param); DeriVector2 p2(line.p2, param); @@ -3019,7 +3100,18 @@ void ConstraintC2LDistance::errorgrad(double* err, double* grad, double* param) // and a positive value makes it decrease. darea = std::signbit(area) ? -darea : darea; - double dh = (darea - h * dlength) / length; + deriValue = (darea - h * dlength) / length; + + return h; +} +void ConstraintC2LDistance::errorgrad(double* err, double* grad, double* param) +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + double h, dh; + h = value(dh, param); if (err) { if (h < *circle.rad) { @@ -3043,6 +3135,18 @@ void ConstraintC2LDistance::errorgrad(double* err, double* grad, double* param) } } } +void ConstraintC2LDistance::evaluate() +{ + double h, dh; + h = value(dh, nullptr); + + if (h < *circle.rad) { + *distance() = *circle.rad - h; + } + else { + *distance() = h - *circle.rad; + } +} // -------------------------------------------------------- // ConstraintP2CDistance @@ -3073,18 +3177,22 @@ void ConstraintP2CDistance::ReconstructGeomPointers() pvecChangedFlag = false; } +double ConstraintP2CDistance::value(double& deriValue, double* param) +{ + DeriVector2 ct(circle.center, param); + DeriVector2 p(pt, param); + DeriVector2 v_length = ct.subtr(p); + + return v_length.length(deriValue); +} void ConstraintP2CDistance::errorgrad(double* err, double* grad, double* param) { if (pvecChangedFlag) { ReconstructGeomPointers(); } - DeriVector2 ct(circle.center, param); - DeriVector2 p(pt, param); - DeriVector2 v_length = ct.subtr(p); - - double dlength; - double length = v_length.length(dlength); + double length, dlength; + length = value(dlength, param); if (err) { *err = *circle.rad + *distance() - length; @@ -3107,6 +3215,13 @@ void ConstraintP2CDistance::errorgrad(double* err, double* grad, double* param) } } } +void ConstraintP2CDistance::evaluate() +{ + double h, dh; + h = value(dh, nullptr); + + *distance() = (h < *circle.rad) ? *circle.rad - h : h - *circle.rad; +} // -------------------------------------------------------- // ConstraintArcLength @@ -3134,6 +3249,19 @@ ConstraintType ConstraintArcLength::getTypeId() return ArcLength; } +void ConstraintArcLength::normalizedAngles(double& start, double& end) const +{ + end = *arc.endAngle; + start = *arc.startAngle; + + // Assume positive angles and CCW arc + while (start < 0.) { + start += 2. * std::numbers::pi; + } + while (end < start) { + end += 2. * std::numbers::pi; + } +} void ConstraintArcLength::errorgrad(double* err, double* grad, double* param) { if (pvecChangedFlag) { @@ -3141,21 +3269,14 @@ void ConstraintArcLength::errorgrad(double* err, double* grad, double* param) } double rad = *arc.rad; - double endA = *arc.endAngle; - double startA = *arc.startAngle; - // Assume positive angles and CCW arc - while (startA < 0.) { - startA += 2. * std::numbers::pi; - } - while (endA < startA) { - endA += 2. * std::numbers::pi; - } + double startA, endA; + normalizedAngles(startA, endA); + if (err) { *err = rad * (endA - startA) - *distance(); } else if (grad) { if (param == distance()) { - // if constraint is not driving it varies on distance(). *grad = -1.; } else { @@ -3166,5 +3287,11 @@ void ConstraintArcLength::errorgrad(double* err, double* grad, double* param) } } } +void ConstraintArcLength::evaluate() +{ + double startA, endA; + normalizedAngles(startA, endA); + *distance() = (endA - startA) * *arc.rad; +} } // namespace GCS diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h index da1b057317..c6df2b8484 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -202,6 +202,14 @@ public: }; // virtual void grad(MAP_pD_D &deriv); --> TODO: vectorized grad version virtual double maxStep(MAP_pD_D& dir, double lim = 1.); + + // Evaluates the value of the constraint and assigns it to + // the value parameter, called on driven constraints to + // find the parameter of interest without solving + // Note: not implemented for constraints which do not have a value + virtual void evaluate() + {} + // 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. @@ -228,6 +236,7 @@ public: ConstraintType getTypeId() override; double error() override; double grad(double*) override; + void evaluate() override; }; // Center of Gravity @@ -402,12 +411,14 @@ private: { return pvec[2]; } + double value(); public: ConstraintDifference(double* p1, double* p2, double* d); ConstraintType getTypeId() override; double error() override; double grad(double*) override; + void evaluate() override; }; // P2PDistance @@ -434,6 +445,7 @@ private: { return pvec[4]; } + double value(); public: ConstraintP2PDistance(Point& p1, Point& p2, double* d); @@ -445,6 +457,7 @@ public: double error() override; double grad(double*) override; double maxStep(MAP_pD_D& dir, double lim = 1.) override; + void evaluate() override; }; // P2PAngle @@ -483,6 +496,7 @@ public: double error() override; double grad(double*) override; double maxStep(MAP_pD_D& dir, double lim = 1.) override; + void evaluate() override; }; // P2LDistance @@ -517,6 +531,7 @@ private: { return pvec[6]; } + double value(); public: ConstraintP2LDistance(Point& p, Line& l, double* d); @@ -529,6 +544,7 @@ public: double grad(double*) override; double maxStep(MAP_pD_D& dir, double lim = 1.) override; double abs(double darea); + void evaluate() override; }; // PointOnLine @@ -762,6 +778,7 @@ public: double error() override; double grad(double*) override; double maxStep(MAP_pD_D& dir, double lim = 1.) override; + void evaluate() override; }; // MidpointOnLine @@ -1142,6 +1159,7 @@ public: ConstraintType getTypeId() override; double error() override; double grad(double*) override; + void evaluate() override; }; // snell's law angles constrainer. Point needs to lie on all three curves to be constraied. @@ -1223,6 +1241,7 @@ public: ConstraintType getTypeId() override; double error() override; double grad(double*) override; + void evaluate() override; }; // TODO: Do we need point here at all? @@ -1268,6 +1287,7 @@ public: ConstraintType getTypeId() override; double error() override; double grad(double*) override; + void evaluate() override; }; class ConstraintEqualLineLength: public Constraint @@ -1296,6 +1316,7 @@ private: // writes pointers in pvec to the parameters of c1, c2 void ReconstructGeomPointers(); void errorgrad(double* err, double* grad, double* param) override; + void evaluate() override; public: ConstraintC2CDistance(Circle& c1, Circle& c2, double* d); @@ -1314,7 +1335,10 @@ private: } // writes pointers in pvec to the parameters of c, l void ReconstructGeomPointers(); + + double value(double& deriValue, double* param); void errorgrad(double* err, double* grad, double* param) override; + void evaluate() override; public: ConstraintC2LDistance(Circle& c, Line& l, double* d); @@ -1332,7 +1356,9 @@ private: return pvec[0]; } void ReconstructGeomPointers(); // writes pointers in pvec to the parameters of c + double value(double& deriValue, double* param); void errorgrad(double* err, double* grad, double* param) override; + void evaluate() override; public: ConstraintP2CDistance(Point& p, Circle& c, double* d); @@ -1349,7 +1375,9 @@ private: return pvec[0]; } void ReconstructGeomPointers(); // writes pointers in pvec to the parameters of a + void normalizedAngles(double& start, double& end) const; void errorgrad(double* err, double* grad, double* param) override; + void evaluate() override; public: ConstraintArcLength(Arc& a, double* d); diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index eb9ebe668b..ef1762fd17 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -536,6 +536,7 @@ void System::clear() reference.clear(); clearSubSystems(); deleteAllContent(clist); + drivenConstraints.clear(); c2p.clear(); p2c.clear(); } @@ -566,6 +567,9 @@ int System::addConstraint(Constraint* constr) if (constr->getTag() >= 0) { // negatively tagged constraints have no impact hasDiagnosis = false; // on the diagnosis } + if (!constr->isDriving()) { + drivenConstraints.push_back(constr); + } clist.push_back(constr); VEC_pD constr_params = constr->params(); @@ -579,13 +583,11 @@ int System::addConstraint(Constraint* constr) void System::removeConstraint(Constraint* constr) { - std::vector::iterator it; - it = std::ranges::find(clist, constr); - if (it == clist.end()) { + if (std::erase(clist, constr) == 0) { return; } + std::erase(drivenConstraints, constr); - clist.erase(it); if (constr->getTag() >= 0) { hasDiagnosis = false; } @@ -1743,11 +1745,13 @@ void System::initSolution(Algorithm alg) std::vector clistR; if (!redundant.empty()) { std::ranges::copy_if(clist, std::back_inserter(clistR), [this](auto constr) { - return this->redundant.count(constr) == 0; + return this->redundant.count(constr) == 0 && constr->isDriving(); }); } else { - clistR = clist; + std::ranges::copy_if(clist, std::back_inserter(clistR), [this](auto constr) { + return constr->isDriving(); + }); } // partitioning into decoupled components @@ -1834,7 +1838,7 @@ void System::initSolution(Algorithm alg) clists[cid], std::back_inserter(clist0), std::back_inserter(clist1), - [](auto constr) { return constr->getTag() >= 0 && constr->isDriving(); } + [](auto constr) { return constr->getTag() >= 0; } ); if (!clist0.empty()) { @@ -4683,6 +4687,13 @@ void System::applySolution() *(it->first) = *(it->second); } } + evaluateDrivenConstraints(); +} +void System::evaluateDrivenConstraints() +{ + for (auto dconstr : drivenConstraints) { + dconstr->evaluate(); + } } void System::undoSolution() diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index 99fb74a03c..2619abc17a 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/src/Mod/Sketcher/App/planegcs/GCS.h @@ -119,6 +119,7 @@ private: std::vector> pDependentParametersGroups; std::vector clist; + std::vector drivenConstraints; std::map c2p; // constraint to parameter adjacency list std::map> p2c; // parameter to constraint adjacency list @@ -595,6 +596,8 @@ public: int solve(SubSystem* subsysA, SubSystem* subsysB, bool isFine = true, bool isRedundantsolving = false); void applySolution(); + void evaluateDrivenConstraints(); + void undoSolution(); // FIXME: looks like XconvergenceFine is not the solver precision, at least in DogLeg // solver.