electron-rare/FreeCAD
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sketcher: Evaluate driven constraints rather than solving them (#26632)

Browse Source
This commit is contained in:
theo-vt
2026-02-23 11:14:22 -06:00
committed by GitHub
parent f153960396
commit 92894ea3a8
4 changed files with 209 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/Mod/Sketcher/App/planegcs/Constraints.cpp
+160 -33
View File
@@ -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
src/Mod/Sketcher/App/planegcs/Constraints.h
+28
View File
@@ -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);
src/Mod/Sketcher/App/planegcs/GCS.cpp
+18 -7
View File
@@ -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<Constraint*>::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<Constraint*> 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()
src/Mod/Sketcher/App/planegcs/GCS.h
+3
View File
@@ -119,6 +119,7 @@ private:
std::vector<std::vector<double*>> pDependentParametersGroups;
std::vector<Constraint*> clist;
std::vector<Constraint*> drivenConstraints;
std::map<Constraint*, VEC_pD> c2p; // constraint to parameter adjacency list
std::map<double*, std::vector<Constraint*>> 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.